Add robot maneuvers to sim radio

ateam_kenobi/src/core/motion/motion_command.hpp

@@ -40,7 +40,9 @@ enum class ControlMode
   GlobalVelocity = 2,
   LocalVelocity = 3,
   GlobalAccel = 4,
-  LocalAccel = 5
+  LocalAccel = 5,
+  HeadingPivot = 6,
+  PointPivot = 7
 };
 
 struct MotionCommand
@@ -53,6 +55,15 @@ struct MotionCommand
   double limit_vel_angular = 0.0;
   double limit_acc_linear = 0.0;
   double limit_acc_angular = 0.0;
+
+  double pivot_target_x = 0.0;
+  double pivot_target_y = 0.0;
+  double pivot_global_theta = 0.0;
+  double pivot_orbit_radius = 0.0;
+  double pivot_inset_angle = 0.0;
+
+  uint8_t pivot_direction = 0;
+  bool pivot_commpute_inset_angle = false;
 };
 
 }  // namespace ateam_kenobi::motion

ateam_kenobi/src/core/motion/motion_executor.cpp

@@ -312,7 +312,41 @@ std::optional<MotionCommand> MotionExecutor::ExecuteIntent(
 {
   (void)overlays;
   (void)world;
-  return PivotToHeading(intent, robot);
+  (void)robot;
+
+  MotionCommand command;
+  command.control_mode = ControlMode::HeadingPivot;
+  command.limit_vel_angular = intent.limits.angular_velocity;
+  command.limit_acc_angular = intent.limits.angular_acceleration;
+  command.pivot_global_theta = intent.target_heading;
+  command.pivot_orbit_radius = intent.radius;
+  command.pivot_inset_angle = intent.inset_angle;
+  command.pivot_direction = static_cast<uint8_t>(intent.direction);
+  command.pivot_commpute_inset_angle = intent.compute_inset_angle;
+
+  return command;
+}
+
+std::optional<MotionCommand> MotionExecutor::ExecuteIntent(
+  const intents::PivotPoint & intent, const Robot & robot, visualization::Overlays & overlays,
+  const World & world)
+{
+  (void)overlays;
+  (void)world;
+  (void)robot;
+
+  MotionCommand command;
+  command.control_mode = ControlMode::PointPivot;
+  command.limit_vel_angular = intent.limits.angular_velocity;
+  command.limit_acc_angular = intent.limits.angular_acceleration;
+  command.pivot_target_x = intent.target_x;
+  command.pivot_target_y = intent.target_y;
+  command.pivot_orbit_radius = intent.radius;
+  command.pivot_inset_angle = intent.inset_angle;
+  command.pivot_direction = static_cast<uint8_t>(intent.direction);
+  command.pivot_commpute_inset_angle = intent.compute_inset_angle;
+
+  return command;
 }
 
 }  // namespace ateam_kenobi::motion

ateam_kenobi/src/core/motion/motion_executor.hpp

@@ -97,6 +97,9 @@ class MotionExecutor
   std::optional<MotionCommand> ExecuteIntent(
     const intents::PivotHeading & intent, const Robot & robot,
     visualization::Overlays & overlays, const World & world);
+  std::optional<MotionCommand> ExecuteIntent(
+    const intents::PivotPoint & intent, const Robot & robot,
+    visualization::Overlays & overlays, const World & world);
 };
 
 }  // namespace ateam_kenobi::motion

ateam_kenobi/src/core/motion/motion_intent.hpp

@@ -40,6 +40,11 @@ enum class Frame
   Local
 };
 
+enum class PivotDirection {
+  Forward = 0,
+  Backward = 1
+};
+
 struct Limits
 {
   double linear_velocity = 0.0;
@@ -113,6 +118,20 @@ struct PivotHeading
 {
   double target_heading;
   double radius = 0.089;  // Estimated radius of bot holding ball
+  double inset_angle = 0.0;
+  PivotDirection direction = PivotDirection::Forward;
+  bool compute_inset_angle = false;
+  Limits limits;
+};
+
+struct PivotPoint
+{
+  double target_x;
+  double target_y;
+  double radius = 0.089;  // Estimated radius of bot holding ball
+  double inset_angle = 0.0;
+  PivotDirection direction = PivotDirection::Forward;
+  bool compute_inset_angle = false;
   Limits limits;
 };
 
@@ -127,7 +146,8 @@ using MotionIntent = std::variant<
   intents::Position,
   intents::PositionFacing,
   intents::PivotVelocity,
-  intents::PivotHeading>;
+  intents::PivotHeading,
+  intents::PivotPoint>;
 
 }  // namespace ateam_kenobi::motion
 

ateam_kenobi/src/kenobi_node.cpp

@@ -247,6 +247,13 @@ class KenobiNode : public rclcpp::Node
         ros_cmd.limit_vel_angular = motion_cmd.limit_vel_angular;
         ros_cmd.limit_acc_linear = motion_cmd.limit_acc_linear;
         ros_cmd.limit_acc_angular = motion_cmd.limit_acc_angular;
+        ros_cmd.pivot_target_x = motion_cmd.pivot_target_x;
+        ros_cmd.pivot_target_y = motion_cmd.pivot_target_y;
+        ros_cmd.pivot_global_theta = motion_cmd.pivot_global_theta;
+        ros_cmd.pivot_orbit_radius = motion_cmd.pivot_orbit_radius;
+        ros_cmd.pivot_inset_angle = motion_cmd.pivot_inset_angle;
+        ros_cmd.pivot_direction = motion_cmd.pivot_direction;
+        ros_cmd.pivot_compute_inset_angle = motion_cmd.pivot_commpute_inset_angle;
       }
     }
 

ateam_kenobi/src/skills/capture.cpp

@@ -69,21 +69,25 @@ RobotCommand Capture::runMoveToBall(
   const World & world,
   const Robot & robot)
 {
+  (void)robot;
+
   motion::intents::PositionFacing intent;
   intent.position = world.ball.pos;
   intent.face_target = world.ball.pos;
   intent.planner_options.avoid_ball = false;
 
-  const auto distance_to_ball = CGAL::approximate_sqrt(CGAL::squared_distance(robot.pos,
-      world.ball.pos));
+  // const auto distance_to_ball = CGAL::approximate_sqrt(CGAL::squared_distance(robot.pos,
+  //     world.ball.pos));
 
-  const auto decel_distance = distance_to_ball - approach_radius_;
+  // const auto decel_distance = distance_to_ball - approach_radius_;
 
-  const auto max_decel_vel = std::sqrt((2.0 * decel_limit_ * decel_distance) +
-      (capture_speed_ * capture_speed_));
+  // const auto max_decel_vel = std::sqrt((2.0 * decel_limit_ * decel_distance) +
+  //     (capture_speed_ * capture_speed_));
 
-  intent.limits.linear_velocity = std::min(max_decel_vel, max_speed_);
+  // intent.limits.linear_velocity = std::min(max_decel_vel, max_speed_);
 
+  intent.limits.linear_velocity = max_speed_;
+  intent.limits.linear_acceleration = decel_limit_;
   RobotCommand command;
   command.motion_intent = intent;
 
@@ -108,14 +112,14 @@ RobotCommand Capture::runCapture(const World & world, const Robot & robot)
   RobotCommand command;
 
   if(world.ball.visible) {
-    motion::intents::LinearVelocityAngularFacing intent;
-    intent.linear = ateam_geometry::Vector{capture_speed_, 0.0};
+    motion::intents::PositionFacing intent;
+    intent.position = world.ball.pos;
     intent.face_target = world.ball.pos;
     command.motion_intent = intent;
   } else {
-    motion::intents::Velocity intent;
-    intent.linear = ateam_geometry::Vector{capture_speed_, 0.0};
-    intent.angular = 0.0;
+    motion::intents::Position intent;
+    intent.position = world.ball.pos;
+    intent.heading = robot.theta;
     command.motion_intent = intent;
   }
 

ateam_kenobi/src/skills/pivot_kick.cpp

@@ -102,6 +102,10 @@ RobotCommand PivotKick::Pivot(const Robot & robot)
   motion::intents::PivotHeading intent;
   intent.radius = 0.095 * 1.05;
   intent.target_heading = robot_to_target_angle;
+  intent.inset_angle = M_PI / 2.0;
+
+  intent.limits.angular_velocity = 2.0;
+  intent.limits.angular_acceleration = 2.0;
 
   RobotCommand command;
   command.motion_intent = intent;

ateam_msgs/msg/RobotMotionCommand.msg

@@ -8,6 +8,9 @@ float64 limit_acc_linear  # (m/s/s)
 float64 limit_acc_angular  # (rad/s/s)
 
 # pivot parameters
+float64 pivot_target_x # (m)
+float64 pivot_target_y # (m)
+float64 pivot_global_theta # (rad)
 float64 pivot_orbit_radius  # (m)
 float64 pivot_inset_angle  # (rad)
 uint8 pivot_direction  # orbit drive direction: PD_FORWARD=0, PD_BACKWARD=1

motion/ateam_controls_testing/scripts/pivot_test.py

@@ -0,0 +1,132 @@
+#! /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.1
+
+# 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),
+    (-0.5, -0.5, 0.0, 0.09975, math.pi/2, 1.0),
+    (-0.5, -0.5, math.pi / 2.0, 0.09975, math.pi/2, 1.0),
+    (-0.5, -0.5, math.pi, 0.09975, math.pi/2, 1.0),
+    (-0.5, -0.5, -math.pi / 2.0, 0.09975, math.pi/2, 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_HEADING_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()

motion/ateam_controls_testing/scripts/waypoints_test.py

@@ -21,6 +21,7 @@
 # THE SOFTWARE.
 
 import argparse
+import math
 import time
 
 from ateam_msgs.msg import RobotMotionCommand, VisionStateRobot

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>