diff --git a/packet/packet_composed/CMakeLists.txt b/packet/packet_composed/CMakeLists.txt new file mode 100644 index 0000000..233c0f1 --- /dev/null +++ b/packet/packet_composed/CMakeLists.txt @@ -0,0 +1,41 @@ +cmake_minimum_required(VERSION 3.8) +project(packet_composed) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) +find_package(rclcpp REQUIRED) +find_package(packet_interfaces REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) + + add_executable(Composed + src/composed_main.cpp + src/composed.cpp + ) + + ament_target_dependencies(Composed + rclcpp + std_msgs + sensor_msgs + packet_interfaces + ) + + target_include_directories(Composed + PUBLIC + $ + $ + ) + install(TARGETS + Composed + DESTINATION lib/${PROJECT_NAME} + ) + +ament_package() + diff --git a/packet/packet_composed/include/packet_composed/sensorspacket.hpp b/packet/packet_composed/include/packet_composed/sensorspacket.hpp new file mode 100644 index 0000000..d383d10 --- /dev/null +++ b/packet/packet_composed/include/packet_composed/sensorspacket.hpp @@ -0,0 +1,47 @@ +#ifndef PACKET_COMPOSED_SENSORSPACKET_HPP +#define PACKET_COMPOSED_SENSORSPACKET_HPP + +#include +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" + +class Composed : public rclcpp::Node { +private: + rclcpp::Publisher::SharedPtr composed_publisher; + rclcpp::Subscription::SharedPtr depth_subscription; + rclcpp::Subscription::SharedPtr imu_subscription; + rclcpp::Subscription::SharedPtr flex1_subscription; + rclcpp::Subscription::SharedPtr flex2_subscription; + rclcpp::Subscription::SharedPtr current_subscription; + rclcpp::Subscription::SharedPtr voltage_subscription; + + rclcpp::TimerBase::SharedPtr _timer; + packet_interfaces::msg::Composed composed_msg; + + void _loop(); + void depth_topic_callback(const packet_interfaces::msg::Depth& msg); + void imu_topic_callback(const sensor_msgs::msg::Imu& msg); + void flex1_topic_callback(const packet_interfaces::msg::Flex1& msg); + void flex2_topic_callback(const packet_interfaces::msg::Flex2& msg); + void current_topic_callback(const packet_interfaces::msg::current& msg); + void voltage_topic_callback(const packet_interfaces::msg::voltage& msg); + + packet_interfaces::msg::Depth _depth; + sensor_msgs::msg::Imu _imu; + packet_interfaces::msg::Flex _flex1; + packet_interfaces::msg::Flex _flex2; + packet_interfaces::msg::Current _current; + packet_interfaces::msg::Voltage _voltage; + + bool _depth_received; + bool _imu_received; + bool _flex1_received; + bool _flex2_received; + bool _current_received; + bool _voltage_received; + +public: + Composed(); +}; + +#endif // PACKET_COMPOSED_SENSORPACKET_HPP diff --git a/packet/packet_composed/package.xml b/packet/packet_composed/package.xml new file mode 100644 index 0000000..0f77539 --- /dev/null +++ b/packet/packet_composed/package.xml @@ -0,0 +1,17 @@ + + + + packet_composed + 0.0.0 + TODO: Package description + norikonakano + MIT + ament_cmake + rclcpp + std_msgs + sensor_msgs + packet_interfaces + + ament_cmake + + diff --git a/packet/packet_composed/src/composed.cpp b/packet/packet_composed/src/composed.cpp new file mode 100644 index 0000000..2b85384 --- /dev/null +++ b/packet/packet_composed/src/composed.cpp @@ -0,0 +1,107 @@ +#include +#include +#include +#include "packet_composed/sensorspacket.hpp" + + +Composed::Composed() : + rclcpp::Node("composed"), + _timer(), + _depth(), + _imu(), + _flex1(), + _flex2(), + _current(), + _voltage(), + _depth_received(false), + _imu_received(false), + _flex1_received(false), + _flex2_received(false), + _current_received(false), + _voltage_received(false) +{ + + using namespace std::chrono_literals; + composed_publisher = this->create_publisher("sensors_composed", 10); + + depth_subscription = this->create_subscription( + "sensors/depth", 10, std::bind(&Composed::depth_topic_callback, this, std::placeholders::_1)); + + imu_subscription = this->create_subscription( + "sensors/imu", 10, std::bind(&Composed::imu_topic_callback, this, std::placeholders::_1)); + + flex1_subscription = this->create_subscription( + "sensors/flex/1", 10, std::bind(&Composed::flex1_topic_callback, this, std::placeholders::_1)); + + flex2_subscription = this->create_subscription( + "sensors/flex/2", 10, std::bind(&Composed::flex2_topic_callback, this, std::placeholders::_1)); + + current_subscription = this->create_subscription( + "sensors/current", 10, std::bind(&Composed::current_topic_callback, this, std::placeholders::_1)); + + voltage_subscription = this->create_subscription( + "sensors/voltage", 10, std::bind(&Composed::voltage_topic_callback, this, std::placeholders::_1)); + + auto loop = std::bind(&Composed::_loop, this); + _timer = this->create_wall_timer(10ms, loop); +} + +void Composed::_loop() { + + if (_depth_received && _imu_received && _flex1_received && _flex2_received && _current_received && _voltage_received) + { + auto composed_msg = packet_interfaces::msg::Composed(); + composed_msg.depth = _depth; + composed_msg.imu = _imu; + composed_msg.flex1 = _flex1; + composed_msg.flex2 = _flex2; + composed_msg.current = _current; + composed_msg.voltage = _voltage; + + composed_publisher->publish(composed_msg); + + } +} + +void Composed::depth_topic_callback(const packet_interfaces::msg::Depth& msg) +{ + RCLCPP_DEBUG(this->get_logger(), "Received Depth message"); + auto composed_msg = packet_interfaces::msg::Composed(); + _depth = msg; + _depth_received = true; +} + +void Composed::imu_topic_callback(const sensor_msgs::msg::Imu& msg) +{ + RCLCPP_DEBUG(this->get_logger(), "Received IMU message"); + _imu = msg; + _imu_received = true; +} + +void Composed::flex1_topic_callback(const packet_interfaces::msg::Flex1& msg) +{ + RCLCPP_DEBUG(this->get_logger(), "Received Flex1 message"); + _flex1 = msg; + _flex1_received = true; +} + +void Composed::flex2_topic_callback(const packet_interfaces::msg::Flex2& msg) +{ + RCLCPP_DEBUG(this->get_logger(), "Received Flex2 message"); + _flex2 = msg; + _flex2_received = true; +} + +void Composed::current_topic_callback(const packet_interfaces::msg::Current2& msg) +{ + RCLCPP_DEBUG(this->get_logger(), "Received Current message"); + _current = msg; + _current_received = true; +} + +void Composed::voltage_topic_callback(const packet_interfaces::msg::Voltage& msg) +{ + RCLCPP_DEBUG(this->get_logger(), "Received Flex2 message"); + _voltage = msg; + _voltage_received = true; +} diff --git a/packet/packet_composed/src/composed_main.cpp b/packet/packet_composed/src/composed_main.cpp new file mode 100644 index 0000000..e8c76fe --- /dev/null +++ b/packet/packet_composed/src/composed_main.cpp @@ -0,0 +1,11 @@ +#include +#include +#include "packet_composed/sensorspacket.hpp" + +int main(int argc, char * argv[]) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +}