diff --git a/radio/ateam_radio_bridge/src/radio_bridge_node.cpp b/radio/ateam_radio_bridge/src/radio_bridge_node.cpp index 046595d58..5c0508be0 100644 --- a/radio/ateam_radio_bridge/src/radio_bridge_node.cpp +++ b/radio/ateam_radio_bridge/src/radio_bridge_node.cpp @@ -66,6 +66,10 @@ class RadioBridgeNode : public rclcpp::Node declare_parameter("net_interface_address", "")), firmware_parameter_server_(*this, connections_) { + 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); + declare_parameters("controls_enabled", { {"body_vel", true}, {"wheel_vel", true}, @@ -125,6 +129,7 @@ class RadioBridgeNode : public rclcpp::Node 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_; @@ -162,7 +167,7 @@ class RadioBridgeNode : public rclcpp::Node motion_command_timestamps_[robot_id] = std::chrono::steady_clock::now(); } - void CloseConnection(const std::size_t & connection_index) + void CloseConnection(const std::size_t & connection_index, bool send_goodbye = true) { std::unique_ptr connection; { @@ -177,12 +182,14 @@ class RadioBridgeNode : public rclcpp::Node RCLCPP_INFO( get_logger(), "Closing connection to robot %ld (%s:%d)", connection_index, connection->GetRemoteIPAddress().c_str(), connection->GetRemotePort()); - const auto packet = CreateEmptyPacket(CC_GOODBYE); - connection->send( - reinterpret_cast(&packet), - GetPacketSize(packet.command_code)); - // Give some time for the message to actually send before closing the connection - std::this_thread::sleep_for(std::chrono::milliseconds(50)); + if (send_goodbye) { + const auto packet = CreateEmptyPacket(CC_GOODBYE); + connection->send( + reinterpret_cast(&packet), + GetPacketSize(packet.command_code)); + // Give some time for the message to actually send before closing the connection + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } } /** @@ -199,6 +206,7 @@ class RadioBridgeNode : public rclcpp::Node connection_publishers_[i]->publish(connection_message); shutdown_requested_[i] = false; reboot_requested_[i] = false; + goodbye_received_[i] = false; continue; } const auto & last_heartbeat_time = last_heartbeat_timestamp_[i]; @@ -209,6 +217,13 @@ class RadioBridgeNode : public rclcpp::Node lock.unlock(); CloseConnection(i); } + if(goodbye_received_[i]) { + RCLCPP_INFO(get_logger(), "Received goodbye from robot %ld.", i); + // 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 } } @@ -295,7 +310,7 @@ class RadioBridgeNode : public rclcpp::Node const auto robot_id = hello_data.robot_id; - if (robot_id > connections_.size()) { + if (robot_id >= connections_.size()) { // invalid robot ID requested const auto reply_packet = CreateEmptyPacket(CC_NACK); discovery_receiver_.SendTo( @@ -361,7 +376,7 @@ class RadioBridgeNode : public rclcpp::Node switch (packet.command_code) { case CC_GOODBYE: // close connection. No need to send our own goodbye - connections_[robot_id].reset(); + goodbye_received_[robot_id] = true; break; case CC_TELEMETRY: {