Launch split & radio bridge fixes

ateam_bringup/launch/bringup_physical.launch.py → 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')
-            )
-        )
     ])

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'
+                )
+            )
+        ),
+    ])

ateam_bringup/launch/bringup_simulation.launch.py

@@ -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'
+                )
             )
-        )
+        ),
     ])

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 DeclareLaunchArgument, IncludeLaunchDescription
 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',

ateam_bringup/launch/kenobi.launch.xml

@@ -0,0 +1,10 @@
+<launch>
+  <arg name="kenobi_debug" default="false"/>
+  <arg name="kenobi_playbook" default=""/>
+
+  <let name="kenobi_prefix" value="$(eval '\'xterm -fg white -bg black -e gdb -ex run --args\' if (\'$(var kenobi_debug)\'.lower() == \'true\') else \'\'')"/>
+  <node name="kenobi_node" pkg="ateam_kenobi" exec="ateam_kenobi_node" respawn="True" launch-prefix="$(var kenobi_prefix)">
+    <param name="playbook" value="$(var kenobi_playbook)"/>
+  </node>
+  <node name="vader" pkg="ateam_bringup" exec="vader.py" respawn="True"/>
+</launch>

ateam_bringup/launch/autonomy.launch.xml → ateam_bringup/launch/state_tracking.launch.xml

@@ -1,8 +1,5 @@
 <launch>
-  <arg name="use_emulated_ballsense" default="false"/>
   <arg name="team_name" default="A-Team"/>
-  <arg name="kenobi_debug" default="false"/>
-  <arg name="kenobi_playbook" default=""/>
   <arg name="vision_offset_robot_x" default="0.0"/>
   <arg name="vision_offset_robot_y" default="0.0"/>
 
@@ -17,10 +14,4 @@
   <node name="game_state_tracker" pkg="ateam_game_state" exec="game_state_tracker_node" respawn="True">
     <param name="gc_team_name" value="$(var team_name)"/>
   </node>
-  <let name="kenobi_prefix" value="$(eval '\'xterm -fg white -bg black -e gdb -ex run --args\' if (\'$(var kenobi_debug)\'.lower() == \'true\') else \'\'')"/>
-  <node name="kenobi_node" pkg="ateam_kenobi" exec="ateam_kenobi_node" respawn="True" launch-prefix="$(var kenobi_prefix)">
-    <param name="use_emulated_ballsense" value="$(var use_emulated_ballsense)"/>
-    <param name="playbook" value="$(var kenobi_playbook)"/>
-  </node>
-  <node name="vader" pkg="ateam_bringup" exec="vader.py" respawn="True"/>
-</launch>
+</launch>

ateam_joystick_control/launch/joystick_controller.launch.xml

@@ -1,5 +1,6 @@
 <launch>
   <arg name="joy_config_file" default="$(find-pkg-share ateam_joystick_control)/config/xbox_joy_config.yaml"/>
+  <arg name="robot_id" default="-1"/>
 
   <node name="joystick_node" pkg="joy" exec="joy_node">
     <param name="deadzone" value="0.1"/>
@@ -9,6 +10,7 @@
     <param from="$(var joy_config_file)"/>
     <param from="$(find-pkg-share ateam_joystick_control)/config/common_params.yaml"/>
     <param name="command_topic_template" value="/robot_motion_commands/robot{}" />
+    <param name="robot_id" value="$(var robot_id)"/>
     <remap from="~/joy" to="/joy"/>
   </node>
 </launch>

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<bool>("use_emulated_ballsense", false);
-
     overlay_publisher_ = create_publisher<ateam_msgs::msg::OverlayArray>(
       "/overlays",
       rclcpp::SystemDefaultsQoS());

radio/ateam_radio_bridge/src/radio_bridge_node.cpp

@@ -30,6 +30,7 @@
 #include <ateam_radio_msgs/msg/connection_status.hpp>
 #include <ateam_radio_msgs/msg/basic_telemetry.hpp>
 #include <ateam_radio_msgs/msg/extended_telemetry.hpp>
+#include <ateam_radio_msgs/msg/error_telemetry.hpp>
 #include <ateam_radio_msgs/srv/get_firmware_parameter.hpp>
 #include <ateam_radio_msgs/srv/set_firmware_parameter.hpp>
 #include <ateam_radio_msgs/srv/send_robot_power_request.hpp>
@@ -74,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<std::string>("discovery_address", "224.4.20.69"),
       declare_parameter<uint16_t>("discovery_port", 42069),
       std::bind(&RadioBridgeNode::DiscoveryMessageCallback, this, std::placeholders::_1,
@@ -117,6 +120,12 @@ class RadioBridgeNode : public rclcpp::Node
       rclcpp::SystemDefaultsQoS(),
       this);
 
+    ateam_common::indexed_topic_helpers::create_indexed_publishers<ateam_radio_msgs::msg::ErrorTelemetry>(
+      error_feedback_publishers_,
+      "~/robot_feedback/error/robot",
+      rclcpp::SystemDefaultsQoS(),
+      this);
+
     power_request_service_ = create_service<ateam_radio_msgs::srv::SendRobotPowerRequest>(
       "~/send_power_request",
       std::bind(&RadioBridgeNode::SendPowerRequestCallback, this, std::placeholders::_1,
@@ -167,6 +176,8 @@ class RadioBridgeNode : public rclcpp::Node
     16> feedback_publishers_;
   std::array<rclcpp::Publisher<ateam_radio_msgs::msg::ExtendedTelemetry>::SharedPtr,
     16> motion_feedback_publishers_;
+  std::array<rclcpp::Publisher<ateam_radio_msgs::msg::ErrorTelemetry>::SharedPtr,
+    16> error_feedback_publishers_;
   ateam_common::MulticastReceiver discovery_receiver_;
   FirmwareParameterServer firmware_parameter_server_;
   rclcpp::Service<ateam_radio_msgs::srv::SendRobotPowerRequest>::SharedPtr power_request_service_;
@@ -364,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;
@@ -538,6 +549,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<ErrorTelemetry>(data_var)) {
+            const auto & telem_data = std::get<ErrorTelemetry>(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);

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)) {

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<std::monostate, HelloRequest, BasicTelemetry,
-    BasicControl, ExtendedTelemetry, ParameterCommand>;
+    BasicControl, ExtendedTelemetry, ErrorTelemetry, ParameterCommand>;
 
 PacketDataVariant ExtractData(const RadioPacket & packet, std::string & error);
 

radio/ateam_radio_msgs/CMakeLists.txt

@@ -24,6 +24,7 @@ set(RADIO_STRUCTS_TO_GENERATE
     GlobalAccelerationCommand
     LocalAccelerationCommand
     BasicTelemetry
+    ErrorTelemetry
     ExtendedTelemetry
     BodyControlTelemetry
     BodyControlExtendedTelemetry