From a9705d8f0422970359e144336a10ea82c2db8286 Mon Sep 17 00:00:00 2001 From: LeonardoBizzoni Date: Sat, 14 Mar 2026 10:34:19 +0100 Subject: [PATCH] ROS2<->STM32 message passing --- .../Core/Inc/firmware/fmw_messages.h | 5 +- .../pioneer3DX_controller/src/p3dx_node.cpp | 80 ++++++++++- .../pioneer3DX_controller/src/p3dx_node.hpp | 8 ++ .../src/stm32_messages.h | 124 ++++++++++++++++++ 4 files changed, 211 insertions(+), 6 deletions(-) create mode 100644 pioneer_workstation_ws/src/pioneer3DX_controller/src/stm32_messages.h diff --git a/pioneer_controller/Core/Inc/firmware/fmw_messages.h b/pioneer_controller/Core/Inc/firmware/fmw_messages.h index 295ec40..7a3d590 100644 --- a/pioneer_controller/Core/Inc/firmware/fmw_messages.h +++ b/pioneer_controller/Core/Inc/firmware/fmw_messages.h @@ -2,7 +2,7 @@ #define FMW_MESSAGE_H typedef union { - struct { + struct FMW_PidConstants_Fields { float proportional; float integral; float derivative; @@ -75,7 +75,7 @@ enum { #pragma pack(push, 1) typedef struct FMW_Message { - struct { + struct FMW_Message_Header { FMW_MessageType type; uint32_t crc; } header; @@ -105,6 +105,7 @@ typedef struct FMW_Message { float angular; } run_set_velocity; + // TODO(lb): add odometry information struct { int32_t ticks_left; int32_t ticks_right; 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 661d4a0..13a0bd3 100644 --- a/pioneer_workstation_ws/src/pioneer3DX_controller/src/p3dx_node.cpp +++ b/pioneer_workstation_ws/src/pioneer3DX_controller/src/p3dx_node.cpp @@ -8,17 +8,48 @@ P3DX_Controller_Node::P3DX_Controller_Node() : subscriber_command_velocity(this->create_subscription("p3dx_command_velocity", 10, std::bind(&P3DX_Controller_Node::callback_subscribe_command_velocity, this, std::placeholders::_1))), - publisher_odometry_timer(this->create_wall_timer(50ms, std::bind(&P3DX_Controller_Node::callback_publish_odometry, this))) + publisher_odometry_timer(this->create_wall_timer(50ms, std::bind(&P3DX_Controller_Node::callback_publish_odometry, this))), + stm32_running(false) { - this->declare_parameter("frame_id_odometry", std::string("p3dx_controller_odometry")); assert(this->serial_fd != -1); + + this->declare_parameter("frame_id_odometry", std::string("p3dx_controller_odometry")); + + // TODO(lb): force the switch to config mode maybe? + // switching to run mode shouldn't happen now, this is just for testing + FMW_Message response, msg_run; + msg_run.header.type = FMW_MessageType_ModeChange_Run; + msg_run.header.crc = (uint32_t)-1; + (void)::write(this->serial_fd, &msg_run, sizeof(FMW_Message)); + 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; } + } + stm32_message_print(&response); + assert(response.header.type == FMW_MessageType_Response); + assert(response.response.result == FMW_Result_Ok || response.response.result == FMW_Result_Error_Command_NotAvailable); + this->stm32_running = true; } void P3DX_Controller_Node::callback_publish_odometry(void) { + if (!this->stm32_running) { return; } + + FMW_Message response, msg_get_status; + msg_get_status.header.type = FMW_MessageType_Run_GetStatus; + msg_get_status.header.crc = (uint32_t)-1; + (void)::write(this->serial_fd, &msg_get_status, sizeof(FMW_Message)); + 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(response.header.type == FMW_MessageType_Response); + stm32_message_print(&response); + + nav_msgs::msg::Odometry output; output.header.frame_id = this->get_parameter("frame_id_odometry").as_string(); - // output.header.stamp = ; // TODO(lb): generate some kind of current timestamp + output.header.stamp = rclcpp::Time(0, 0, this->get_clock()->now().get_clock_type()); // output.pose.pose.position.x = ; // TODO(lb): fill with pioneer data // output.pose.pose.position.y = ; // TODO(lb): fill with pioneer data @@ -37,10 +68,51 @@ void P3DX_Controller_Node::callback_publish_odometry(void) assert(output.twist.twist.angular.z == 0); this->publisher_odometry->publish(output); - RCLCPP_INFO(this->get_logger(), "Hello, World!"); } void P3DX_Controller_Node::callback_subscribe_command_velocity(const geometry_msgs::msg::Twist::SharedPtr cmd) { (void)cmd; } + +void P3DX_Controller_Node::stm32_message_print(const FMW_Message *msg) +{ + static const char *message_types[] = { + #define X(Variant, Id) #Variant, + FMW_MESSAGE_TYPE_VARIANTS() + #undef X + }; + + static const char *result_types[] = { + #define X(Variant, Id) #Variant, + FMW_RESULT_VARIANTS() + #undef X + }; + + char buffer[512] = {0}; + int32_t buffer_len = snprintf(buffer, sizeof(buffer), + "FMW_Message {" + "\n header.type = %s" + "\n header.crc = %u", + message_types[msg->header.type], msg->header.crc); + assert(buffer_len > 0); + assert(buffer_len < sizeof(buffer)); + + switch (msg->header.type) { + case FMW_MessageType_Response: { + buffer_len += snprintf(buffer + buffer_len, sizeof(buffer) - buffer_len, + "\n response:" + "\n result = %s" + "\n delta_millis = %d" + "\n ticks_left = %d" + "\n ticks_right = %d", + result_types[msg->response.result], msg->response.delta_millis, + msg->response.ticks_left, msg->response.ticks_right); + assert(buffer_len < sizeof(buffer)); + } break; + default: { + assert(false && "unreachable"); + } break; + } + RCLCPP_INFO(this->get_logger(), "%s\n}", buffer); +} 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 164a193..f860f6c 100644 --- a/pioneer_workstation_ws/src/pioneer3DX_controller/src/p3dx_node.hpp +++ b/pioneer_workstation_ws/src/pioneer3DX_controller/src/p3dx_node.hpp @@ -4,6 +4,10 @@ #include #include +extern "C" { +#include "stm32_messages.h" +} + #include "main.hpp" struct P3DX_Controller_Node: public rclcpp::Node { @@ -11,11 +15,15 @@ struct P3DX_Controller_Node: public rclcpp::Node { rclcpp::Publisher::SharedPtr publisher_odometry; rclcpp::Subscription::SharedPtr subscriber_command_velocity; rclcpp::TimerBase::SharedPtr publisher_odometry_timer; + bool stm32_running; P3DX_Controller_Node(void); void callback_publish_odometry(void); void callback_subscribe_command_velocity(const geometry_msgs::msg::Twist::SharedPtr cmd); + + void stm32_message_print(const FMW_Message *msg); }; + #endif diff --git a/pioneer_workstation_ws/src/pioneer3DX_controller/src/stm32_messages.h b/pioneer_workstation_ws/src/pioneer3DX_controller/src/stm32_messages.h new file mode 100644 index 0000000..7a3d590 --- /dev/null +++ b/pioneer_workstation_ws/src/pioneer3DX_controller/src/stm32_messages.h @@ -0,0 +1,124 @@ +#ifndef FMW_MESSAGE_H +#define FMW_MESSAGE_H + +typedef union { + struct FMW_PidConstants_Fields { + float proportional; + float integral; + float derivative; + }; + float values[3]; +} FMW_PidConstants; + +// NOTE(lb): The expansion of this macro relies on another macro called "X" (hence the name X-macro). +// X isn't define yet but, for it to work with this macro expansion, it must be a function-like macro +// with exactly 2 arguemnts: 1) the enumation variant name. 2) the corresponding numerical id. +#define FMW_MESSAGE_TYPE_VARIANTS() \ + X(FMW_MessageType_None, 0) \ + X(FMW_MessageType_Response, 1) \ + X(FMW_MessageType_ModeChange_Config, 2) \ + X(FMW_MessageType_ModeChange_Run, 3) \ + X(FMW_MessageType_Config_Robot, 4) \ + X(FMW_MessageType_Config_PID, 5) \ + X(FMW_MessageType_Config_LED, 6) \ + X(FMW_MessageType_Run_GetStatus, 7) \ + X(FMW_MessageType_Run_SetVelocity, 8) \ + X(FMW_MessageType_COUNT, 9) + +// NOTE(lb): Here i want to take all of the variants that are generated from the macro +// expension of FMW_MESSAGE_TYPE_VARIANTS and use them to populate the enum definition. +// For it to be a valid enum definition you need to take every `X(VariantName, Id)` +// and turn them into `VariantName = Id,`. +// It's important to undefine `X` after use to avoid redefinition and, more importantly, +// to use it in another X-macro expansion by accident. +// This seems pointless but when paired with `#VariantName` you can automatically +// turn all of the symbolic variant names into strings and now printing the variant +// can be done via an array lookup `[VariantName] = #VariantName` instead of a runtime check. +typedef uint8_t FMW_MessageType; +enum { +#define X(Variant, Id) Variant = Id, + FMW_MESSAGE_TYPE_VARIANTS() +#undef X +}; + +#define FMW_RESULT_VARIANTS() \ + X(FMW_Result_Ok, 0) \ + X(FMW_Result_Error_InvalidArguments, 1) \ + X(FMW_Result_Error_FaultPinTriggered, 2) \ + X(FMW_Result_Error_UART_Crc, 3) \ + X(FMW_Result_Error_UART_NegativeTimeout, 4) \ + X(FMW_Result_Error_UART_ReceiveTimeoutElapsed, 5) \ + X(FMW_Result_Error_UART_Parity, 6) \ + X(FMW_Result_Error_UART_Frame, 7) \ + X(FMW_Result_Error_UART_Noise, 8) \ + X(FMW_Result_Error_UART_Overrun, 9) \ + X(FMW_Result_Error_Encoder_InvalidTimer, 10) \ + X(FMW_Result_Error_Encoder_NonPositiveTicksPerRevolution, 11) \ + X(FMW_Result_Error_Encoder_NonPositiveWheelCircumference, 12) \ + X(FMW_Result_Error_Encoder_GetTick, 13) \ + X(FMW_Result_Error_Buzzer_Timer, 14) \ + X(FMW_Result_Error_MessageHandler_InvalidState, 15) \ + X(FMW_Result_Error_MessageHandler_Config_NonPositiveBaseline, 16) \ + X(FMW_Result_Error_MessageHandler_Config_NonPositiveWheelCircumference, 17) \ + X(FMW_Result_Error_MessageHandler_Config_NonPositiveTicksPerRevolution, 18) \ + X(FMW_Result_Error_MessageHandler_Config_NonPositiveLEDUpdatePeriod, 19) \ + X(FMW_Result_Error_Command_NotRecognized, 20) \ + X(FMW_Result_Error_Command_NotAvailable, 21) \ + X(FMW_Result_COUNT, 22) + +typedef uint8_t FMW_Result; +enum { +#define X(Variant, Id) Variant = Id, + FMW_RESULT_VARIANTS() +#undef X +}; + +#pragma pack(push, 1) +typedef struct FMW_Message { + struct FMW_Message_Header { + FMW_MessageType type; + uint32_t crc; + } header; + + union { + struct { + float baseline; + float wheel_circumference_left; + float wheel_circumference_right; + uint32_t ticks_per_revolution_left; + uint32_t ticks_per_revolution_right; + } config_robot; + struct { + FMW_PidConstants left; + FMW_PidConstants right; + FMW_PidConstants cross; + } config_pid; + struct { + float voltage_red; + float voltage_orange; + float voltage_hysteresis; + uint32_t update_period; + } config_led; + + struct { + float linear; + float angular; + } run_set_velocity; + + // TODO(lb): add odometry information + struct { + int32_t ticks_left; + int32_t ticks_right; + uint16_t delta_millis; + FMW_Result result; + } response; + }; +} FMW_Message; +#pragma pack(pop) + +static_assert(sizeof(uint8_t) == 1); +static_assert(sizeof(uint16_t) == 2); +static_assert(sizeof(uint32_t) == 4); +static_assert(sizeof(float) == 4); + +#endif /* INC_COMMUNICATION_OTTO_MESSAGES_H_ */ -- 2.52.0