diff --git a/device/nucleo_communicate/include/nucleo_communicate/channel.hpp b/device/nucleo_communicate/include/nucleo_communicate/channel.hpp index 301ffd3..0067b7e 100644 --- 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 +#include #include #include @@ -22,6 +23,8 @@ class Channel : public rclcpp::Node { nucleo_com::SerialPort serial; std::mutex serial_mutex; + std::optional order_failed_at; + rclcpp::Subscription::SharedPtr initialize_subscription; rclcpp::Subscription::SharedPtr suspend_subscription; rclcpp::Subscription::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 index c14fd08..a15dae7 100644 --- 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 #include #include #include @@ -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; - auto receive() -> nucleo_com::RecvData; + auto receive() -> std::optional; /// 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 index 61d88eb..1899b51 100644 --- a/device/nucleo_communicate/src/channel.cpp +++ b/device/nucleo_communicate/src/channel.cpp @@ -1,6 +1,8 @@ #include #include #include +#include +#include #include #include @@ -18,64 +20,81 @@ using packet_interfaces::msg::Voltage; void channel::Channel::initialize_subscription_callback(const std_msgs::msg::Empty&) { const std::lock_guard _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 _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 _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 _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 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(state_data); + state_msg.state = static_cast(state_data.value()); this->state_publisher->publish(state_msg); } + const std::optional 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 index 5e63f75..db42103 100644 --- 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((msg.bldc[0] >> 0) & 0xFF), - static_cast((msg.bldc[0] >> 8) & 0xFF), - static_cast((msg.bldc[1] >> 0) & 0xFF), - static_cast((msg.bldc[1] >> 8) & 0xFF), - static_cast((msg.bldc[2] >> 0) & 0xFF), - static_cast((msg.bldc[2] >> 8) & 0xFF), - static_cast((msg.bldc[3] >> 0) & 0xFF), - static_cast((msg.bldc[3] >> 8) & 0xFF), - static_cast((msg.servo[0] >> 0) & 0xFF), - static_cast((msg.servo[0] >> 8) & 0xFF), - static_cast((msg.servo[1] >> 0) & 0xFF), - static_cast((msg.servo[1] >> 8) & 0xFF), - static_cast((msg.servo[2] >> 0) & 0xFF), - static_cast((msg.servo[2] >> 8) & 0xFF), - static_cast((msg.servo[3] >> 0) & 0xFF), - static_cast((msg.servo[3] >> 8) & 0xFF), - }); + return SendData( + { + static_cast((msg.bldc[0] >> 0) & 0xFF), + static_cast((msg.bldc[0] >> 8) & 0xFF), + static_cast((msg.bldc[1] >> 0) & 0xFF), + static_cast((msg.bldc[1] >> 8) & 0xFF), + static_cast((msg.bldc[2] >> 0) & 0xFF), + static_cast((msg.bldc[2] >> 8) & 0xFF), + static_cast((msg.bldc[3] >> 0) & 0xFF), + static_cast((msg.bldc[3] >> 8) & 0xFF), + static_cast((msg.servo[0] >> 0) & 0xFF), + static_cast((msg.servo[0] >> 8) & 0xFF), + static_cast((msg.servo[1] >> 0) & 0xFF), + static_cast((msg.servo[1] >> 8) & 0xFF), + static_cast((msg.servo[2] >> 0) & 0xFF), + static_cast((msg.servo[2] >> 8) & 0xFF), + static_cast((msg.servo[3] >> 0) & 0xFF), + static_cast((msg.servo[3] >> 8) & 0xFF), + } + ); } diff --git a/device/nucleo_communicate/src/serial_port.cpp b/device/nucleo_communicate/src/serial_port.cpp index bdf4e41..2b91ed6 100644 --- 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 { 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(recv_data); } -auto SerialPort::receive() -> RecvData { +auto SerialPort::receive() -> std::optional { 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; }