diff --git a/ateam_kenobi/src/core/motion/motion_command.hpp b/ateam_kenobi/src/core/motion/motion_command.hpp index 8b0a868c..58ea2bd3 100644 --- a/ateam_kenobi/src/core/motion/motion_command.hpp +++ b/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 diff --git a/ateam_kenobi/src/core/motion/motion_executor.cpp b/ateam_kenobi/src/core/motion/motion_executor.cpp index c9af3c89..b93c75a3 100644 --- a/ateam_kenobi/src/core/motion/motion_executor.cpp +++ b/ateam_kenobi/src/core/motion/motion_executor.cpp @@ -312,7 +312,41 @@ std::optional 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(intent.direction); + command.pivot_commpute_inset_angle = intent.compute_inset_angle; + + return command; +} + +std::optional 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(intent.direction); + command.pivot_commpute_inset_angle = intent.compute_inset_angle; + + return command; } } // namespace ateam_kenobi::motion diff --git a/ateam_kenobi/src/core/motion/motion_executor.hpp b/ateam_kenobi/src/core/motion/motion_executor.hpp index fedfd5d6..d4577d22 100644 --- a/ateam_kenobi/src/core/motion/motion_executor.hpp +++ b/ateam_kenobi/src/core/motion/motion_executor.hpp @@ -97,6 +97,9 @@ class MotionExecutor std::optional ExecuteIntent( const intents::PivotHeading & intent, const Robot & robot, visualization::Overlays & overlays, const World & world); + std::optional ExecuteIntent( + const intents::PivotPoint & intent, const Robot & robot, + visualization::Overlays & overlays, const World & world); }; } // namespace ateam_kenobi::motion diff --git a/ateam_kenobi/src/core/motion/motion_intent.hpp b/ateam_kenobi/src/core/motion/motion_intent.hpp index eb9051b1..73ee013a 100644 --- a/ateam_kenobi/src/core/motion/motion_intent.hpp +++ b/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 diff --git a/ateam_kenobi/src/kenobi_node.cpp b/ateam_kenobi/src/kenobi_node.cpp index 443806c8..c96a27bf 100644 --- a/ateam_kenobi/src/kenobi_node.cpp +++ b/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; } } diff --git a/ateam_kenobi/src/skills/capture.cpp b/ateam_kenobi/src/skills/capture.cpp index dff715ff..dd5515a4 100644 --- a/ateam_kenobi/src/skills/capture.cpp +++ b/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; } diff --git a/ateam_kenobi/src/skills/pivot_kick.cpp b/ateam_kenobi/src/skills/pivot_kick.cpp index 69141020..e6956b82 100644 --- a/ateam_kenobi/src/skills/pivot_kick.cpp +++ b/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; diff --git a/ateam_msgs/msg/RobotMotionCommand.msg b/ateam_msgs/msg/RobotMotionCommand.msg index b7eaa9b2..ff7cc8e9 100644 --- a/ateam_msgs/msg/RobotMotionCommand.msg +++ b/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 diff --git a/motion/ateam_controls_testing/scripts/pivot_test.py b/motion/ateam_controls_testing/scripts/pivot_test.py new file mode 100755 index 00000000..2b0e6269 --- /dev/null +++ b/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() diff --git a/motion/ateam_controls_testing/scripts/waypoints_test.py b/motion/ateam_controls_testing/scripts/waypoints_test.py index db6f0e18..fa51217a 100755 --- a/motion/ateam_controls_testing/scripts/waypoints_test.py +++ b/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 diff --git a/radio/ateam_ssl_simulation_radio_bridge/CMakeLists.txt b/radio/ateam_ssl_simulation_radio_bridge/CMakeLists.txt index b68d98e9..1f46aa08 100644 --- a/radio/ateam_ssl_simulation_radio_bridge/CMakeLists.txt +++ b/radio/ateam_ssl_simulation_radio_bridge/CMakeLists.txt @@ -11,6 +11,8 @@ 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) @@ -18,6 +20,7 @@ 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 diff --git a/radio/ateam_ssl_simulation_radio_bridge/package.xml b/radio/ateam_ssl_simulation_radio_bridge/package.xml index 12ebcc55..f33ef804 100644 --- a/radio/ateam_ssl_simulation_radio_bridge/package.xml +++ b/radio/ateam_ssl_simulation_radio_bridge/package.xml @@ -14,6 +14,8 @@ ateam_common ateam_msgs ateam_radio_msgs + ateam_controls + ateam_geometry tf2 tf2_geometry_msgs ssl_league_protobufs diff --git a/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.cpp b/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.cpp index 3d9b1c8c..25071396 100644 --- a/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.cpp +++ b/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.cpp @@ -24,6 +24,7 @@ #include #include #include +#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; } diff --git a/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.hpp b/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.hpp index 4cd2e3af..f21cef21 100644 --- a/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.hpp +++ b/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.hpp @@ -31,6 +31,9 @@ #include #include +#include + +#include "robot_maneuvers.hpp" namespace ateam_ssl_simulation_radio_bridge::message_conversions { @@ -38,7 +41,8 @@ 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); diff --git a/radio/ateam_ssl_simulation_radio_bridge/src/pid.hpp b/radio/ateam_ssl_simulation_radio_bridge/src/pid.hpp new file mode 100644 index 00000000..e0a262b9 --- /dev/null +++ b/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 +#include + +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_ diff --git a/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.cpp b/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.cpp new file mode 100644 index 00000000..a4e1bb7f --- /dev/null +++ b/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.cpp @@ -0,0 +1,435 @@ +// 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. + +#include +#include "robot_maneuvers.hpp" +#include + +namespace ateam_ssl_simulation_radio_bridge::robot_maneuvers +{ + +void ManeuverExecutor::execute_maneuver( + RobotMoveCommand * robot_move_command, + const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) +{ + switch(ros_msg.body_control_mode) { + case ateam_msgs::msg::RobotMotionCommand::BCM_OFF: + bcm_off_maneuver(); + return; // Ignores acceleration limits set in message and immediately stops the robot + + // Trajectory Maneuvers + case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_POSITION: + case ateam_msgs::msg::RobotMotionCommand::BCM_HEADING_PIVOT: + case ateam_msgs::msg::RobotMotionCommand::BCM_POINT_PIVOT: + trajectory_maneuver(ros_msg, robot); + break; + + // Global Maneuvers + case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_VELOCITY: + global_velocity_maneuver(ros_msg); + break; + case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_ACCEL: + global_acceleration_maneuver(ros_msg); + break; + + // Local Maneuvers + case ateam_msgs::msg::RobotMotionCommand::BCM_LOCAL_VELOCITY: + local_velocity_maneuver(ros_msg, robot); + break; + case ateam_msgs::msg::RobotMotionCommand::BCM_LOCAL_ACCEL: + local_acceleration_maneuver(ros_msg, robot); + break; + } + + finalize_command(robot_move_command, ros_msg, robot); +} + +void ManeuverExecutor::bcm_off_maneuver() +{ + current_global_command_ = ateam_msgs::msg::Twist2D(); + prev_global_command_ = ateam_msgs::msg::Twist2D(); + prev_update_time_ = std::chrono::steady_clock::now(); +} + +// Trajectory Maneuvers +void ManeuverExecutor::trajectory_maneuver( + const ateam_msgs::msg::RobotMotionCommand & ros_msg, + ateam_msgs::msg::GameStateRobot robot) +{ + // Update trajectory state to current time + float dt = static_cast(get_dt()); + tick_trajectory(dt); + Vector6C_t state = get_trajectory_state(); + + double vision_yaw = get_yaw(robot.pose); + + // Check current position against predicted position + double current_dist = std::hypot(robot.pose.position.x - state.data[0], + robot.pose.position.y - state.data[1]); + bool current_pos_needs_replan = current_dist > 0.5 || + angles::shortest_angular_distance(vision_yaw, state.data[2]) > 0.3; + + bool maneuver_command_changed = (command_ != ros_msg); + bool replanning = maneuver_command_changed || current_pos_needs_replan; + if (replanning) { + Vector6C_t starting_state = state; // start from trajectory predicted state + + if (current_pos_needs_replan) { + // Predicted robot state is inaccurate + starting_state = Vector6C_t{ + static_cast(robot.pose.position.x), + static_cast(robot.pose.position.y), + static_cast(vision_yaw), + static_cast(prev_global_command_.x), + static_cast(prev_global_command_.y), + static_cast(prev_global_command_.theta), + }; + } else if (!command_uses_trajectory()) { + // If previous command did not use trajectory controller use prev commanded velocities instead + starting_state.data[3] = static_cast(prev_global_command_.x); + starting_state.data[4] = static_cast(prev_global_command_.y); + starting_state.data[5] = static_cast(prev_global_command_.theta); + } + + command_ = ros_msg; + plan_trajectory(starting_state); + state = get_trajectory_state(); + + pid_x_traj_.reset(); + pid_y_traj_.reset(); + pid_theta_traj_.reset(); + + pid_x_target_.reset(); + pid_y_target_.reset(); + pid_theta_target_.reset(); + } + + ateam_msgs::msg::Twist2D global_target_err = ateam_msgs::msg::Twist2D(); + global_target_err.x = command_.pose.x - robot.pose.position.x; + global_target_err.y = command_.pose.y - robot.pose.position.y; + global_target_err.theta = angles::shortest_angular_distance(get_yaw(robot.pose), + command_.pose.theta); + + ateam_msgs::msg::Twist2D global_traj_err = ateam_msgs::msg::Twist2D(); + global_traj_err.x = state.data[0] - robot.pose.position.x; + global_traj_err.y = state.data[1] - robot.pose.position.y; + global_traj_err.theta = angles::shortest_angular_distance(get_yaw(robot.pose), state.data[2]); + + ateam_msgs::msg::Twist2D global_feedforward = ateam_msgs::msg::Twist2D(); + ateam_msgs::msg::Twist2D global_feedback = ateam_msgs::msg::Twist2D(); + + // Default Control Behavior + global_feedforward.x = state.data[3]; + global_feedforward.y = state.data[4]; + global_feedforward.theta = state.data[5]; + + global_feedback.x = pid_x_traj_.compute_command(global_traj_err.x, dt); + global_feedback.y = pid_y_traj_.compute_command(global_traj_err.y, dt); + global_feedback.theta = pid_theta_traj_.compute_command(global_traj_err.theta, dt); + + // Custom trajectory control behavior + switch(command_.body_control_mode) { + case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_POSITION: + { + global_feedback.theta = 0.0; + + // Handle slight xy overshoot and final position offset due to vision filter delay + const double threshold = 0.05; + if (abs(global_target_err.x) < threshold) { + global_feedforward.x = 0.0; + global_feedback.x = pid_x_target_.compute_command(global_target_err.x, dt); + } + + if (abs(global_target_err.y) < threshold) { + global_feedforward.y = 0.0; + global_feedback.y = pid_y_target_.compute_command(global_target_err.y, dt); + } + break; + } + + case ateam_msgs::msg::RobotMotionCommand::BCM_HEADING_PIVOT: + case ateam_msgs::msg::RobotMotionCommand::BCM_POINT_PIVOT: + std::cerr << "radius: " << command_.pivot_orbit_radius << std::endl; + global_feedback.x = 0.0; + global_feedback.y = 0.0; + break; + } + + current_global_command_.x = global_feedforward.x + global_feedback.x; + current_global_command_.y = global_feedforward.y + global_feedback.y; + current_global_command_.theta = global_feedforward.theta + global_feedback.theta; + + std::cerr << "theta: " << get_yaw(robot.pose) << ", final err: " << global_target_err.theta << ", traj err: " << global_traj_err.theta << std::endl; +} + +// Global Maneuvers +void ManeuverExecutor::global_velocity_maneuver(const ateam_msgs::msg::RobotMotionCommand & ros_msg) +{ + current_global_command_ = ros_msg.velocity; +} + +void ManeuverExecutor::global_acceleration_maneuver( + const ateam_msgs::msg::RobotMotionCommand & ros_msg) +{ + const double dt = get_dt(); + + current_global_command_.x = prev_global_command_.x + dt * ros_msg.acceleration.x; + current_global_command_.y = prev_global_command_.y + dt * ros_msg.acceleration.y; + current_global_command_.theta = prev_global_command_.theta + dt * ros_msg.acceleration.theta; +} + +// Local Maneuvers +void ManeuverExecutor::local_velocity_maneuver( + const ateam_msgs::msg::RobotMotionCommand & ros_msg, + ateam_msgs::msg::GameStateRobot robot) +{ + current_global_command_ = rotate_frame(ros_msg.velocity, -get_yaw(robot.pose)); +} + +void ManeuverExecutor::local_acceleration_maneuver( + const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) +{ + ateam_msgs::msg::Twist2D global_accel = rotate_frame(ros_msg.acceleration, -get_yaw(robot.pose)); + const double dt = get_dt(); + + current_global_command_.x = prev_global_command_.x + dt * global_accel.x; + current_global_command_.y = prev_global_command_.y + dt * global_accel.y; + current_global_command_.theta = prev_global_command_.theta + dt * global_accel.theta; +} + +void ManeuverExecutor::finalize_command( + RobotMoveCommand * robot_move_command, + const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) +{ + command_ = ros_msg; + double dt = get_dt(); + + ateam_msgs::msg::Twist2D local_command; + if (command_uses_trajectory()) { + // Motion limits for trajectory type maneuvers are mostly handled by the planner + Vector6C_t state = get_trajectory_state(); + local_command = rotate_frame(current_global_command_, state.data[2]); + } else { + // Handle non-trajectory type maneuver motion limits + traj_params_ = generate_trajectory_params(command_); + current_global_command_ = apply_xy_motion_limits(prev_global_command_, current_global_command_, + traj_params_.max_vel_linear, traj_params_.max_accel_linear, dt); + current_global_command_.theta = apply_1d_motion_limits(prev_global_command_.theta, + current_global_command_.theta, traj_params_.max_vel_angular, traj_params_.max_accel_angular, + dt); + + local_command = rotate_frame(current_global_command_, get_yaw(robot.pose)); + } + + MoveLocalVelocity * local_velocity_command = robot_move_command->mutable_local_velocity(); + local_velocity_command->set_forward(local_command.x); + local_velocity_command->set_left(local_command.y); + local_velocity_command->set_angular(local_command.theta); + + prev_global_command_ = current_global_command_; + prev_update_time_ = std::chrono::steady_clock::now(); +} + +ateam_msgs::msg::Twist2D ManeuverExecutor::apply_xy_motion_limits( + ateam_msgs::msg::Twist2D prev, + ateam_msgs::msg::Twist2D command, double vel_limit, double acc_limit, double dt) +{ + ateam_geometry::Vector prev_xy{prev.x, prev.y}; + ateam_geometry::Vector command_xy{command.x, command.y}; + ateam_geometry::Vector requested_acceleration = command_xy - prev_xy; + + double acceleration_magnitude = std::clamp(ateam_geometry::norm(requested_acceleration), 0.0, + dt * acc_limit); + ateam_geometry::Vector acc_limited_command = prev_xy + + (acceleration_magnitude * ateam_geometry::normalize(requested_acceleration)); + + double final_magnitude = std::clamp(ateam_geometry::norm(acc_limited_command), 0.0, vel_limit); + ateam_geometry::Vector final_xy = final_magnitude * + ateam_geometry::normalize(acc_limited_command); + + command.x = final_xy.x(); + command.y = final_xy.y(); + + return command; +} + +double ManeuverExecutor::apply_1d_motion_limits( + double prev, double command, double vel_limit, + double acc_limit, double dt) +{ + double requested_acceleration = command - prev; + double acc_limited_command = prev + std::clamp(requested_acceleration, -dt * acc_limit, + dt * acc_limit); + return std::clamp(acc_limited_command, -vel_limit, vel_limit); +} + +double ManeuverExecutor::get_dt() +{ + // const std::chrono::duration elapsed = + // std::chrono::steady_clock::now() - prev_update_time_; + // return elapsed.count(); + return 0.01; +} + +double ManeuverExecutor::get_yaw(geometry_msgs::msg::Pose pose) +{ + tf2::Quaternion quat; + tf2::fromMsg(pose.orientation, quat); + double yaw, pitch, roll; + tf2::Matrix3x3(quat).getEulerYPR(yaw, pitch, roll); + + return yaw; +} + +ateam_msgs::msg::Twist2D ManeuverExecutor::rotate_frame( + ateam_msgs::msg::Twist2D input_frame, + double angle) +{ + ateam_msgs::msg::Twist2D output_frame = ateam_msgs::msg::Twist2D(); + output_frame.x = cos(-angle) * input_frame.x - sin(-angle) * input_frame.y; + output_frame.y = sin(-angle) * input_frame.x + cos(-angle) * input_frame.y; + output_frame.theta = input_frame.theta; + return output_frame; +} + +TrajectoryParams_t ManeuverExecutor::generate_trajectory_params( + const ateam_msgs::msg::RobotMotionCommand & ros_msg) +{ + TrajectoryParams_t traj_params = ateam_controls_default_traj_params(); + + if (ros_msg.limit_vel_linear != 0.0) { + traj_params.max_vel_linear = ros_msg.limit_vel_linear; + } + if (ros_msg.limit_vel_angular != 0.0) { + traj_params.max_vel_angular = ros_msg.limit_vel_angular; + } + if (ros_msg.limit_acc_linear != 0.0) { + traj_params.max_accel_linear = ros_msg.limit_acc_linear; + } + if (ros_msg.limit_acc_angular != 0.0) { + traj_params.max_accel_angular = ros_msg.limit_acc_angular; + } + + return traj_params; +} + +PivotParams_t ManeuverExecutor::generate_pivot_params( + const ateam_msgs::msg::RobotMotionCommand & ros_msg) +{ + PivotParams_t pivot_params = ateam_controls_default_pivot_params(); + if (ros_msg.limit_vel_angular != 0.0) { + pivot_params.max_vel_angular = ros_msg.limit_vel_angular; + } + if (ros_msg.limit_acc_angular != 0.0) { + pivot_params.max_accel_angular = ros_msg.limit_acc_angular; + } + + pivot_params.orbit_radius = ros_msg.pivot_orbit_radius; + pivot_params.inset_angle = ros_msg.pivot_inset_angle; + pivot_params.compute_inset_angle = ros_msg.pivot_compute_inset_angle; + pivot_params.direction = static_cast(ros_msg.pivot_direction); + + return pivot_params; +} + +bool ManeuverExecutor::command_uses_trajectory() +{ + switch(command_.body_control_mode) { + case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_POSITION: + case ateam_msgs::msg::RobotMotionCommand::BCM_HEADING_PIVOT: + case ateam_msgs::msg::RobotMotionCommand::BCM_POINT_PIVOT: + return true; + + default: + return false; + } +} + +Vector6C_t ManeuverExecutor::get_trajectory_state() +{ + switch(command_.body_control_mode) { + case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_POSITION: + return position_trajectory_.state; + case ateam_msgs::msg::RobotMotionCommand::BCM_HEADING_PIVOT: + case ateam_msgs::msg::RobotMotionCommand::BCM_POINT_PIVOT: + return pivot_trajectory_.state; + default: + return Vector6C_t(); + } +} + +void ManeuverExecutor::plan_trajectory(Vector6C_t starting_state) +{ + switch (command_.body_control_mode) { + case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_POSITION: + { + const Vector3C_t target_pose{ + static_cast(command_.pose.x), + static_cast(command_.pose.y), + static_cast(command_.pose.theta) + }; + traj_params_ = generate_trajectory_params(command_); + ateam_controls_traj_from_target_pose( + starting_state, + target_pose, + traj_params_, + &position_trajectory_ + ); + break; + } + case ateam_msgs::msg::RobotMotionCommand::BCM_HEADING_PIVOT: + { + pivot_params_ = generate_pivot_params(command_); + ateam_controls_pivot_traj_from_target_heading( + starting_state, + command_.pivot_global_theta, + pivot_params_, + &pivot_trajectory_ + ); + break; + } + case ateam_msgs::msg::RobotMotionCommand::BCM_POINT_PIVOT: + { + pivot_params_ = generate_pivot_params(command_); + ateam_controls_pivot_traj_from_target_point( + starting_state, + command_.pivot_target_x, + command_.pivot_target_y, + pivot_params_, + &pivot_trajectory_ + ); + break; + } + } +} + +void ManeuverExecutor::tick_trajectory(float dt) +{ + switch (command_.body_control_mode) { + case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_POSITION: + ateam_controls_traj_tick(&position_trajectory_, dt); + break; + case ateam_msgs::msg::RobotMotionCommand::BCM_HEADING_PIVOT: + case ateam_msgs::msg::RobotMotionCommand::BCM_POINT_PIVOT: + ateam_controls_pivot_traj_tick(&pivot_trajectory_, dt); + break; + } +} +} // namespace ateam_ssl_simulation_radio_bridge::robot_maneuvers diff --git a/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.hpp b/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.hpp new file mode 100644 index 00000000..06548e81 --- /dev/null +++ b/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.hpp @@ -0,0 +1,131 @@ +// 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 ROBOT_MANEUVERS_HPP_ +#define ROBOT_MANEUVERS_HPP_ + +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include "pid.hpp" +#include "ateam_controls/ateam_controls.h" + +namespace ateam_ssl_simulation_radio_bridge::robot_maneuvers +{ +class ManeuverExecutor { +public: + ManeuverExecutor() + { + traj_params_ = ateam_controls_default_traj_params(); + prev_update_time_ = std::chrono::steady_clock::now(); + + pid_x_traj_ = PID(3.0, 0.0, 0.0); + pid_y_traj_ = PID(3.0, 0.0, 0.0); + pid_theta_traj_ = PID(0.1, 0.0, 0.0); + + pid_x_target_ = PID(4.0, 0.0, 0.001); + pid_y_target_ = PID(4.0, 0.0, 0.001); + pid_theta_target_ = PID(0.0, 0.0, 0.0); + } + + void set_command(ateam_msgs::msg::RobotMotionCommand command) + { + command_ = command; + } + + void execute_maneuver( + RobotMoveCommand * robot_move_command, + const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot); + +private: + void bcm_off_maneuver(); + + // Trajectory Maneuvers + void trajectory_maneuver( + const ateam_msgs::msg::RobotMotionCommand & ros_msg, + ateam_msgs::msg::GameStateRobot robot); + + // Global Maneuvers + void global_velocity_maneuver(const ateam_msgs::msg::RobotMotionCommand & ros_msg); + void global_acceleration_maneuver(const ateam_msgs::msg::RobotMotionCommand & ros_msg); + + // Local Maneuvers + void local_velocity_maneuver( + const ateam_msgs::msg::RobotMotionCommand & ros_msg, + ateam_msgs::msg::GameStateRobot robot); + void local_acceleration_maneuver( + const ateam_msgs::msg::RobotMotionCommand & ros_msg, + ateam_msgs::msg::GameStateRobot robot); + + void finalize_command( + RobotMoveCommand * robot_move_command, + const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot); + + ateam_msgs::msg::Twist2D apply_xy_motion_limits( + ateam_msgs::msg::Twist2D prev, + ateam_msgs::msg::Twist2D command, double vel_limit, double acc_limit, double dt); + double apply_1d_motion_limits( + double prev, double commanded, double vel_limit, double acc_limit, + double dt); + + double get_dt(); + double get_yaw(geometry_msgs::msg::Pose pose); + ateam_msgs::msg::Twist2D rotate_frame(ateam_msgs::msg::Twist2D input_frame, double angle); + + TrajectoryParams_t generate_trajectory_params( + const ateam_msgs::msg::RobotMotionCommand & ros_msg); + PivotParams_t generate_pivot_params( + const ateam_msgs::msg::RobotMotionCommand & ros_msg); + bool command_uses_trajectory(); + Vector6C_t get_trajectory_state(); + void plan_trajectory(Vector6C_t starting_state); + void tick_trajectory(float dt); + + PID pid_x_traj_; + PID pid_y_traj_; + PID pid_theta_traj_; + + PID pid_x_target_; + PID pid_y_target_; + PID pid_theta_target_; + + ateam_msgs::msg::RobotMotionCommand command_; // Current ros command, updates every frame + ateam_msgs::msg::Twist2D current_global_command_; // Velocity to be commanded this frame + ateam_msgs::msg::Twist2D prev_global_command_; // Velocity commanded previous frame + std::chrono::steady_clock::time_point prev_update_time_; + + BangBangTraj3D_t position_trajectory_; + TrajectoryParams_t traj_params_; + + PivotTrajectory_t pivot_trajectory_; + PivotParams_t pivot_params_; +}; + +} // namespace ateam_ssl_simulation_radio_bridge::robot_maneuvers + +#endif // ROBOT_MANEUVERS_HPP_ diff --git a/radio/ateam_ssl_simulation_radio_bridge/src/ssl_simulation_radio_bridge_node.cpp b/radio/ateam_ssl_simulation_radio_bridge/src/ssl_simulation_radio_bridge_node.cpp index 989bae0e..3785dac4 100644 --- a/radio/ateam_ssl_simulation_radio_bridge/src/ssl_simulation_radio_bridge_node.cpp +++ b/radio/ateam_ssl_simulation_radio_bridge/src/ssl_simulation_radio_bridge_node.cpp @@ -38,9 +38,11 @@ #include #include #include +#include #include #include "message_conversions.hpp" +#include "robot_maneuvers.hpp" namespace ateam_ssl_simulation_radio_bridge { @@ -62,8 +64,9 @@ class SSLSimulationRadioBridgeNode : public rclcpp::Node declare_parameter("ssl_sim_control_port", 10300); declare_parameter("ssl_sim_blue_port", 10301); declare_parameter("ssl_sim_yellow_port", 10302); + declare_parameter("command_timeout_ms", 100); - team_color_change_callback(ateam_common::TeamColor::Blue); + team_color_change_callback(ateam_common::TeamColor::Unknown); create_indexed_subscribers ( @@ -85,19 +88,20 @@ class SSLSimulationRadioBridgeNode : public rclcpp::Node rclcpp::SystemDefaultsQoS(), this); + world_subscription_ = create_subscription( + std::string(Topics::kWorld), rclcpp::SystemDefaultsQoS(), + std::bind(&SSLSimulationRadioBridgeNode::world_callback, this, std::placeholders::_1)); + send_simulator_control_service_ = create_service("~/send_simulator_control_packet", std::bind(&SSLSimulationRadioBridgeNode::handle_send_simulator_control, this, std::placeholders::_1, std::placeholders::_2)); std::ranges::fill(command_timestamps_, std::chrono::steady_clock::now()); - zero_command_timer_ = + send_command_timer_ = create_wall_timer( - std::chrono::milliseconds( - declare_parameter( - "command_timeout_ms", - 100)), - std::bind(&SSLSimulationRadioBridgeNode::zero_command_timer_callback, this)); + std::chrono::milliseconds(10), + std::bind(&SSLSimulationRadioBridgeNode::send_command_timer_callback, this)); udp_sim_control_ = std::make_unique( @@ -160,16 +164,29 @@ class SSLSimulationRadioBridgeNode : public rclcpp::Node std::bind_front(&SSLSimulationRadioBridgeNode::feedback_callback, this)); } + void world_callback(const ateam_msgs::msg::GameStateWorld & msg) + { + world_ = msg; + } + void send_command(const ateam_msgs::msg::RobotMotionCommand & msg, const int robot_id) { if (!udp_robot_control_) { return; } - RobotControl robots_control = message_conversions::fromMsg(msg, robot_id, get_logger()); - std::vector buffer; - buffer.resize(robots_control.ByteSizeLong()); - if (robots_control.SerializeToArray(buffer.data(), buffer.size())) { - udp_robot_control_->send(static_cast(buffer.data()), buffer.size()); + + const auto robot = world_.our_robots[robot_id]; + // Somehow nonvisible robots have their id set to 0 in the world topic + // easier to just not interact with them + if (robot.visible) { + auto & maneuver_executor = manuever_executors_[robot_id]; + RobotControl robots_control = message_conversions::fromMsg(msg, robot, maneuver_executor, + get_logger()); + std::vector buffer; + buffer.resize(robots_control.ByteSizeLong()); + if (robots_control.SerializeToArray(buffer.data(), buffer.size())) { + udp_robot_control_->send(static_cast(buffer.data()), buffer.size()); + } } } @@ -178,7 +195,7 @@ class SSLSimulationRadioBridgeNode : public rclcpp::Node int robot_id) { command_timestamps_[robot_id] = std::chrono::steady_clock::now(); - send_command(*robot_commands_msg, robot_id); + commands_[robot_id] = *robot_commands_msg; } void feedback_callback(const uint8_t * buffer, size_t bytes_received) @@ -203,18 +220,7 @@ class SSLSimulationRadioBridgeNode : public rclcpp::Node } } - void send_zero_command(const int id) - { - ateam_msgs::msg::RobotMotionCommand msg; - msg.dribbler_speed = 0.0; - msg.kick_request = ateam_msgs::msg::RobotMotionCommand::KR_DISABLE; - msg.velocity.x = 0.0; - msg.velocity.y = 0.0; - msg.velocity.theta = 0.0; - send_command(msg, id); - } - - void zero_command_timer_callback() + void send_command_timer_callback() { const auto timeout_duration = std::chrono::milliseconds( get_parameter( @@ -222,8 +228,12 @@ class SSLSimulationRadioBridgeNode : public rclcpp::Node const auto timeout_time = std::chrono::steady_clock::now() - timeout_duration; for (auto id = 0; id < 16; ++id) { if (command_timestamps_[id] < timeout_time) { - send_zero_command(id); + ateam_msgs::msg::RobotMotionCommand zero_command = ateam_msgs::msg::RobotMotionCommand(); + zero_command.kick_request = ateam_msgs::msg::RobotMotionCommand::KR_DISABLE; + commands_[id] = zero_command; } + + send_command(commands_[id], id); } } @@ -237,9 +247,14 @@ class SSLSimulationRadioBridgeNode : public rclcpp::Node 16> feedback_publishers_; std::array::SharedPtr, 16> connection_publishers_; + rclcpp::Subscription::SharedPtr world_subscription_; + ateam_msgs::msg::GameStateWorld world_; + std::array commands_; + std::array manuever_executors_; rclcpp::Service::SharedPtr send_simulator_control_service_; - rclcpp::TimerBase::SharedPtr zero_command_timer_; + rclcpp::TimerBase::SharedPtr send_command_timer_; std::array command_timestamps_; };