Skip to content

Resets onboard controller during side changes #395

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
May 17, 2026
Merged

Resets onboard controller during side changes #395

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
45df955
Updates comms submodule
barulicm May 17, 2026
527bba4
Adds logic to send controller reset when field side changes.
barulicm May 17, 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
11 changes: 10 additions & 1 deletion 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 @@ -72,6 +72,7 @@ class RadioBridgeNode : public rclcpp::Node
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)),
last_side_change_timestamp_(std::chrono::steady_clock::now()),
game_controller_listener_(*this,
std::bind_front(&RadioBridgeNode::TeamColorChangeCallback, this)),
discovery_receiver_(declare_parameter<std::string>("discovery_address", "224.4.20.69"),
Expand Down Expand Up @@ -154,6 +155,7 @@ class RadioBridgeNode : public rclcpp::Node
std::array<std::chrono::steady_clock::time_point, 16> vision_state_timestamps_;
std::array<bool, 16> shutdown_requested_;
std::array<bool, 16> reboot_requested_;
std::chrono::steady_clock::time_point last_side_change_timestamp_;
ateam_common::GameControllerListener game_controller_listener_;
std::array<rclcpp::Subscription<ateam_msgs::msg::RobotMotionCommand>::SharedPtr,
16> motion_command_subscriptions_;
Expand Down Expand Up @@ -260,11 +262,12 @@ class RadioBridgeNode : public rclcpp::Node
void SendCommandsCallback()
{
const std::lock_guard lock(mutex_);
const auto now = std::chrono::steady_clock::now();
Copy link
Copy Markdown
Contributor

@chachmu chachmu May 17, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Did we ever standardize on using chrono or ros clock for our timestamps everywhere?

Copy link
Copy Markdown
Contributor Author

@barulicm barulicm May 17, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Not sure if we ever formally decided, but right now we only use chrono everywhere.

for (auto id = 0; id < 16; ++id) {
if (connections_[id] == nullptr) {
continue;
}
if ((std::chrono::steady_clock::now() - motion_command_timestamps_[id]) >
if ((now - motion_command_timestamps_[id]) >
command_timeout_threshold_)
{
RCLCPP_WARN(get_logger(), "Robot %d command topic inactive. Sending zeros.", id);
Expand All @@ -282,6 +285,7 @@ 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.reset_controller = (last_side_change_timestamp_ + sustain_timeout_threshold_) >= now;
control_msg.reserved1 = 0;
FillVisionUpdate(control_msg, vision_states_[id], vision_state_timestamps_[id]);
control_msg.kick_request = static_cast<KickRequest>(motion_commands_[id].kick_request);
Expand Down Expand Up @@ -567,6 +571,11 @@ class RadioBridgeNode : public rclcpp::Node
SetupVisionSubscribers(color);
}

void TeamSideChangeCallback(const ateam_common::TeamSide)
{
last_side_change_timestamp_ = std::chrono::steady_clock::now();
}

void SendPowerRequestCallback(
const std::shared_ptr<ateam_radio_msgs::srv::SendRobotPowerRequest::Request> request,
std::shared_ptr<ateam_radio_msgs::srv::SendRobotPowerRequest::Response> response)
Expand Down
14 changes: 4 additions & 10 deletions radio/ateam_radio_bridge/src/rnp_packet_helpers.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -289,19 +289,13 @@ ParameterDataFormat GetParameterDataFormatForParameter(const ParameterName & par
return VEC2_F32;
case TRAJ_RECOMPUTE_ERROR:
return VEC4_F32;
case TRAJ_MAX:
return VEC4_F32;
case POSE_FB_PIDII_X:
return VEC5_F32;
case POSE_FB_PIDII_Y:
return VEC5_F32;
case POSE_FB_PIDII_THETA:
case POSE_FB_PIDII_LINEAR:
return VEC5_F32;
case TWIST_FB_PIDII_X:
case POSE_FB_PIDII_ANGULAR:
return VEC5_F32;
case TWIST_FB_PIDII_Y:
case TWIST_FB_PIDII_LINEAR:
return VEC5_F32;
case TWIST_FB_PIDII_THETA:
case TWIST_FB_PIDII_ANGULAR:
return VEC5_F32;
default:
throw std::invalid_argument("GetParameterDataFormatForParameter: Unrecognized parameter name.");
Expand Down
2 changes: 1 addition & 1 deletion radio/ateam_radio_msgs/software-communication
Open in desktop
Submodule software-communication updated 5 files
+2 −1 ateam-common-packets/include/basic_control.h
+2 −1 ateam-common-packets/include/basic_telemetry.h
+4 −10 ateam-common-packets/include/robot_parameters.h
+1 −1 ateam-common-packets/rust-lib/src/lib.rs
+46 −50 ateam-common-packets/rust-lib/src/radio.rs
Loading