make pacledt-compose'Node'

packet/packet_composed/.vscode/c_cpp_properties.json

@@ -0,0 +1,23 @@
+{
+ "configurations": [
+ {
+ "browse": {
+ "databaseFilename": "${default}",
+ "limitSymbolsToIncludedHeaders": false
+ },
+ "includePath": [
+ "/home/norikonakano/24jam_ws/install/packet_interfaces/include/**",
+ "/opt/ros/iron/include/**",
+ "/home/norikonakano/24jam_ws/src/2024_cavolinia/packet/packet_composed/include/**",
+ "include/**",
+ "/usr/include/**"
+ ],
+ "name": "ROS",
+ "intelliSenseMode": "gcc-x64",
+ "compilerPath": "/usr/bin/gcc",
+ "cStandard": "gnu11",
+ "cppStandard": "c++14"
+ }
+ ],
+ "version": 4
+}

packet/packet_composed/.vscode/settings.json

@@ -0,0 +1,16 @@
+{
+ "python.autoComplete.extraPaths": [
+ "/home/norikonakano/24jam_ws/install/pi_i2c/lib/python3.10/site-packages",
+ "/home/norikonakano/24jam_ws/install/nucleo_communicate_py/lib/python3.10/site-packages",
+ "/home/norikonakano/24jam_ws/install/packet_interfaces/lib/python3.10/site-packages",
+ "/home/norikonakano/24jam_ws/install/joystick/lib/python3.10/site-packages",
+ "/opt/ros/iron/lib/python3.10/site-packages"
+ ],
+ "python.analysis.extraPaths": [
+ "/home/norikonakano/24jam_ws/install/pi_i2c/lib/python3.10/site-packages",
+ "/home/norikonakano/24jam_ws/install/nucleo_communicate_py/lib/python3.10/site-packages",
+ "/home/norikonakano/24jam_ws/install/packet_interfaces/lib/python3.10/site-packages",
+ "/home/norikonakano/24jam_ws/install/joystick/lib/python3.10/site-packages",
+ "/opt/ros/iron/lib/python3.10/site-packages"
+ ]
+}

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,40 @@
+#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<std_msgs::msg::String>::SharedPtr _publisher;
+ 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);
+
+
+ // さらにいくつかのメソッド, プロパティが必要になる
+
+public:
+ Composed();
+};
+
+#endif // PACKET_COMPOSED_SENSORPACKET_HPP

packet/packet_composed/package.xml

@@ -0,0 +1,24 @@
+<?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>
+
+ <exec_depend>rclcpp</exec_depend>
+ <exec_depend>std_msgs</exec_depend>
+ <exec_depend>sensor_msgs</exec_depend>
+ <exec_depend>packet_interfaces</exec_depend>
+
+ <export>
+ <build_type>ament_cmake</build_type>
+ </export>
+</package>

packet/packet_composed/src/composed.cpp

@@ -0,0 +1,109 @@
+#include <chrono>
+#include <functional>
+#include <string>
+#include "packet_composed/sensorspacket.hpp"
+
+
+Composed::Composed() :
+ rclcpp::Node("composed"),
+ _publisher(),
+ _timer(), 
+ _count(0) 
+{
+ //composed_publisherをコンストラクタで初期化した。
+ using namespace std::chrono_literals;
+ _publisher = this->create_publisher<std_msgs::msg::String>("/sensors_composed", 10);
+ composed_publisher = this->create_publisher<packet_interfaces::msg::Composed>("composed_topic", 10);
+
+ depth_subscription = this->create_subscription<packet_interfaces::msg::Depth>(
+ "depth_topic", 10, std::bind(&Composed::depth_topic_callback, this, std::placeholders::_1));
+
+ imu_subscription = this->create_subscription<sensor_msgs::msg::Imu>(
+ "imu_topic", 10, std::bind(&Composed::imu_topic_callback, this, std::placeholders::_1));
+
+ flex1_subscription = this->create_subscription<packet_interfaces::msg::Flex>(
+ "flex1_topic", 10, std::bind(&Composed::flex1_topic_callback, this, std::placeholders::_1));
+
+ flex2_subscription = this->create_subscription<packet_interfaces::msg::Flex>(
+ "flex2_topic", 10, std::bind(&Composed::flex2_topic_callback, this, std::placeholders::_1));
+
+ current_subscription = this->create_subscription<packet_interfaces::msg::Current>(
+ "current_topic", 10, std::bind(&Composed::current_topic_callback, this, std::placeholders::_1));
+
+ voltage_subscription = this->create_subscription<packet_interfaces::msg::Voltage>(
+ "voltage_topic", 10, std::bind(&Composed::voltage_topic_callback, this, std::placeholders::_1));
+
+ auto loop = std::bind(&Composed::_loop, this);
+ _timer = this->create_wall_timer(500ms, loop);
+}
+
+void Composed::_loop() {
+ 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());
+ _publisher->publish(msg);
+ _count++;
+}
+
+void Composed::depth_topic_callback(const packet_interfaces::msg::Depth& msg)
+{
+ RCLCPP_INFO(this->get_logger(), "Received Depth message");
+ // Composedメッセージにデータを追加
+ auto composed_msg = packet_interfaces::msg::Composed();
+ composed_msg.depth = msg;
+ // 他のデータも追加する
+ composed_publisher->publish(composed_msg);
+}
+
+void Composed::imu_topic_callback(const sensor_msgs::msg::Imu& msg)
+{
+ RCLCPP_INFO(this->get_logger(), "Received IMU message");
+ // Composedメッセージにデータを追加
+ auto composed_msg = packet_interfaces::msg::Composed();
+ composed_msg.imu = msg;
+ // 他のデータも追加する
+ composed_publisher->publish(composed_msg);
+}
+
+void Composed::flex1_topic_callback(const packet_interfaces::msg::Flex1& msg)
+{
+ RCLCPP_INFO(this->get_logger(), "Received Flex1 message");
+ // Composedメッセージにデータを追加
+ auto composed_msg = packet_interfaces::msg::Composed();
+ composed_msg.flex1 = msg;
+ // 他のデータも追加する
+ composed_publisher->publish(composed_msg);
+}
+
+void Composed::flex2_topic_callback(const packet_interfaces::msg::Flex2& msg)
+{
+ RCLCPP_INFO(this->get_logger(), "Received Flex2 message");
+ // Composedメッセージにデータを追加
+ auto composed_msg = packet_interfaces::msg::Composed();
+ composed_msg.flex2 = msg;
+ // 他のデータも追加する
+ composed_publisher->publish(composed_msg);
+}
+
+void Composed::current_topic_callback(const packet_interfaces::msg::Current2& msg)
+{
+ RCLCPP_INFO(this->get_logger(), "Received Current message");
+ // Composedメッセージにデータを追加
+ auto composed_msg = packet_interfaces::msg::Composed();
+ composed_msg.Current = msg;
+ // 他のデータも追加する
+ composed_publisher->publish(composed_msg);
+}
+
+void Composed::voltage_topic_callback(const packet_interfaces::msg::Voltage& msg)
+{
+ RCLCPP_INFO(this->get_logger(), "Received Flex2 message");
+ // Composedメッセージにデータを追加
+ auto composed_msg = packet_interfaces::msg::Composed();
+ composed_msg.voltage = msg;
+ // 他のデータも追加する
+ composed_publisher->publish(composed_msg);
+}
+

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;
+}