From 45df95561bbcacc4c0731f2e51b822bb706c6adb Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Sun, 17 May 2026 18:56:39 -0400 Subject: [PATCH 1/2] Updates comms submodule --- .../ateam_radio_bridge/src/rnp_packet_helpers.cpp | 14 ++++---------- radio/ateam_radio_msgs/software-communication | 2 +- 2 files changed, 5 insertions(+), 11 deletions(-) diff --git a/radio/ateam_radio_bridge/src/rnp_packet_helpers.cpp b/radio/ateam_radio_bridge/src/rnp_packet_helpers.cpp index 4b1a5b7b..53c79e94 100644 --- a/radio/ateam_radio_bridge/src/rnp_packet_helpers.cpp +++ b/radio/ateam_radio_bridge/src/rnp_packet_helpers.cpp @@ -289,19 +289,13 @@ ParameterDataFormat GetParameterDataFormatForParameter(const ParameterName & par return VEC2_F32; case TRAJ_RECOMPUTE_ERROR: return VEC4_F32; - case TRAJ_MAX: - return VEC4_F32; - case POSE_FB_PIDII_X: - return VEC5_F32; - case POSE_FB_PIDII_Y: - return VEC5_F32; - case POSE_FB_PIDII_THETA: + case POSE_FB_PIDII_LINEAR: return VEC5_F32; - case TWIST_FB_PIDII_X: + case POSE_FB_PIDII_ANGULAR: return VEC5_F32; - case TWIST_FB_PIDII_Y: + case TWIST_FB_PIDII_LINEAR: return VEC5_F32; - case TWIST_FB_PIDII_THETA: + case TWIST_FB_PIDII_ANGULAR: return VEC5_F32; default: throw std::invalid_argument("GetParameterDataFormatForParameter: Unrecognized parameter name."); diff --git a/radio/ateam_radio_msgs/software-communication b/radio/ateam_radio_msgs/software-communication index b969e32d..c5676357 160000 --- a/radio/ateam_radio_msgs/software-communication +++ b/radio/ateam_radio_msgs/software-communication @@ -1 +1 @@ -Subproject commit b969e32db321430eb058c72444e161ec3ae7e084 +Subproject commit c5676357809484c2a0d90612b1626fb5f781898f From 527bba4aa76f47ac5b1550ad814170131fd4e856 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Sun, 17 May 2026 18:57:16 -0400 Subject: [PATCH 2/2] Adds logic to send controller reset when field side changes. --- radio/ateam_radio_bridge/src/radio_bridge_node.cpp | 11 ++++++++++- 1 file changed, 10 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 99b750e3..c84bc45e 100644 --- a/radio/ateam_radio_bridge/src/radio_bridge_node.cpp +++ b/radio/ateam_radio_bridge/src/radio_bridge_node.cpp @@ -72,6 +72,7 @@ class RadioBridgeNode : public rclcpp::Node 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)), + last_side_change_timestamp_(std::chrono::steady_clock::now()), game_controller_listener_(*this, std::bind_front(&RadioBridgeNode::TeamColorChangeCallback, this)), discovery_receiver_(declare_parameter("discovery_address", "224.4.20.69"), @@ -154,6 +155,7 @@ class RadioBridgeNode : public rclcpp::Node std::array vision_state_timestamps_; std::array shutdown_requested_; std::array reboot_requested_; + std::chrono::steady_clock::time_point last_side_change_timestamp_; ateam_common::GameControllerListener game_controller_listener_; std::array::SharedPtr, 16> motion_command_subscriptions_; @@ -260,11 +262,12 @@ class RadioBridgeNode : public rclcpp::Node void SendCommandsCallback() { const std::lock_guard lock(mutex_); + const auto now = std::chrono::steady_clock::now(); for (auto id = 0; id < 16; ++id) { if (connections_[id] == nullptr) { continue; } - if ((std::chrono::steady_clock::now() - motion_command_timestamps_[id]) > + if ((now - motion_command_timestamps_[id]) > command_timeout_threshold_) { RCLCPP_WARN(get_logger(), "Robot %d command topic inactive. Sending zeros.", id); @@ -282,6 +285,7 @@ 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.reset_controller = (last_side_change_timestamp_ + sustain_timeout_threshold_) >= now; control_msg.reserved1 = 0; FillVisionUpdate(control_msg, vision_states_[id], vision_state_timestamps_[id]); control_msg.kick_request = static_cast(motion_commands_[id].kick_request); @@ -567,6 +571,11 @@ class RadioBridgeNode : public rclcpp::Node SetupVisionSubscribers(color); } + void TeamSideChangeCallback(const ateam_common::TeamSide) + { + last_side_change_timestamp_ = std::chrono::steady_clock::now(); + } + void SendPowerRequestCallback( const std::shared_ptr request, std::shared_ptr response)