From: LeonardoBizzoni Date: Mon, 16 Mar 2026 10:19:19 +0000 (+0100) Subject: actually implemented odometry + odometry pub + velocity command sub X-Git-Url: http://git.leonardobizzoni.com/?a=commitdiff_plain;h=b6e34623fb18de3335c925ac83debe017ba13ef3;p=pioneer-stm32 actually implemented odometry + odometry pub + velocity command sub --- diff --git a/pioneer_controller/Core/Inc/firmware/fmw_core.h b/pioneer_controller/Core/Inc/firmware/fmw_core.h index 87a1fc8..df23782 100644 --- a/pioneer_controller/Core/Inc/firmware/fmw_core.h +++ b/pioneer_controller/Core/Inc/firmware/fmw_core.h @@ -36,11 +36,11 @@ enum { }; typedef struct { - float baseline; // NOTE(lb): measured in meters - float velocity_linear; + float baseline; // NOTE(lb): measured in meters + Vec2Float position; // NOTE(lb): measured in meters + Vec2Float orientation; + Vec2Float velocity_linear; // NOTE(lb): measured in m/s float velocity_angular; - float setpoint_right; - float setpoint_left; } FMW_Odometry; typedef struct { @@ -86,16 +86,16 @@ void fmw_motors_enable(FMW_Motor motors[], int32_t count) __attribute__((n void fmw_motors_disable(FMW_Motor motors[], int32_t count) __attribute__((nonnull)); void fmw_motor_set_speed(FMW_Motor *motor, int32_t duty_cycle) __attribute__((nonnull)); -void fmw_encoders_init(FMW_Encoder encoders[], int32_t count) __attribute__((nonnull)); -void fmw_encoders_deinit(FMW_Encoder encoders[], int32_t count) __attribute__((nonnull)); -void fmw_encoder_update(FMW_Encoder *encoder) __attribute__((nonnull)); -float fmw_encoder_get_linear_velocity(const FMW_Encoder *encoder) __attribute__((warn_unused_result, nonnull)); -void fmw_encoder_count_reset(FMW_Encoder *encoder) __attribute__((nonnull)); -int32_t fmw_encoder_count_get(const FMW_Encoder *encoder) __attribute__((warn_unused_result, nonnull)); +void fmw_encoders_init(FMW_Encoder encoders[], int32_t count) __attribute__((nonnull)); +void fmw_encoders_deinit(FMW_Encoder encoders[], int32_t count) __attribute__((nonnull)); +void fmw_encoders_update(FMW_Encoder encoders[], int32_t count) __attribute__((nonnull)); +float fmw_encoder_get_linear_velocity(const FMW_Encoder *encoder, float meters_traveled) __attribute__((warn_unused_result, nonnull)); +void fmw_encoder_count_reset(FMW_Encoder *encoder) __attribute__((nonnull)); +int32_t fmw_encoder_count_get(const FMW_Encoder *encoder) __attribute__((warn_unused_result, nonnull)); -int32_t fmw_pid_update(FMW_PidController *pid, float velocity) __attribute__((warn_unused_result, nonnull)); +void fmw_odometry_pose_update(FMW_Odometry *odometry, float meters_traveled_left, float meters_traveled_right) __attribute__((nonnull)); -void fmw_odometry_setpoint_from_velocities(FMW_Odometry *odometry, float linear, float angular) __attribute__((nonnull)); +int32_t fmw_pid_update(FMW_PidController *pid, float velocity) __attribute__((warn_unused_result, nonnull)); void fmw_led_init(FMW_Led *led) __attribute__((nonnull)); void fmw_led_deinit(FMW_Led *led) __attribute__((nonnull)); @@ -103,11 +103,12 @@ void fmw_led_update(FMW_Led *led) __attribute__((nonnull)); void fmw_buzzers_set(FMW_Buzzer buzzer[], int32_t count, bool on) __attribute__((nonnull)); -FMW_Result fmw_message_uart_receive(UART_HandleTypeDef *huart, FMW_Message *msg, int32_t wait_ms) __attribute__((warn_unused_result, nonnull)); +FMW_Result fmw_message_uart_receive(UART_HandleTypeDef *huart, FMW_Message *msg, int32_t wait_ms) __attribute__((warn_unused_result, nonnull)); HAL_StatusTypeDef fmw_message_uart_send(UART_HandleTypeDef *huart, CRC_HandleTypeDef *hcrc, - FMW_Message *msg, int32_t wait_ms) __attribute__((nonnull, warn_unused_result)); + FMW_Message *msg, int32_t wait_ms) __attribute__((nonnull, warn_unused_result)); +FMW_Message fmw_message_from_uart_error(const UART_HandleTypeDef *huart) __attribute__((nonnull, warn_unused_result)); -FMW_Message fmw_message_from_uart_error(const UART_HandleTypeDef *huart); +Vec2Float fmw_setpoint_from_velocities(const FMW_Odometry *odometry, float linear, float angular) __attribute__((nonnull)); #define FMW_LED_UPDATE_PERIOD 200 #define FMW_DEBOUNCE_DELAY 200 diff --git a/pioneer_controller/Core/Inc/firmware/fmw_messages.h b/pioneer_controller/Core/Inc/firmware/fmw_messages.h index 99a30e5..b2c08f0 100644 --- a/pioneer_controller/Core/Inc/firmware/fmw_messages.h +++ b/pioneer_controller/Core/Inc/firmware/fmw_messages.h @@ -107,10 +107,15 @@ typedef struct FMW_Message { float angular; } run_set_velocity; - // TODO(lb): add odometry information struct { int32_t ticks_left; int32_t ticks_right; + float position_x; + float position_y; + float orientation_x; + float orientation_y; + float velocity_linear; + float velocity_angular; uint16_t delta_millis; FMW_Result result; } response; @@ -118,9 +123,10 @@ typedef struct FMW_Message { } 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); +static_assert(sizeof(uint8_t) == 1); +static_assert(sizeof(uint16_t) == 2); +static_assert(sizeof(uint32_t) == 4); +static_assert(sizeof(float) == 4); +static_assert(sizeof(FMW_Message) == 41); #endif /* INC_COMMUNICATION_OTTO_MESSAGES_H_ */ diff --git a/pioneer_controller/Core/Inc/main.h b/pioneer_controller/Core/Inc/main.h index 6e24afa..5ace7eb 100644 --- a/pioneer_controller/Core/Inc/main.h +++ b/pioneer_controller/Core/Inc/main.h @@ -62,7 +62,21 @@ extern int32_t pid_min; #define ARRLENGTH(Arr) (sizeof((Arr)) / sizeof(*(Arr))) +typedef union Vec2Float Vec2Float; +union Vec2Float { + float values[2]; + struct { float x, y; }; + struct { float left, right; }; +}; + +typedef union Mat3Float Mat3Float; +union Mat3Float { + float values[3][3]; + float sequence[9]; +}; + void start(void) __attribute__((noreturn)); +Mat3Float mat3float_multiply(Mat3Float lhs, Mat3Float rhs); /* USER CODE END EM */ diff --git a/pioneer_controller/Core/Src/firmware/fwm_core.c b/pioneer_controller/Core/Src/firmware/fwm_core.c index 8177220..7f94b83 100644 --- a/pioneer_controller/Core/Src/firmware/fwm_core.c +++ b/pioneer_controller/Core/Src/firmware/fwm_core.c @@ -2,6 +2,7 @@ #include "firmware/fmw_inc.h" #include +#include static struct { FMW_Motor *motors; @@ -59,6 +60,15 @@ FMW_Message fmw_message_from_uart_error(const UART_HandleTypeDef *huart) { return res; } +Vec2Float fmw_setpoint_from_velocities(const FMW_Odometry *odometry, float linear, float angular) { + FMW_ASSERT(odometry->baseline > 0.f, .callback = fmw_hook_assert_fail); + Vec2Float res = { + .left = linear - (odometry->baseline * angular) / 2.f, + .right = linear + (odometry->baseline * angular) / 2.f, + }; + return res; +} + // ============================================================ // Motor controller void fmw_motors_init(FMW_Motor motors[], int32_t count) { @@ -206,23 +216,20 @@ void fmw_encoders_deinit(FMW_Encoder encoders[], int32_t count) { } } -void fmw_encoder_update(FMW_Encoder *encoder) { - encoder->previous_millis = encoder->current_millis; - encoder->current_millis = HAL_GetTick(); - encoder->ticks = fmw_encoder_count_get(encoder); - fmw_encoder_count_reset(encoder); - FMW_ASSERT(encoder->current_millis >= encoder->previous_millis, .callback = fmw_hook_assert_fail); +void fmw_encoders_update(FMW_Encoder encoders[], int32_t count) { + for (int32_t i = 0; i < count; ++i) { + encoders[i].previous_millis = encoders[i].current_millis; + encoders[i].current_millis = HAL_GetTick(); + encoders[i].ticks = fmw_encoder_count_get(&encoders[i]); + fmw_encoder_count_reset(&encoders[i]); + FMW_ASSERT(encoders[i].current_millis >= encoders[i].previous_millis, .callback = fmw_hook_assert_fail); + } } -float fmw_encoder_get_linear_velocity(const FMW_Encoder *encoder) { +float fmw_encoder_get_linear_velocity(const FMW_Encoder *encoder, float meters_traveled) { float deltatime = encoder->current_millis - encoder->previous_millis; FMW_ASSERT(deltatime > 0.f, .callback = fmw_hook_assert_fail); - float meters = FMW_METERS_FROM_TICKS(encoder->ticks, - encoder->wheel_circumference, - encoder->ticks_per_revolution); - FMW_ASSERT(meters >= 0.f, .callback = fmw_hook_assert_fail); - float linear_velocity = meters / (deltatime / 1000.f); - FMW_ASSERT(linear_velocity >= 0.f, .callback = fmw_hook_assert_fail); + float linear_velocity = meters_traveled / (deltatime / 1000.f); return linear_velocity; } @@ -233,15 +240,56 @@ void fmw_encoder_count_reset(FMW_Encoder *encoder) { int32_t fmw_encoder_count_get(const FMW_Encoder *encoder) { FMW_ASSERT(encoder->timer != NULL, .callback = fmw_hook_assert_fail); - return (int32_t)__HAL_TIM_GET_COUNTER(encoder->timer) - (encoder->timer->Init.Period / 2); + int32_t res = (int32_t)__HAL_TIM_GET_COUNTER(encoder->timer) - (encoder->timer->Init.Period / 2); + return res; } // ============================================================ // Odometry -void fmw_odometry_setpoint_from_velocities(FMW_Odometry *odometry, float linear, float angular) { - FMW_ASSERT(odometry->baseline > 0.f, .callback = fmw_hook_assert_fail); - odometry->setpoint_left = linear - (odometry->baseline * angular) / 2; - odometry->setpoint_right = linear + (odometry->baseline * angular) / 2; +void fmw_odometry_pose_update(FMW_Odometry *odometry, float meters_traveled_left, float meters_traveled_right) { + static Mat3Float robot_pose = {{{1.f, 0.f, 0.f}, {0.f, 1.f, 0.f}, {0.f, 0.f, 1.f}}}; + static const float tolerance = 0.005f; + + Mat3Float robot_robotranslation = {0}; + Mat3Float robot_from_cir = {0}; + Mat3Float cir_rotation = {0}; + Mat3Float cir_from_robot = {0}; + + // calcolo dell'angolo di rotazione + float theta = -(meters_traveled_left - meters_traveled_right) / odometry->baseline; + + // soglia di tolleranza -> controllo la direzione del robot se va dritto + if (ABS(theta) < tolerance) { + /* traslazione lungo x, calcolo della matrice di rototraslazione per traiettoria dritta */ + robot_robotranslation.values[0][0] = 1; robot_robotranslation.values[0][1] = 0; robot_robotranslation.values[0][2] = (meters_traveled_left + meters_traveled_right) / 2; + robot_robotranslation.values[1][0] = 0; robot_robotranslation.values[1][1] = 1; robot_robotranslation.values[1][2] = 0; + robot_robotranslation.values[2][0] = 0; robot_robotranslation.values[2][1] = 0; robot_robotranslation.values[2][2] = 1; + } else { + // traiettoria lungo una curva + // distanza del centro del robot dal CIR + float d_cir = (meters_traveled_right / theta) - (odometry->baseline / 2.f); + // matrice cir_from_robot di traslazione al CIR + cir_from_robot.values[0][0] = 1; cir_from_robot.values[0][1] = 0; cir_from_robot.values[0][2] = 0; + cir_from_robot.values[1][0] = 0; cir_from_robot.values[1][1] = 1; cir_from_robot.values[1][2] = -d_cir; + cir_from_robot.values[2][0] = 0; cir_from_robot.values[2][1] = 0; cir_from_robot.values[2][2] = 1; + // matrice r di rotazione attorno al CIR + cir_rotation.values[0][0] = cosf(theta); cir_rotation.values[0][1] = -sinf(theta); cir_rotation.values[0][2] = 0; + cir_rotation.values[1][0] = sinf(theta); cir_rotation.values[1][1] = cosf(theta); cir_rotation.values[1][2] = 0; + cir_rotation.values[2][0] = 0; cir_rotation.values[2][1] = 0; cir_rotation.values[2][2] = 1; + // matrice robot_from_cir di ri-traslazione dal CIR + robot_from_cir.values[0][0] = 1; robot_from_cir.values[0][1] = 0; robot_from_cir.values[0][2] = 0; + robot_from_cir.values[1][0] = 0; robot_from_cir.values[1][1] = 1; robot_from_cir.values[1][2] = d_cir; + robot_from_cir.values[2][0] = 0; robot_from_cir.values[2][1] = 0; robot_from_cir.values[2][2] = 1; + // calcolo matrice di ROTOTRASLAZIONE corrente + robot_robotranslation = mat3float_multiply(robot_from_cir, mat3float_multiply(cir_rotation, cir_from_robot)); + } + + // calcolo (nuova) ROBOT_POSE + robot_pose = mat3float_multiply(robot_pose, robot_robotranslation); + odometry->position.x = robot_pose.values[0][2]; + odometry->position.y = robot_pose.values[1][2]; + odometry->orientation.x = robot_pose.values[0][0]; + odometry->orientation.y = robot_pose.values[0][1]; } // ============================================================ diff --git a/pioneer_controller/Core/Src/main.c b/pioneer_controller/Core/Src/main.c index 32a9450..7fe6158 100644 --- a/pioneer_controller/Core/Src/main.c +++ b/pioneer_controller/Core/Src/main.c @@ -815,6 +815,20 @@ void start(void) { } } +Mat3Float mat3float_multiply(Mat3Float lhs, Mat3Float rhs) { + Mat3Float output = {0}; + output.values[0][0] = (lhs.values[0][0] * rhs.values[0][0]) + (lhs.values[0][1] * rhs.values[1][0]) + (lhs.values[0][2] * rhs.values[2][0]); + output.values[0][1] = (lhs.values[0][0] * rhs.values[0][1]) + (lhs.values[0][1] * rhs.values[1][1]) + (lhs.values[0][2] * rhs.values[2][1]); + output.values[0][2] = (lhs.values[0][0] * rhs.values[0][2]) + (lhs.values[0][1] * rhs.values[1][2]) + (lhs.values[0][2] * rhs.values[2][2]); + output.values[1][0] = (lhs.values[1][0] * rhs.values[0][0]) + (lhs.values[1][1] * rhs.values[1][0]) + (lhs.values[1][2] * rhs.values[2][0]); + output.values[1][1] = (lhs.values[1][0] * rhs.values[0][1]) + (lhs.values[1][1] * rhs.values[1][1]) + (lhs.values[1][2] * rhs.values[2][1]); + output.values[1][2] = (lhs.values[1][0] * rhs.values[0][2]) + (lhs.values[1][1] * rhs.values[1][2]) + (lhs.values[1][2] * rhs.values[2][2]); + output.values[2][0] = (lhs.values[2][0] * rhs.values[0][0]) + (lhs.values[2][1] * rhs.values[1][0]) + (lhs.values[2][2] * rhs.values[2][0]); + output.values[2][1] = (lhs.values[2][0] * rhs.values[0][1]) + (lhs.values[2][1] * rhs.values[1][1]) + (lhs.values[2][2] * rhs.values[2][1]); + output.values[2][2] = (lhs.values[2][0] * rhs.values[0][2]) + (lhs.values[2][1] * rhs.values[1][2]) + (lhs.values[2][2] * rhs.values[2][2]); + return output; +} + FMW_Result message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) { // NOTE(lb): the `msg->header.crc != -1` checks are just because i haven't // implemented CRC into the program that sends these messages. @@ -899,10 +913,10 @@ FMW_Result message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) { case FMW_Mode_Run: { switch (msg->header.type) { case FMW_MessageType_Run_SetVelocity: { - fmw_odometry_setpoint_from_velocities(&odometry, msg->run_set_velocity.linear, msg->run_set_velocity.angular); - pid_left.setpoint = odometry.setpoint_left; - pid_right.setpoint = odometry.setpoint_right; - pid_cross.setpoint = odometry.setpoint_left - odometry.setpoint_right; + Vec2Float setpoint = fmw_setpoint_from_velocities(&odometry, msg->run_set_velocity.linear, msg->run_set_velocity.angular); + pid_left.setpoint = setpoint.left; + pid_right.setpoint = setpoint.right; + pid_cross.setpoint = setpoint.left - setpoint.right; } // fallthrough case FMW_MessageType_Run_GetStatus: { // NOTE(lb): SetVelocity continues here as well because the previous case doesn't end with a `break`. @@ -923,6 +937,12 @@ FMW_Result message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) { msg.response.delta_millis = time_millis_delta; msg.response.ticks_left = current_ticks_left; msg.response.ticks_right = current_ticks_right; + msg.response.position_x = odometry.position.x; + msg.response.position_y = odometry.position.y; + msg.response.orientation_x = odometry.orientation.x; + msg.response.orientation_y = odometry.orientation.y; + msg.response.velocity_linear = (odometry.velocity_linear.left + odometry.velocity_linear.right) / 2.f; + msg.response.velocity_angular = odometry.velocity_angular; msg.header.crc = HAL_CRC_Calculate(hcrc, (uint32_t*)&msg, sizeof msg); (void)HAL_UART_Transmit(UART_HANDLE_PTR_USED_FOR_MESSAGE_EXCHANGE_PROTOCOL, (uint8_t*)&msg, sizeof msg, HAL_MAX_DELAY); return FMW_Result_Ok; @@ -973,17 +993,23 @@ FMW_Result message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) { void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { if (htim != &htim6) { return; } - // NOTE(lb): metrics taken for transmission - ticks_left += fmw_encoder_count_get(&encoders.left); - ticks_right += fmw_encoder_count_get(&encoders.right); - { // PID control - fmw_encoder_update(&encoders.left); - float velocity_left = fmw_encoder_get_linear_velocity(&encoders.left); - int32_t dutycycle_left = fmw_pid_update(&pid_left, velocity_left); + fmw_encoders_update(encoders.values, ARRLENGTH(encoders.values)); + ticks_left += encoders.left.ticks; + ticks_right += encoders.right.ticks; - fmw_encoder_update(&encoders.right); - float velocity_right = fmw_encoder_get_linear_velocity(&encoders.right); + float meters_traveled_left = FMW_METERS_FROM_TICKS(encoders.left.ticks, encoders.left.wheel_circumference, encoders.left.ticks_per_revolution); + float meters_traveled_right = FMW_METERS_FROM_TICKS(encoders.right.ticks, encoders.right.wheel_circumference, encoders.right.ticks_per_revolution); + + fmw_odometry_pose_update(&odometry, meters_traveled_left, meters_traveled_right); + + float velocity_left = fmw_encoder_get_linear_velocity(&encoders.left, meters_traveled_left); + float velocity_right = fmw_encoder_get_linear_velocity(&encoders.right, meters_traveled_right); + odometry.velocity_linear.left = velocity_left; + odometry.velocity_linear.right = velocity_right; + odometry.velocity_angular = (velocity_right - velocity_left) / odometry.baseline; + + int32_t dutycycle_left = fmw_pid_update(&pid_left, velocity_left); int32_t dutycycle_right = fmw_pid_update(&pid_right, velocity_right); int32_t dutycycle_cross = fmw_pid_update(&pid_cross, velocity_left - velocity_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 09a3b5b..a1a6a68 100644 --- a/pioneer_workstation_ws/src/pioneer3dx_controller/src/p3dx_node.cpp +++ b/pioneer_workstation_ws/src/pioneer3dx_controller/src/p3dx_node.cpp @@ -37,21 +37,10 @@ void P3DX_Controller_Node::callback_publish_odometry(void) { if (this->stm32_config_mode) { return; } - FMW_Message response, msg_get_status; + FMW_Message msg_get_status; msg_get_status.header.type = FMW_MessageType_Run_GetStatus; msg_get_status.header.crc = (uint32_t)-1; - 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(); + FMW_Message response = this->stm32_message_send(&msg_get_status); assert(response.header.type == FMW_MessageType_Response); this->stm32_message_print(&response); if (response.response.result == FMW_Result_Error_Command_NotAvailable) { @@ -65,28 +54,31 @@ void P3DX_Controller_Node::callback_publish_odometry(void) output.header.frame_id = this->get_parameter("frame_id_odometry").as_string(); 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 - // output.pose.pose.orientation.x = ; // TODO(lb): fill with pioneer data - // output.pose.pose.orientation.y = ; // TODO(lb): fill with pioneer data + output.pose.pose.position.x = response.response.position_x; + output.pose.pose.position.y = response.response.position_y; + output.pose.pose.orientation.x = response.response.orientation_x; + output.pose.pose.orientation.y = response.response.orientation_y; - // output.twist.twist.linear.x = ; // TODO(lb): fill with pioneer data - // output.twist.twist.linear.y = ; // TODO(lb): fill with pioneer data - // output.twist.twist.angular.x = ; // TODO(lb): fill with pioneer data - // output.twist.twist.angular.y = ; // TODO(lb): fill with pioneer data - - assert(output.pose.pose.position.z == 0); - assert(output.pose.pose.orientation.z == 0); - assert(output.pose.pose.orientation.w == 1); - assert(output.twist.twist.linear.z == 0); - assert(output.twist.twist.angular.z == 0); + output.twist.twist.linear.x = response.response.velocity_linear; + output.twist.twist.angular.x = response.response.velocity_angular; this->publisher_odometry->publish(output); } void P3DX_Controller_Node::callback_subscribe_command_velocity(const geometry_msgs::msg::Twist::SharedPtr cmd) { - (void)cmd; + if (this->stm32_config_mode) { return; } + + FMW_Message msg; + ::memset(&msg, 0, sizeof(msg)); + msg.header.type = FMW_MessageType_Run_SetVelocity; + msg.header.crc = (uint32_t)-1; + msg.run_set_velocity.linear = cmd->linear.x; + msg.run_set_velocity.angular = cmd->angular.x; + + FMW_Message response_msg = this->stm32_message_send(&msg); + assert(response_msg.header.type == FMW_MessageType_Response); + assert(response_msg.response.result == FMW_Result_Ok); } void P3DX_Controller_Node::callback_service_command_change_mode_s(const std::shared_ptr request, @@ -252,12 +244,21 @@ void P3DX_Controller_Node::stm32_message_print(const FMW_Message *msg) 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", + "\n result = %s" + "\n delta_millis = %d" + "\n ticks_left = %d" + "\n ticks_right = %d" + "\n position_x = %f" + "\n position_y = %f" + "\n orientation_x = %f" + "\n orientation_y = %f" + "\n velocity_linear = %f" + "\n velocity_angular = %f", result_types[msg->response.result], msg->response.delta_millis, - msg->response.ticks_left, msg->response.ticks_right); + msg->response.ticks_left, msg->response.ticks_right, + msg->response.position_x, msg->response.position_y, + msg->response.orientation_x, msg->response.orientation_y, + msg->response.velocity_linear, msg->response.velocity_angular); assert(buffer_len < (int32_t)sizeof(buffer)); } break; default: { diff --git a/pioneer_workstation_ws/src/pioneer3dx_controller/src/stm32_messages.h b/pioneer_workstation_ws/src/pioneer3dx_controller/src/stm32_messages.h index 99a30e5..b2c08f0 100644 --- a/pioneer_workstation_ws/src/pioneer3dx_controller/src/stm32_messages.h +++ b/pioneer_workstation_ws/src/pioneer3dx_controller/src/stm32_messages.h @@ -107,10 +107,15 @@ typedef struct FMW_Message { float angular; } run_set_velocity; - // TODO(lb): add odometry information struct { int32_t ticks_left; int32_t ticks_right; + float position_x; + float position_y; + float orientation_x; + float orientation_y; + float velocity_linear; + float velocity_angular; uint16_t delta_millis; FMW_Result result; } response; @@ -118,9 +123,10 @@ typedef struct FMW_Message { } 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); +static_assert(sizeof(uint8_t) == 1); +static_assert(sizeof(uint16_t) == 2); +static_assert(sizeof(uint32_t) == 4); +static_assert(sizeof(float) == 4); +static_assert(sizeof(FMW_Message) == 41); #endif /* INC_COMMUNICATION_OTTO_MESSAGES_H_ */