diff --git a/radio/ateam_radio_bridge/CMakeLists.txt b/radio/ateam_radio_bridge/CMakeLists.txt index bc981b4f..c16b1e5e 100644 --- a/radio/ateam_radio_bridge/CMakeLists.txt +++ b/radio/ateam_radio_bridge/CMakeLists.txt @@ -11,10 +11,12 @@ find_package(rclcpp_components REQUIRED) find_package(ateam_radio_msgs REQUIRED) find_package(ateam_msgs REQUIRED) find_package(ateam_common REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) add_library(${PROJECT_NAME} SHARED src/firmware_parameter_server.cpp src/ip_address_helpers.cpp + src/nan_helpers.cpp src/radio_bridge_node.cpp src/rnp_packet_helpers.cpp ) @@ -27,6 +29,7 @@ ament_target_dependencies(${PROJECT_NAME} ateam_radio_msgs ateam_msgs ateam_common + tf2_geometry_msgs ) rclcpp_components_register_node( ${PROJECT_NAME} diff --git a/radio/ateam_radio_bridge/package.xml b/radio/ateam_radio_bridge/package.xml index 4e57f067..ab9b0a30 100644 --- a/radio/ateam_radio_bridge/package.xml +++ b/radio/ateam_radio_bridge/package.xml @@ -15,6 +15,7 @@ ateam_radio_msgs ateam_msgs ateam_common + tf2_geometry_msgs ament_lint_auto ament_lint_common diff --git a/radio/ateam_radio_bridge/src/nan_helpers.cpp b/radio/ateam_radio_bridge/src/nan_helpers.cpp new file mode 100644 index 00000000..c4081eb2 --- /dev/null +++ b/radio/ateam_radio_bridge/src/nan_helpers.cpp @@ -0,0 +1,103 @@ +// 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. + +#include "nan_helpers.hpp" +#include +#include + +namespace ateam_radio_bridge +{ + +void ReplaceNanWithZero(double & val, rclcpp::Logger logger) +{ + if (std::isnan(val)) { + RCLCPP_WARN(logger, "Radio bridge is replacing NaNs!"); + val = 0.0; + } +} + +void ReplaceNanWithZero(float & val, rclcpp::Logger logger) +{ + if (std::isnan(val)) { + RCLCPP_WARN(logger, "Radio bridge is replacing NaNs!"); + val = 0.0; + } +} + +void ReplaceNanWithZero(ateam_msgs::msg::Twist2D & twist, rclcpp::Logger logger) +{ + ReplaceNanWithZero(twist.x, logger); + ReplaceNanWithZero(twist.y, logger); + ReplaceNanWithZero(twist.theta, logger); +} + +void ReplaceNanWithZero(ateam_msgs::msg::RobotMotionCommand & command, rclcpp::Logger logger) +{ + ReplaceNanWithZero(command.pose, logger); + ReplaceNanWithZero(command.velocity, logger); + ReplaceNanWithZero(command.acceleration, logger); + ReplaceNanWithZero(command.limit_vel_linear, logger); + ReplaceNanWithZero(command.limit_vel_angular, logger); + ReplaceNanWithZero(command.limit_acc_linear, logger); + ReplaceNanWithZero(command.limit_acc_angular, logger); + ReplaceNanWithZero(command.kick_speed, logger); + ReplaceNanWithZero(command.dribbler_speed, logger); +} + +void ReplaceNanWithZero(ateam_msgs::msg::VisionStateRobot & vision_state, rclcpp::Logger logger) +{ + ReplaceNanWithZero(vision_state.pose, logger); + ReplaceNanWithZero(vision_state.twist, logger); + ReplaceNanWithZero(vision_state.twist_body, logger); + ReplaceNanWithZero(vision_state.accel, logger); +} + +void ReplaceNanWithZero(geometry_msgs::msg::Pose & pose, rclcpp::Logger logger) +{ + ReplaceNanWithZero(pose.position.x, logger); + ReplaceNanWithZero(pose.position.y, logger); + ReplaceNanWithZero(pose.position.z, logger); + ReplaceNanWithZero(pose.orientation.x, logger); + ReplaceNanWithZero(pose.orientation.y, logger); + ReplaceNanWithZero(pose.orientation.z, logger); + ReplaceNanWithZero(pose.orientation.w, logger); +} + +void ReplaceNanWithZero(geometry_msgs::msg::Twist & twist, rclcpp::Logger logger) +{ + ReplaceNanWithZero(twist.linear.x, logger); + ReplaceNanWithZero(twist.linear.y, logger); + ReplaceNanWithZero(twist.linear.z, logger); + ReplaceNanWithZero(twist.angular.x, logger); + ReplaceNanWithZero(twist.angular.y, logger); + ReplaceNanWithZero(twist.angular.z, logger); +} + +void ReplaceNanWithZero(geometry_msgs::msg::Accel & accel, rclcpp::Logger logger) +{ + ReplaceNanWithZero(accel.linear.x, logger); + ReplaceNanWithZero(accel.linear.y, logger); + ReplaceNanWithZero(accel.linear.z, logger); + ReplaceNanWithZero(accel.angular.x, logger); + ReplaceNanWithZero(accel.angular.y, logger); + ReplaceNanWithZero(accel.angular.z, logger); +} + +} // namespace ateam_radio_bridge diff --git a/radio/ateam_radio_bridge/src/nan_helpers.hpp b/radio/ateam_radio_bridge/src/nan_helpers.hpp new file mode 100644 index 00000000..1d8a8fed --- /dev/null +++ b/radio/ateam_radio_bridge/src/nan_helpers.hpp @@ -0,0 +1,55 @@ +// 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. + +#ifndef ATEAM_RADIO_BRIDGE__RADIO_BRIDGE_NODE_HPP_ +#define ATEAM_RADIO_BRIDGE__RADIO_BRIDGE_NODE_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#define REPLACE_NAN_WITH_ZERO(val) ateam_radio_bridge::ReplaceNanWithZero(val, get_logger()) + +namespace ateam_radio_bridge +{ + +void ReplaceNanWithZero(double & val, rclcpp::Logger logger); + +void ReplaceNanWithZero(float & val, rclcpp::Logger logger); + +void ReplaceNanWithZero(ateam_msgs::msg::Twist2D & twist, rclcpp::Logger logger); + +void ReplaceNanWithZero(ateam_msgs::msg::RobotMotionCommand & command, rclcpp::Logger logger); + +void ReplaceNanWithZero(ateam_msgs::msg::VisionStateRobot & vision_state, rclcpp::Logger logger); + +void ReplaceNanWithZero(geometry_msgs::msg::Pose & pose, rclcpp::Logger logger); + +void ReplaceNanWithZero(geometry_msgs::msg::Twist & twist, rclcpp::Logger logger); + +void ReplaceNanWithZero(geometry_msgs::msg::Accel & accel, rclcpp::Logger logger); + +} // namespace ateam_radio_bridge + +#endif // ATEAM_RADIO_BRIDGE__RADIO_BRIDGE_NODE_HPP_ diff --git a/radio/ateam_radio_bridge/src/radio_bridge_node.cpp b/radio/ateam_radio_bridge/src/radio_bridge_node.cpp index 1e6c5576..99b750e3 100644 --- a/radio/ateam_radio_bridge/src/radio_bridge_node.cpp +++ b/radio/ateam_radio_bridge/src/radio_bridge_node.cpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -35,13 +36,16 @@ #include #include #include +#include #include #include #include #include +#include #include "rnp_packet_helpers.hpp" #include "ip_address_helpers.hpp" +#include "nan_helpers.hpp" #include "firmware_parameter_server.hpp" // TODO(barulicm) add warning if we see another instance of this running via multicast @@ -66,6 +70,7 @@ class RadioBridgeNode : public rclcpp::Node : rclcpp::Node("radio_bridge", options), sustain_timeout_threshold_(declare_parameter("sustain_timeout_ms", 250)), 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)), game_controller_listener_(*this, std::bind_front(&RadioBridgeNode::TeamColorChangeCallback, this)), @@ -82,8 +87,8 @@ class RadioBridgeNode : public rclcpp::Node declare_parameters("controls_enabled", { {"body_vel", true}, - {"wheel_vel", true}, - {"wheel_torque", false} + {"wheel_vel", false}, + {"wheel_torque", true} }); ateam_common::indexed_topic_helpers::create_indexed_subscribers( @@ -128,6 +133,8 @@ class RadioBridgeNode : public rclcpp::Node std::chrono::duration(1.0 / declare_parameter("command_frequency", 60.0)), std::bind(&RadioBridgeNode::SendCommandsCallback, this)); + SetupVisionSubscribers(game_controller_listener_.GetTeamColor()); + RCLCPP_INFO(get_logger(), "Radio bridge node ready."); } @@ -135,6 +142,7 @@ class RadioBridgeNode : public rclcpp::Node // Incoming timeouts const std::chrono::milliseconds sustain_timeout_threshold_; const std::chrono::milliseconds connect_timeout_threshold_; + const std::chrono::milliseconds vision_state_staleness_threshold_; // Outgoing timeouts const std::chrono::milliseconds command_timeout_threshold_; @@ -142,11 +150,15 @@ class RadioBridgeNode : public rclcpp::Node std::mutex mutex_; std::array motion_commands_; std::array motion_command_timestamps_; + std::array vision_states_; + std::array vision_state_timestamps_; std::array shutdown_requested_; std::array reboot_requested_; ateam_common::GameControllerListener game_controller_listener_; std::array::SharedPtr, 16> motion_command_subscriptions_; + std::array::SharedPtr, + 16> vision_state_subscriptions_; std::array::SharedPtr, 16> connection_publishers_; std::array::SharedPtr, @@ -162,26 +174,6 @@ class RadioBridgeNode : public rclcpp::Node rclcpp::TimerBase::SharedPtr connection_check_timer_; rclcpp::TimerBase::SharedPtr command_send_timer_; - void ReplaceNanWithZero(double & val) { - if (std::isnan(val)) { - RCLCPP_WARN(get_logger(), "Radio bridge is replacing NaNs!"); - val = 0.0; - } - } - - void ReplaceNanWithZero(float & val) { - if (std::isnan(val)) { - RCLCPP_WARN(get_logger(), "Radio bridge is replacing NaNs!"); - val = 0.0f; - } - } - - void ReplaceNanWithZero(ateam_msgs::msg::Twist2D & twist) { - ReplaceNanWithZero(twist.x); - ReplaceNanWithZero(twist.y); - ReplaceNanWithZero(twist.theta); - } - void MotionCommandCallback( const ateam_msgs::msg::RobotMotionCommand::SharedPtr command_msg, int robot_id) @@ -189,18 +181,20 @@ class RadioBridgeNode : public rclcpp::Node const std::lock_guard lock(mutex_); motion_commands_[robot_id] = *command_msg; auto & command = motion_commands_[robot_id]; - ReplaceNanWithZero(command.pose); - ReplaceNanWithZero(command.velocity); - ReplaceNanWithZero(command.acceleration); - ReplaceNanWithZero(command.limit_vel_linear); - ReplaceNanWithZero(command.limit_vel_angular); - ReplaceNanWithZero(command.limit_acc_linear); - ReplaceNanWithZero(command.limit_acc_angular); - ReplaceNanWithZero(command.kick_speed); - ReplaceNanWithZero(command.dribbler_speed); + REPLACE_NAN_WITH_ZERO(command); motion_command_timestamps_[robot_id] = std::chrono::steady_clock::now(); } + void VisionStateCallback( + const ateam_msgs::msg::VisionStateRobot::SharedPtr vision_msg, + int robot_id) + { + const std::lock_guard lock(mutex_); + vision_states_[robot_id] = *vision_msg; + REPLACE_NAN_WITH_ZERO(vision_states_[robot_id]); + vision_state_timestamps_[robot_id] = std::chrono::steady_clock::now(); + } + void CloseConnection(const std::size_t & connection_index, bool send_goodbye = true) { std::unique_ptr connection; @@ -275,7 +269,9 @@ class RadioBridgeNode : public rclcpp::Node { RCLCPP_WARN(get_logger(), "Robot %d command topic inactive. Sending zeros.", id); motion_commands_[id] = ateam_msgs::msg::RobotMotionCommand(); + motion_commands_[id].body_control_mode = ateam_msgs::msg::RobotMotionCommand::BCM_OFF; motion_commands_[id].kick_request = ateam_msgs::msg::RobotMotionCommand::KR_DISABLE; + motion_commands_[id].dribbler_speed = 0.0; } BasicControl control_msg; control_msg.request_shutdown = shutdown_requested_[id]; @@ -286,11 +282,8 @@ 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.vision_update = 0; control_msg.reserved1 = 0; - control_msg.vision_position_update[0] = 0.0f; - control_msg.vision_position_update[1] = 0.0f; - control_msg.vision_position_update[2] = 0.0f; + FillVisionUpdate(control_msg, vision_states_[id], vision_state_timestamps_[id]); control_msg.kick_request = static_cast(motion_commands_[id].kick_request); control_msg.play_song = 0; control_msg.reserved2[0] = 0; @@ -365,6 +358,21 @@ 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_) { + control_msg.vision_update = 0; + control_msg.vision_position_update[0] = 0; + control_msg.vision_position_update[1] = 0; + control_msg.vision_position_update[2] = 0; + return; + } + control_msg.vision_update = 1; + control_msg.vision_position_update[0] = static_cast(vision_state.pose.position.x); + control_msg.vision_position_update[1] = static_cast(vision_state.pose.position.y); + control_msg.vision_position_update[2] = static_cast(GetYaw(vision_state.pose)); + } + void DiscoveryMessageCallback( const std::string & sender_address, const uint16_t sender_port, uint8_t * udp_packet_data, size_t udp_packet_size) @@ -396,20 +404,22 @@ class RadioBridgeNode : public rclcpp::Node HelloRequest hello_data = std::get(data_variant); - const uint32_t incoming_coms_hash = hello_data.coms_hash[0] | (hello_data.coms_hash[1] << 8) | - (hello_data.coms_hash[2] << 16) | (hello_data.coms_hash[3] << 24); - if (incoming_coms_hash != ateam_radio_msgs::kComsHash) { - RCLCPP_WARN(get_logger(), "Ignoring discovery packet. Packet version hash mismatch."); - return; - } + // Commented out until firmware implements git hash populating - if (ateam_radio_msgs::kComsDirty) { - RCLCPP_WARN(get_logger(), "Local packet version is dirty. Compatibility check may be unreliable."); - } + // const uint32_t incoming_coms_hash = hello_data.coms_hash[0] | (hello_data.coms_hash[1] << 8) | + // (hello_data.coms_hash[2] << 16) | (hello_data.coms_hash[3] << 24); + // if (incoming_coms_hash != ateam_radio_msgs::kComsHash) { + // RCLCPP_WARN(get_logger(), "Ignoring discovery packet. Packet version hash mismatch."); + // return; + // } - if (hello_data.coms_repo_dirty) { - RCLCPP_WARN(get_logger(), "Remote robot's packet version is dirty. Compatibility check may be unreliable."); - } + // if (ateam_radio_msgs::kComsDirty) { + // RCLCPP_WARN(get_logger(), "Local packet version is dirty. Compatibility check may be unreliable."); + // } + + // if (hello_data.coms_repo_dirty) { + // RCLCPP_WARN(get_logger(), "Remote robot's packet version is dirty. Compatibility check may be unreliable."); + // } if (!(game_controller_listener_.GetTeamColor() == ateam_common::TeamColor::Blue && hello_data.color == TC_BLUE) && @@ -549,11 +559,12 @@ class RadioBridgeNode : public rclcpp::Node } } - void TeamColorChangeCallback(const ateam_common::TeamColor) + void TeamColorChangeCallback(const ateam_common::TeamColor color) { for (auto i = 0ul; i < connections_.size(); ++i) { CloseConnection(i); } + SetupVisionSubscribers(color); } void SendPowerRequestCallback( @@ -606,6 +617,31 @@ class RadioBridgeNode : public rclcpp::Node } } + void SetupVisionSubscribers(const ateam_common::TeamColor color) { + for(auto & sub : vision_state_subscriptions_) { + sub.reset(); + } + if(color == ateam_common::TeamColor::Unknown) { + return; + } + const auto topic_prefix = color == ateam_common::TeamColor::Yellow ? Topics::kYellowTeamRobotPrefix : Topics::kBlueTeamRobotPrefix; + ateam_common::indexed_topic_helpers::create_indexed_subscribers( + vision_state_subscriptions_, + topic_prefix, + rclcpp::SystemDefaultsQoS(), + &RadioBridgeNode::VisionStateCallback, + this); + } + + double GetYaw(const geometry_msgs::msg::Pose & pose) + { + tf2::Quaternion quat; + tf2::fromMsg(pose.orientation, quat); + double yaw, pitch, roll; + tf2::Matrix3x3(quat).getEulerYPR(yaw, pitch, roll); + return yaw; + } + }; } // namespace ateam_radio_bridge diff --git a/radio/ateam_radio_bridge/src/rnp_packet_helpers.cpp b/radio/ateam_radio_bridge/src/rnp_packet_helpers.cpp index bb76bc62..4b1a5b7b 100644 --- a/radio/ateam_radio_bridge/src/rnp_packet_helpers.cpp +++ b/radio/ateam_radio_bridge/src/rnp_packet_helpers.cpp @@ -59,21 +59,23 @@ std::size_t GetPacketSize(const CommandCode & command_code) case CC_HELLO_REQ: return packet_header_size + sizeof(HelloRequest); break; - case CC_TELEMETRY: - return packet_header_size + sizeof(BasicTelemetry); - break; - case CC_CONTROL: - return packet_header_size + sizeof(BasicControl); - break; case CC_HELLO_RESP: return packet_header_size + sizeof(HelloResponse); break; + case CC_TELEMETRY: + return packet_header_size + sizeof(BasicTelemetry); + break; case CC_CONTROL_DEBUG_TELEMETRY: return packet_header_size + sizeof(ExtendedTelemetry); break; case CC_ROBOT_PARAMETER_COMMAND: return packet_header_size + sizeof(ParameterCommand); break; + case CC_ERROR_TELEMETRY: + return packet_header_size + sizeof(ErrorTelemetry); + case CC_CONTROL: + return packet_header_size + sizeof(BasicControl); + break; default: throw std::invalid_argument("Unrecognized command code: " + std::to_string(command_code)); } diff --git a/radio/ateam_radio_bridge/test/launch_tests/bridge_command_test.py b/radio/ateam_radio_bridge/test/launch_tests/bridge_command_test.py index bb50bd4c..ef3760d0 100644 --- a/radio/ateam_radio_bridge/test/launch_tests/bridge_command_test.py +++ b/radio/ateam_radio_bridge/test/launch_tests/bridge_command_test.py @@ -59,6 +59,11 @@ def setUpClass(cls): "/radio_bridge/robot_motion_commands/robot0", qos_profile_system_default, ) + cls.vis_pub = cls.node.create_publisher( + ateam_msgs.msg.VisionStateRobot, + "/yellow_team/robot0", + qos_profile_system_default, + ) @classmethod def tearDownClass(cls): @@ -67,12 +72,13 @@ def tearDownClass(cls): cls.node.destroy_node() rclpy.shutdown(context=cls.context) - def test_commands(self): + def test_0_connection(self): connect_timeout = time.time() + 1 while not self.robot.isConnected() and time.time() < connect_timeout: time.sleep(0.1) self.assertTrue(self.robot.isConnected()) + def test_1_commands(self): cmd_msg = ateam_msgs.msg.RobotMotionCommand() cmd_msg.body_control_mode = ateam_msgs.msg.RobotMotionCommand.BCM_LOCAL_VELOCITY cmd_msg.velocity.x = 2.0 @@ -92,6 +98,39 @@ def test_commands(self): if abs(vel_x_linear - 2.0) < 0.1: # Pass the test return + + def test_2_vision_updates(self): + vis_msg = ateam_msgs.msg.VisionStateRobot() + vis_msg.pose.position.x = 1.0 + vis_msg.pose.position.y = 2.0 + vis_msg.pose.orientation.x = 0.0 + vis_msg.pose.orientation.y = 0.0 + vis_msg.pose.orientation.z = 0.707 + vis_msg.pose.orientation.w = 0.707 + self.vis_pub.publish(vis_msg) + + timeout = time.time() + 1 + while True: + time.sleep(0.017) # 60Hz + if time.time() > timeout: + self.fail("timed out waiting for valid vision update packet") + break + self.vis_pub.publish(vis_msg) + last_packet = self.robot.getLastCmdMessage() + if len(last_packet) != 64: + continue + # Extract vision updates + vision_update_flag = struct.unpack("