diff --git a/radio/ateam_radio_bridge/CMakeLists.txt b/radio/ateam_radio_bridge/CMakeLists.txt index cdffff551..bc981b4fa 100644 --- a/radio/ateam_radio_bridge/CMakeLists.txt +++ b/radio/ateam_radio_bridge/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.8) project(ateam_radio_bridge) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Werror) endif() find_package(ament_cmake REQUIRED) diff --git a/radio/ateam_radio_bridge/src/radio_bridge_node.cpp b/radio/ateam_radio_bridge/src/radio_bridge_node.cpp index 5c0508be0..7da95b046 100644 --- a/radio/ateam_radio_bridge/src/radio_bridge_node.cpp +++ b/radio/ateam_radio_bridge/src/radio_bridge_node.cpp @@ -50,12 +50,21 @@ using namespace std::string_literals; namespace ateam_radio_bridge { +enum class ConnectionState +{ + Disconnected, + Connecting, + Connected, + ReadyToClose +}; + class RadioBridgeNode : public rclcpp::Node { public: RadioBridgeNode(const rclcpp::NodeOptions & options) : rclcpp::Node("radio_bridge", options), - timeout_threshold_(declare_parameter("timeout_ms", 250)), + sustain_timeout_threshold_(declare_parameter("sustain_timeout_ms", 250)), + connect_timeout_threshold_(declare_parameter("connect_timeout_ms", 750)), command_timeout_threshold_(declare_parameter("command_timeout_ms", 100)), game_controller_listener_(*this, std::bind_front(&RadioBridgeNode::TeamColorChangeCallback, this)), @@ -68,7 +77,7 @@ class RadioBridgeNode : public rclcpp::Node { std::fill(shutdown_requested_.begin(), shutdown_requested_.end(), false); std::fill(reboot_requested_.begin(), reboot_requested_.end(), false); - std::fill(goodbye_received_.begin(), goodbye_received_.end(), false); + std::fill(connection_states_.begin(), connection_states_.end(), ConnectionState::Disconnected); declare_parameters("controls_enabled", { {"body_vel", true}, @@ -122,14 +131,18 @@ class RadioBridgeNode : public rclcpp::Node } private: - const std::chrono::milliseconds timeout_threshold_; + // Incoming timeouts + const std::chrono::milliseconds sustain_timeout_threshold_; + const std::chrono::milliseconds connect_timeout_threshold_; + + // Outgoing timeouts const std::chrono::milliseconds command_timeout_threshold_; + std::mutex mutex_; std::array motion_commands_; std::array motion_command_timestamps_; std::array shutdown_requested_; std::array reboot_requested_; - std::array goodbye_received_; ateam_common::GameControllerListener game_controller_listener_; std::array::SharedPtr, 16> motion_command_subscriptions_; @@ -143,7 +156,8 @@ class RadioBridgeNode : public rclcpp::Node FirmwareParameterServer firmware_parameter_server_; rclcpp::Service::SharedPtr power_request_service_; std::array, 16> connections_; - std::array last_heartbeat_timestamp_; + std::array last_heartbeat_timestamps_; + std::array connection_states_; rclcpp::TimerBase::SharedPtr connection_check_timer_; rclcpp::TimerBase::SharedPtr command_send_timer_; @@ -173,6 +187,7 @@ class RadioBridgeNode : public rclcpp::Node { std::lock_guard lock(mutex_); connections_.at(connection_index).swap(connection); + connection_states_[connection_index] = ConnectionState::Disconnected; } if(!connection) { // Connection already closed @@ -206,22 +221,22 @@ class RadioBridgeNode : public rclcpp::Node connection_publishers_[i]->publish(connection_message); shutdown_requested_[i] = false; reboot_requested_[i] = false; - goodbye_received_[i] = false; + connection_states_[i] = ConnectionState::Disconnected; continue; } - const auto & last_heartbeat_time = last_heartbeat_timestamp_[i]; + const auto & last_heartbeat_time = last_heartbeat_timestamps_[i]; const auto time_since_heartbeat = std::chrono::steady_clock::now() - last_heartbeat_time; - if (time_since_heartbeat > timeout_threshold_) { + const auto effective_timeout = connection_states_[i] == ConnectionState::Connected ? + sustain_timeout_threshold_ : connect_timeout_threshold_; + if (time_since_heartbeat > effective_timeout) { RCLCPP_WARN(get_logger(), "Connection to robot %ld timed out.", i); // release lock early so CloseConnection can grab it lock.unlock(); CloseConnection(i); } - if(goodbye_received_[i]) { - RCLCPP_INFO(get_logger(), "Received goodbye from robot %ld.", i); + if(connection_states_[i] == ConnectionState::ReadyToClose) { // release lock early so CloseConnection can grab it lock.unlock(); - // don't send goodbye because we already received one from the robot CloseConnection(i, false); } // lock released by destructor @@ -340,7 +355,8 @@ class RadioBridgeNode : public rclcpp::Node sender_address.c_str(), sender_port); motion_command_timestamps_[robot_id] = {}; - last_heartbeat_timestamp_[robot_id] = std::chrono::steady_clock::now(); + last_heartbeat_timestamps_[robot_id] = std::chrono::steady_clock::now(); + connection_states_[robot_id] = ConnectionState::Connecting; connections_[hello_data.robot_id] = std::make_unique( sender_address, sender_port, std::bind( @@ -376,11 +392,12 @@ class RadioBridgeNode : public rclcpp::Node switch (packet.command_code) { case CC_GOODBYE: // close connection. No need to send our own goodbye - goodbye_received_[robot_id] = true; + RCLCPP_INFO(get_logger(), "Received goodbye from robot %d.", robot_id); + connection_states_[robot_id] = ConnectionState::ReadyToClose; break; case CC_TELEMETRY: { - last_heartbeat_timestamp_[robot_id] = std::chrono::steady_clock::now(); + OnHeartbeatQualifiedMessageReceived(robot_id); const auto data_var = ExtractData(packet, error); if (!error.empty()) { RCLCPP_WARN(get_logger(), "Ignoring basic telemetry message from robot %d. %s", robot_id, error.c_str()); @@ -424,7 +441,7 @@ class RadioBridgeNode : public rclcpp::Node break; } case CC_KEEPALIVE: - last_heartbeat_timestamp_[robot_id] = std::chrono::steady_clock::now(); + OnHeartbeatQualifiedMessageReceived(robot_id); break; default: RCLCPP_WARN( @@ -484,6 +501,13 @@ class RadioBridgeNode : public rclcpp::Node response->success = true; } + void OnHeartbeatQualifiedMessageReceived(int robot_id) { + last_heartbeat_timestamps_[robot_id] = std::chrono::steady_clock::now(); + if(connection_states_[robot_id] == ConnectionState::Connecting) { + connection_states_[robot_id] = ConnectionState::Connected; + } + } + }; } // namespace ateam_radio_bridge diff --git a/radio/ateam_radio_bridge/test/launch_tests/CMakeLists.txt b/radio/ateam_radio_bridge/test/launch_tests/CMakeLists.txt index 74dd619f5..e4028ff68 100644 --- a/radio/ateam_radio_bridge/test/launch_tests/CMakeLists.txt +++ b/radio/ateam_radio_bridge/test/launch_tests/CMakeLists.txt @@ -11,6 +11,10 @@ add_launch_test(bridge_command_test.py APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_SOURCE_DIR} TIMEOUT 10 ) +add_launch_test(bridge_goodbye_test.py + APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_SOURCE_DIR} + TIMEOUT 10 +) install(PROGRAMS mock_robot.py 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 3b98a39f5..a98bacc40 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 @@ -91,3 +91,8 @@ def test_commands(self): if abs(vel_x_linear - 2.0) < 0.1: # Pass the test return + +@launch_testing.post_shutdown_test() +class TestProcessExit(unittest.TestCase): + def test_exit_codes(self, proc_info): + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/radio/ateam_radio_bridge/test/launch_tests/bridge_discovery_test.py b/radio/ateam_radio_bridge/test/launch_tests/bridge_discovery_test.py index e5e509b6b..499efb633 100644 --- a/radio/ateam_radio_bridge/test/launch_tests/bridge_discovery_test.py +++ b/radio/ateam_radio_bridge/test/launch_tests/bridge_discovery_test.py @@ -79,3 +79,8 @@ def test_discoveryResponse(self): "Hello response should not hold \ port 0", ) + +@launch_testing.post_shutdown_test() +class TestProcessExit(unittest.TestCase): + def test_exit_codes(self, proc_info): + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/radio/ateam_radio_bridge/test/launch_tests/bridge_feedback_test.py b/radio/ateam_radio_bridge/test/launch_tests/bridge_feedback_test.py index 976ab52af..7bcf142e3 100644 --- a/radio/ateam_radio_bridge/test/launch_tests/bridge_feedback_test.py +++ b/radio/ateam_radio_bridge/test/launch_tests/bridge_feedback_test.py @@ -77,3 +77,8 @@ def test_feedback(self): # Just checks a few fields to make sure it's a reasonably valid message self.assertEqual(message.sequence_number, 1) self.assertAlmostEqual(message.battery_percent, 100) + +@launch_testing.post_shutdown_test() +class TestProcessExit(unittest.TestCase): + def test_exit_codes(self, proc_info): + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/radio/ateam_radio_bridge/test/launch_tests/bridge_goodbye_test.py b/radio/ateam_radio_bridge/test/launch_tests/bridge_goodbye_test.py new file mode 100644 index 000000000..d2ba56b4b --- /dev/null +++ b/radio/ateam_radio_bridge/test/launch_tests/bridge_goodbye_test.py @@ -0,0 +1,69 @@ +"""Tests the bridge's ability to handle goodbye packets from the robot.""" + +import time +import unittest + +import launch +from launch.actions import TimerAction + +import launch_ros.actions + +import launch_testing + +from mock_robot import MockRobot + +import pytest + + +discovery_address = "224.4.20.70" +discovery_port = 42069 + + +@pytest.mark.rostest +def generate_test_description(): + return ( + launch.LaunchDescription( + [ + launch_ros.actions.Node( + package="ateam_radio_bridge", + executable="radio_bridge_node", + parameters=[ + {"discovery_address": discovery_address}, + {"default_team_color": "yellow"}, + {"net_interface_address": ""}, + ], + ), + TimerAction(period=0.1, actions=[launch_testing.actions.ReadyToTest()]), + ] + ), + locals(), + ) + + +class TestRadioBridgeNode(unittest.TestCase): + @classmethod + def setUpClass(cls): + cls.robot = MockRobot( + discovery_address=discovery_address, discovery_port=discovery_port + ) + cls.robot.startAsync() + + @classmethod + def tearDownClass(cls): + cls.robot.stopAsync() + + def test_feedback(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()) + + # Let connection run for a bit + time.sleep(0.1) + + self.robot.sendGoodbyeAndShutdown() + +@launch_testing.post_shutdown_test() +class TestProcessExit(unittest.TestCase): + def test_exit_codes(self, proc_info): + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/radio/ateam_radio_bridge/test/launch_tests/mock_robot.py b/radio/ateam_radio_bridge/test/launch_tests/mock_robot.py index 2a462d525..1c30882d3 100644 --- a/radio/ateam_radio_bridge/test/launch_tests/mock_robot.py +++ b/radio/ateam_radio_bridge/test/launch_tests/mock_robot.py @@ -3,6 +3,7 @@ import socket import struct +import time from threading import Thread @@ -18,6 +19,7 @@ def __init__( self._last_cmd_packet = None self._robot_id = robot_id self._color = color + self._goodbye_requested = False self._async_running = False self._async_thread = None self._bridge_endpoint = ('', 0) @@ -31,12 +33,22 @@ def isConnected(self) -> bool: def getLastCmdMessage(self) -> bytes: return self._last_cmd_packet + def sendGoodbyeAndShutdown(self) -> None: + if not self._connected: + return + self._goodbye_requested = True + time.sleep(0.1) + self.stopAsync() + def run(self, run_async=False): while True: if run_async and not self._async_running: return if not self._connected: self._runDiscovery() + elif self._goodbye_requested: + self._sendGoodbyePacket() + self._goodbye_requested = False else: self._runConnected() @@ -96,6 +108,17 @@ def _sendTelemetryPacket(self): 100, # Kicker Charge Percent ) self._socket.sendto(packet, self._bridge_endpoint) + + def _sendGoodbyePacket(self): + packet = struct.pack( + 'IHHBH', + 0, # CRC + 0, # Version Major + 1, # Version Minor + 3, # CC Goodbye, + 0, # Data Length + ) + self._socket.sendto(packet, self._bridge_endpoint) def _runConnected(self): try: