Read echoed header

rogy-AquaLab · Aug 24, 2024 · 132d6f8 · 132d6f8
1 parent db3248b
commit 132d6f8
Show file tree

Hide file tree

Showing 5 changed files with 89 additions and 44 deletions.
diff --git a/device/nucleo_communicate/include/nucleo_communicate/channel.hpp b/device/nucleo_communicate/include/nucleo_communicate/channel.hpp
@@ -2,6 +2,7 @@
 #define NUCLEO_COMMUNICATE_CHANNEL_HPP
 
 #include <mutex>
+#include <optional>
 
 #include <packet_interfaces/msg/current.hpp>
 #include <packet_interfaces/msg/flex.hpp>
@@ -22,6 +23,8 @@ class Channel : public rclcpp::Node {
     nucleo_com::SerialPort serial;
     std::mutex             serial_mutex;
 
+    std::optional<rclcpp::Time> order_failed_at;
+
     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;

diff --git a/device/nucleo_communicate/include/nucleo_communicate/serial_port.hpp b/device/nucleo_communicate/include/nucleo_communicate/serial_port.hpp
@@ -1,6 +1,7 @@
 #ifndef NUCLEO_COMMUNICATE_SERIAL_PORT_HPP
 #define NUCLEO_COMMUNICATE_SERIAL_PORT_HPP
 
+#include <optional>
 #include <stdio.h>
 #include <string>
 #include <termios.h>
@@ -25,16 +26,16 @@ class SerialPort {
     void setup();
 
     // https://github.com/rogy-AquaLab/2024_umiusi_nucleo
-    void send(const nucleo_com::SendData& data);
+    bool send(const nucleo_com::SendData& data);
 
-    auto receive_state() -> nucleo_com::NucleoState;
+    auto receive_state() -> std::optional<nucleo_com::NucleoState>;
 
-    auto receive() -> nucleo_com::RecvData;
+    auto receive() -> std::optional<nucleo_com::RecvData>;
 
     /// nucleoに初期化命令を送る *SerialPortの初期化ではない
-    void initialize();
+    bool initialize();
 
-    void suspend();
+    bool suspend();
 };
 
 }

diff --git a/device/nucleo_communicate/src/channel.cpp b/device/nucleo_communicate/src/channel.cpp
@@ -1,6 +1,8 @@
 #include <chrono>
 #include <functional>
 #include <mutex>
+#include <optional>
+#include <rclcpp/logging.hpp>
 #include <utility>
 
 #include <rclcpp/time.hpp>
@@ -18,64 +20,81 @@ using packet_interfaces::msg::Voltage;
 
 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");
+    const bool                        success = this->serial.initialize();
+    if (success) {
+        RCLCPP_INFO(this->get_logger(), "Sent initialize order");
+    } else {
+        RCLCPP_ERROR(this->get_logger(), "Failed to send 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");
+    const bool                        success = this->serial.suspend();
+    if (success) {
+        RCLCPP_INFO(this->get_logger(), "Sent suspend order");
+    } else {
+        RCLCPP_ERROR(this->get_logger(), "Failed to send suspend order");
+    }
 }
 
 void channel::Channel::power_subscription_callback(const Power& msg) {
     const std::lock_guard<std::mutex> _guard(this->serial_mutex);
 
-    const nucleo_com::SendData data = nucleo_com::SendData::from_msg(msg);
-    this->serial.send(data);
-    RCLCPP_INFO(this->get_logger(), "Sent power order");
+    const nucleo_com::SendData data    = nucleo_com::SendData::from_msg(msg);
+    const bool                 success = this->serial.send(data);
+    if (success) {
+        RCLCPP_INFO(this->get_logger(), "Sent power order");
+    } else {
+        RCLCPP_ERROR(this->get_logger(), "Failed to send power order");
+    }
 }
 
 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::NucleoState state_data = this->serial.receive_state();
-    const nucleo_com::RecvData    data       = this->serial.receive();
-    {
+    const rclcpp::Time now = this->get_clock()->now();
+
+    const std::optional<nucleo_com::NucleoState> state_data
+        = this->serial.receive_state();
+    if (state_data.has_value()) {
         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);
+        state_msg.state           = static_cast<std::uint8_t>(state_data.value());
         this->state_publisher->publish(state_msg);
     }
+    const std::optional<nucleo_com::RecvData> data = this->serial.receive();
+    if (!data.has_value()) {
+        return;
+    }
     {
         Flex flex1_msg;
         flex1_msg.header.frame_id = "nucleo_flex_1";
         flex1_msg.header.stamp    = now;
-        flex1_msg.value           = data.flex1_value();
+        flex1_msg.value           = data->flex1_value();
         this->flex1_publisher->publish(std::move(flex1_msg));
     }
     {
         Flex flex2_msg;
         flex2_msg.header.frame_id = "nucleo_flex_2";
         flex2_msg.header.stamp    = now;
-        flex2_msg.value           = data.flex2_value();
+        flex2_msg.value           = data->flex2_value();
         this->flex2_publisher->publish(std::move(flex2_msg));
     }
     {
         Current current_msg;
         current_msg.header.frame_id = "nucleo_current";
         current_msg.header.stamp    = now;
-        current_msg.value           = data.current_value();
+        current_msg.value           = data->current_value();
         this->current_publisher->publish(std::move(current_msg));
     }
     {
         Voltage voltage_msg;
         voltage_msg.header.frame_id = "nucleo_voltage";
         voltage_msg.header.stamp    = now;
-        voltage_msg.value           = data.voltage_value();
+        voltage_msg.value           = data->voltage_value();
         this->voltage_publisher->publish(std::move(voltage_msg));
     }
 }
@@ -84,6 +103,7 @@ channel::Channel::Channel(const rclcpp::NodeOptions& options) :
     rclcpp::Node("channel", options),
     serial("/dev/ttyACM0"),
     serial_mutex(),
+    order_failed_at(std::nullopt),
     initialize_subscription(nullptr),
     suspend_subscription(nullptr),
     power_subscription(nullptr),

diff --git a/device/nucleo_communicate/src/send_data.cpp b/device/nucleo_communicate/src/send_data.cpp
@@ -5,22 +5,24 @@ using nucleo_com::SendData;
 SendData::SendData(SendData::BufferType&& buf) : buffer(buf) {}
 
 auto SendData::from_msg(const packet_interfaces::msg::Power& msg) -> SendData {
-    return SendData({
-        static_cast<std::uint8_t>((msg.bldc[0] >> 0) & 0xFF),
-        static_cast<std::uint8_t>((msg.bldc[0] >> 8) & 0xFF),
-        static_cast<std::uint8_t>((msg.bldc[1] >> 0) & 0xFF),
-        static_cast<std::uint8_t>((msg.bldc[1] >> 8) & 0xFF),
-        static_cast<std::uint8_t>((msg.bldc[2] >> 0) & 0xFF),
-        static_cast<std::uint8_t>((msg.bldc[2] >> 8) & 0xFF),
-        static_cast<std::uint8_t>((msg.bldc[3] >> 0) & 0xFF),
-        static_cast<std::uint8_t>((msg.bldc[3] >> 8) & 0xFF),
-        static_cast<std::uint8_t>((msg.servo[0] >> 0) & 0xFF),
-        static_cast<std::uint8_t>((msg.servo[0] >> 8) & 0xFF),
-        static_cast<std::uint8_t>((msg.servo[1] >> 0) & 0xFF),
-        static_cast<std::uint8_t>((msg.servo[1] >> 8) & 0xFF),
-        static_cast<std::uint8_t>((msg.servo[2] >> 0) & 0xFF),
-        static_cast<std::uint8_t>((msg.servo[2] >> 8) & 0xFF),
-        static_cast<std::uint8_t>((msg.servo[3] >> 0) & 0xFF),
-        static_cast<std::uint8_t>((msg.servo[3] >> 8) & 0xFF),
-    });
+    return SendData(
+        {
+            static_cast<std::uint8_t>((msg.bldc[0] >> 0) & 0xFF),
+            static_cast<std::uint8_t>((msg.bldc[0] >> 8) & 0xFF),
+            static_cast<std::uint8_t>((msg.bldc[1] >> 0) & 0xFF),
+            static_cast<std::uint8_t>((msg.bldc[1] >> 8) & 0xFF),
+            static_cast<std::uint8_t>((msg.bldc[2] >> 0) & 0xFF),
+            static_cast<std::uint8_t>((msg.bldc[2] >> 8) & 0xFF),
+            static_cast<std::uint8_t>((msg.bldc[3] >> 0) & 0xFF),
+            static_cast<std::uint8_t>((msg.bldc[3] >> 8) & 0xFF),
+            static_cast<std::uint8_t>((msg.servo[0] >> 0) & 0xFF),
+            static_cast<std::uint8_t>((msg.servo[0] >> 8) & 0xFF),
+            static_cast<std::uint8_t>((msg.servo[1] >> 0) & 0xFF),
+            static_cast<std::uint8_t>((msg.servo[1] >> 8) & 0xFF),
+            static_cast<std::uint8_t>((msg.servo[2] >> 0) & 0xFF),
+            static_cast<std::uint8_t>((msg.servo[2] >> 8) & 0xFF),
+            static_cast<std::uint8_t>((msg.servo[3] >> 0) & 0xFF),
+            static_cast<std::uint8_t>((msg.servo[3] >> 8) & 0xFF),
+        }
+    );
 }
diff --git a/device/nucleo_communicate/src/serial_port.cpp b/device/nucleo_communicate/src/serial_port.cpp
@@ -32,7 +32,7 @@ void SerialPort::setup() {
     ioctl(fd, TCSETS, &tio);
 }
 
-void SerialPort::send(const SendData& data) {
+bool SerialPort::send(const SendData& data) {
     std::uint8_t header = 0x00;
     fwrite(&header, sizeof(std::uint8_t), 1, this->serial);
     fwrite(
@@ -41,17 +41,25 @@ void SerialPort::send(const SendData& data) {
         data.buffer.size(),
         this->serial
     );
+    std::uint8_t res_header = ~header;
+    fread(&res_header, sizeof(std::uint8_t), 1, this->serial);
+    return res_header == header;
 }
 
-auto SerialPort::receive_state() -> NucleoState {
+auto SerialPort::receive_state() -> std::optional<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);
+    std::uint8_t res_header = ~header;
+    fread(&res_header, sizeof(std::uint8_t), 1, this->serial);
+    if (res_header != header) {
+        return std::nullopt;
+    }
     return static_cast<NucleoState>(recv_data);
 }
 
-auto SerialPort::receive() -> RecvData {
+auto SerialPort::receive() -> std::optional<RecvData> {
     std::uint8_t header = 0x01;
     fwrite(&header, sizeof(std::uint8_t), 1, this->serial);
     RecvData::BufferType buffer{};
@@ -61,15 +69,26 @@ auto SerialPort::receive() -> RecvData {
         buffer.size(),
         this->serial
     );
+    std::uint8_t res_header = ~header;
+    fread(&res_header, sizeof(std::uint8_t), 1, this->serial);
+    if (res_header != header) {
+        return std::nullopt;
+    }
     return RecvData(std::move(buffer));
 }
 
-void SerialPort::initialize() {
+bool SerialPort::initialize() {
     std::uint8_t header = 0xFE;
     fwrite(&header, sizeof(std::uint8_t), 1, this->serial);
+    std::uint8_t res_header = ~header;
+    fread(&res_header, sizeof(std::uint8_t), 1, this->serial);
+    return res_header == header;
 }
 
-void SerialPort::suspend() {
+bool SerialPort::suspend() {
     std::uint8_t header = 0xFF;
     fwrite(&header, sizeof(std::uint8_t), 1, this->serial);
+    std::uint8_t res_header = ~header;
+    fread(&res_header, sizeof(std::uint8_t), 1, this->serial);
+    return res_header == header;
 }