update comms to add pivot maneuver

ateam_msgs/msg/RobotMotionCommand.msg

@@ -7,6 +7,10 @@ float64 limit_vel_angular  # (rad/s)
 float64 limit_acc_linear  # (m/s/s)
 float64 limit_acc_angular  # (rad/s/s)
 
+# pivot maneuver parameters (used when body_control_mode == BCM_PIVOT)
+float64 pivot_orbit_radius  # (m)
+float64 pivot_heading_lag  # (rad)
+
 uint8 kick_request
 float32 kick_speed  # (m/s)
 float32 dribbler_speed  # (rpm)
@@ -28,3 +32,4 @@ uint8 BCM_GLOBAL_VELOCITY = 2
 uint8 BCM_LOCAL_VELOCITY = 3
 uint8 BCM_GLOBAL_ACCEL = 4
 uint8 BCM_LOCAL_ACCEL = 5
+uint8 BCM_PIVOT = 6

radio/ateam_radio_bridge/src/radio_bridge_node.cpp

@@ -366,6 +366,18 @@ class RadioBridgeNode : public rclcpp::Node
             static_cast<float>(command.acceleration.theta),
           };
           break;
+        case ateam_msgs::msg::RobotMotionCommand::BCM_PIVOT:
+          control_msg.body_control_mode = BCM_PIVOT;
+          control_msg.cmd.pivot = {
+            static_cast<float>(command.pose.x),
+            static_cast<float>(command.pose.y),
+            static_cast<float>(command.pose.theta),
+            static_cast<float>(command.limit_vel_angular),
+            static_cast<float>(command.limit_acc_angular),
+            static_cast<float>(command.pivot_orbit_radius),
+            static_cast<float>(command.pivot_heading_lag)
+          };
+          break;
         default:
           RCLCPP_WARN(get_logger(), "Unknown body control mode: %d", command.body_control_mode);
           control_msg.body_control_mode = BCM_OFF;

radio/ateam_radio_msgs/CMakeLists.txt

@@ -23,6 +23,7 @@ set(RADIO_STRUCTS_TO_GENERATE
     LocalVelocityCommand
     GlobalAccelerationCommand
     LocalAccelerationCommand
+    PivotCommand
     BasicTelemetry
     ErrorTelemetry
     ExtendedTelemetry