make pacledt-compose'Node'

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(<dependency> 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
+ $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+ $<INSTALL_INTERFACE:include>
+ )
+ install(TARGETS
+ Composed
+ DESTINATION lib/${PROJECT_NAME}
+ )
+
+ament_package()
+

packet/packet_composed/include/packet_composed/sensorspacket.hpp

@@ -0,0 +1,53 @@
+#ifndef PACKET_COMPOSED_SENSORSPACKET_HPP
+#define PACKET_COMPOSED_SENSORSPACKET_HPP
+
+#include <memory>
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
+
+class Composed : public rclcpp::Node {
+private:
+ rclcpp::Publisher<packet_interfaces::msg::Composed>::SharedPtr composed_publisher;
+ rclcpp::Subscription<packet_interfaces::msg::Depth>::SharedPtr depth_subscription;
+ rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_subscription;
+ rclcpp::Subscription<packet_interfaces::msg::Flex>::SharedPtr flex1_subscription;
+ rclcpp::Subscription<packet_interfaces::msg::Flex>::SharedPtr flex2_subscription;
+ rclcpp::Subscription<packet_interfaces::msg::Current>::SharedPtr current_subscription;
+ rclcpp::Subscription<packet_interfaces::msg::Voltage>::SharedPtr voltage_subscription;
+
+ //まとめてpublishするためにタイマーのつもり
+ rclcpp::TimerBase::SharedPtr _timer;
+ size_t _count;
+ 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

packet/packet_composed/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+ <name>packet_composed</name>
+ <version>0.0.0</version>
+ <description>TODO: Package description</description>
+ <maintainer email="[email protected]">norikonakano</maintainer>
+ <license>MIT</license>
+ <buildtool_depend>ament_cmake</buildtool_depend>
+ <depend>rclcpp</depend>
+ <depend>std_msgs</depend>
+ <depend>sensor_msgs</depend>
+ <depend>packet_interfaces</depend>
+ <depend>packet_interfaces</depend>
+ <export>
+ <build_type>ament_cmake</build_type>
+ </export>
+</package>

packet/packet_composed/src/composed.cpp

@@ -0,0 +1,164 @@
+#include <chrono>
+#include <functional>
+#include <string>
+#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)
+ _count(0) 
+{
+ //publisher,subscriptionを作る。
+
+ using namespace std::chrono_literals;
+ composed_publisher = this->create_publisher<packet_interfaces::msg::Composed>("sensors_composed", 10);
+
+ depth_subscription = this->create_subscription<packet_interfaces::msg::Depth>(
+ "sensors/depth", 10, std::bind(&Composed::depth_topic_callback, this, std::placeholders::_1));
+
+ imu_subscription = this->create_subscription<sensor_msgs::msg::Imu>(
+ "sensors/imu", 10, std::bind(&Composed::imu_topic_callback, this, std::placeholders::_1));
+
+ flex1_subscription = this->create_subscription<packet_interfaces::msg::Flex>(
+ "sensors/flex/1", 10, std::bind(&Composed::flex1_topic_callback, this, std::placeholders::_1));
+
+ flex2_subscription = this->create_subscription<packet_interfaces::msg::Flex>(
+ "sensors/flex/2", 10, std::bind(&Composed::flex2_topic_callback, this, std::placeholders::_1));
+
+ current_subscription = this->create_subscription<packet_interfaces::msg::Current>(
+ "sensors/current", 10, std::bind(&Composed::current_topic_callback, this, std::placeholders::_1));
+
+ voltage_subscription = this->create_subscription<packet_interfaces::msg::Voltage>(
+ "sensors/voltage", 10, std::bind(&Composed::, this, std::placeholders::_1));
+
+ auto loop = std::bind(&Composed::_loop, this);
+ _timer = this->create_wall_timer(500ms, loop);
+}
+
+void Composed::_loop() {
+
+ //下のif文のcallbackとtimerのcallbackは非同期に呼ばれるからできなさそう。。。↓
+ 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); 
+
+ // メッセージを再度受信させる。
+ _depth_received = false;
+ _imu_received = false;
+ _flex1_received = false;
+ _flex2_received = false;
+ _current_received = false;
+ _voltage_received = false;
+
+ //文字列はもういらない？？
+ // std::stringstream ss;
+ // ss << "Hello, world! " << _count;
+ // std_msgs::msg::String msg{};
+ // msg.data = ss.str();
+ // RCLCPP_INFO(this->get_logger(), "say %s", ss.str().c_str());
+ // composed_publisher->publish(composed_msg);
+ // _count++;
+ }
+}
+
+void Composed::depth_topic_callback(const packet_interfaces::msg::Depth& msg)
+{
+ RCLCPP_DEBUG(this->get_logger(), "Received Depth message");
+ // Composedメッセージにデータを追加
+ auto composed_msg = packet_interfaces::msg::Composed();
+ // composed_msg.depth = msg;
+
+ _depth = msg; // 受信した深度データを保存 
+ _depth_received = true;
+
+ // 他のデータも追加する
+ // composed_publisher->publish(composed_msg); ここでやらず_loopでする？
+}
+
+void Composed::imu_topic_callback(const sensor_msgs::msg::Imu& msg)
+{
+ RCLCPP_DEBUG(this->get_logger(), "Received IMU message");
+ // Composedメッセージにデータを追加
+ // auto composed_msg = packet_interfaces::msg::Composed();
+ // composed_msg.imu = msg;
+
+ _imu = msg; // 受信したIMUデータを保存
+ _imu_received = true;
+
+ // 他のデータも追加する
+ // composed_publisher->publish(composed_msg); ここでやらず_loopでする？
+}
+
+void Composed::flex1_topic_callback(const packet_interfaces::msg::Flex1& msg)
+{
+ RCLCPP_DEBUG(this->get_logger(), "Received Flex1 message");
+ // Composedメッセージにデータを追加
+ // auto composed_msg = packet_interfaces::msg::Composed();
+ // composed_msg.flex1 = msg;
+
+ _flex1 = msg; // 受信したFlex1データを保存
+ _flex1_received = true; 
+
+ //composed_publisher->publish(composed_msg); ここでやらず_loopでする？
+}
+
+void Composed::flex2_topic_callback(const packet_interfaces::msg::Flex2& msg)
+{
+ RCLCPP_DEBUG(this->get_logger(), "Received Flex2 message");
+ // Composedメッセージにデータを追加
+ // auto composed_msg = packet_interfaces::msg::Composed();
+ // composed_msg.flex2 = msg;
+
+ _flex2 = msg; // 受信したFlex2データを保存
+ _flex2_received = true;
+
+ //composed_publisher->publish(composed_msg); ここでやらず_loopでする？
+}
+
+void Composed::current_topic_callback(const packet_interfaces::msg::Current2& msg)
+{
+ RCLCPP_DEBUG(this->get_logger(), "Received Current message");
+ // Composedメッセージにデータを追加
+ // auto composed_msg = packet_interfaces::msg::Composed();
+ // composed_msg.Current = msg;
+
+ _current = msg; // 受信したCurrentデータを保存
+ _current_received = true;
+
+ //composed_publisher->publish(composed_msg); ここでやらず_loopでする？
+}
+
+void Composed::voltage_topic_callback(const packet_interfaces::msg::Voltage& msg)
+{
+ RCLCPP_DEBUG(this->get_logger(), "Received Flex2 message");
+ // Composedメッセージにデータを追加
+ // auto composed_msg = packet_interfaces::msg::Composed();
+ // composed_msg.voltage = msg;
+
+ _voltage = msg; // 受信したVoltageデータを保存
+ _voltage_received = true;
+
+ //composed_publisher->publish(composed_msg); ここでやらず_loopでする？
+}
+

packet/packet_composed/src/composed_main.cpp

@@ -0,0 +1,11 @@
+#include <memory>
+#include <rclcpp/rclcpp.hpp>
+#include "packet_composed/sensorspacket.hpp" 
+
+int main(int argc, char * argv[]) {
+ rclcpp::init(argc, argv);
+ auto node = std::make_shared<Composed>();
+ rclcpp::spin(node);
+ rclcpp::shutdown();
+ return 0;
+}