Merge pull request #65 from rogy-AquaLab/nucleo-newstate

Nucleo newstate

device/nucleo_communicate/README.md

@@ -8,7 +8,7 @@ Nucleoとの通信周り
 
 ## Executables
 
-- `nucleo-chanel`: `Channel`Nodeをspin
+- `nucleo-channel`: `Channel`Nodeをspin
 
 ## Launches
 

device/nucleo_communicate/include/nucleo_communicate/channel.hpp

@@ -5,6 +5,7 @@
 
 #include <packet_interfaces/msg/current.hpp>
 #include <packet_interfaces/msg/flex.hpp>
+#include <packet_interfaces/msg/nucleo_state.hpp>
 #include <packet_interfaces/msg/power.hpp>
 #include <packet_interfaces/msg/voltage.hpp>
 #include <rclcpp/node.hpp>
@@ -21,17 +22,20 @@ class Channel : public rclcpp::Node {
     nucleo_com::SerialPort serial;
     std::mutex             serial_mutex;
 
-    rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr          quit_subscription;
+    rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr initialize_subscription;
+    rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr suspend_subscription;
     rclcpp::Subscription<packet_interfaces::msg::Power>::SharedPtr power_subscription;
 
-    rclcpp::Publisher<packet_interfaces::msg::Flex>::SharedPtr    flex1_publisher;
-    rclcpp::Publisher<packet_interfaces::msg::Flex>::SharedPtr    flex2_publisher;
-    rclcpp::Publisher<packet_interfaces::msg::Current>::SharedPtr current_publisher;
-    rclcpp::Publisher<packet_interfaces::msg::Voltage>::SharedPtr voltage_publisher;
+    rclcpp::Publisher<packet_interfaces::msg::NucleoState>::SharedPtr state_publisher;
+    rclcpp::Publisher<packet_interfaces::msg::Flex>::SharedPtr        flex1_publisher;
+    rclcpp::Publisher<packet_interfaces::msg::Flex>::SharedPtr        flex2_publisher;
+    rclcpp::Publisher<packet_interfaces::msg::Current>::SharedPtr     current_publisher;
+    rclcpp::Publisher<packet_interfaces::msg::Voltage>::SharedPtr     voltage_publisher;
 
     rclcpp::TimerBase::SharedPtr timer;
 
-    void quit_subscription_callback(const std_msgs::msg::Empty& _msg);
+    void initialize_subscription_callback(const std_msgs::msg::Empty& _msg);
+    void suspend_subscription_callback(const std_msgs::msg::Empty& _msg);
     void power_subscription_callback(const packet_interfaces::msg::Power& msg);
 
     void timer_callback();

device/nucleo_communicate/include/nucleo_communicate/nucleo_state.hpp

@@ -0,0 +1,18 @@
+#ifndef NUCLEO_COMMUNICATE_NUCLEO_STATE_HPP
+#define NUCLEO_COMMUNICATE_NUCLEO_STATE_HPP
+
+#include <cstdint>
+
+namespace nucleo_com {
+
+// https://github.com/rogy-AquaLab/2024_umiusi_nucleo/blob/4ccdc0d/include/umiusi/state.hpp
+
+enum class NucleoState: std::uint8_t {
+    INITIALIZING = 0,
+    SUSPEND = 1,
+    RUNNING = 2,
+};
+
+}
+
+#endif // NUCLEO_COMMUNICATE_NUCLEO_STATE_HPP

device/nucleo_communicate/include/nucleo_communicate/serial_port.hpp

@@ -5,6 +5,7 @@
 #include <string>
 #include <termios.h>
 
+#include "nucleo_communicate/nucleo_state.hpp"
 #include "nucleo_communicate/recv_data.hpp"
 #include "nucleo_communicate/send_data.hpp"
 
@@ -26,9 +27,14 @@ class SerialPort {
     // https://github.com/rogy-AquaLab/2024_umiusi_nucleo
     void send(const nucleo_com::SendData& data);
 
+    auto receive_state() -> nucleo_com::NucleoState;
+
     auto receive() -> nucleo_com::RecvData;
 
-    void quit();
+    /// nucleoに初期化命令を送る *SerialPortの初期化ではない
+    void initialize();
+
+    void suspend();
 };
 
 }

device/nucleo_communicate/launch/channel_launch.py

@@ -8,12 +8,14 @@ def generate_launch_description():
         executable="nucleo-channel",
         namespace="device",
         remappings=[
+            ("/device/nucleo_state", "/packet/nucleo_state"),
             ("/device/flex_1", "/packet/sensors/flex_1"),
             ("/device/flex_2", "/packet/sensors/flex_2"),
             ("/device/current", "/packet/sensors/current"),
             ("/device/voltage", "/packet/sensors/voltage"),
             ("/device/power", "/packet/order/power"),
-            ("/device/quit", "/packet/order/quit"),
+            ("/device/initialize", "/packet/order/initialize"),
+            ("/device/suspend", "/packet/order/suspend"),
         ],
     )
     return LaunchDescription([channel])

device/nucleo_communicate/src/channel.cpp

@@ -12,12 +12,20 @@
 
 using packet_interfaces::msg::Current;
 using packet_interfaces::msg::Flex;
+using packet_interfaces::msg::NucleoState;
 using packet_interfaces::msg::Power;
 using packet_interfaces::msg::Voltage;
 
-void channel::Channel::quit_subscription_callback(const std_msgs::msg::Empty&) {
-    this->serial.quit();
-    RCLCPP_INFO(this->get_logger(), "Sent quit order");
+void channel::Channel::initialize_subscription_callback(const std_msgs::msg::Empty&) {
+    const std::lock_guard<std::mutex> _guard(this->serial_mutex);
+    this->serial.initialize();
+    RCLCPP_INFO(this->get_logger(), "Sent initialize order");
+}
+
+void channel::Channel::suspend_subscription_callback(const std_msgs::msg::Empty&) {
+    const std::lock_guard<std::mutex> _guard(this->serial_mutex);
+    this->serial.suspend();
+    RCLCPP_INFO(this->get_logger(), "Sent suspend order");
 }
 
 void channel::Channel::power_subscription_callback(const Power& msg) {
@@ -32,8 +40,16 @@ void channel::Channel::timer_callback() {
     const std::lock_guard<std::mutex> _guard(this->serial_mutex);
 
     RCLCPP_DEBUG(this->get_logger(), "tick");
-    const rclcpp::Time         now  = this->get_clock()->now();
-    const nucleo_com::RecvData data = this->serial.receive();
+    const rclcpp::Time            now        = this->get_clock()->now();
+    const nucleo_com::NucleoState state_data = this->serial.receive_state();
+    const nucleo_com::RecvData    data       = this->serial.receive();
+    {
+        NucleoState state_msg;
+        state_msg.header.frame_id = "nucleo_state";
+        state_msg.header.stamp    = now;
+        state_msg.state           = static_cast<std::uint8_t>(state_data);
+        this->state_publisher->publish(state_msg);
+    }
     {
         Flex flex1_msg;
         flex1_msg.header.frame_id = "nucleo_flex_1";
@@ -68,8 +84,10 @@ channel::Channel::Channel(const rclcpp::NodeOptions& options) :
     rclcpp::Node("channel", options),
     serial("/dev/ttyACM0"),
     serial_mutex(),
-    quit_subscription(nullptr),
+    initialize_subscription(nullptr),
+    suspend_subscription(nullptr),
     power_subscription(nullptr),
+    state_publisher(nullptr),
     flex1_publisher(nullptr),
     flex2_publisher(nullptr),
     current_publisher(nullptr),
@@ -80,20 +98,28 @@ channel::Channel::Channel(const rclcpp::NodeOptions& options) :
 
     this->serial.setup();
 
-    this->quit_subscription = this->create_subscription<std_msgs::msg::Empty>(
-        "quit", 10, std::bind(&channel::Channel::quit_subscription_callback, this, _1)
+    this->initialize_subscription = this->create_subscription<std_msgs::msg::Empty>(
+        "initialize",
+        10,
+        std::bind(&channel::Channel::initialize_subscription_callback, this, _1)
+    );
+    this->suspend_subscription = this->create_subscription<std_msgs::msg::Empty>(
+        "suspend",
+        10,
+        std::bind(&channel::Channel::suspend_subscription_callback, this, _1)
     );
     this->power_subscription = this->create_subscription<Power>(
         "power", 10, std::bind(&channel::Channel::power_subscription_callback, this, _1)
     );
 
+    this->state_publisher   = this->create_publisher<NucleoState>("nucleo_state", 10);
     this->flex1_publisher   = this->create_publisher<Flex>("flex_1", 10);
     this->flex2_publisher   = this->create_publisher<Flex>("flex_2", 10);
     this->current_publisher = this->create_publisher<Current>("current", 10);
     this->voltage_publisher = this->create_publisher<Voltage>("voltage", 10);
 
     this->timer = this->create_wall_timer(
-        40ms, std::bind(&channel::Channel::timer_callback, this)
+        70ms, std::bind(&channel::Channel::timer_callback, this)
     );
 }
 

device/nucleo_communicate/src/serial_port.cpp

@@ -43,6 +43,14 @@ void SerialPort::send(const SendData& data) {
     );
 }
 
+auto SerialPort::receive_state() -> NucleoState {
+    std::uint8_t header = 0x02;
+    fwrite(&header, sizeof(std::uint8_t), 1, this->serial);
+    std::uint8_t recv_data = 0;
+    fread(&recv_data, sizeof(std::uint8_t), 1, this->serial);
+    return static_cast<NucleoState>(recv_data);
+}
+
 auto SerialPort::receive() -> RecvData {
     std::uint8_t header = 0x01;
     fwrite(&header, sizeof(std::uint8_t), 1, this->serial);
@@ -56,7 +64,12 @@ auto SerialPort::receive() -> RecvData {
     return RecvData(std::move(buffer));
 }
 
-void SerialPort::quit() {
+void SerialPort::initialize() {
+    std::uint8_t header = 0xFE;
+    fwrite(&header, sizeof(std::uint8_t), 1, this->serial);
+}
+
+void SerialPort::suspend() {
     std::uint8_t header = 0xFF;
     fwrite(&header, sizeof(std::uint8_t), 1, this->serial);
 }

packet/packet_interfaces/CMakeLists.txt

@@ -20,6 +20,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
     msg/Depth.msg
     msg/Flex.msg
     msg/LedColor.msg
+    msg/NucleoState.msg
     msg/Power.msg
     msg/Voltage.msg
     DEPENDENCIES std_msgs sensor_msgs

packet/packet_interfaces/README.md

@@ -15,4 +15,4 @@ device-app間で受け渡すメッセージ型
 (see also)
 
 - sensor_msgs/msg/Image: カメライメージ (**これはComposedに含めない**)
-- std_msgs/msg/Empty: quit命令の型
+- std_msgs/msg/Empty: nucleoのsuspend, initialize命令の型

packet/packet_interfaces/msg/NucleoState.msg

@@ -0,0 +1,5 @@
+std_msgs/Header header
+
+# nucleoの状態
+# https://github.com/rogy-AquaLab/2024_umiusi_nucleo/tree/4ccdc0d?tab=readme-ov-file#nucleo-status-0x02
+byte state