From 0a028a96b605aa9878bc58da042c5d6d76f18f5c Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 5 May 2026 22:05:09 -0400 Subject: [PATCH 01/12] Removes leftover interfaces from ssl_ros_bridge split --- ateam_msgs/CMakeLists.txt | 5 ----- ateam_msgs/srv/ReconnectTeamClient.srv | 9 --------- ateam_msgs/srv/SetDesiredKeeper.srv | 6 ------ ateam_msgs/srv/SetTeamAdvantageChoice.srv | 10 ---------- ateam_msgs/srv/SubstituteBot.srv | 5 ----- 5 files changed, 35 deletions(-) delete mode 100644 ateam_msgs/srv/ReconnectTeamClient.srv delete mode 100644 ateam_msgs/srv/SetDesiredKeeper.srv delete mode 100644 ateam_msgs/srv/SetTeamAdvantageChoice.srv delete mode 100644 ateam_msgs/srv/SubstituteBot.srv diff --git a/ateam_msgs/CMakeLists.txt b/ateam_msgs/CMakeLists.txt index 139cc0d92..f100d7ad5 100755 --- a/ateam_msgs/CMakeLists.txt +++ b/ateam_msgs/CMakeLists.txt @@ -16,7 +16,6 @@ rosidl_generate_interfaces(${PROJECT_NAME} msg/OverlayArray.msg msg/RefereeInfo.msg msg/RobotMotionCommand.msg - msg/TeamClientConnectionStatus.msg msg/GameStateBall.msg msg/GameStateRobot.msg @@ -37,13 +36,9 @@ rosidl_generate_interfaces(${PROJECT_NAME} msg/PlayInfo.msg srv/GetFirmwareParameter.srv - srv/ReconnectTeamClient.srv - srv/SetDesiredKeeper.srv srv/SetFirmwareParameter.srv srv/SetOverridePlay.srv srv/SetPlayEnabled.srv - srv/SetTeamAdvantageChoice.srv - srv/SubstituteBot.srv srv/SetIgnoreFieldSide.srv srv/SendSimulatorControlPacket.srv diff --git a/ateam_msgs/srv/ReconnectTeamClient.srv b/ateam_msgs/srv/ReconnectTeamClient.srv deleted file mode 100644 index 251ec6234..000000000 --- a/ateam_msgs/srv/ReconnectTeamClient.srv +++ /dev/null @@ -1,9 +0,0 @@ -# Request - -# Address of the server to connect to. If blank, will reconnect to the last used address. -string server_address "" - ---- -# Response - -bool success diff --git a/ateam_msgs/srv/SetDesiredKeeper.srv b/ateam_msgs/srv/SetDesiredKeeper.srv deleted file mode 100644 index 8a54d577b..000000000 --- a/ateam_msgs/srv/SetDesiredKeeper.srv +++ /dev/null @@ -1,6 +0,0 @@ -# Request -int32 desired_keeper ---- -# Response -bool success -string reason diff --git a/ateam_msgs/srv/SetTeamAdvantageChoice.srv b/ateam_msgs/srv/SetTeamAdvantageChoice.srv deleted file mode 100644 index 6cb00d6c4..000000000 --- a/ateam_msgs/srv/SetTeamAdvantageChoice.srv +++ /dev/null @@ -1,10 +0,0 @@ -# Request -uint8 choice - -uint8 STOP=0 -uint8 CONTINUE=1 - ---- -# Response -bool success -string reason diff --git a/ateam_msgs/srv/SubstituteBot.srv b/ateam_msgs/srv/SubstituteBot.srv deleted file mode 100644 index 7b785ef4e..000000000 --- a/ateam_msgs/srv/SubstituteBot.srv +++ /dev/null @@ -1,5 +0,0 @@ -# Request ---- -# Response -bool success -string reason From 9a77103e27a4f4c4d8bf3ad31a6172725c09c57e Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 5 May 2026 22:26:12 -0400 Subject: [PATCH 02/12] Adds body control modes to RobotMotionCommand --- ateam_msgs/CMakeLists.txt | 1 + ateam_msgs/msg/RobotMotionCommand.msg | 28 +++++++++++++++++---------- ateam_msgs/msg/Twist2D.msg | 3 +++ 3 files changed, 22 insertions(+), 10 deletions(-) create mode 100644 ateam_msgs/msg/Twist2D.msg diff --git a/ateam_msgs/CMakeLists.txt b/ateam_msgs/CMakeLists.txt index f100d7ad5..be02fcf5c 100755 --- a/ateam_msgs/CMakeLists.txt +++ b/ateam_msgs/CMakeLists.txt @@ -16,6 +16,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} msg/OverlayArray.msg msg/RefereeInfo.msg msg/RobotMotionCommand.msg + msg/Twist2D.msg msg/GameStateBall.msg msg/GameStateRobot.msg diff --git a/ateam_msgs/msg/RobotMotionCommand.msg b/ateam_msgs/msg/RobotMotionCommand.msg index ce22894d1..b47071a9f 100644 --- a/ateam_msgs/msg/RobotMotionCommand.msg +++ b/ateam_msgs/msg/RobotMotionCommand.msg @@ -1,11 +1,15 @@ -# Global velocity commands -# XY in translation (m/s) -# Z in rotation (rad/s) -geometry_msgs/Twist twist -uint8 twist_frame 0 +uint8 body_control_mode +Twist2D pose # (m, rad) +Twist2D velocity # (m/s, rad/s) +Twist2D acceleration # (m/s/s, rad/s/s) +float64 limit_vel_linear # (m/s) +float64 limit_vel_angular # (rad/s) +float64 limit_acc_linear # (m/s/s) +float64 limit_acc_angular # (rad/s/s) + uint8 kick_request -float32 kick_speed # (m/s) -float32 dribbler_speed # (rpm) +float32 kick_speed # (m/s) +float32 dribbler_speed # (rpm) # kick options uint8 KR_ARM = 0 @@ -17,6 +21,10 @@ uint8 KR_CHIP_NOW = 5 uint8 KR_CHIP_TOUCH = 6 uint8 KR_CHIP_CAPTURED = 7 -# frame options -uint8 FRAME_WORLD = 0 -uint8 FRAME_BODY = 1 +# body control modes +uint8 BCM_OFF = 0 +uint8 BCM_GLOBAL_POSITION = 1 +uint8 BCM_GLOBAL_VELOCITY = 2 +uint8 BCM_LOCAL_VELOCITY = 3 +uint8 BCM_GLOBAL_ACCEL = 4 +uint8 BCM_LOCAL_ACCEL = 5 diff --git a/ateam_msgs/msg/Twist2D.msg b/ateam_msgs/msg/Twist2D.msg new file mode 100644 index 000000000..3acb85874 --- /dev/null +++ b/ateam_msgs/msg/Twist2D.msg @@ -0,0 +1,3 @@ +float64 x +float64 y +float64 theta From 11532b8f7c0788e236a8f556f7fda954a68acdc9 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 5 May 2026 22:26:24 -0400 Subject: [PATCH 03/12] Adds BCM support to joystick control node --- .../src/joystick_control_node.cpp | 7 +- .../joystick_control_node_test.py | 70 ++++--------------- 2 files changed, 19 insertions(+), 58 deletions(-) diff --git a/ateam_joystick_control/src/joystick_control_node.cpp b/ateam_joystick_control/src/joystick_control_node.cpp index 02b58efda..68cda57af 100644 --- a/ateam_joystick_control/src/joystick_control_node.cpp +++ b/ateam_joystick_control/src/joystick_control_node.cpp @@ -156,11 +156,12 @@ class JoystickControlNode : public rclcpp::Node ateam_msgs::msg::RobotMotionCommand command_message; - command_message.twist.linear.x = get_parameter("mapping.linear.x.scale").as_double() * + command_message.body_control_mode = ateam_msgs::msg::RobotMotionCommand::BCM_LOCAL_VELOCITY; + command_message.velocity.x = get_parameter("mapping.linear.x.scale").as_double() * joy_message->axes[linear_x_axis_]; - command_message.twist.linear.y = get_parameter("mapping.linear.y.scale").as_double() * + command_message.velocity.y = get_parameter("mapping.linear.y.scale").as_double() * joy_message->axes[linear_y_axis_]; - command_message.twist.angular.z = get_parameter("mapping.angular.z.scale").as_double() * + command_message.velocity.theta = get_parameter("mapping.angular.z.scale").as_double() * joy_message->axes[angular_z_axis_]; if (kick_trigger_(*joy_message)) { diff --git a/ateam_joystick_control/test/launch_tests/joystick_control_node_test.py b/ateam_joystick_control/test/launch_tests/joystick_control_node_test.py index ca8b5f6a5..bbcf8b824 100644 --- a/ateam_joystick_control/test/launch_tests/joystick_control_node_test.py +++ b/ateam_joystick_control/test/launch_tests/joystick_control_node_test.py @@ -126,43 +126,15 @@ def setRobotId(self, robot_id): return TestJoystickControlNode.set_parameter_client \ .call(set_param_request) - def test_0_defaultRobotIdIsZero(self): - joy_msg = sensor_msgs.msg.Joy() - joy_msg.axes = [ - 1.0, - 1.0, - 0.0, - 1.0, - 0.0, - 0.0, - 0.0, - 0.0 - ] - joy_msg.buttons = [0] * 11 - - timeout = time.time() + 1 - while TestJoystickControlNode.received_msg_0 is None and \ - TestJoystickControlNode.received_msg_1 is None: - TestJoystickControlNode.pub.publish(joy_msg) - time.sleep(0.1) - if time.time() >= timeout: - break - - self.assertIsNotNone(TestJoystickControlNode.received_msg_0) - self.assertIsNone(TestJoystickControlNode.received_msg_1) - - self.assertAlmostEqual( - TestJoystickControlNode.received_msg_0.twist.linear.x, 1.0) - self.assertAlmostEqual( - TestJoystickControlNode.received_msg_0.twist.linear.y, 1.0) - self.assertAlmostEqual( - TestJoystickControlNode.received_msg_0.twist.linear.z, 0.0) - self.assertAlmostEqual( - TestJoystickControlNode.received_msg_0.twist.angular.x, 0.0) - self.assertAlmostEqual( - TestJoystickControlNode.received_msg_0.twist.angular.y, 0.0) - self.assertAlmostEqual( - TestJoystickControlNode.received_msg_0.twist.angular.z, 1.0) + def test_0_prepScenario(self): + # The control node defaults to ID -1, so we need to switch it to 0 first to make the rest + # of the test sequence work + set_param_response = self.setRobotId(0) + if not set_param_response.results[0].successful: + TestJoystickControlNode.node.get_logger() \ + .error(f'Setting parameter failed with reason: \ + {set_param_response.results[0].reason}') + self.assertTrue(set_param_response.results[0].successful) def test_1_shouldSendStopWhenJoystickSwitchesId(self): set_param_response = self.setRobotId(1) @@ -180,17 +152,11 @@ def test_1_shouldSendStopWhenJoystickSwitchesId(self): self.assertIsNotNone(TestJoystickControlNode.received_msg_0) self.assertAlmostEqual( - TestJoystickControlNode.received_msg_0.twist.linear.x, 0.0) - self.assertAlmostEqual( - TestJoystickControlNode.received_msg_0.twist.linear.y, 0.0) - self.assertAlmostEqual( - TestJoystickControlNode.received_msg_0.twist.linear.z, 0.0) + TestJoystickControlNode.received_msg_0.velocity.x, 0.0) self.assertAlmostEqual( - TestJoystickControlNode.received_msg_0.twist.angular.x, 0.0) + TestJoystickControlNode.received_msg_0.velocity.y, 0.0) self.assertAlmostEqual( - TestJoystickControlNode.received_msg_0.twist.angular.y, 0.0) - self.assertAlmostEqual( - TestJoystickControlNode.received_msg_0.twist.angular.z, 0.0) + TestJoystickControlNode.received_msg_0.velocity.theta, 0.0) def test_2_afterIdChangeCommandsShouldGoToNewRobot(self): joy_msg = sensor_msgs.msg.Joy() @@ -215,17 +181,11 @@ def test_2_afterIdChangeCommandsShouldGoToNewRobot(self): self.assertIsNotNone(TestJoystickControlNode.received_msg_1) self.assertAlmostEqual( - TestJoystickControlNode.received_msg_1.twist.linear.x, -1.0) - self.assertAlmostEqual( - TestJoystickControlNode.received_msg_1.twist.linear.y, -1.0) - self.assertAlmostEqual( - TestJoystickControlNode.received_msg_1.twist.linear.z, 0.0) - self.assertAlmostEqual( - TestJoystickControlNode.received_msg_1.twist.angular.x, 0.0) + TestJoystickControlNode.received_msg_1.velocity.x, -1.0) self.assertAlmostEqual( - TestJoystickControlNode.received_msg_1.twist.angular.y, 0.0) + TestJoystickControlNode.received_msg_1.velocity.y, -1.0) self.assertAlmostEqual( - TestJoystickControlNode.received_msg_1.twist.angular.z, -1.0) + TestJoystickControlNode.received_msg_1.velocity.theta, -1.0) def test_3_invalidRobotIdsShouldBeRejected(self): self.assertFalse(self.setRobotId(-2).results[0].successful) From d2e51a0e5d8ba51d0b10f24c6cdd0f1cab1b3a94 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 5 May 2026 22:33:44 -0400 Subject: [PATCH 04/12] Updates state_tracking --- .../src/game_state_tracker/game_state_tracker.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/state_tracking/ateam_game_state/src/game_state_tracker/game_state_tracker.cpp b/state_tracking/ateam_game_state/src/game_state_tracker/game_state_tracker.cpp index 779b5d8ee..c00c4a579 100644 --- a/state_tracking/ateam_game_state/src/game_state_tracker/game_state_tracker.cpp +++ b/state_tracking/ateam_game_state/src/game_state_tracker/game_state_tracker.cpp @@ -214,9 +214,12 @@ class GameStateTracker : public rclcpp::Node { void RobotCommandCallback(const ateam_msgs::msg::RobotMotionCommand::SharedPtr msg, int id) { - world_.our_robots[id].prev_command_vel = ateam_geometry::Vector(msg->twist.linear.x, - msg->twist.linear.y); - world_.our_robots[id].prev_command_omega = msg->twist.angular.z; + // TODO(barulicm): This assumes we only use the local velocity mode + // This is fine for now because by the time we support other modes in Kenobi, + // we shouldn't depend on these fields anyway + world_.our_robots[id].prev_command_vel = ateam_geometry::Vector(msg->velocity.x, + msg->velocity.y); + world_.our_robots[id].prev_command_omega = msg->velocity.theta; } void UpdateRefInfo() From 5a9a4ada29d73774a80965b6bea1e04ea578778d Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 5 May 2026 22:39:41 -0400 Subject: [PATCH 05/12] Updates ateam_motion_benchmark --- .../src/angular_velocity_benchmark.cpp | 8 ++++---- motion/ateam_motion_benchmark/src/velocity_benchmark.cpp | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/motion/ateam_motion_benchmark/src/angular_velocity_benchmark.cpp b/motion/ateam_motion_benchmark/src/angular_velocity_benchmark.cpp index 1304cadf7..2f7104dfc 100644 --- a/motion/ateam_motion_benchmark/src/angular_velocity_benchmark.cpp +++ b/motion/ateam_motion_benchmark/src/angular_velocity_benchmark.cpp @@ -346,8 +346,8 @@ class AngularVelocityBenchmarkNode : public rclcpp::Node command_speed_ = speed; - command.twist.angular.z = command_speed_; - command.twist.linear.y = -1 * command_speed_ * options_.radius; + command.velocity.theta = command_speed_; + command.velocity.y = -1 * command_speed_ * options_.radius; command.kick_request = ateam_msgs::msg::RobotMotionCommand::KR_DISABLE; @@ -376,8 +376,8 @@ class AngularVelocityBenchmarkNode : public rclcpp::Node { timer_.reset(); ateam_msgs::msg::RobotMotionCommand command; - command.twist.linear.x = 0.0; - command.twist.linear.y = 0.0; + command.velocity.x = 0.0; + command.velocity.y = 0.0; command.kick_request = ateam_msgs::msg::RobotMotionCommand::KR_DISABLE; command_pub_->publish(command); std::this_thread::sleep_for(std::chrono::milliseconds(100)); diff --git a/motion/ateam_motion_benchmark/src/velocity_benchmark.cpp b/motion/ateam_motion_benchmark/src/velocity_benchmark.cpp index 3d4a65ce6..a177cd5b1 100644 --- a/motion/ateam_motion_benchmark/src/velocity_benchmark.cpp +++ b/motion/ateam_motion_benchmark/src/velocity_benchmark.cpp @@ -350,9 +350,9 @@ class VelocityBenchmarkNode : public rclcpp::Node command_speed_ = speed; if(options_.axis == Axis::X) { - command.twist.linear.x = speed; + command.velocity.x = speed; } else { - command.twist.linear.y = speed; + command.velocity.y = speed; } command.kick_request = ateam_msgs::msg::RobotMotionCommand::KR_DISABLE; @@ -390,8 +390,8 @@ class VelocityBenchmarkNode : public rclcpp::Node { timer_.reset(); ateam_msgs::msg::RobotMotionCommand command; - command.twist.linear.x = 0.0; - command.twist.linear.y = 0.0; + command.velocity.x = 0.0; + command.velocity.y = 0.0; command.kick_request = ateam_msgs::msg::RobotMotionCommand::KR_DISABLE; command_pub_->publish(command); std::this_thread::sleep_for(std::chrono::milliseconds(100)); From ab24763f87ca5ba2113227f9140c31cc48385d6b Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 5 May 2026 22:48:17 -0400 Subject: [PATCH 06/12] Updates simulation bridge --- .../src/message_conversions.cpp | 6 +++--- .../src/ssl_simulation_radio_bridge_node.cpp | 5 +++-- 2 files changed, 6 insertions(+), 5 deletions(-) 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 2a87e1fc6..3d9b1c8c7 100644 --- a/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.cpp +++ b/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.cpp @@ -80,9 +80,9 @@ 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.twist.linear.x, logger)); - local_velocity_command->set_left(ReplaceNanWithZero(ros_msg.twist.linear.y, logger)); - local_velocity_command->set_angular(ReplaceNanWithZero(ros_msg.twist.angular.z, logger)); + 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)); return robots_control; } 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 02b125477..989bae0e5 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 @@ -208,8 +208,9 @@ class SSLSimulationRadioBridgeNode : public rclcpp::Node ateam_msgs::msg::RobotMotionCommand msg; msg.dribbler_speed = 0.0; msg.kick_request = ateam_msgs::msg::RobotMotionCommand::KR_DISABLE; - msg.twist.linear.x = 0.0; - msg.twist.angular.z = 0.0; + msg.velocity.x = 0.0; + msg.velocity.y = 0.0; + msg.velocity.theta = 0.0; send_command(msg, id); } From fb8d3bf07913dd9fb4e9c7045e8691d7fcbeed99 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 5 May 2026 22:52:32 -0400 Subject: [PATCH 07/12] Minimal support updates to radio bridge --- .../src/radio_bridge_node.cpp | 21 +++++++++++++------ .../test/launch_tests/bridge_command_test.py | 3 ++- 2 files changed, 17 insertions(+), 7 deletions(-) diff --git a/radio/ateam_radio_bridge/src/radio_bridge_node.cpp b/radio/ateam_radio_bridge/src/radio_bridge_node.cpp index a6680322e..0585b03c9 100644 --- a/radio/ateam_radio_bridge/src/radio_bridge_node.cpp +++ b/radio/ateam_radio_bridge/src/radio_bridge_node.cpp @@ -176,9 +176,18 @@ class RadioBridgeNode : public rclcpp::Node const std::lock_guard lock(mutex_); motion_commands_[robot_id] = *command_msg; auto & command = motion_commands_[robot_id]; - ReplaceNanWithZero(command.twist.linear.x); - ReplaceNanWithZero(command.twist.linear.y); - ReplaceNanWithZero(command.twist.angular.z); + ReplaceNanWithZero(command.pose.x); + ReplaceNanWithZero(command.pose.y); + ReplaceNanWithZero(command.pose.theta); + ReplaceNanWithZero(command.velocity.x); + ReplaceNanWithZero(command.velocity.y); + ReplaceNanWithZero(command.velocity.theta); + ReplaceNanWithZero(command.acceleration.x); + ReplaceNanWithZero(command.acceleration.y); + ReplaceNanWithZero(command.acceleration.theta); + if(command.body_control_mode != ateam_msgs::msg::RobotMotionCommand::BCM_LOCAL_VELOCITY) { + RCLCPP_WARN(get_logger(), "Received non-local-velocity command. This is not fully supported by the radio bridge and may cause issues."); + } motion_command_timestamps_[robot_id] = std::chrono::steady_clock::now(); } @@ -279,9 +288,9 @@ class RadioBridgeNode : public rclcpp::Node 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(motion_commands_[id].twist.linear.x), - static_cast(motion_commands_[id].twist.linear.y), - static_cast(motion_commands_[id].twist.angular.z), + static_cast(motion_commands_[id].velocity.x), + static_cast(motion_commands_[id].velocity.y), + static_cast(motion_commands_[id].velocity.theta), 0.0, // TODO(barulicm): max_linear_acc 0.0 // TODO(barulicm): max_angular_acc }; diff --git a/radio/ateam_radio_bridge/test/launch_tests/bridge_command_test.py b/radio/ateam_radio_bridge/test/launch_tests/bridge_command_test.py index e84ea8997..bb50bd4cf 100644 --- a/radio/ateam_radio_bridge/test/launch_tests/bridge_command_test.py +++ b/radio/ateam_radio_bridge/test/launch_tests/bridge_command_test.py @@ -74,7 +74,8 @@ def test_commands(self): self.assertTrue(self.robot.isConnected()) cmd_msg = ateam_msgs.msg.RobotMotionCommand() - cmd_msg.twist.linear.x = 2.0 + cmd_msg.body_control_mode = ateam_msgs.msg.RobotMotionCommand.BCM_LOCAL_VELOCITY + cmd_msg.velocity.x = 2.0 timeout = time.time() + 1 while True: From c54d7174c35d01941f005ec3a998e81b9aeac67b Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 5 May 2026 23:07:02 -0400 Subject: [PATCH 08/12] MVP updates to kenobi --- ateam_bringup/launch/autonomy.launch.xml | 2 -- .../src/core/defense_area_enforcement.cpp | 14 ++++++++------ ateam_kenobi/src/kenobi_node.cpp | 17 +++++------------ 3 files changed, 13 insertions(+), 20 deletions(-) diff --git a/ateam_bringup/launch/autonomy.launch.xml b/ateam_bringup/launch/autonomy.launch.xml index 6f0a5f00f..d7a7ae962 100644 --- a/ateam_bringup/launch/autonomy.launch.xml +++ b/ateam_bringup/launch/autonomy.launch.xml @@ -1,5 +1,4 @@ - @@ -20,7 +19,6 @@ - diff --git a/ateam_kenobi/src/core/defense_area_enforcement.cpp b/ateam_kenobi/src/core/defense_area_enforcement.cpp index 6fc8f2e78..caed6e9ef 100644 --- a/ateam_kenobi/src/core/defense_area_enforcement.cpp +++ b/ateam_kenobi/src/core/defense_area_enforcement.cpp @@ -43,8 +43,8 @@ void EnforceDefenseAreaKeepout( } auto & command = *maybe_command; if (WouldVelocityCauseCollision(world, robot_id, command)) { - command.twist.linear.x = 0.0; - command.twist.linear.y = 0.0; + command.velocity.x = 0.0; + command.velocity.y = 0.0; } } } @@ -72,11 +72,13 @@ bool WouldVelocityCauseCollision( const double delta_t = 0.01; - ateam_geometry::Vector velocity{motion_command.twist.linear.x, - motion_command.twist.linear.y}; - if (motion_command.twist_frame == ateam_msgs::msg::RobotMotionCommand::FRAME_BODY) { - velocity = ateam_kenobi::motion::LocalToWorldFrame(velocity, world.our_robots[robot_id]); + if (motion_command.body_control_mode != ateam_msgs::msg::RobotMotionCommand::BCM_LOCAL_VELOCITY) { + std::cerr << + "WARNING: Non-local velocity motion commands not yet supported by defense area enforcement. " + "Skipping check.\n"; + return false; } + ateam_geometry::Vector velocity{motion_command.velocity.x, motion_command.velocity.y}; const ateam_geometry::Vector displacement = velocity * delta_t; diff --git a/ateam_kenobi/src/kenobi_node.cpp b/ateam_kenobi/src/kenobi_node.cpp index ebbbe965d..7447f8282 100644 --- a/ateam_kenobi/src/kenobi_node.cpp +++ b/ateam_kenobi/src/kenobi_node.cpp @@ -69,7 +69,6 @@ class KenobiNode : public rclcpp::Node overlays_(""), motion_executor_(get_logger().get_child("motion")) { - declare_parameter("use_world_velocities", false); declare_parameter("use_emulated_ballsense", false); overlay_publisher_ = create_publisher( @@ -223,8 +222,6 @@ class KenobiNode : public rclcpp::Node }); const auto motion_commands = motion_executor_.RunFrame(motion_intents, overlays_, world); - const auto use_world_vels = get_parameter("use_world_velocities").as_bool(); - std::array, 16> ros_commands; for(auto id = 0ul; id < commands.size(); ++id) { auto & maybe_cmd = commands[id]; @@ -232,21 +229,17 @@ class KenobiNode : public rclcpp::Node if (!maybe_cmd || !maybe_motion_cmd) { ros_commands[id] = std::nullopt; } else { - const auto & robot = world.our_robots[id]; const auto & cmd = maybe_cmd.value(); const auto & motion_cmd = maybe_motion_cmd.value(); - const auto linear_vel = use_world_vels ? - motion::LocalToWorldFrame(motion_cmd.linear, robot) : motion_cmd.linear; + const auto linear_vel = motion_cmd.linear; auto & ros_cmd = ros_commands[id].emplace(); ros_cmd.dribbler_speed = cmd.dribbler_speed; ros_cmd.kick_request = static_cast(cmd.kick); ros_cmd.kick_speed = cmd.kick_speed; - ros_cmd.twist.linear.x = linear_vel.x(); - ros_cmd.twist.linear.y = linear_vel.y(); - ros_cmd.twist.angular.z = motion_cmd.angular; - ros_cmd.twist_frame = - use_world_vels ? ateam_msgs::msg::RobotMotionCommand::FRAME_WORLD : - ateam_msgs::msg::RobotMotionCommand::FRAME_BODY; + ros_cmd.body_control_mode = ateam_msgs::msg::RobotMotionCommand::BCM_LOCAL_VELOCITY; + ros_cmd.velocity.x = linear_vel.x(); + ros_cmd.velocity.y = linear_vel.y(); + ros_cmd.velocity.theta = motion_cmd.angular; } } From 0fea897ee48b0ab1734312d5b12260a10bddf2b0 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Wed, 6 May 2026 01:02:00 -0400 Subject: [PATCH 09/12] Adds support for additional control modes to radio bridge --- .../src/radio_bridge_node.cpp | 70 ++++++++++++++++--- 1 file changed, 62 insertions(+), 8 deletions(-) diff --git a/radio/ateam_radio_bridge/src/radio_bridge_node.cpp b/radio/ateam_radio_bridge/src/radio_bridge_node.cpp index 0585b03c9..65a00dc92 100644 --- a/radio/ateam_radio_bridge/src/radio_bridge_node.cpp +++ b/radio/ateam_radio_bridge/src/radio_bridge_node.cpp @@ -281,19 +281,13 @@ class RadioBridgeNode : public rclcpp::Node 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(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(motion_commands_[id].velocity.x), - static_cast(motion_commands_[id].velocity.y), - static_cast(motion_commands_[id].velocity.theta), - 0.0, // TODO(barulicm): max_linear_acc - 0.0 // TODO(barulicm): max_angular_acc - }; + FillBodyControl(control_msg, motion_commands_[id]); + const auto control_packet = CreatePacket(CC_CONTROL, control_msg); connections_[id]->send( reinterpret_cast(&control_packet), @@ -301,6 +295,66 @@ class RadioBridgeNode : public rclcpp::Node } } + void FillBodyControl(BasicControl & control_msg, const ateam_msgs::msg::RobotMotionCommand & command) { + switch(command.body_control_mode) { + case ateam_msgs::msg::RobotMotionCommand::BCM_OFF: + control_msg.body_control_mode = BCM_OFF; + break; + case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_POSITION: + control_msg.body_control_mode = BCM_GLOBAL_POSITION; + control_msg.cmd.global_pos = { + static_cast(command.pose.x), + static_cast(command.pose.y), + static_cast(command.pose.theta), + static_cast(command.limit_vel_linear), + static_cast(command.limit_vel_angular), + static_cast(command.limit_acc_linear), + static_cast(command.limit_acc_angular) + }; + break; + case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_VELOCITY: + control_msg.body_control_mode = BCM_GLOBAL_VELOCITY; + control_msg.cmd.global_vel = { + static_cast(command.velocity.x), + static_cast(command.velocity.y), + static_cast(command.velocity.theta), + static_cast(command.limit_vel_linear), + static_cast(command.limit_vel_angular) + }; + break; + case ateam_msgs::msg::RobotMotionCommand::BCM_LOCAL_VELOCITY: + control_msg.body_control_mode = BCM_LOCAL_VELOCITY; + control_msg.cmd.local_vel = { + static_cast(command.velocity.x), + static_cast(command.velocity.y), + static_cast(command.velocity.theta), + static_cast(command.limit_vel_linear), + static_cast(command.limit_vel_angular) + }; + break; + case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_ACCEL: + control_msg.body_control_mode = BCM_GLOBAL_ACCEL; + control_msg.cmd.global_acc = { + static_cast(command.acceleration.x), + static_cast(command.acceleration.y), + static_cast(command.acceleration.theta), + }; + break; + case ateam_msgs::msg::RobotMotionCommand::BCM_LOCAL_ACCEL: + control_msg.body_control_mode = BCM_LOCAL_ACCEL; + control_msg.cmd.local_acc = { + static_cast(command.acceleration.x), + static_cast(command.acceleration.y), + static_cast(command.acceleration.theta), + }; + break; + default: + RCLCPP_WARN(get_logger(), "Unknown body control mode: %d", command.body_control_mode); + control_msg.body_control_mode = BCM_OFF; + break; + } + } + void DiscoveryMessageCallback( const std::string & sender_address, const uint16_t sender_port, uint8_t * udp_packet_data, size_t udp_packet_size) From e2d520d2e92d31f0f4d2ebd53e8f831252e7f748 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Wed, 6 May 2026 01:12:48 -0400 Subject: [PATCH 10/12] Removes leftover warning and adds missing nan checks. --- .../src/radio_bridge_node.cpp | 34 ++++++++++++------- 1 file changed, 22 insertions(+), 12 deletions(-) diff --git a/radio/ateam_radio_bridge/src/radio_bridge_node.cpp b/radio/ateam_radio_bridge/src/radio_bridge_node.cpp index 65a00dc92..1e6c55762 100644 --- a/radio/ateam_radio_bridge/src/radio_bridge_node.cpp +++ b/radio/ateam_radio_bridge/src/radio_bridge_node.cpp @@ -169,6 +169,19 @@ class RadioBridgeNode : public rclcpp::Node } } + void ReplaceNanWithZero(float & val) { + if (std::isnan(val)) { + RCLCPP_WARN(get_logger(), "Radio bridge is replacing NaNs!"); + val = 0.0f; + } + } + + void ReplaceNanWithZero(ateam_msgs::msg::Twist2D & twist) { + ReplaceNanWithZero(twist.x); + ReplaceNanWithZero(twist.y); + ReplaceNanWithZero(twist.theta); + } + void MotionCommandCallback( const ateam_msgs::msg::RobotMotionCommand::SharedPtr command_msg, int robot_id) @@ -176,18 +189,15 @@ class RadioBridgeNode : public rclcpp::Node const std::lock_guard lock(mutex_); motion_commands_[robot_id] = *command_msg; auto & command = motion_commands_[robot_id]; - ReplaceNanWithZero(command.pose.x); - ReplaceNanWithZero(command.pose.y); - ReplaceNanWithZero(command.pose.theta); - ReplaceNanWithZero(command.velocity.x); - ReplaceNanWithZero(command.velocity.y); - ReplaceNanWithZero(command.velocity.theta); - ReplaceNanWithZero(command.acceleration.x); - ReplaceNanWithZero(command.acceleration.y); - ReplaceNanWithZero(command.acceleration.theta); - if(command.body_control_mode != ateam_msgs::msg::RobotMotionCommand::BCM_LOCAL_VELOCITY) { - RCLCPP_WARN(get_logger(), "Received non-local-velocity command. This is not fully supported by the radio bridge and may cause issues."); - } + ReplaceNanWithZero(command.pose); + ReplaceNanWithZero(command.velocity); + ReplaceNanWithZero(command.acceleration); + ReplaceNanWithZero(command.limit_vel_linear); + ReplaceNanWithZero(command.limit_vel_angular); + ReplaceNanWithZero(command.limit_acc_linear); + ReplaceNanWithZero(command.limit_acc_angular); + ReplaceNanWithZero(command.kick_speed); + ReplaceNanWithZero(command.dribbler_speed); motion_command_timestamps_[robot_id] = std::chrono::steady_clock::now(); } From a090a4e1334754a46749d0f6418077ed14dac230 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Wed, 6 May 2026 02:40:27 -0400 Subject: [PATCH 11/12] Implements vision updates in radio bridge --- radio/ateam_radio_bridge/CMakeLists.txt | 3 + radio/ateam_radio_bridge/package.xml | 1 + radio/ateam_radio_bridge/src/nan_helpers.cpp | 103 ++++++++++++++++++ radio/ateam_radio_bridge/src/nan_helpers.hpp | 55 ++++++++++ .../src/radio_bridge_node.cpp | 100 +++++++++++------ .../test/launch_tests/bridge_command_test.py | 41 ++++++- 6 files changed, 268 insertions(+), 35 deletions(-) create mode 100644 radio/ateam_radio_bridge/src/nan_helpers.cpp create mode 100644 radio/ateam_radio_bridge/src/nan_helpers.hpp diff --git a/radio/ateam_radio_bridge/CMakeLists.txt b/radio/ateam_radio_bridge/CMakeLists.txt index bc981b4fa..c16b1e5eb 100644 --- a/radio/ateam_radio_bridge/CMakeLists.txt +++ b/radio/ateam_radio_bridge/CMakeLists.txt @@ -11,10 +11,12 @@ find_package(rclcpp_components REQUIRED) find_package(ateam_radio_msgs REQUIRED) find_package(ateam_msgs REQUIRED) find_package(ateam_common REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) add_library(${PROJECT_NAME} SHARED src/firmware_parameter_server.cpp src/ip_address_helpers.cpp + src/nan_helpers.cpp src/radio_bridge_node.cpp src/rnp_packet_helpers.cpp ) @@ -27,6 +29,7 @@ ament_target_dependencies(${PROJECT_NAME} ateam_radio_msgs ateam_msgs ateam_common + tf2_geometry_msgs ) rclcpp_components_register_node( ${PROJECT_NAME} diff --git a/radio/ateam_radio_bridge/package.xml b/radio/ateam_radio_bridge/package.xml index 4e57f0676..ab9b0a30a 100644 --- a/radio/ateam_radio_bridge/package.xml +++ b/radio/ateam_radio_bridge/package.xml @@ -15,6 +15,7 @@ ateam_radio_msgs ateam_msgs ateam_common + tf2_geometry_msgs ament_lint_auto ament_lint_common diff --git a/radio/ateam_radio_bridge/src/nan_helpers.cpp b/radio/ateam_radio_bridge/src/nan_helpers.cpp new file mode 100644 index 000000000..c4081eb21 --- /dev/null +++ b/radio/ateam_radio_bridge/src/nan_helpers.cpp @@ -0,0 +1,103 @@ +// 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 "nan_helpers.hpp" +#include +#include + +namespace ateam_radio_bridge +{ + +void ReplaceNanWithZero(double & val, rclcpp::Logger logger) +{ + if (std::isnan(val)) { + RCLCPP_WARN(logger, "Radio bridge is replacing NaNs!"); + val = 0.0; + } +} + +void ReplaceNanWithZero(float & val, rclcpp::Logger logger) +{ + if (std::isnan(val)) { + RCLCPP_WARN(logger, "Radio bridge is replacing NaNs!"); + val = 0.0; + } +} + +void ReplaceNanWithZero(ateam_msgs::msg::Twist2D & twist, rclcpp::Logger logger) +{ + ReplaceNanWithZero(twist.x, logger); + ReplaceNanWithZero(twist.y, logger); + ReplaceNanWithZero(twist.theta, logger); +} + +void ReplaceNanWithZero(ateam_msgs::msg::RobotMotionCommand & command, rclcpp::Logger logger) +{ + ReplaceNanWithZero(command.pose, logger); + ReplaceNanWithZero(command.velocity, logger); + ReplaceNanWithZero(command.acceleration, logger); + ReplaceNanWithZero(command.limit_vel_linear, logger); + ReplaceNanWithZero(command.limit_vel_angular, logger); + ReplaceNanWithZero(command.limit_acc_linear, logger); + ReplaceNanWithZero(command.limit_acc_angular, logger); + ReplaceNanWithZero(command.kick_speed, logger); + ReplaceNanWithZero(command.dribbler_speed, logger); +} + +void ReplaceNanWithZero(ateam_msgs::msg::VisionStateRobot & vision_state, rclcpp::Logger logger) +{ + ReplaceNanWithZero(vision_state.pose, logger); + ReplaceNanWithZero(vision_state.twist, logger); + ReplaceNanWithZero(vision_state.twist_body, logger); + ReplaceNanWithZero(vision_state.accel, logger); +} + +void ReplaceNanWithZero(geometry_msgs::msg::Pose & pose, rclcpp::Logger logger) +{ + ReplaceNanWithZero(pose.position.x, logger); + ReplaceNanWithZero(pose.position.y, logger); + ReplaceNanWithZero(pose.position.z, logger); + ReplaceNanWithZero(pose.orientation.x, logger); + ReplaceNanWithZero(pose.orientation.y, logger); + ReplaceNanWithZero(pose.orientation.z, logger); + ReplaceNanWithZero(pose.orientation.w, logger); +} + +void ReplaceNanWithZero(geometry_msgs::msg::Twist & twist, rclcpp::Logger logger) +{ + ReplaceNanWithZero(twist.linear.x, logger); + ReplaceNanWithZero(twist.linear.y, logger); + ReplaceNanWithZero(twist.linear.z, logger); + ReplaceNanWithZero(twist.angular.x, logger); + ReplaceNanWithZero(twist.angular.y, logger); + ReplaceNanWithZero(twist.angular.z, logger); +} + +void ReplaceNanWithZero(geometry_msgs::msg::Accel & accel, rclcpp::Logger logger) +{ + ReplaceNanWithZero(accel.linear.x, logger); + ReplaceNanWithZero(accel.linear.y, logger); + ReplaceNanWithZero(accel.linear.z, logger); + ReplaceNanWithZero(accel.angular.x, logger); + ReplaceNanWithZero(accel.angular.y, logger); + ReplaceNanWithZero(accel.angular.z, logger); +} + +} // namespace ateam_radio_bridge diff --git a/radio/ateam_radio_bridge/src/nan_helpers.hpp b/radio/ateam_radio_bridge/src/nan_helpers.hpp new file mode 100644 index 000000000..1d8a8fed7 --- /dev/null +++ b/radio/ateam_radio_bridge/src/nan_helpers.hpp @@ -0,0 +1,55 @@ +// 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 ATEAM_RADIO_BRIDGE__RADIO_BRIDGE_NODE_HPP_ +#define ATEAM_RADIO_BRIDGE__RADIO_BRIDGE_NODE_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#define REPLACE_NAN_WITH_ZERO(val) ateam_radio_bridge::ReplaceNanWithZero(val, get_logger()) + +namespace ateam_radio_bridge +{ + +void ReplaceNanWithZero(double & val, rclcpp::Logger logger); + +void ReplaceNanWithZero(float & val, rclcpp::Logger logger); + +void ReplaceNanWithZero(ateam_msgs::msg::Twist2D & twist, rclcpp::Logger logger); + +void ReplaceNanWithZero(ateam_msgs::msg::RobotMotionCommand & command, rclcpp::Logger logger); + +void ReplaceNanWithZero(ateam_msgs::msg::VisionStateRobot & vision_state, rclcpp::Logger logger); + +void ReplaceNanWithZero(geometry_msgs::msg::Pose & pose, rclcpp::Logger logger); + +void ReplaceNanWithZero(geometry_msgs::msg::Twist & twist, rclcpp::Logger logger); + +void ReplaceNanWithZero(geometry_msgs::msg::Accel & accel, rclcpp::Logger logger); + +} // namespace ateam_radio_bridge + +#endif // ATEAM_RADIO_BRIDGE__RADIO_BRIDGE_NODE_HPP_ diff --git a/radio/ateam_radio_bridge/src/radio_bridge_node.cpp b/radio/ateam_radio_bridge/src/radio_bridge_node.cpp index 1e6c55762..dcc3ced5b 100644 --- a/radio/ateam_radio_bridge/src/radio_bridge_node.cpp +++ b/radio/ateam_radio_bridge/src/radio_bridge_node.cpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -35,13 +36,16 @@ #include #include #include +#include #include #include #include #include +#include #include "rnp_packet_helpers.hpp" #include "ip_address_helpers.hpp" +#include "nan_helpers.hpp" #include "firmware_parameter_server.hpp" // TODO(barulicm) add warning if we see another instance of this running via multicast @@ -66,6 +70,7 @@ class RadioBridgeNode : public rclcpp::Node : rclcpp::Node("radio_bridge", options), sustain_timeout_threshold_(declare_parameter("sustain_timeout_ms", 250)), connect_timeout_threshold_(declare_parameter("connect_timeout_ms", 750)), + vision_state_staleness_threshold_(declare_parameter("vision_state_staleness_ms", 100)), command_timeout_threshold_(declare_parameter("command_timeout_ms", 100)), game_controller_listener_(*this, std::bind_front(&RadioBridgeNode::TeamColorChangeCallback, this)), @@ -128,6 +133,8 @@ class RadioBridgeNode : public rclcpp::Node std::chrono::duration(1.0 / declare_parameter("command_frequency", 60.0)), std::bind(&RadioBridgeNode::SendCommandsCallback, this)); + SetupVisionSubscribers(game_controller_listener_.GetTeamColor()); + RCLCPP_INFO(get_logger(), "Radio bridge node ready."); } @@ -135,6 +142,7 @@ class RadioBridgeNode : public rclcpp::Node // Incoming timeouts const std::chrono::milliseconds sustain_timeout_threshold_; const std::chrono::milliseconds connect_timeout_threshold_; + const std::chrono::milliseconds vision_state_staleness_threshold_; // Outgoing timeouts const std::chrono::milliseconds command_timeout_threshold_; @@ -142,11 +150,15 @@ class RadioBridgeNode : public rclcpp::Node std::mutex mutex_; std::array motion_commands_; std::array motion_command_timestamps_; + std::array vision_states_; + std::array vision_state_timestamps_; std::array shutdown_requested_; std::array reboot_requested_; ateam_common::GameControllerListener game_controller_listener_; std::array::SharedPtr, 16> motion_command_subscriptions_; + std::array::SharedPtr, + 16> vision_state_subscriptions_; std::array::SharedPtr, 16> connection_publishers_; std::array::SharedPtr, @@ -162,26 +174,6 @@ class RadioBridgeNode : public rclcpp::Node rclcpp::TimerBase::SharedPtr connection_check_timer_; rclcpp::TimerBase::SharedPtr command_send_timer_; - void ReplaceNanWithZero(double & val) { - if (std::isnan(val)) { - RCLCPP_WARN(get_logger(), "Radio bridge is replacing NaNs!"); - val = 0.0; - } - } - - void ReplaceNanWithZero(float & val) { - if (std::isnan(val)) { - RCLCPP_WARN(get_logger(), "Radio bridge is replacing NaNs!"); - val = 0.0f; - } - } - - void ReplaceNanWithZero(ateam_msgs::msg::Twist2D & twist) { - ReplaceNanWithZero(twist.x); - ReplaceNanWithZero(twist.y); - ReplaceNanWithZero(twist.theta); - } - void MotionCommandCallback( const ateam_msgs::msg::RobotMotionCommand::SharedPtr command_msg, int robot_id) @@ -189,18 +181,21 @@ class RadioBridgeNode : public rclcpp::Node const std::lock_guard lock(mutex_); motion_commands_[robot_id] = *command_msg; auto & command = motion_commands_[robot_id]; - ReplaceNanWithZero(command.pose); - ReplaceNanWithZero(command.velocity); - ReplaceNanWithZero(command.acceleration); - ReplaceNanWithZero(command.limit_vel_linear); - ReplaceNanWithZero(command.limit_vel_angular); - ReplaceNanWithZero(command.limit_acc_linear); - ReplaceNanWithZero(command.limit_acc_angular); - ReplaceNanWithZero(command.kick_speed); - ReplaceNanWithZero(command.dribbler_speed); + REPLACE_NAN_WITH_ZERO(command); motion_command_timestamps_[robot_id] = std::chrono::steady_clock::now(); } + void VisionStateCallback( + const ateam_msgs::msg::VisionStateRobot::SharedPtr vision_msg, + int robot_id) + { + const std::lock_guard lock(mutex_); + vision_states_[robot_id] = *vision_msg; + REPLACE_NAN_WITH_ZERO(vision_states_[robot_id]); + vision_state_timestamps_[robot_id] = std::chrono::steady_clock::now(); + RCLCPP_INFO(get_logger(), "Received vision state for robot %d.", robot_id); + } + void CloseConnection(const std::size_t & connection_index, bool send_goodbye = true) { std::unique_ptr connection; @@ -275,7 +270,9 @@ class RadioBridgeNode : public rclcpp::Node { RCLCPP_WARN(get_logger(), "Robot %d command topic inactive. Sending zeros.", id); motion_commands_[id] = ateam_msgs::msg::RobotMotionCommand(); + motion_commands_[id].body_control_mode = ateam_msgs::msg::RobotMotionCommand::BCM_OFF; motion_commands_[id].kick_request = ateam_msgs::msg::RobotMotionCommand::KR_DISABLE; + motion_commands_[id].dribbler_speed = 0.0; } BasicControl control_msg; control_msg.request_shutdown = shutdown_requested_[id]; @@ -286,11 +283,8 @@ class RadioBridgeNode : public rclcpp::Node 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.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; + FillVisionUpdate(control_msg, vision_states_[id], vision_state_timestamps_[id]); control_msg.kick_request = static_cast(motion_commands_[id].kick_request); control_msg.play_song = 0; control_msg.reserved2[0] = 0; @@ -365,6 +359,18 @@ class RadioBridgeNode : public rclcpp::Node } } + void FillVisionUpdate(BasicControl & control_msg, const ateam_msgs::msg::VisionStateRobot & vision_state, const std::chrono::steady_clock::time_point & timestamp) { + const auto now = std::chrono::steady_clock::now(); + if (now - timestamp > vision_state_staleness_threshold_) { + control_msg.vision_update = 0; + return; + } + control_msg.vision_update = 1; + control_msg.vision_position_update[0] = static_cast(vision_state.pose.position.x); + control_msg.vision_position_update[1] = static_cast(vision_state.pose.position.y); + control_msg.vision_position_update[2] = static_cast(GetYaw(vision_state.pose)); + } + void DiscoveryMessageCallback( const std::string & sender_address, const uint16_t sender_port, uint8_t * udp_packet_data, size_t udp_packet_size) @@ -549,11 +555,12 @@ class RadioBridgeNode : public rclcpp::Node } } - void TeamColorChangeCallback(const ateam_common::TeamColor) + void TeamColorChangeCallback(const ateam_common::TeamColor color) { for (auto i = 0ul; i < connections_.size(); ++i) { CloseConnection(i); } + SetupVisionSubscribers(color); } void SendPowerRequestCallback( @@ -606,6 +613,31 @@ class RadioBridgeNode : public rclcpp::Node } } + void SetupVisionSubscribers(const ateam_common::TeamColor color) { + for(auto & sub : vision_state_subscriptions_) { + sub.reset(); + } + if(color == ateam_common::TeamColor::Unknown) { + return; + } + const auto topic_prefix = color == ateam_common::TeamColor::Yellow ? Topics::kYellowTeamRobotPrefix : Topics::kBlueTeamRobotPrefix; + ateam_common::indexed_topic_helpers::create_indexed_subscribers( + vision_state_subscriptions_, + topic_prefix, + rclcpp::SystemDefaultsQoS(), + &RadioBridgeNode::VisionStateCallback, + this); + } + + double GetYaw(const 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; + } + }; } // namespace ateam_radio_bridge diff --git a/radio/ateam_radio_bridge/test/launch_tests/bridge_command_test.py b/radio/ateam_radio_bridge/test/launch_tests/bridge_command_test.py index bb50bd4cf..ef3760d07 100644 --- a/radio/ateam_radio_bridge/test/launch_tests/bridge_command_test.py +++ b/radio/ateam_radio_bridge/test/launch_tests/bridge_command_test.py @@ -59,6 +59,11 @@ def setUpClass(cls): "/radio_bridge/robot_motion_commands/robot0", qos_profile_system_default, ) + cls.vis_pub = cls.node.create_publisher( + ateam_msgs.msg.VisionStateRobot, + "/yellow_team/robot0", + qos_profile_system_default, + ) @classmethod def tearDownClass(cls): @@ -67,12 +72,13 @@ def tearDownClass(cls): cls.node.destroy_node() rclpy.shutdown(context=cls.context) - def test_commands(self): + def test_0_connection(self): connect_timeout = time.time() + 1 while not self.robot.isConnected() and time.time() < connect_timeout: time.sleep(0.1) self.assertTrue(self.robot.isConnected()) + def test_1_commands(self): cmd_msg = ateam_msgs.msg.RobotMotionCommand() cmd_msg.body_control_mode = ateam_msgs.msg.RobotMotionCommand.BCM_LOCAL_VELOCITY cmd_msg.velocity.x = 2.0 @@ -92,6 +98,39 @@ def test_commands(self): if abs(vel_x_linear - 2.0) < 0.1: # Pass the test return + + def test_2_vision_updates(self): + vis_msg = ateam_msgs.msg.VisionStateRobot() + vis_msg.pose.position.x = 1.0 + vis_msg.pose.position.y = 2.0 + vis_msg.pose.orientation.x = 0.0 + vis_msg.pose.orientation.y = 0.0 + vis_msg.pose.orientation.z = 0.707 + vis_msg.pose.orientation.w = 0.707 + self.vis_pub.publish(vis_msg) + + timeout = time.time() + 1 + while True: + time.sleep(0.017) # 60Hz + if time.time() > timeout: + self.fail("timed out waiting for valid vision update packet") + break + self.vis_pub.publish(vis_msg) + last_packet = self.robot.getLastCmdMessage() + if len(last_packet) != 64: + continue + # Extract vision updates + vision_update_flag = struct.unpack(" Date: Wed, 13 May 2026 00:39:11 -0400 Subject: [PATCH 12/12] Fixes from physical bot testing in Marietta --- .../src/radio_bridge_node.cpp | 34 +++++++++++-------- .../src/rnp_packet_helpers.cpp | 14 ++++---- 2 files changed, 27 insertions(+), 21 deletions(-) diff --git a/radio/ateam_radio_bridge/src/radio_bridge_node.cpp b/radio/ateam_radio_bridge/src/radio_bridge_node.cpp index dcc3ced5b..99b750e3c 100644 --- a/radio/ateam_radio_bridge/src/radio_bridge_node.cpp +++ b/radio/ateam_radio_bridge/src/radio_bridge_node.cpp @@ -87,8 +87,8 @@ class RadioBridgeNode : public rclcpp::Node declare_parameters("controls_enabled", { {"body_vel", true}, - {"wheel_vel", true}, - {"wheel_torque", false} + {"wheel_vel", false}, + {"wheel_torque", true} }); ateam_common::indexed_topic_helpers::create_indexed_subscribers( @@ -193,7 +193,6 @@ class RadioBridgeNode : public rclcpp::Node vision_states_[robot_id] = *vision_msg; REPLACE_NAN_WITH_ZERO(vision_states_[robot_id]); vision_state_timestamps_[robot_id] = std::chrono::steady_clock::now(); - RCLCPP_INFO(get_logger(), "Received vision state for robot %d.", robot_id); } void CloseConnection(const std::size_t & connection_index, bool send_goodbye = true) @@ -363,6 +362,9 @@ class RadioBridgeNode : public rclcpp::Node const auto now = std::chrono::steady_clock::now(); if (now - timestamp > vision_state_staleness_threshold_) { control_msg.vision_update = 0; + control_msg.vision_position_update[0] = 0; + control_msg.vision_position_update[1] = 0; + control_msg.vision_position_update[2] = 0; return; } control_msg.vision_update = 1; @@ -402,20 +404,22 @@ class RadioBridgeNode : public rclcpp::Node HelloRequest hello_data = std::get(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; - } + // Commented out until firmware implements git hash populating - if (ateam_radio_msgs::kComsDirty) { - RCLCPP_WARN(get_logger(), "Local packet version is dirty. Compatibility check may be unreliable."); - } + // 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 (hello_data.coms_repo_dirty) { - RCLCPP_WARN(get_logger(), "Remote robot's packet version is dirty. Compatibility check may be unreliable."); - } + // 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) && diff --git a/radio/ateam_radio_bridge/src/rnp_packet_helpers.cpp b/radio/ateam_radio_bridge/src/rnp_packet_helpers.cpp index bb76bc62f..4b1a5b7be 100644 --- a/radio/ateam_radio_bridge/src/rnp_packet_helpers.cpp +++ b/radio/ateam_radio_bridge/src/rnp_packet_helpers.cpp @@ -59,21 +59,23 @@ std::size_t GetPacketSize(const CommandCode & command_code) case CC_HELLO_REQ: return packet_header_size + sizeof(HelloRequest); break; - case CC_TELEMETRY: - return packet_header_size + sizeof(BasicTelemetry); - break; - case CC_CONTROL: - return packet_header_size + sizeof(BasicControl); - break; case CC_HELLO_RESP: return packet_header_size + sizeof(HelloResponse); break; + case CC_TELEMETRY: + return packet_header_size + sizeof(BasicTelemetry); + break; case CC_CONTROL_DEBUG_TELEMETRY: return packet_header_size + sizeof(ExtendedTelemetry); break; case CC_ROBOT_PARAMETER_COMMAND: return packet_header_size + sizeof(ParameterCommand); break; + case CC_ERROR_TELEMETRY: + return packet_header_size + sizeof(ErrorTelemetry); + case CC_CONTROL: + return packet_header_size + sizeof(BasicControl); + break; default: throw std::invalid_argument("Unrecognized command code: " + std::to_string(command_code)); }