From 336530efbcda1c17741b562e0d4347a9245874d1 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Sun, 17 May 2026 19:07:14 -0400 Subject: [PATCH 1/6] Adds robot_id launch argument to joystick launch stack. --- ateam_bringup/launch/joystick_only_stack.launch.py | 12 ++++++++++-- .../launch/joystick_controller.launch.xml | 2 ++ 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/ateam_bringup/launch/joystick_only_stack.launch.py b/ateam_bringup/launch/joystick_only_stack.launch.py index 623c05c5a..b760a420f 100644 --- a/ateam_bringup/launch/joystick_only_stack.launch.py +++ b/ateam_bringup/launch/joystick_only_stack.launch.py @@ -21,18 +21,26 @@ from ateam_bringup.substitutions import PackageLaunchFileSubstitution from ateam_bringup.utils import remap_indexed_topics import launch -from launch.actions import IncludeLaunchDescription +from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument from launch.launch_description_sources import FrontendLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node def generate_launch_description(): return launch.LaunchDescription([ + DeclareLaunchArgument( + name='robot_id', + default_value="-1" + ), IncludeLaunchDescription( FrontendLaunchDescriptionSource( PackageLaunchFileSubstitution('ateam_joystick_control', 'joystick_controller.launch.xml') - ) + ), + launch_arguments={ + 'robot_id': LaunchConfiguration('robot_id') + }.items() ), Node( package='ateam_radio_bridge', diff --git a/ateam_joystick_control/launch/joystick_controller.launch.xml b/ateam_joystick_control/launch/joystick_controller.launch.xml index 85d8401f0..5f9cdf523 100644 --- a/ateam_joystick_control/launch/joystick_controller.launch.xml +++ b/ateam_joystick_control/launch/joystick_controller.launch.xml @@ -1,5 +1,6 @@ + @@ -9,6 +10,7 @@ + From a50faee0f868cee5e6824a8ddbea1eee8cee9053 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Sun, 17 May 2026 20:31:35 -0400 Subject: [PATCH 2/6] Splits physical bringup files into core and game variants --- ...nch.py => bringup_physical_core.launch.py} | 42 ++++++++--------- .../launch/bringup_physical_game.launch.py | 47 +++++++++++++++++++ .../launch/bringup_simulation.launch.py | 34 ++++++++------ ateam_bringup/launch/kenobi.launch.xml | 10 ++++ ...y.launch.xml => state_tracking.launch.xml} | 11 +---- ateam_kenobi/src/kenobi_node.cpp | 2 - 6 files changed, 99 insertions(+), 47 deletions(-) rename ateam_bringup/launch/{bringup_physical.launch.py => bringup_physical_core.launch.py} (95%) create mode 100644 ateam_bringup/launch/bringup_physical_game.launch.py create mode 100644 ateam_bringup/launch/kenobi.launch.xml rename ateam_bringup/launch/{autonomy.launch.xml => state_tracking.launch.xml} (55%) diff --git a/ateam_bringup/launch/bringup_physical.launch.py b/ateam_bringup/launch/bringup_physical_core.launch.py similarity index 95% rename from ateam_bringup/launch/bringup_physical.launch.py rename to ateam_bringup/launch/bringup_physical_core.launch.py index b93e91cd2..b857db609 100644 --- a/ateam_bringup/launch/bringup_physical.launch.py +++ b/ateam_bringup/launch/bringup_physical_core.launch.py @@ -1,4 +1,4 @@ -# Copyright 2021 A Team +# 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 @@ -44,12 +44,6 @@ def generate_launch_description(): DeclareLaunchArgument('team_name', default_value='A-Team'), DeclareLaunchArgument('use_local_gc', default_value='False'), - Node( - package='ateam_bringup', - executable='scream_if_wifi_enabled.sh', - name='wifi_checker', - ), - GroupAction( condition=IfCondition(LaunchConfiguration('use_local_gc')), scoped=False, @@ -73,18 +67,32 @@ def generate_launch_description(): IncludeLaunchDescription( FrontendLaunchDescriptionSource( PackageLaunchFileSubstitution('ateam_bringup', - 'autonomy.launch.xml')), + 'ui.launch.xml')) + ), + + IncludeLaunchDescription( + FrontendLaunchDescriptionSource( + PackageLaunchFileSubstitution('ateam_joystick_control', + 'joystick_controller.launch.xml') + ) + ), + + IncludeLaunchDescription( + FrontendLaunchDescriptionSource( + PackageLaunchFileSubstitution('ateam_bringup', + 'state_tracking.launch.xml') + ), launch_arguments={ 'team_name': LaunchConfiguration('team_name'), 'vision_offset_robot_x': '0.0', - 'vision_offset_robot_y': '0.0', + 'vision_offset_robot_y': '0.0' }.items() ), - IncludeLaunchDescription( - FrontendLaunchDescriptionSource( - PackageLaunchFileSubstitution('ateam_bringup', - 'ui.launch.xml')) + Node( + package='ateam_bringup', + executable='scream_if_wifi_enabled.sh', + name='wifi_checker', ), Node( @@ -102,13 +110,5 @@ def generate_launch_description(): ('~/robot_feedback/extended/robot', '/robot_feedback/extended/robot'), ('~/robot_feedback/connection/robot', '/robot_feedback/connection/robot') ]), - # prefix=['xterm -bg black -fg white -e gdb -ex run --args'] ), - - IncludeLaunchDescription( - FrontendLaunchDescriptionSource( - PackageLaunchFileSubstitution('ateam_joystick_control', - 'joystick_controller.launch.xml') - ) - ) ]) diff --git a/ateam_bringup/launch/bringup_physical_game.launch.py b/ateam_bringup/launch/bringup_physical_game.launch.py new file mode 100644 index 000000000..d5258876f --- /dev/null +++ b/ateam_bringup/launch/bringup_physical_game.launch.py @@ -0,0 +1,47 @@ +# 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. + +from ateam_bringup.substitutions import PackageLaunchFileSubstitution +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import ( + FrontendLaunchDescriptionSource, + PythonLaunchDescriptionSource +) + + +def generate_launch_description(): + return LaunchDescription([ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PackageLaunchFileSubstitution( + 'ateam_bringup', 'bringup_physical_core.launch.py' + ) + ) + ), + + IncludeLaunchDescription( + FrontendLaunchDescriptionSource( + PackageLaunchFileSubstitution( + 'ateam_bringup', 'kenobi.launch.xml' + ) + ) + ), + ]) diff --git a/ateam_bringup/launch/bringup_simulation.launch.py b/ateam_bringup/launch/bringup_simulation.launch.py index c0e67048b..81d1ab006 100644 --- a/ateam_bringup/launch/bringup_simulation.launch.py +++ b/ateam_bringup/launch/bringup_simulation.launch.py @@ -23,7 +23,7 @@ from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.conditions import IfCondition from launch.launch_description_sources import FrontendLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration +from launch.substitutions import IfElseSubstitution, LaunchConfiguration from launch_ros.actions import Node @@ -69,20 +69,15 @@ def generate_launch_description(): IncludeLaunchDescription( FrontendLaunchDescriptionSource( PackageLaunchFileSubstitution('ateam_bringup', - 'autonomy.launch.xml')), - launch_arguments={ - 'team_name': LaunchConfiguration('team_name'), - 'use_emulated_ballsense': 'False', - 'vision_offset_robot_x': '0.0', - 'vision_offset_robot_y': '0.0', - }.items() + 'ui.launch.xml')), + condition=IfCondition(LaunchConfiguration('start_ui')) ), IncludeLaunchDescription( FrontendLaunchDescriptionSource( - PackageLaunchFileSubstitution('ateam_bringup', - 'ui.launch.xml')), - condition=IfCondition(LaunchConfiguration('start_ui')) + PackageLaunchFileSubstitution('ateam_joystick_control', + 'joystick_controller.launch.xml') + ) ), Node( @@ -97,8 +92,19 @@ def generate_launch_description(): IncludeLaunchDescription( FrontendLaunchDescriptionSource( - PackageLaunchFileSubstitution('ateam_joystick_control', - 'joystick_controller.launch.xml') + PackageLaunchFileSubstitution('ateam_bringup', + 'state_tracking.launch.xml') + ), + launch_arguments={ + 'team_name': LaunchConfiguration('team_name'), + }.items() + ), + + IncludeLaunchDescription( + FrontendLaunchDescriptionSource( + PackageLaunchFileSubstitution( + 'ateam_bringup', 'kenobi.launch.xml' + ) ) - ) + ), ]) diff --git a/ateam_bringup/launch/kenobi.launch.xml b/ateam_bringup/launch/kenobi.launch.xml new file mode 100644 index 000000000..3cb9ef11c --- /dev/null +++ b/ateam_bringup/launch/kenobi.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/ateam_bringup/launch/autonomy.launch.xml b/ateam_bringup/launch/state_tracking.launch.xml similarity index 55% rename from ateam_bringup/launch/autonomy.launch.xml rename to ateam_bringup/launch/state_tracking.launch.xml index d7a7ae962..1ae047ce8 100644 --- a/ateam_bringup/launch/autonomy.launch.xml +++ b/ateam_bringup/launch/state_tracking.launch.xml @@ -1,8 +1,5 @@ - - - @@ -17,10 +14,4 @@ - - - - - - - + \ No newline at end of file diff --git a/ateam_kenobi/src/kenobi_node.cpp b/ateam_kenobi/src/kenobi_node.cpp index 7447f8282..486d0fae0 100644 --- a/ateam_kenobi/src/kenobi_node.cpp +++ b/ateam_kenobi/src/kenobi_node.cpp @@ -69,8 +69,6 @@ class KenobiNode : public rclcpp::Node overlays_(""), motion_executor_(get_logger().get_child("motion")) { - declare_parameter("use_emulated_ballsense", false); - overlay_publisher_ = create_publisher( "/overlays", rclcpp::SystemDefaultsQoS()); From 10ea49287baebefa690fd6dfa16aafa701d6d1a0 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Mon, 18 May 2026 00:18:16 -0400 Subject: [PATCH 3/6] Adds error telemetry publishing to radio bridge --- .../src/radio_bridge_node.cpp | 24 +++++++++++++++++++ .../src/rnp_packet_helpers.cpp | 10 ++++++++ .../src/rnp_packet_helpers.hpp | 2 +- radio/ateam_radio_msgs/CMakeLists.txt | 1 + 4 files changed, 36 insertions(+), 1 deletion(-) diff --git a/radio/ateam_radio_bridge/src/radio_bridge_node.cpp b/radio/ateam_radio_bridge/src/radio_bridge_node.cpp index c84bc45e7..d5ee700dd 100644 --- a/radio/ateam_radio_bridge/src/radio_bridge_node.cpp +++ b/radio/ateam_radio_bridge/src/radio_bridge_node.cpp @@ -30,6 +30,7 @@ #include #include #include +#include #include #include #include @@ -117,6 +118,12 @@ class RadioBridgeNode : public rclcpp::Node rclcpp::SystemDefaultsQoS(), this); + ateam_common::indexed_topic_helpers::create_indexed_publishers( + error_feedback_publishers_, + "~/robot_feedback/error/robot", + rclcpp::SystemDefaultsQoS(), + this); + power_request_service_ = create_service( "~/send_power_request", std::bind(&RadioBridgeNode::SendPowerRequestCallback, this, std::placeholders::_1, @@ -167,6 +174,8 @@ class RadioBridgeNode : public rclcpp::Node 16> feedback_publishers_; std::array::SharedPtr, 16> motion_feedback_publishers_; + std::array::SharedPtr, + 16> error_feedback_publishers_; ateam_common::MulticastReceiver discovery_receiver_; FirmwareParameterServer firmware_parameter_server_; rclcpp::Service::SharedPtr power_request_service_; @@ -538,6 +547,21 @@ class RadioBridgeNode : public rclcpp::Node } break; } + case CC_ERROR_TELEMETRY: + { + const auto data_var = ExtractData(packet, error); + if (!error.empty()) { + RCLCPP_WARN(get_logger(), "Ignoring error telemetry message from robot %d. %s", robot_id, error.c_str()); + return; + } + + if (std::holds_alternative(data_var)) { + const auto & telem_data = std::get(data_var); + error_feedback_publishers_[robot_id]->publish(ateam_radio_msgs::Convert(telem_data)); + RCLCPP_WARN(get_logger(), "Error message from robot %d: %s", robot_id, telem_data.error_message); + } + break; + } case CC_ROBOT_PARAMETER_COMMAND: { const auto data_var = ExtractData(packet, error); diff --git a/radio/ateam_radio_bridge/src/rnp_packet_helpers.cpp b/radio/ateam_radio_bridge/src/rnp_packet_helpers.cpp index 53c79e946..980ef89b7 100644 --- a/radio/ateam_radio_bridge/src/rnp_packet_helpers.cpp +++ b/radio/ateam_radio_bridge/src/rnp_packet_helpers.cpp @@ -242,6 +242,16 @@ PacketDataVariant ExtractData(const RadioPacket & packet, std::string & error) var = packet.data.extended_telemetry; break; } + case CC_ERROR_TELEMETRY: + { + // TODO(barulicm): Restore this sanity check after firmware fixes the packets they're sending us. + // if (packet.header.data_length != sizeof(ErrorTelemetry)) { + // error = "Incorrect data length for ErrorTelemtry type. Expected " + std::to_string(sizeof(ErrorTelemetry)) + " but got " + std::to_string(packet.header.data_length); + // break; + // } + var = packet.data.error_telemetry; + break; + } case CC_ROBOT_PARAMETER_COMMAND: { if (packet.header.data_length != sizeof(ParameterCommand)) { diff --git a/radio/ateam_radio_bridge/src/rnp_packet_helpers.hpp b/radio/ateam_radio_bridge/src/rnp_packet_helpers.hpp index d3c061b9d..71bb8282e 100644 --- a/radio/ateam_radio_bridge/src/rnp_packet_helpers.hpp +++ b/radio/ateam_radio_bridge/src/rnp_packet_helpers.hpp @@ -76,7 +76,7 @@ RadioPacket CreateEmptyPacket(const CommandCode command_code); RadioPacket ParsePacket(const uint8_t * data, const std::size_t data_length, std::string & error); using PacketDataVariant = std::variant; + BasicControl, ExtendedTelemetry, ErrorTelemetry, ParameterCommand>; PacketDataVariant ExtractData(const RadioPacket & packet, std::string & error); diff --git a/radio/ateam_radio_msgs/CMakeLists.txt b/radio/ateam_radio_msgs/CMakeLists.txt index 9737d4d3e..bbf2c618a 100644 --- a/radio/ateam_radio_msgs/CMakeLists.txt +++ b/radio/ateam_radio_msgs/CMakeLists.txt @@ -24,6 +24,7 @@ set(RADIO_STRUCTS_TO_GENERATE GlobalAccelerationCommand LocalAccelerationCommand BasicTelemetry + ErrorTelemetry ExtendedTelemetry BodyControlTelemetry BodyControlExtendedTelemetry From 9a5a810ad7aaf1e92b89ab21cbba138f673c50e9 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Mon, 18 May 2026 00:18:42 -0400 Subject: [PATCH 4/6] Adds team side callback to gc listener constructor --- radio/ateam_radio_bridge/src/radio_bridge_node.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/radio/ateam_radio_bridge/src/radio_bridge_node.cpp b/radio/ateam_radio_bridge/src/radio_bridge_node.cpp index d5ee700dd..a637aa872 100644 --- a/radio/ateam_radio_bridge/src/radio_bridge_node.cpp +++ b/radio/ateam_radio_bridge/src/radio_bridge_node.cpp @@ -75,7 +75,9 @@ class RadioBridgeNode : public rclcpp::Node command_timeout_threshold_(declare_parameter("command_timeout_ms", 100)), last_side_change_timestamp_(std::chrono::steady_clock::now()), game_controller_listener_(*this, - std::bind_front(&RadioBridgeNode::TeamColorChangeCallback, this)), + std::bind_front(&RadioBridgeNode::TeamColorChangeCallback, this), + std::bind_front(&RadioBridgeNode::TeamSideChangeCallback, this) + ), discovery_receiver_(declare_parameter("discovery_address", "224.4.20.69"), declare_parameter("discovery_port", 42069), std::bind(&RadioBridgeNode::DiscoveryMessageCallback, this, std::placeholders::_1, From 3078c8d1085c21b0682e91c20f3b4b517921d84c Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Mon, 18 May 2026 00:19:04 -0400 Subject: [PATCH 5/6] Adds visibility check to radio bridge vision packet filler --- radio/ateam_radio_bridge/src/radio_bridge_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/radio/ateam_radio_bridge/src/radio_bridge_node.cpp b/radio/ateam_radio_bridge/src/radio_bridge_node.cpp index a637aa872..0884b9bb2 100644 --- a/radio/ateam_radio_bridge/src/radio_bridge_node.cpp +++ b/radio/ateam_radio_bridge/src/radio_bridge_node.cpp @@ -375,7 +375,7 @@ 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_) { + if (now - timestamp > vision_state_staleness_threshold_ || !vision_state.visible) { control_msg.vision_update = 0; control_msg.vision_position_update[0] = 0; control_msg.vision_position_update[1] = 0; From 2192640d636ec7bcd400395f36450ae0a2c020a1 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Mon, 18 May 2026 01:32:33 -0400 Subject: [PATCH 6/6] Fixes linting failures. --- ateam_bringup/launch/bringup_simulation.launch.py | 2 +- ateam_bringup/launch/joystick_only_stack.launch.py | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ateam_bringup/launch/bringup_simulation.launch.py b/ateam_bringup/launch/bringup_simulation.launch.py index 81d1ab006..f565118b7 100644 --- a/ateam_bringup/launch/bringup_simulation.launch.py +++ b/ateam_bringup/launch/bringup_simulation.launch.py @@ -23,7 +23,7 @@ from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.conditions import IfCondition from launch.launch_description_sources import FrontendLaunchDescriptionSource -from launch.substitutions import IfElseSubstitution, LaunchConfiguration +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node diff --git a/ateam_bringup/launch/joystick_only_stack.launch.py b/ateam_bringup/launch/joystick_only_stack.launch.py index b760a420f..c64ac878b 100644 --- a/ateam_bringup/launch/joystick_only_stack.launch.py +++ b/ateam_bringup/launch/joystick_only_stack.launch.py @@ -21,7 +21,7 @@ from ateam_bringup.substitutions import PackageLaunchFileSubstitution from ateam_bringup.utils import remap_indexed_topics import launch -from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.launch_description_sources import FrontendLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node @@ -31,7 +31,7 @@ def generate_launch_description(): return launch.LaunchDescription([ DeclareLaunchArgument( name='robot_id', - default_value="-1" + default_value='-1' ), IncludeLaunchDescription( FrontendLaunchDescriptionSource(