Add robot maneuvers to sim radio

ateam_kenobi/src/skills/capture.cpp

@@ -109,11 +109,13 @@ RobotCommand Capture::runCapture(const World & world, const Robot & robot)
 
   if(world.ball.visible) {
     motion::intents::LinearVelocityAngularFacing intent;
+    intent.frame = ateam_kenobi::motion::Frame::Local;
     intent.linear = ateam_geometry::Vector{capture_speed_, 0.0};
     intent.face_target = world.ball.pos;
     command.motion_intent = intent;
   } else {
     motion::intents::Velocity intent;
+    intent.frame = ateam_kenobi::motion::Frame::Local;
     intent.linear = ateam_geometry::Vector{capture_speed_, 0.0};
     intent.angular = 0.0;
     command.motion_intent = intent;

ateam_msgs/msg/RobotMotionCommand.msg

@@ -8,6 +8,7 @@ float64 limit_acc_linear  # (m/s/s)
 float64 limit_acc_angular  # (rad/s/s)
 
 # pivot parameters
+float64 pivot_global_theta # (rad)
 float64 pivot_orbit_radius  # (m)
 float64 pivot_inset_angle  # (rad)
 

motion/ateam_controls_testing/scripts/pivot_test.py

@@ -0,0 +1,128 @@
+#! /usr/bin/env python3
+
+# 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.
+
+import argparse
+import math
+import time
+
+from ateam_msgs.msg import RobotMotionCommand, VisionStateRobot
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import qos_profile_system_default
+from transforms3d.euler import quat2euler
+
+linear_threshold = 10
+angular_threshold = 0.0349
+
+# x, y, theta, orbit_radius, inset angle, hold time
+waypoints = [
+    (-0.5, -0.5, 0.0, 0.5, 0.0, 1.0),
+    (-0.5, -0.5, math.pi, 0.5, 0.0, 1.0),
+]
+
+current_index = 0
+waypoint_hold_start_time = None
+vision_robot_state_msg = None
+
+
+def vision_callback(msg: VisionStateRobot):
+    """Store latest message."""
+    global vision_robot_state_msg
+    vision_robot_state_msg = msg
+
+
+def publish_waypoint_command(index: int):
+    waypoint = waypoints[index]
+    command_msg = RobotMotionCommand()
+    command_msg.body_control_mode = RobotMotionCommand.BCM_PIVOT
+    command_msg.pose.x = waypoint[0]
+    command_msg.pose.y = waypoint[1]
+    command_msg.pose.theta = waypoint[2]
+    command_msg.kick_request = RobotMotionCommand.KR_DISABLE
+    command_msg.pivot_global_theta = waypoint[2]
+    command_msg.pivot_orbit_radius = waypoint[3]
+    command_msg.pivot_inset_angle = waypoint[4]
+    command_msg.limit_acc_linear = 3.0
+    command_msg.limit_vel_linear = 3.0
+    command_msg.limit_acc_angular = 8.0
+    command_msg.limit_vel_angular = 4.0
+    command_pub.publish(command_msg)
+
+
+def is_at_waypoint(index: int):
+    waypoint = waypoints[index]
+    #  get yaw from quat
+    q = vision_robot_state_msg.pose.orientation
+    _, _, theta = quat2euler([q.w, q.x, q.y, q.z])
+    return (
+        vision_robot_state_msg.visible
+        and abs(vision_robot_state_msg.pose.position.x - waypoint[0]) < linear_threshold
+        and abs(vision_robot_state_msg.pose.position.y - waypoint[1]) < linear_threshold
+        and abs(theta - waypoint[2]) < angular_threshold
+    )
+
+
+if __name__ == '__main__':
+    argparser = argparse.ArgumentParser(
+        description='Test node for sending pivots to the robot'
+    )
+    argparser.add_argument('robot_id', type=int)
+    argparser.add_argument('--color', '-c', type=str, default='blue')
+    args = argparser.parse_args()
+
+    rclpy.init()
+    node = Node('pivot_test')
+
+    command_pub = node.create_publisher(
+        RobotMotionCommand,
+        f'/robot_motion_commands/robot{args.robot_id}',
+        qos_profile_system_default,
+    )
+    vision_sub = node.create_subscription(
+        VisionStateRobot,
+        f'/{args.color}_team/robot{args.robot_id}',
+        vision_callback,
+        qos_profile_system_default,
+    )
+
+    while rclpy.ok():
+        rclpy.spin_once(node)
+        publish_waypoint_command(0)
+        if vision_robot_state_msg is None:
+            continue
+        if is_at_waypoint(0):
+            node.get_logger().info('Reached waypoint 0. Starting cycle.')
+            break
+
+    while rclpy.ok():
+        rclpy.spin_once(node)
+        if is_at_waypoint(current_index):
+            if waypoint_hold_start_time is None:
+                waypoint_hold_start_time = time.time()
+            elif (time.time() - waypoint_hold_start_time) > waypoints[current_index][3]:
+                current_index = (current_index + 1) % len(waypoints)
+                waypoint_hold_start_time = None
+                node.get_logger().info(f'Moving to waypoint {current_index}')
+        publish_waypoint_command(current_index)
+
+    node.destroy_node()
+    rclpy.shutdown()

radio/ateam_ssl_simulation_radio_bridge/CMakeLists.txt

@@ -11,13 +11,16 @@ find_package(rclcpp_components REQUIRED)
 find_package(ateam_common REQUIRED)
 find_package(ateam_msgs REQUIRED)
 find_package(ateam_radio_msgs REQUIRED)
+find_package(ateam_controls REQUIRED)
+find_package(ateam_geometry REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(ssl_league_protobufs REQUIRED)
 
 add_library(${PROJECT_NAME} SHARED
   src/ssl_simulation_radio_bridge_node.cpp
   src/message_conversions.cpp
+  src/robot_maneuvers.cpp
 )
 set_target_properties(${PROJECT_NAME} PROPERTIES CXX_STANDARD 20)
 ament_target_dependencies(${PROJECT_NAME}
@@ -26,6 +29,8 @@ ament_target_dependencies(${PROJECT_NAME}
   ateam_common
   ateam_msgs
   ateam_radio_msgs
+  ateam_controls
+  ateam_geometry
   tf2
   tf2_geometry_msgs
   ssl_league_protobufs

radio/ateam_ssl_simulation_radio_bridge/package.xml

@@ -14,6 +14,8 @@
   <depend>ateam_common</depend>
   <depend>ateam_msgs</depend>
   <depend>ateam_radio_msgs</depend>
+  <depend>ateam_controls</depend>
+  <depend>ateam_geometry</depend>
   <depend>tf2</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>ssl_league_protobufs</depend>

radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.cpp

@@ -24,6 +24,7 @@
 #include <tf2/LinearMath/Quaternion.h>
 #include <stdexcept>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include "robot_maneuvers.hpp"
 
 namespace ateam_ssl_simulation_radio_bridge::message_conversions
 {
@@ -52,13 +53,14 @@ double ReplaceNanWithZero(const double val, rclcpp::Logger logger)
 }
 
 RobotControl fromMsg(
-  const ateam_msgs::msg::RobotMotionCommand & ros_msg, int robot_id,
+  const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot,
+  ateam_ssl_simulation_radio_bridge::robot_maneuvers::ManeuverExecutor & maneuver_executor,
   rclcpp::Logger logger)
 {
   RobotControl robots_control;
   RobotCommand * proto_robot_command = robots_control.add_robot_commands();
 
-  proto_robot_command->set_id(robot_id);
+  proto_robot_command->set_id(robot.id);
   proto_robot_command->set_dribbler_speed(ReplaceNanWithZero(9.5492968 * ros_msg.dribbler_speed,
       logger));
 
@@ -78,11 +80,7 @@ RobotControl fromMsg(
   }
 
   RobotMoveCommand * robot_move_command = proto_robot_command->mutable_move_command();
-  MoveLocalVelocity * local_velocity_command = robot_move_command->mutable_local_velocity();
-
-  local_velocity_command->set_forward(ReplaceNanWithZero(ros_msg.velocity.x, logger));
-  local_velocity_command->set_left(ReplaceNanWithZero(ros_msg.velocity.y, logger));
-  local_velocity_command->set_angular(ReplaceNanWithZero(ros_msg.velocity.theta, logger));
+  maneuver_executor.execute_maneuver(robot_move_command, ros_msg, robot);
 
   return robots_control;
 }

radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.hpp

@@ -31,14 +31,18 @@
 
 #include <ateam_radio_msgs/msg/basic_telemetry.hpp>
 #include <ateam_msgs/msg/robot_motion_command.hpp>
+#include <ateam_msgs/msg/game_state_robot.hpp>
+
+#include "robot_maneuvers.hpp"
 
 namespace ateam_ssl_simulation_radio_bridge::message_conversions
 {
 
 ateam_radio_msgs::msg::BasicTelemetry fromProto(const RobotFeedback & proto_msg);
 
 RobotControl fromMsg(
-  const ateam_msgs::msg::RobotMotionCommand & ros_msg, int robot_id,
+  const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot,
+  ateam_ssl_simulation_radio_bridge::robot_maneuvers::ManeuverExecutor & maneuver_executor,
   rclcpp::Logger logger);
 
 SimulatorControl fromMsg(const ssl_league_msgs::msg::SimulatorControl & ros_msg);

radio/ateam_ssl_simulation_radio_bridge/src/pid.hpp

@@ -0,0 +1,93 @@
+// 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 PID_HPP_
+#define PID_HPP_
+
+#include <algorithm>
+#include <cmath>
+
+class PID
+{
+public:
+  PID() = default;
+  PID(double p, double i, double d, double i_max = 0, double i_min = 0)
+  {
+    set_gains(p, i, d, i_max, i_min);
+  }
+
+
+  double compute_command(const double error, const double dt)
+  {
+    if (std::isnan(error)) {
+      prev_error_ = 0;
+      return 0;
+    }
+
+    integral_ += error * dt;
+    if (use_antiwindup_) {
+      integral_ = std::clamp(integral_, i_min_, i_max_);
+    }
+
+    double derivative = (error - prev_error_) / dt;
+
+    double command = (kp_ * error) + (ki_ * integral_) + (kd_ * derivative);
+    prev_error_ = error;
+
+    return command;
+  }
+
+  void set_gains(double p, double i, double d, double i_max = 0, double i_min = 0)
+  {
+    kp_ = p;
+    ki_ = i;
+    kd_ = d;
+
+    if (i_min == 0 && i_max == 0) {
+      use_antiwindup_ = false;
+    } else {
+      use_antiwindup_ = true;
+      i_min_ = i_min;
+      i_max_ = i_max;
+    }
+
+    integral_ = 0;
+  }
+
+  void reset()
+  {
+    integral_ = 0;
+    prev_error_ = 0;
+  }
+
+private:
+  double kp_ = 0;
+  double ki_ = 0;
+  double kd_ = 0;
+
+  bool use_antiwindup_ = false;
+  double i_max_ = 0;
+  double i_min_ = 0;
+
+  double prev_error_ = 0;
+  double integral_ = 0;
+};
+
+#endif  // PID_HPP_