Radio bridge connection states

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)

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<bool>("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<ateam_msgs::msg::RobotMotionCommand, 16> motion_commands_;
   std::array<std::chrono::steady_clock::time_point, 16> motion_command_timestamps_;
   std::array<bool, 16> shutdown_requested_;
   std::array<bool, 16> reboot_requested_;
-  std::array<bool, 16> goodbye_received_;
   ateam_common::GameControllerListener game_controller_listener_;
   std::array<rclcpp::Subscription<ateam_msgs::msg::RobotMotionCommand>::SharedPtr,
     16> motion_command_subscriptions_;
@@ -143,7 +156,8 @@ class RadioBridgeNode : public rclcpp::Node
   FirmwareParameterServer firmware_parameter_server_;
   rclcpp::Service<ateam_radio_msgs::srv::SendRobotPowerRequest>::SharedPtr power_request_service_;
   std::array<std::unique_ptr<ateam_common::BiDirectionalUDP>, 16> connections_;
-  std::array<std::chrono::steady_clock::time_point, 16> last_heartbeat_timestamp_;
+  std::array<std::chrono::steady_clock::time_point, 16> last_heartbeat_timestamps_;
+  std::array<ConnectionState, 16> 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<ateam_common::BiDirectionalUDP>(
       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

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

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)

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)

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)

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)

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: