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..f565118b7 100644
--- a/ateam_bringup/launch/bringup_simulation.launch.py
+++ b/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'
+ )
)
- )
+ ),
])
diff --git a/ateam_bringup/launch/joystick_only_stack.launch.py b/ateam_bringup/launch/joystick_only_stack.launch.py
index 623c05c5a..c64ac878b 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 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',
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_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 @@
+
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());
diff --git a/radio/ateam_radio_bridge/src/radio_bridge_node.cpp b/radio/ateam_radio_bridge/src/radio_bridge_node.cpp
index c84bc45e7..0884b9bb2 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
@@ -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("discovery_address", "224.4.20.69"),
declare_parameter("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(
+ 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 +176,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_;
@@ -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(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