From 108a34805712737df474a5ccc38311bf2aa79196 Mon Sep 17 00:00:00 2001 From: LeonardoBizzoni Date: Sun, 15 Mar 2026 17:23:19 +0100 Subject: [PATCH] added the other config cmds + `serial_fd` access in critical section --- .../src/pioneer3dx_controller/src/main.cpp | 6 +- .../pioneer3dx_controller/src/p3dx_node.cpp | 58 ++++++++++++++----- .../pioneer3dx_controller/src/p3dx_node.hpp | 6 +- 3 files changed, 52 insertions(+), 18 deletions(-) diff --git a/pioneer_workstation_ws/src/pioneer3dx_controller/src/main.cpp b/pioneer_workstation_ws/src/pioneer3dx_controller/src/main.cpp index 6e4819b..10e6263 100644 --- a/pioneer_workstation_ws/src/pioneer3dx_controller/src/main.cpp +++ b/pioneer_workstation_ws/src/pioneer3dx_controller/src/main.cpp @@ -8,11 +8,15 @@ std::shared_ptr p3dx_controller = nullptr; int32_t main(int32_t argc, const char *argv[]) { + int32_t serial_fd = serial_open("/dev/ttyACM0"); + assert(serial_fd != -1); + rclcpp::init(argc, argv); - p3dx_controller = std::make_shared(); + p3dx_controller = std::make_shared(serial_fd); assert(p3dx_controller != nullptr); rclcpp::spin(p3dx_controller); rclcpp::shutdown(); + close(serial_fd); } [[nodiscard]] int32_t serial_open(const char *portname) diff --git a/pioneer_workstation_ws/src/pioneer3dx_controller/src/p3dx_node.cpp b/pioneer_workstation_ws/src/pioneer3dx_controller/src/p3dx_node.cpp index dc76ee2..8d5972f 100644 --- a/pioneer_workstation_ws/src/pioneer3dx_controller/src/p3dx_node.cpp +++ b/pioneer_workstation_ws/src/pioneer3dx_controller/src/p3dx_node.cpp @@ -1,10 +1,19 @@ #include "p3dx_node.hpp" +#ifdef assert +# undef assert +#endif +#define assert(cond) \ + do { \ + if (!(cond)) { __builtin_trap(); } \ + } while (false) + using namespace std::chrono_literals; -P3DX_Controller_Node::P3DX_Controller_Node() : +P3DX_Controller_Node::P3DX_Controller_Node(int32_t serial_fd) : rclcpp::Node("P3DX_Controller_Node"), - serial_fd(serial_open("/dev/ttyACM0")), + serial_fd(serial_fd), + stm32_config_mode(false), publisher_odometry(this->create_publisher("odometry", 10)), subscriber_velocity(this->create_subscription("p3dx_command_velocity", 10, std::bind(&P3DX_Controller_Node::callback_subscribe_command_velocity, @@ -26,21 +35,29 @@ P3DX_Controller_Node::P3DX_Controller_Node() : void P3DX_Controller_Node::callback_publish_odometry(void) { - if (!this->stm32_running) { return; } + if (this->stm32_config_mode) { return; } -#if 0 FMW_Message response, msg_get_status; msg_get_status.header.type = FMW_MessageType_Run_GetStatus; msg_get_status.header.crc = (uint32_t)-1; - size_t bytes_written = ::write(this->serial_fd, &msg_get_status, sizeof(FMW_Message)); - (void)bytes_written; + this->serial_mutex.lock(); + ssize_t bytes_written = ::write(this->serial_fd, &msg_get_status, sizeof(FMW_Message)); + assert(bytes_written != -1); + assert(bytes_written == sizeof(msg_get_status)); for (uint32_t bytes_read = 0; bytes_read < sizeof(response);) { ssize_t n = ::read(this->serial_fd, ((uint8_t*)&response) + bytes_read, sizeof(response) - bytes_read); if (n >= 0) { bytes_read += (uint32_t)n; } + assert(n != -1); } + this->serial_mutex.unlock(); assert(response.header.type == FMW_MessageType_Response); - stm32_message_print(&response); - + this->stm32_message_print(&response); + if (response.response.result == FMW_Result_Error_Command_NotAvailable) { + this->stm32_config_mode = true; + return; + } else { + this->stm32_config_mode = false; + } nav_msgs::msg::Odometry output; output.header.frame_id = this->get_parameter("frame_id_odometry").as_string(); @@ -63,7 +80,6 @@ void P3DX_Controller_Node::callback_publish_odometry(void) assert(output.twist.twist.angular.z == 0); this->publisher_odometry->publish(output); -#endif } void P3DX_Controller_Node::callback_subscribe_command_velocity(const geometry_msgs::msg::Twist::SharedPtr cmd) @@ -74,18 +90,21 @@ void P3DX_Controller_Node::callback_subscribe_command_velocity(const geometry_ms void P3DX_Controller_Node::callback_service_command_change_mode_s(const std::shared_ptr request, std::shared_ptr response) { - RCLCPP_INFO(p3dx_controller->get_logger(), "%d", request->mode); + RCLCPP_INFO(p3dx_controller->get_logger(), "Requested mode: %d", request->mode); P3DX_Cmd_ChangeMode_Kind mode = (P3DX_Cmd_ChangeMode_Kind)request->mode; if (mode == P3DX_Cmd_ChangeMode_Kind::None || mode >= P3DX_Cmd_ChangeMode_Kind::COUNT) { RCLCPP_INFO(p3dx_controller->get_logger(), "invalid mode received"); response->status_response = (uint8_t)FMW_Result_Error_Command_NotRecognized; } else { + FMW_MessageType type; switch (mode) { case P3DX_Cmd_ChangeMode_Kind::Config: { RCLCPP_INFO(p3dx_controller->get_logger(), "switching to config mode"); + type = FMW_MessageType_ModeChange_Config; } break; case P3DX_Cmd_ChangeMode_Kind::Run: { RCLCPP_INFO(p3dx_controller->get_logger(), "switching to run mode"); + type = FMW_MessageType_ModeChange_Run; } break; default: { assert(false && "unreachable"); @@ -94,12 +113,15 @@ void P3DX_Controller_Node::callback_service_command_change_mode_s(const std::sha FMW_Message msg; ::memset(&msg, 0, sizeof(msg)); - msg.header.type = (mode == P3DX_Cmd_ChangeMode_Kind::Config ? FMW_MessageType_ModeChange_Config : FMW_MessageType_ModeChange_Run); + msg.header.type = type; msg.header.crc = (uint32_t)-1; FMW_Message response_msg = p3dx_controller->stm32_message_send(&msg); assert(response_msg.header.type == FMW_MessageType_Response); response->status_response = (uint8_t)response_msg.response.result; + if (response_msg.response.result == FMW_Result_Ok) { + p3dx_controller->stm32_config_mode = (mode == P3DX_Cmd_ChangeMode_Kind::Config); + } } } @@ -180,14 +202,20 @@ FMW_Message P3DX_Controller_Node::stm32_message_send(const FMW_Message *msg) FMW_Message res; ::memset(&res, 0, sizeof(res)); - // TODO(lb): only works once for some reason - size_t bytes_written = ::write(this->serial_fd, msg, sizeof(FMW_Message)); - (void)bytes_written; + assert(msg != NULL); + assert(this->serial_fd > 0); + this->serial_mutex.lock(); + ssize_t bytes_written = ::write(this->serial_fd, msg, sizeof(FMW_Message)); + assert(bytes_written != -1); + assert(bytes_written > 0); + assert(bytes_written == sizeof(FMW_Message)); RCLCPP_INFO(this->get_logger(), "message sent to stm32"); - for (uint32_t bytes_read = 0; bytes_read < sizeof(msg);) { + for (uint32_t bytes_read = 0; bytes_read < sizeof(*msg);) { ssize_t n = ::read(this->serial_fd, ((uint8_t*)&res) + bytes_read, sizeof(res) - bytes_read); + assert(n != -1); if (n >= 0) { bytes_read += (uint32_t)n; } } + this->serial_mutex.unlock(); this->stm32_message_print(&res); assert(res.header.type == FMW_MessageType_Response); return res; diff --git a/pioneer_workstation_ws/src/pioneer3dx_controller/src/p3dx_node.hpp b/pioneer_workstation_ws/src/pioneer3dx_controller/src/p3dx_node.hpp index bd9fff5..a147267 100644 --- a/pioneer_workstation_ws/src/pioneer3dx_controller/src/p3dx_node.hpp +++ b/pioneer_workstation_ws/src/pioneer3dx_controller/src/p3dx_node.hpp @@ -1,6 +1,7 @@ #ifndef P3DX_NODE_HPP #define P3DX_NODE_HPP +#include #include #include @@ -26,7 +27,8 @@ enum P3DX_Cmd_ChangeMode_Kind: uint8_t { struct P3DX_Controller_Node: public rclcpp::Node { const int32_t serial_fd; - bool stm32_running; + bool stm32_config_mode; + std::mutex serial_mutex; rclcpp::Publisher::SharedPtr publisher_odometry; rclcpp::Subscription::SharedPtr subscriber_velocity; @@ -36,7 +38,7 @@ struct P3DX_Controller_Node: public rclcpp::Node { rclcpp::Service::SharedPtr service_config_led; rclcpp::TimerBase::SharedPtr timer_publisher_odometry; - P3DX_Controller_Node(void); + P3DX_Controller_Node(int32_t serial_fd); void callback_publish_odometry(void); void callback_subscribe_command_velocity(const geometry_msgs::msg::Twist::SharedPtr cmd); -- 2.52.0