New Packets

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<std::chrono::duration<double>>(

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<std::chrono::duration<double>>(

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<const uint8_t *>(&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<const uint8_t *>(&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.";

radio/ateam_radio_bridge/src/radio_bridge_node.cpp

@@ -33,6 +33,7 @@
 #include <ateam_radio_msgs/srv/set_firmware_parameter.hpp>
 #include <ateam_radio_msgs/srv/send_robot_power_request.hpp>
 #include <ateam_radio_msgs/conversion.hpp>
+#include <ateam_radio_msgs/version.hpp>
 #include <ateam_msgs/msg/robot_motion_command.hpp>
 #include <ateam_common/indexed_topic_helpers.hpp>
 #include <ateam_common/multicast_receiver.hpp>
@@ -201,7 +202,7 @@ class RadioBridgeNode : public rclcpp::Node
       const auto packet = CreateEmptyPacket(CC_GOODBYE);
       connection->send(
         reinterpret_cast<const uint8_t *>(&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<KickRequest>(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<float>(motion_commands_[id].twist.linear.x),
+        static_cast<float>(motion_commands_[id].twist.linear.y),
+        static_cast<float>(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<const uint8_t *>(&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<HelloRequest>(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<const char *>(&reply_packet), GetPacketSize(reply_packet.command_code));
+        reinterpret_cast<const char *>(&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<const char *>(&reply_packet), GetPacketSize(reply_packet.command_code));
+        reinterpret_cast<const char *>(&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<const char *>(&reply_packet), GetPacketSize(reply_packet.command_code));
+      reinterpret_cast<const char *>(&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;
     }
   }