diff --git a/motion/ateam_motion_benchmark/src/angular_velocity_benchmark.cpp b/motion/ateam_motion_benchmark/src/angular_velocity_benchmark.cpp index 37bedaf34..1304cadf7 100644 --- a/motion/ateam_motion_benchmark/src/angular_velocity_benchmark.cpp +++ b/motion/ateam_motion_benchmark/src/angular_velocity_benchmark.cpp @@ -355,8 +355,9 @@ class AngularVelocityBenchmarkNode : public rclcpp::Node const auto vision_speed = robot.state->twist_body.angular.z; const auto vision_perp_speed = robot.state->twist_body.linear.y; - const auto firmware_speed = robot.motion_feedback->cgkf_body_velocity_state_estimate[2]; - const auto firmware_perp_speed = robot.motion_feedback->cgkf_body_velocity_state_estimate[0]; + // TODO These values are not in the new packet definitions? + const auto firmware_speed = 0.0; // robot.motion_feedback-> cgkf_body_velocity_state_estimate[2]; + const auto firmware_perp_speed = 0.0; // robot.motion_feedback->cgkf_body_velocity_state_estimate[0]; DataEntry entry{ .time = std::chrono::duration_cast>( diff --git a/motion/ateam_motion_benchmark/src/velocity_benchmark.cpp b/motion/ateam_motion_benchmark/src/velocity_benchmark.cpp index 4d5f7db2b..3d4a65ce6 100644 --- a/motion/ateam_motion_benchmark/src/velocity_benchmark.cpp +++ b/motion/ateam_motion_benchmark/src/velocity_benchmark.cpp @@ -363,12 +363,15 @@ class VelocityBenchmarkNode : public rclcpp::Node robot.state->twist_body.linear.x : robot.state->twist_body.linear.y; const auto vision_perp_speed = options_.axis == Axis::X ? robot.state->twist_body.linear.y : robot.state->twist_body.linear.x; - const auto firmware_speed = options_.axis == Axis::X ? - robot.motion_feedback->cgkf_body_velocity_state_estimate[0] : - robot.motion_feedback->cgkf_body_velocity_state_estimate[1]; - const auto firmware_perp_speed = options_.axis == Axis::X ? - robot.motion_feedback->cgkf_body_velocity_state_estimate[1] : - robot.motion_feedback->cgkf_body_velocity_state_estimate[0]; + // TODO These values are not in the new packet definitions? + // const auto firmware_speed = options_.axis == Axis::X ? + // robot.motion_feedback->cgkf_body_velocity_state_estimate[0] : + // robot.motion_feedback->cgkf_body_velocity_state_estimate[1]; + // const auto firmware_perp_speed = options_.axis == Axis::X ? + // robot.motion_feedback->cgkf_body_velocity_state_estimate[1] : + // robot.motion_feedback->cgkf_body_velocity_state_estimate[0]; + const auto firmware_speed = 0.0; + const auto firmware_perp_speed = 0.0; DataEntry entry{ .time = std::chrono::duration_cast>( diff --git a/radio/ateam_radio_bridge/src/firmware_parameter_server.cpp b/radio/ateam_radio_bridge/src/firmware_parameter_server.cpp index 3de4fafeb..ec164df2d 100644 --- a/radio/ateam_radio_bridge/src/firmware_parameter_server.cpp +++ b/radio/ateam_radio_bridge/src/firmware_parameter_server.cpp @@ -72,7 +72,7 @@ void FirmwareParameterServer::GetFirmwareParameterCallback(const ateam_msgs::srv response_ready_ = false; connections_[robot_id]->send( reinterpret_cast(&command_packet), - GetPacketSize(command_packet.command_code)); + GetPacketSize(command_packet.header.command_code)); if(!response_cv_.wait_for(lock, std::chrono::milliseconds(300), [this]{ return response_ready_.load(); })) { response->success = false; response->reason = "Timed out waiting for reply."; @@ -124,7 +124,7 @@ void FirmwareParameterServer::SetFirmwareParameterCallback(const ateam_msgs::srv response_ready_ = false; connections_[robot_id]->send( reinterpret_cast(&command_packet), - GetPacketSize(command_packet.command_code)); + GetPacketSize(command_packet.header.command_code)); if(!response_cv_.wait_for(lock, std::chrono::milliseconds(100), [this]{ return response_ready_.load(); })) { response->success = false; response->reason = "Timed out waiting for reply."; diff --git a/radio/ateam_radio_bridge/src/radio_bridge_node.cpp b/radio/ateam_radio_bridge/src/radio_bridge_node.cpp index 7da95b046..a6680322e 100644 --- a/radio/ateam_radio_bridge/src/radio_bridge_node.cpp +++ b/radio/ateam_radio_bridge/src/radio_bridge_node.cpp @@ -33,6 +33,7 @@ #include #include #include +#include #include #include #include @@ -201,7 +202,7 @@ class RadioBridgeNode : public rclcpp::Node const auto packet = CreateEmptyPacket(CC_GOODBYE); connection->send( reinterpret_cast(&packet), - GetPacketSize(packet.command_code)); + GetPacketSize(packet.header.command_code)); // Give some time for the message to actually send before closing the connection std::this_thread::sleep_for(std::chrono::milliseconds(50)); } @@ -263,22 +264,31 @@ class RadioBridgeNode : public rclcpp::Node control_msg.game_state_in_stop = game_controller_listener_.GetGameCommand() == ateam_common::GameCommand::Stop; control_msg.emergency_stop = false; - control_msg.body_vel_controls_enabled = get_parameter("controls_enabled.body_vel").as_bool(); control_msg.wheel_vel_control_enabled = get_parameter("controls_enabled.wheel_vel").as_bool(); control_msg.wheel_torque_control_enabled = get_parameter("controls_enabled.wheel_torque").as_bool(); - control_msg.play_song = 0; - control_msg.vel_x_linear = motion_commands_[id].twist.linear.x; - control_msg.vel_y_linear = motion_commands_[id].twist.linear.y; - control_msg.vel_z_angular = motion_commands_[id].twist.angular.z; - control_msg.dribbler_speed = motion_commands_[id].dribbler_speed; - control_msg.dribbler_multiplier = 55; + control_msg.vision_update = 0; + control_msg.reserved1 = 0; + control_msg.vision_position_update[0] = 0.0f; + control_msg.vision_position_update[1] = 0.0f; + control_msg.vision_position_update[2] = 0.0f; + control_msg.body_control_mode = BCM_LOCAL_VELOCITY; control_msg.kick_request = static_cast(motion_commands_[id].kick_request); + control_msg.play_song = 0; + control_msg.reserved2[0] = 0; control_msg.kick_vel = motion_commands_[id].kick_speed; + control_msg.dribbler_speed = motion_commands_[id].dribbler_speed; + control_msg.cmd.local_vel = { + static_cast(motion_commands_[id].twist.linear.x), + static_cast(motion_commands_[id].twist.linear.y), + static_cast(motion_commands_[id].twist.angular.z), + 0.0, // TODO(barulicm): max_linear_acc + 0.0 // TODO(barulicm): max_angular_acc + }; const auto control_packet = CreatePacket(CC_CONTROL, control_msg); connections_[id]->send( reinterpret_cast(&control_packet), - GetPacketSize(control_packet.command_code)); + GetPacketSize(control_packet.header.command_code)); } } @@ -293,10 +303,10 @@ class RadioBridgeNode : public rclcpp::Node return; } - if (packet.command_code != CC_HELLO_REQ) { + if (packet.header.command_code != CC_HELLO_REQ) { RCLCPP_WARN( get_logger(), "Ignoring discovery packet. Unexpected command code: %d", - packet.command_code); + packet.header.command_code); return; } @@ -313,6 +323,21 @@ class RadioBridgeNode : public rclcpp::Node HelloRequest hello_data = std::get(data_variant); + const uint32_t incoming_coms_hash = hello_data.coms_hash[0] | (hello_data.coms_hash[1] << 8) | + (hello_data.coms_hash[2] << 16) | (hello_data.coms_hash[3] << 24); + if (incoming_coms_hash != ateam_radio_msgs::kComsHash) { + RCLCPP_WARN(get_logger(), "Ignoring discovery packet. Packet version hash mismatch."); + return; + } + + if (ateam_radio_msgs::kComsDirty) { + RCLCPP_WARN(get_logger(), "Local packet version is dirty. Compatibility check may be unreliable."); + } + + if (hello_data.coms_repo_dirty) { + RCLCPP_WARN(get_logger(), "Remote robot's packet version is dirty. Compatibility check may be unreliable."); + } + if (!(game_controller_listener_.GetTeamColor() == ateam_common::TeamColor::Blue && hello_data.color == TC_BLUE) && !(game_controller_listener_.GetTeamColor() == ateam_common::TeamColor::Yellow && @@ -330,7 +355,7 @@ class RadioBridgeNode : public rclcpp::Node const auto reply_packet = CreateEmptyPacket(CC_NACK); discovery_receiver_.SendTo( sender_address, sender_port, - reinterpret_cast(&reply_packet), GetPacketSize(reply_packet.command_code)); + reinterpret_cast(&reply_packet), GetPacketSize(reply_packet.header.command_code)); RCLCPP_WARN(get_logger(), "Rejecting discovery packet. Invalid robot ID: %d", robot_id); return; } @@ -344,7 +369,7 @@ class RadioBridgeNode : public rclcpp::Node const auto reply_packet = CreateEmptyPacket(CC_NACK); discovery_receiver_.SendTo( sender_address, sender_port, - reinterpret_cast(&reply_packet), GetPacketSize(reply_packet.command_code)); + reinterpret_cast(&reply_packet), GetPacketSize(reply_packet.header.command_code)); RCLCPP_WARN(get_logger(), "Rejecting discovery packet. Robot ID already connected: %d", robot_id); return; @@ -373,7 +398,7 @@ class RadioBridgeNode : public rclcpp::Node const auto reply_packet = CreatePacket(CC_HELLO_RESP, response); discovery_receiver_.SendTo( sender_address, sender_port, - reinterpret_cast(&reply_packet), GetPacketSize(reply_packet.command_code)); + reinterpret_cast(&reply_packet), GetPacketSize(reply_packet.header.command_code)); } void RobotIncomingPacketCallback( @@ -389,7 +414,7 @@ class RadioBridgeNode : public rclcpp::Node const std::lock_guard lock(mutex_); - switch (packet.command_code) { + switch (packet.header.command_code) { case CC_GOODBYE: // close connection. No need to send our own goodbye RCLCPP_INFO(get_logger(), "Received goodbye from robot %d.", robot_id); @@ -446,7 +471,7 @@ class RadioBridgeNode : public rclcpp::Node default: RCLCPP_WARN( get_logger(), "Ignoring telemetry message from robot %d. Unsupported command code: %d", - robot_id, packet.command_code); + robot_id, packet.header.command_code); return; } } diff --git a/radio/ateam_radio_bridge/src/rnp_packet_helpers.cpp b/radio/ateam_radio_bridge/src/rnp_packet_helpers.cpp index 191538dbc..bb76bc62f 100644 --- a/radio/ateam_radio_bridge/src/rnp_packet_helpers.cpp +++ b/radio/ateam_radio_bridge/src/rnp_packet_helpers.cpp @@ -41,40 +41,41 @@ namespace ateam_radio_bridge */ std::size_t GetPacketSize(const CommandCode & command_code) { + const auto packet_header_size = sizeof(RadioHeader); // For now, packet size only depends on command code switch (command_code) { case CC_ACK: - return kPacketHeaderSize; + return packet_header_size; break; case CC_NACK: - return kPacketHeaderSize; + return packet_header_size; break; case CC_GOODBYE: - return kPacketHeaderSize; + return packet_header_size; break; case CC_KEEPALIVE: - return kPacketHeaderSize; + return packet_header_size; break; case CC_HELLO_REQ: - return kPacketHeaderSize + sizeof(HelloRequest); + return packet_header_size + sizeof(HelloRequest); break; case CC_TELEMETRY: - return kPacketHeaderSize + sizeof(BasicTelemetry); + return packet_header_size + sizeof(BasicTelemetry); break; case CC_CONTROL: - return kPacketHeaderSize + sizeof(BasicControl); + return packet_header_size + sizeof(BasicControl); break; case CC_HELLO_RESP: - return kPacketHeaderSize + sizeof(HelloResponse); + return packet_header_size + sizeof(HelloResponse); break; case CC_CONTROL_DEBUG_TELEMETRY: - return kPacketHeaderSize + sizeof(ExtendedTelemetry); + return packet_header_size + sizeof(ExtendedTelemetry); break; case CC_ROBOT_PARAMETER_COMMAND: - return kPacketHeaderSize + sizeof(ParameterCommand); + return packet_header_size + sizeof(ParameterCommand); break; default: - throw std::invalid_argument("Unrecognized command code."); + throw std::invalid_argument("Unrecognized command code: " + std::to_string(command_code)); } } @@ -89,13 +90,13 @@ std::size_t GetPacketSize(const CommandCode & command_code) */ void SetCRC(RadioPacket & packet) { - const auto crc_size = sizeof(packet.crc32); - const auto packet_size = GetPacketSize(packet.command_code); + const auto crc_size = sizeof(packet.header.crc32); + const auto packet_size = GetPacketSize(packet.header.command_code); boost::crc_32_type crc; crc.process_bytes( reinterpret_cast(reinterpret_cast(&packet) + crc_size), packet_size - crc_size); - packet.crc32 = crc.checksum(); + packet.header.crc32 = crc.checksum(); } /** @@ -109,13 +110,13 @@ void SetCRC(RadioPacket & packet) */ bool HasCorrectCRC(const RadioPacket & packet) { - const auto crc_size = sizeof(packet.crc32); - const auto packet_size = GetPacketSize(packet.command_code); + const auto crc_size = sizeof(packet.header.crc32); + const auto packet_size = GetPacketSize(packet.header.command_code); boost::crc_32_type crc; crc.process_bytes( reinterpret_cast(reinterpret_cast(&packet) + crc_size), packet_size - crc_size); - return packet.crc32 == crc.checksum(); + return packet.header.crc32 == crc.checksum(); } /** @@ -132,11 +133,12 @@ bool HasCorrectCRC(const RadioPacket & packet) RadioPacket CreateEmptyPacket(const CommandCode command_code) { RadioPacket packet{ - 0, - kProtocolVersionMajor, - kProtocolVersionMinor, - command_code, - 0, + RadioHeader{ + 0, // crc32 + command_code, + 0, // reserved + 0 // data length + }, {} }; @@ -160,11 +162,19 @@ RadioPacket CreateEmptyPacket(const CommandCode command_code) */ RadioPacket ParsePacket(const uint8_t * data, const std::size_t data_length, std::string & error) { + const auto packet_header_size = sizeof(RadioHeader); + RadioPacket packet; - std::copy_n(data, kPacketHeaderSize, reinterpret_cast(&packet)); + std::copy_n(data, packet_header_size, reinterpret_cast(&packet)); - const auto packet_size = GetPacketSize(packet.command_code); + std::size_t packet_size = 0; + try { + packet_size = GetPacketSize(packet.header.command_code); + } catch (const std::invalid_argument & e) { + error = "Error getting packet size: " + std::string(e.what()); + return {}; + } if (data_length != packet_size) { error = "Wrong number of bytes. Expected " + std::to_string(packet_size) + " but got " + @@ -172,16 +182,8 @@ RadioPacket ParsePacket(const uint8_t * data, const std::size_t data_length, std return {}; } - if (packet.major_version != kProtocolVersionMajor || - packet.minor_version != kProtocolVersionMinor) - { - // TODO(barulicm) What should our version compatability rules actually be? This assumes they must match. - error = "Protocol versions do not match."; - return {}; - } - std::copy_n( - data + kPacketHeaderSize, packet_size - kPacketHeaderSize, + data + packet_header_size, packet_size - packet_header_size, reinterpret_cast(&packet.data)); // TODO(barulicm) Firmware doesn't implement CRCs yet @@ -210,10 +212,10 @@ PacketDataVariant ExtractData(const RadioPacket & packet, std::string & error) { PacketDataVariant var; - switch (packet.command_code) { + switch (packet.header.command_code) { case CC_HELLO_REQ: { - if (packet.data_length != sizeof(HelloRequest)) { + if (packet.header.data_length != sizeof(HelloRequest)) { error = "Incorrect data length for HelloData type."; break; } @@ -222,7 +224,7 @@ PacketDataVariant ExtractData(const RadioPacket & packet, std::string & error) } case CC_TELEMETRY: { - if (packet.data_length != sizeof(BasicTelemetry)) { + if (packet.header.data_length != sizeof(BasicTelemetry)) { error = "Incorrect data length for BasicTelemetry type."; break; } @@ -231,16 +233,16 @@ PacketDataVariant ExtractData(const RadioPacket & packet, std::string & error) } case CC_CONTROL_DEBUG_TELEMETRY: { - if (packet.data_length != sizeof(ExtendedTelemetry)) { + if (packet.header.data_length != sizeof(ExtendedTelemetry)) { error = "Incorrect data length for ExtendedTelemetry type."; break; } - var = packet.data.control_debug_telemetry; + var = packet.data.extended_telemetry; break; } case CC_ROBOT_PARAMETER_COMMAND: { - if (packet.data_length != sizeof(ParameterCommand)) { + if (packet.header.data_length != sizeof(ParameterCommand)) { error = "Incorrect data length for ParameterCommand type."; break; } @@ -249,7 +251,7 @@ PacketDataVariant ExtractData(const RadioPacket & packet, std::string & error) } case CC_CONTROL: { - if (packet.data_length != sizeof(BasicControl)) { + if (packet.header.data_length != sizeof(BasicControl)) { error = "Incorrect data length for BasicControl type."; break; } @@ -267,28 +269,38 @@ PacketDataVariant ExtractData(const RadioPacket & packet, std::string & error) ParameterDataFormat GetParameterDataFormatForParameter(const ParameterName & parameter) { switch(parameter) { - case VEL_PID_X: - return PID_LIMITED_INTEGRAL_F32; - case VEL_PID_Y: - return PID_LIMITED_INTEGRAL_F32; - case ANGULAR_VEL_PID_Z: - return PID_LIMITED_INTEGRAL_F32; - case VEL_CGKF_ENCODER_NOISE: - return F32; - case VEL_CGKF_GYRO_NOISE: - return F32; - case VEL_CGKF_PROCESS_NOISE: - return F32; - case VEL_CGFK_INITIAL_COVARIANCE: - return F32; - case VEL_CGKF_K_MATRIX: - return MATRIX_F32; - case RC_BODY_VEL_LIMIT: - return VEC3_F32; - case RC_BODY_ACC_LIMIT: - return VEC3_F32; - case RC_WHEEL_ACC_LIMIT: + case KF_PROCESS_STD: + return VEC4_F32; + case KF_MEASUREMENT_STD: return VEC4_F32; + case KF_MAX_STATE: + return VEC4_F32; + case PHYS_WHEEL: + return VEC4_F32; + case PHYS_INERTIA: + return VEC2_F32; + case PHYS_MOTOR_MODEL: + return VEC2_F32; + case PHYS_FRICTION_MODEL: + return VEC4_F32; + case POSE_CONTROL_GAIN: + return VEC2_F32; + case TRAJ_RECOMPUTE_ERROR: + return VEC4_F32; + case TRAJ_MAX: + return VEC4_F32; + case POSE_FB_PIDII_X: + return VEC5_F32; + case POSE_FB_PIDII_Y: + return VEC5_F32; + case POSE_FB_PIDII_THETA: + return VEC5_F32; + case TWIST_FB_PIDII_X: + return VEC5_F32; + case TWIST_FB_PIDII_Y: + return VEC5_F32; + case TWIST_FB_PIDII_THETA: + return VEC5_F32; default: throw std::invalid_argument("GetParameterDataFormatForParameter: Unrecognized parameter name."); } @@ -299,16 +311,14 @@ std::size_t GetDataSizeForParameterFormat(const ParameterDataFormat & format) switch(format) { case F32: return 1; + case VEC2_F32: + return 2; case VEC3_F32: return 3; case VEC4_F32: return 4; - case PID_F32: - return 3; - case PID_LIMITED_INTEGRAL_F32: + case VEC5_F32: return 5; - case MATRIX_F32: - return 25; default: throw std::invalid_argument("GetDataSizeForParameterFormat: Unrecognized parameter data format."); } @@ -320,16 +330,14 @@ float* GetParameterDataForSetFormat(ParameterCommand & command) switch(command.data_format) { case F32: return &command.data.f32; + case VEC2_F32: + return command.data.vec2_f32; case VEC3_F32: return command.data.vec3_f32; case VEC4_F32: return command.data.vec4_f32; - case PID_F32: - return command.data.pid_f32; - case PID_LIMITED_INTEGRAL_F32: - return command.data.pidii_f32; - case MATRIX_F32: - return command.data.matrix_f32; + case VEC5_F32: + return command.data.vec5_f32; default: throw std::invalid_argument("GetParameterDataForSetFormat: Unrecognized parameter data format."); } diff --git a/radio/ateam_radio_bridge/src/rnp_packet_helpers.hpp b/radio/ateam_radio_bridge/src/rnp_packet_helpers.hpp index f773bfa55..d3c061b9d 100644 --- a/radio/ateam_radio_bridge/src/rnp_packet_helpers.hpp +++ b/radio/ateam_radio_bridge/src/rnp_packet_helpers.hpp @@ -29,7 +29,7 @@ #include #include #include -#include +#include #include #include #include @@ -39,8 +39,6 @@ namespace ateam_radio_bridge std::size_t GetPacketSize(const CommandCode & packet); -constexpr std::size_t kPacketHeaderSize = 12; - void SetCRC(RadioPacket & packet); bool HasCorrectCRC(const RadioPacket & packet); @@ -48,20 +46,21 @@ bool HasCorrectCRC(const RadioPacket & packet); template void SetDataPayload(RadioPacket & packet, const DataTypeType & payload) { - packet.data_length = sizeof(DataTypeType); + packet.header.data_length = sizeof(DataTypeType); auto payload_ptr = reinterpret_cast(&payload); - std::copy_n(payload_ptr, packet.data_length, reinterpret_cast(&packet.data)); + std::copy_n(payload_ptr, packet.header.data_length, reinterpret_cast(&packet.data)); } template RadioPacket CreatePacket(const CommandCode command_code, const DataTypeType & data) { RadioPacket packet{ - 0, - kProtocolVersionMajor, - kProtocolVersionMinor, - command_code, - 0, + RadioHeader{ + 0, // crc32 will be set later + command_code, + 0, // reserved + 0, // data length + }, {} }; diff --git a/radio/ateam_radio_bridge/test/launch_tests/bridge_command_test.py b/radio/ateam_radio_bridge/test/launch_tests/bridge_command_test.py index a98bacc40..e84ea8997 100644 --- a/radio/ateam_radio_bridge/test/launch_tests/bridge_command_test.py +++ b/radio/ateam_radio_bridge/test/launch_tests/bridge_command_test.py @@ -84,10 +84,10 @@ def test_commands(self): break self.cmd_pub.publish(cmd_msg) last_packet = self.robot.getLastCmdMessage() - if len(last_packet) != 40: + if len(last_packet) != 64: continue # Extract BasicControl.vel_x_linear - vel_x_linear = struct.unpack(" +#include #include @@ -29,32 +30,34 @@ TEST(SetCRC, BasicTest) { RadioPacket packet{ - 0, - 1, - 2, - CC_ACK, - 0, + { + 0, + CC_ACK, + 0, + 0 + }, {} }; ateam_radio_bridge::SetCRC(packet); - EXPECT_EQ(packet.crc32, 1560025497u); + EXPECT_EQ(packet.header.crc32, 2583214201u); } // Do we reject (some) bad CRC values? TEST(HasCorrectCRC, BasicTest) { RadioPacket packet{ - 1560025497, - 1, - 2, - CC_ACK, - 0, + { + 2583214201, + CC_ACK, + 0, + 0 + }, {} }; EXPECT_TRUE(ateam_radio_bridge::HasCorrectCRC(packet)); - packet.crc32 = 12; // arbitrary bad CRC value + packet.header.crc32 = 12; // arbitrary bad CRC value EXPECT_FALSE(ateam_radio_bridge::HasCorrectCRC(packet)); } @@ -63,11 +66,19 @@ TEST(SetDataPayload, HelloRequest) { RadioPacket packet; HelloRequest payload{ - 8, - TC_BLUE + 8, // robot ID + TC_BLUE, + 0, // coms repo dirty + 0, // controls repo dirty + 0, // firmware repo dirty + 0, // bitfield reserved + 0, // reserved + {0, 0, 0, 0}, // coms hash + {0, 0, 0, 0}, // controls hash + {0, 0, 0, 0} // firmware hash }; ateam_radio_bridge::SetDataPayload(packet, payload); - EXPECT_EQ(packet.data_length, 2); + EXPECT_EQ(packet.header.data_length, 16); EXPECT_EQ(packet.data.hello_request.robot_id, 8); EXPECT_EQ(packet.data.hello_request.color, TC_BLUE); } @@ -76,15 +87,21 @@ TEST(SetDataPayload, HelloRequest) TEST(CreatePacket, HelloRequest) { HelloRequest payload{ - 12, - TC_YELLOW + 12, // robot ID + TC_YELLOW, + 0, // coms repo dirty + 0, // controls repo dirty + 0, // firmware repo dirty + 0, // bitfield reserved + 0, // reserved + {0, 0, 0, 0}, // coms hash + {0, 0, 0, 0}, // controls hash + {0, 0, 0, 0} // firmware hash }; RadioPacket packet = ateam_radio_bridge::CreatePacket(CC_HELLO_REQ, payload); - EXPECT_EQ(packet.crc32, 1906667910u); - EXPECT_EQ(packet.major_version, kProtocolVersionMajor); - EXPECT_EQ(packet.minor_version, kProtocolVersionMinor); - EXPECT_EQ(packet.command_code, CC_HELLO_REQ); - EXPECT_EQ(packet.data_length, 2); + EXPECT_EQ(packet.header.crc32, 1293447015u); + EXPECT_EQ(packet.header.command_code, CC_HELLO_REQ); + EXPECT_EQ(packet.header.data_length, 16); EXPECT_EQ(packet.data.hello_request.robot_id, 12); EXPECT_EQ(packet.data.hello_request.color, TC_YELLOW); } @@ -93,54 +110,65 @@ TEST(CreatePacket, HelloRequest) TEST(CreateEmptyPacket, Ack) { RadioPacket packet = ateam_radio_bridge::CreateEmptyPacket(CC_ACK); - EXPECT_EQ(packet.crc32, 381840297u); - EXPECT_EQ(packet.major_version, kProtocolVersionMajor); - EXPECT_EQ(packet.minor_version, kProtocolVersionMinor); - EXPECT_EQ(packet.command_code, CC_ACK); - EXPECT_EQ(packet.data_length, 0); + EXPECT_EQ(packet.header.crc32, 2583214201u); + EXPECT_EQ(packet.header.command_code, CC_ACK); + EXPECT_EQ(packet.header.data_length, 0); } // Are we parsing packets correctly? TEST(ParsePacket, HelloRequest) { - std::array data = { - 0, - 0, - 0, - 0, - kProtocolVersionMajor & 0xFF, - kProtocolVersionMajor >> 8, - kProtocolVersionMinor & 0xFF, - kProtocolVersionMinor >> 8, - CC_HELLO_REQ, - 0, - 2, - 0, - 15, - 1 + std::array data = { + 0, 0, 0, 0, // crc + CC_HELLO_REQ, // command code + 0, // reserved + 16, 0, // data length + 15, // robot ID + 1, // team color + 5, // dirty flags + 0, // reserved + 1, 2, 3, 4, // coms hash + 5, 6, 7, 8, // controls hash + 9, 10, 11, 12 // firmware hash }; std::string error_msg; RadioPacket packet = ateam_radio_bridge::ParsePacket(data.data(), data.size(), error_msg); EXPECT_TRUE(error_msg.empty()) << "Unexpected error message: " << error_msg; - EXPECT_EQ(packet.crc32, 0u); - EXPECT_EQ(packet.major_version, kProtocolVersionMajor); - EXPECT_EQ(packet.minor_version, kProtocolVersionMinor); - EXPECT_EQ(packet.command_code, CC_HELLO_REQ); - EXPECT_EQ(packet.data_length, 2); + EXPECT_EQ(packet.header.crc32, 0u); + EXPECT_EQ(packet.header.command_code, CC_HELLO_REQ); + EXPECT_EQ(packet.header.data_length, 16); EXPECT_EQ(packet.data.hello_request.robot_id, 15); EXPECT_EQ(packet.data.hello_request.color, TC_BLUE); + EXPECT_EQ(packet.data.hello_request.coms_repo_dirty, 1); + EXPECT_EQ(packet.data.hello_request.controls_repo_dirty, 0); + EXPECT_EQ(packet.data.hello_request.firmware_repo_dirty, 1); + EXPECT_THAT(packet.data.hello_request.coms_hash, testing::ElementsAreArray({1, 2, 3, 4})); + EXPECT_THAT(packet.data.hello_request.controls_hash, testing::ElementsAreArray({5, 6, 7, 8})); + EXPECT_THAT(packet.data.hello_request.firmware_hash, testing::ElementsAreArray({9, 10, 11, 12})); } // Are we able to extract data from a packet correctly? TEST(ExtractData, HelloRequest) { RadioPacket packet{ - 0, - 0, - 0, - CC_HELLO_REQ, - 2, - HelloRequest{4, TC_BLUE} + { + 0, + CC_HELLO_REQ, + 0, + 16 + }, + HelloRequest{ + 4, // robot ID + TC_BLUE, + 0, // coms repo dirty + 0, // controls repo dirty + 0, // firmware repo dirty + 0, // bitfield reserved + 0, // reserved + {0, 0, 0, 0}, // coms hash + {0, 0, 0, 0}, // controls hash + {0, 0, 0, 0} // firmware hash + } }; std::string error_msg; ateam_radio_bridge::PacketDataVariant data_var = diff --git a/radio/ateam_radio_msgs/CMakeLists.txt b/radio/ateam_radio_msgs/CMakeLists.txt index d93bc9f4b..9737d4d3e 100644 --- a/radio/ateam_radio_msgs/CMakeLists.txt +++ b/radio/ateam_radio_msgs/CMakeLists.txt @@ -3,6 +3,7 @@ project(ateam_radio_msgs) include(cmake/generate_msgs.cmake) include(cmake/generate_conversion_code.cmake) +include(cmake/generate_version_header.cmake) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) @@ -17,17 +18,27 @@ set(GENERATED_DIR ${CMAKE_CURRENT_BINARY_DIR}/ateam_generated) set(RADIO_PACKETS_INCLUDE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/software-communication/ateam-common-packets/include) set(RADIO_STRUCTS_TO_GENERATE + GlobalPositionCommand + GlobalVelocityCommand + LocalVelocityCommand + GlobalAccelerationCommand + LocalAccelerationCommand BasicTelemetry ExtendedTelemetry + BodyControlTelemetry + BodyControlExtendedTelemetry KickerTelemetry MotorTelemetry + CcmTelemetry + CcmVelocityTelemetry + CcmCurrentTelemetry PowerTelemetry BatteryInfo ) generate_msgs( SOURCE ${RADIO_PACKETS_INCLUDE_DIR}/radio.h - DESTINATION ${GENERATED_DIR}/msg + DESTINATION ${GENERATED_DIR} STRUCTS ${RADIO_STRUCTS_TO_GENERATE} ) @@ -37,17 +48,21 @@ generate_conversion_code( STRUCTS ${RADIO_STRUCTS_TO_GENERATE} ) +generate_version_header() + file(GLOB CONFIG_DEPENDS CONFIGURE_DEPENDS "${RADIO_PACKETS_INCLUDE_DIR}/*.*") set_property(DIRECTORY APPEND PROPERTY CMAKE_CONFIGURE_DEPENDS ${CONFIG_DEPENDS}) +file(GLOB _raw_generated_msgs "${GENERATED_DIR}/msg/*.msg") +set(_generated_msgs "") +foreach(_msg ${_raw_generated_msgs}) + string(REPLACE "${GENERATED_DIR}/" "" _rel "${_msg}") + list(APPEND _generated_msgs "${GENERATED_DIR}:${_rel}") +endforeach() + set(MSG_TARGET ${PROJECT_NAME}) rosidl_generate_interfaces(${MSG_TARGET} - ${GENERATED_DIR}:msg/BasicTelemetry.msg - ${GENERATED_DIR}:msg/ExtendedTelemetry.msg - ${GENERATED_DIR}:msg/KickerTelemetry.msg - ${GENERATED_DIR}:msg/MotorTelemetry.msg - ${GENERATED_DIR}:msg/PowerTelemetry.msg - ${GENERATED_DIR}:msg/BatteryInfo.msg + ${_generated_msgs} msg/ConnectionStatus.msg @@ -82,9 +97,8 @@ install( DIRECTORY ${GENERATED_DIR}/include/ DESTINATION include/ateam_radio_msgs/ateam_radio_msgs ) -file(GLOB SYMLINKED_INCLUDES include/ateam_radio_msgs/packets/*) install( - FILES ${SYMLINKED_INCLUDES} + DIRECTORY ${RADIO_PACKETS_INCLUDE_DIR}/ DESTINATION include/ateam_radio_msgs/ateam_radio_msgs/packets ) install( @@ -98,7 +112,7 @@ install( if(BUILD_TESTING) file(GLOB_RECURSE submodule_files software-communication/*) - file(GLOB symlinked_headers include/ateam_radio_msgs/packets/*) + file(GLOB_RECURSE symlinked_headers include/ateam_radio_msgs/packets/*) set(_linter_excludes software-communication ${submodule_files} diff --git a/radio/ateam_radio_msgs/cmake/generate_msgs.cmake b/radio/ateam_radio_msgs/cmake/generate_msgs.cmake index 294bf44f8..67aa2a0d7 100644 --- a/radio/ateam_radio_msgs/cmake/generate_msgs.cmake +++ b/radio/ateam_radio_msgs/cmake/generate_msgs.cmake @@ -32,20 +32,17 @@ function(generate_msgs) message(FATAL_ERROR "At least one struct must be specified.") endif() - file(MAKE_DIRECTORY "${arg_DESTINATION}") + set(output_msg_dir "${arg_DESTINATION}/msg") + + file(MAKE_DIRECTORY "${output_msg_dir}") set(_generate_msgs_script "${CMAKE_CURRENT_SOURCE_DIR}/scripts/generate_msgs.py") if(NOT EXISTS "${_generate_msgs_script}") message(FATAL_ERROR "Script ${_generate_msgs_script} does not exist.") endif() - set(${generated_msgs_files} "") - foreach(struct ${arg_STRUCTS}) - list(APPEND generated_msgs_files "${arg_DESTINATION}/${struct}.msg") - endforeach() - execute_process( - COMMAND python3 ${_generate_msgs_script} ${arg_DESTINATION} ${arg_SOURCE} ${arg_STRUCTS} + COMMAND python3 ${_generate_msgs_script} ${output_msg_dir} ${arg_SOURCE} ${arg_STRUCTS} RESULT_VARIABLE result OUTPUT_VARIABLE output ERROR_VARIABLE error diff --git a/radio/ateam_radio_msgs/cmake/generate_version_header.cmake b/radio/ateam_radio_msgs/cmake/generate_version_header.cmake new file mode 100644 index 000000000..ff246e6cb --- /dev/null +++ b/radio/ateam_radio_msgs/cmake/generate_version_header.cmake @@ -0,0 +1,58 @@ +# Copyright 2026 A Team +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + + +function(generate_version_header) + set(SOFT_COMS_SUBMODULE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/software-communication") + + find_package(Git REQUIRED) + + execute_process( + COMMAND ${GIT_EXECUTABLE} rev-parse HEAD + WORKING_DIRECTORY ${SOFT_COMS_SUBMODULE_DIR} + OUTPUT_VARIABLE SOFT_COMS_SUBMODULE_SHA + OUTPUT_STRIP_TRAILING_WHITESPACE + ERROR_QUIET + ) + + string(SUBSTRING "${SOFT_COMS_SUBMODULE_SHA}" 0 8 SOFT_COMS_SUBMODULE_SHA_SHORT) + set(_hex_string "0x${SOFT_COMS_SUBMODULE_SHA_SHORT}") + math(EXPR SOFT_COMS_SUBMODULE_SHA_SHORT_NUM "${_hex_string} + 0") + + execute_process( + COMMAND ${GIT_EXECUTABLE} status --porcelain + WORKING_DIRECTORY ${SOFT_COMS_SUBMODULE_DIR} + OUTPUT_VARIABLE SOFT_COMS_SUBMODULE_STATUS + OUTPUT_STRIP_TRAILING_WHITESPACE + ) + + if(SOFT_COMS_SUBMODULE_STATUS) + set(SOFT_COMS_SUBMODULE_DIRTY true) + else() + set(SOFT_COMS_SUBMODULE_DIRTY false) + endif() + + configure_file( + ${CMAKE_CURRENT_SOURCE_DIR}/templates/version.hpp.in + "${CMAKE_CURRENT_BINARY_DIR}/ateam_generated/include/version.hpp" + @ONLY + ) + +endfunction() diff --git a/radio/ateam_radio_msgs/scripts/generate_conversion_code.py b/radio/ateam_radio_msgs/scripts/generate_conversion_code.py index a1c907826..f1a10586b 100644 --- a/radio/ateam_radio_msgs/scripts/generate_conversion_code.py +++ b/radio/ateam_radio_msgs/scripts/generate_conversion_code.py @@ -28,15 +28,69 @@ import clang.cindex +def _is_elaborated_union(field_node): + """Return True if the field's type is a typedef'd union.""" + if field_node.type.kind != clang.cindex.TypeKind.ELABORATED: + return False + # get_declaration() on a typedef'd union returns TYPEDEF_DECL, not UNION_DECL. + # The canonical type strips the typedef layer and gives us the underlying record. + canon = field_node.type.get_canonical() + return ( + canon.kind == clang.cindex.TypeKind.RECORD + and canon.get_declaration().kind == clang.cindex.CursorKind.UNION_DECL + ) + + +def collect_union_member_types(file_path, struct_names): + """Return a list of struct type names used as union members inside struct_names types.""" + index = clang.cindex.Index.create() + tu = index.parse(file_path) + extras = [] + struct_like = (clang.cindex.CursorKind.STRUCT_DECL, clang.cindex.CursorKind.UNION_DECL) + for node in tu.cursor.get_children(): + if node.location.file is None or node.location.file.name != file_path: + continue + if node.kind not in struct_like or node.spelling not in struct_names: + continue + for field in node.get_children(): + if field.kind != clang.cindex.CursorKind.FIELD_DECL: + continue + if not _is_elaborated_union(field): + continue + for member in field.type.get_canonical().get_declaration().get_children(): + if member.kind == clang.cindex.CursorKind.FIELD_DECL: + extras.append(member.type.spelling) + return extras + + def generate_conversion_code(output_directory, header_file, struct_names): """Generate conversion functions.""" + include_root = pathlib.Path(header_file).parent index = clang.cindex.Index.create() - translation_unit = index.parse(header_file) - generate_header_file(output_directory, translation_unit, struct_names) - generate_implementation_file(output_directory, translation_unit, struct_names) + header_tus = [ + (h, index.parse(str(h))) for h in sorted(include_root.rglob('*.h')) + ] + # Expand struct_names with types used as union members so Convert() functions are + # generated for them — they aren't in the original list but each needs its own + # converter because ROS2 unions are expanded into separate named fields. + all_struct_names = list(struct_names) + for header_path, _ in header_tus: + for extra in collect_union_member_types(str(header_path), struct_names): + if extra not in all_struct_names: + all_struct_names.append(extra) + generate_header_file(output_directory, header_tus, all_struct_names, include_root) + generate_implementation_file(output_directory, header_tus, all_struct_names) + + +def _nodes_in_file(translation_unit, header_path): + """Yield only top-level nodes whose definition is in header_path.""" + path_str = str(header_path) + for node in translation_unit.cursor.get_children(): + if node.location.file and node.location.file.name == path_str: + yield node -def generate_header_file(output_directory, translation_unit, struct_names): +def generate_header_file(output_directory, header_tus, struct_names, include_root): """Generate header file for conversion functions.""" header_text = ( '// Auto-generated conversion functions for ROS2 messages\n' @@ -46,20 +100,24 @@ def generate_header_file(output_directory, translation_unit, struct_names): for struct_name in struct_names: header_text += '#include \n' - for node in translation_unit.cursor.get_children(): - if node.kind == clang.cindex.CursorKind.STRUCT_DECL: - msg_name = node.spelling - if msg_name not in struct_names: + _struct_like = ( + clang.cindex.CursorKind.STRUCT_DECL, + clang.cindex.CursorKind.UNION_DECL, + ) + for header_path, translation_unit in header_tus: + for node in _nodes_in_file(translation_unit, header_path): + if node.kind not in _struct_like: + continue + if node.spelling not in struct_names: continue - declaration_file = pathlib.Path( - node.get_definition().location.file.name - ).name - header_text += f'#include \n' + relative_path = header_path.relative_to(include_root) + header_text += f'#include \n' header_text += 'namespace ateam_radio_msgs {\n' - for node in translation_unit.cursor.get_children(): - if node.kind == clang.cindex.CursorKind.STRUCT_DECL: - msg_name = node.spelling - if msg_name not in struct_names: + for header_path, translation_unit in header_tus: + for node in _nodes_in_file(translation_unit, header_path): + if node.kind not in _struct_like: + continue + if node.spelling not in struct_names: continue header_text += generate_conversion_function_declaration(node) + '\n' header_text += '} // namespace ateam_radio_msgs\n\n' @@ -70,21 +128,29 @@ def generate_header_file(output_directory, translation_unit, struct_names): f.write(header_text) -def generate_implementation_file(output_directory, translation_unit, struct_names): +def generate_implementation_file(output_directory, header_tus, struct_names): """Generate implementation file for conversion functions.""" enums = [] impl_text = '#include "conversion.hpp"\n\n' 'namespace ateam_radio_msgs {\n\n' - for node in translation_unit.cursor.get_children(): - match node.kind: - case clang.cindex.CursorKind.ENUM_DECL: - enums.append(collect_enum_details(node)) - case clang.cindex.CursorKind.STRUCT_DECL: - msg_name = node.spelling - if msg_name not in struct_names: + for header_path, translation_unit in header_tus: + for node in translation_unit.cursor.get_children(): + match node.kind: + case clang.cindex.CursorKind.ENUM_DECL: + # Accumulate enums with a file filter so each enum is collected + # exactly once (each header's TU re-exposes enums from its includes). + if node.location.file and node.location.file.name == str(header_path): + enums.append(collect_enum_details(node)) + case clang.cindex.CursorKind.STRUCT_DECL | clang.cindex.CursorKind.UNION_DECL: + if node.location.file is None \ + or node.location.file.name != str(header_path): + continue + if node.spelling not in struct_names: + continue + impl_text += generate_conversion_function_implementation( + node, enums, struct_names + ) + case _: continue - impl_text += generate_conversion_function_implementation(node, enums) - case _: - continue impl_text += '} // namespace ateam_radio_bridge\n' os.makedirs(f'{output_directory}/src', exist_ok=True) file_path = f'{output_directory}/src/conversion.cpp' @@ -99,7 +165,7 @@ def generate_conversion_function_declaration(struct_node): return f'{return_type} Convert(const {struct_node.spelling} & {param_name});' -def generate_conversion_function_implementation(struct_node, enums): +def generate_conversion_function_implementation(struct_node, enums, struct_names): """Generate the implementation of a conversion function for the given struct.""" param_name = camel_case_to_snake_case(struct_node.spelling) return_type = f'ateam_radio_msgs::msg::{struct_node.spelling}' @@ -107,25 +173,85 @@ def generate_conversion_function_implementation(struct_node, enums): f'{return_type} Convert(const {struct_node.spelling} & {param_name}) {{\n' ) impl_text += f' {return_type} msg;\n' + # Track the most recently seen enum field; by convention the enum selector + # immediately precedes its union field in the struct declaration, so this is + # the right field to switch on when we hit a union. + last_enum_field = None for field in struct_node.get_children(): if field.kind != clang.cindex.CursorKind.FIELD_DECL: continue if field.spelling.startswith('_'): continue - impl_text += generate_field_copy_line(field, param_name, enums) + if field.type.spelling in [e['type_name'] for e in enums]: + last_enum_field = field + impl_text += generate_field_copy_line( + field, param_name, enums, struct_names, last_enum_field + ) impl_text += ' return msg;\n' impl_text += '}\n' return impl_text -def generate_field_copy_line(field_node, param_name, enums): +def generate_union_switch_copy_lines(field_node, param_name, struct_names, selector_field, enums): + """Generate a switch on selector_field to convert only the active union member.""" + field_name = field_node.spelling + union_decl = field_node.type.get_canonical().get_declaration() + members = [ + m for m in union_decl.get_children() + if m.kind == clang.cindex.CursorKind.FIELD_DECL + and m.type.spelling in struct_names + ] + enum_details = next(e for e in enums if e['type_name'] == selector_field.type.spelling) + # Convention: enum value 0 means "none/off" (no active union member). + # Remaining values sorted ascending map positionally to union members in + # declaration order, so BCM_GLOBAL_POSITION(1)->global_pos, + # BCM_GLOBAL_VELOCITY(2)->global_vel, etc. + non_zero_cases = sorted( + [(name, val) for name, val in enum_details['values'] if val != 0], + key=lambda x: x[1], + ) + result = f' switch ({param_name}.{selector_field.spelling}) {{\n' + for (case_name, _), member in zip(non_zero_cases, members): + msg_field = f'{field_name}_{member.spelling}' + result += f' case {case_name}:\n' + result += ( + f' msg.{msg_field} = ' + f'Convert({param_name}.{field_name}.{member.spelling});\n' + ) + result += ' break;\n' + result += ' default:\n' + result += ' break;\n' + result += ' }\n' + return result + + +def generate_field_copy_line(field_node, param_name, enums, struct_names, selector_field=None): """Generate a line of code to copy a field from the struct to the message.""" field_name = field_node.spelling if field_node.type.kind == clang.cindex.TypeKind.CONSTANTARRAY: return f' std::ranges::copy({param_name}.{field_node.spelling}, ' \ f'std::back_inserter(msg.{field_name}));\n' elif field_node.type.kind == clang.cindex.TypeKind.ELABORATED: - if field_node.type.spelling in [ + if _is_elaborated_union(field_node): + if selector_field is not None: + return generate_union_switch_copy_lines( + field_node, param_name, struct_names, selector_field, enums + ) + # No selector enum found — fall back to converting all members unconditionally. + union_decl = field_node.type.get_canonical().get_declaration() + result = '' + for member in union_decl.get_children(): + if member.kind != clang.cindex.CursorKind.FIELD_DECL: + continue + if member.type.spelling not in struct_names: + continue + msg_field = f'{field_name}_{member.spelling}' + result += ( + f' msg.{msg_field} = ' + f'Convert({param_name}.{field_name}.{member.spelling});\n' + ) + return result + elif field_node.type.spelling in [ 'uint8_t', 'uint16_t', 'uint32_t', @@ -138,10 +264,12 @@ def generate_field_copy_line(field_node, param_name, enums): return f' msg.{field_name} = {param_name}.{field_node.spelling};\n' elif field_node.type.spelling in [e['type_name'] for e in enums]: return f' msg.{field_name} = {param_name}.{field_node.spelling};\n' - else: + elif field_node.type.spelling in struct_names: return ( f' msg.{field_name} = Convert({param_name}.{field_node.spelling});\n' ) + else: + return '' else: return f' msg.{field_name} = {param_name}.{field_node.spelling};\n' diff --git a/radio/ateam_radio_msgs/scripts/generate_msgs.py b/radio/ateam_radio_msgs/scripts/generate_msgs.py index 2ad51cb0b..af61f8c16 100644 --- a/radio/ateam_radio_msgs/scripts/generate_msgs.py +++ b/radio/ateam_radio_msgs/scripts/generate_msgs.py @@ -26,6 +26,41 @@ import clang.cindex +def _is_elaborated_union(field_node): + """Return True if the field's type is a typedef'd union.""" + if field_node.type.kind != clang.cindex.TypeKind.ELABORATED: + return False + # get_declaration() on a typedef'd union returns TYPEDEF_DECL, not UNION_DECL. + # The canonical type strips the typedef layer and gives us the underlying record. + canon = field_node.type.get_canonical() + return ( + canon.kind == clang.cindex.TypeKind.RECORD + and canon.get_declaration().kind == clang.cindex.CursorKind.UNION_DECL + ) + + +def collect_union_member_types(file_path, struct_names): + """Return a list of struct type names used as union members inside struct_names types.""" + index = clang.cindex.Index.create() + tu = index.parse(file_path) + extras = [] + struct_like = (clang.cindex.CursorKind.STRUCT_DECL, clang.cindex.CursorKind.UNION_DECL) + for node in tu.cursor.get_children(): + if node.location.file is None or node.location.file.name != file_path: + continue + if node.kind not in struct_like or node.spelling not in struct_names: + continue + for field in node.get_children(): + if field.kind != clang.cindex.CursorKind.FIELD_DECL: + continue + if not _is_elaborated_union(field): + continue + for member in field.type.get_canonical().get_declaration().get_children(): + if member.kind == clang.cindex.CursorKind.FIELD_DECL: + extras.append(member.type.spelling) + return extras + + def generate_msgs_for_file(output_dir, file_path, struct_names): """Generate ROS2 message definitions from a C header file.""" index = clang.cindex.Index.create() @@ -36,21 +71,26 @@ def generate_msgs_for_file(output_dir, file_path, struct_names): for node in translation_unit.cursor.get_children(): match node.kind: case clang.cindex.CursorKind.ENUM_DECL: + # Intentionally no file filter: enums defined in included headers (e.g. + # BodyControlMode in basic_control.h) must be visible when processing files + # that include them, so enum field types resolve correctly. enums.append(collect_enum_details(node)) - case clang.cindex.CursorKind.STRUCT_DECL: + case clang.cindex.CursorKind.STRUCT_DECL | clang.cindex.CursorKind.UNION_DECL: + if node.location.file is None or node.location.file.name != file_path: + continue msg_name = node.spelling if msg_name not in struct_names: continue declaration_file = pathlib.Path( node.get_definition().location.file.name ).name - msg = generate_msg_for_struct(node, enums) + msg = generate_msg_for_struct(node, enums, struct_names) write_msg_to_file(output_dir, msg, declaration_file) case _: continue -def generate_msg_for_struct(struct_ast_node, enums): +def generate_msg_for_struct(struct_ast_node, enums, struct_names): """Generate a ROS2 message definition for a single struct AST node.""" type_name = struct_ast_node.spelling declarations = [] @@ -59,7 +99,7 @@ def generate_msg_for_struct(struct_ast_node, enums): continue if field.spelling.startswith('_'): continue - add_field_declarations(declarations, field, enums) + add_field_declarations(declarations, field, enums, struct_names) return {'type_name': type_name, 'declarations': declarations} @@ -75,19 +115,39 @@ def write_msg_to_file(output_dir, msg, input_file_path): f.write(f'{declaration}\n') -def add_field_declarations(declarations, field_node, enums): +def add_field_declarations(declarations, field_node, enums, struct_names): """Add declarations for a field node to the list of declarations.""" if field_node.is_bitfield(): add_bitfield_declaration(declarations, field_node) elif field_node.type.kind == clang.cindex.TypeKind.CONSTANTARRAY: add_array_declaration(declarations, field_node) + elif _is_elaborated_union(field_node): + add_union_field_declarations(declarations, field_node, struct_names) elif field_node.type.spelling in [e['type_name'] for e in enums]: add_enum_declaration(declarations, field_node, enums) else: ros2_type = get_ros2_basic_type(field_node.type) + # Skip nested struct types not in struct_names: rosidl would fail to resolve + # them, and they are intentionally excluded from generation (e.g. RadioHeader). + if ros2_type.startswith('ateam_radio_msgs/') \ + and ros2_type.removeprefix('ateam_radio_msgs/') not in struct_names: + return declarations.append(f'{ros2_type} {field_node.spelling}') +def add_union_field_declarations(declarations, field_node, struct_names): + """Expand a union field into one declaration per union member.""" + field_prefix = field_node.spelling + union_decl = field_node.type.get_canonical().get_declaration() + for member in union_decl.get_children(): + if member.kind != clang.cindex.CursorKind.FIELD_DECL: + continue + member_type = member.type.spelling + if member_type not in struct_names: + continue + declarations.append(f'ateam_radio_msgs/{member_type} {field_prefix}_{member.spelling}') + + def add_bitfield_declaration(declarations, field_node): """Add a declaration for a bitfield field node.""" type_name = '' @@ -118,6 +178,8 @@ def add_array_declaration(declarations, field_node): def add_enum_declaration(declarations, field_node, enums): """Add a declaration for an enum field node.""" enum_details = [e for e in enums if e['type_name'] == field_node.type.spelling][0] + # ROS2 messages don't support enum types, so we use the underlying integer type and + # inline the named constants as message-level constants below the field. declarations.append(f'{enum_details["underlying_type"]} {field_node.spelling}') for value_name, value in enum_details['values']: declarations.append( @@ -192,4 +254,19 @@ def get_ros2_basic_type(field_type): ' [ ...]' ) sys.exit(1) - generate_msgs_for_file(sys.argv[1], sys.argv[2], sys.argv[3:]) + output_dir = sys.argv[1] + source = pathlib.Path(sys.argv[2]) + struct_names = list(sys.argv[3:]) + include_dir = source.parent + headers = sorted(include_dir.rglob('*.h')) + # Two-pass: first discover the struct types used as union members (e.g. + # ExtendedGlobalPositionTelemetry inside BodyControlSkillExtendedTelemetry). + # These aren't in the original struct_names list but need their own .msg files + # because ROS2 doesn't support unions — we expand each union into separate fields. + all_struct_names = list(struct_names) + for header in headers: + for extra in collect_union_member_types(str(header), struct_names): + if extra not in all_struct_names: + all_struct_names.append(extra) + for header in headers: + generate_msgs_for_file(output_dir, str(header), all_struct_names) diff --git a/radio/ateam_radio_msgs/software-communication b/radio/ateam_radio_msgs/software-communication index 0fc77eee7..b969e32db 160000 --- a/radio/ateam_radio_msgs/software-communication +++ b/radio/ateam_radio_msgs/software-communication @@ -1 +1 @@ -Subproject commit 0fc77eee7b20b4113e452816be8dd7bc42609f21 +Subproject commit b969e32db321430eb058c72444e161ec3ae7e084 diff --git a/radio/ateam_radio_msgs/templates/version.hpp.in b/radio/ateam_radio_msgs/templates/version.hpp.in new file mode 100644 index 000000000..7095e7a4b --- /dev/null +++ b/radio/ateam_radio_msgs/templates/version.hpp.in @@ -0,0 +1,31 @@ +// Copyright 2026 A Team +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#ifndef ATEAM_RADIO_MSGS__VERSION_HPP_ +#define ATEAM_RADIO_MSGS__VERSION_HPP_ + +namespace ateam_radio_msgs +{ + constexpr uint32_t kComsHash = @SOFT_COMS_SUBMODULE_SHA_SHORT_NUM@; + + constexpr bool kComsDirty = @SOFT_COMS_SUBMODULE_DIRTY@; +} + +#endif // ATEAM_RADIO_MSGS__VERSION_HPP_