Skip to content

Fix small radio bridge bugs #385

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
barulicm merged 2 commits into from
Apr 28, 2026
+24 −9
Merged

Fix small radio bridge bugs #385

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
6d56898
Addresses a few small radio bridge bugs.
barulicm Apr 13, 2026
5a8c2f3
Avoids sending reply goodbye when robot closes connection.
barulicm Apr 13, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
33 changes: 24 additions & 9 deletions radio/ateam_radio_bridge/src/radio_bridge_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -66,6 +66,10 @@ class RadioBridgeNode : public rclcpp::Node
declare_parameter<std::string>("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<bool>("controls_enabled", {
{"body_vel", true},
{"wheel_vel", true},
Expand Down Expand Up @@ -125,6 +129,7 @@ class RadioBridgeNode : public rclcpp::Node
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_;
Expand Down Expand Up @@ -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<ateam_common::BiDirectionalUDP> connection;
{
Expand All @@ -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<const uint8_t *>(&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<const uint8_t *>(&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));
}
}

/**
Expand All @@ -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];
Expand All @@ -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
}
}
Expand Down Expand Up @@ -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(
Expand Down Expand Up @@ -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:
{
Expand Down
Loading