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