cxh/sweeper_e800
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Auto commit at 2025-06-18 17:32:49

Browse Source
This commit is contained in:
cxh 2025-06-18 17:32:49 +08:00
parent bffaa1f396
commit d37c19a471
1 changed files with 6 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/ctrl_arbiter/src/ctrl_arbiter.cpp
View File

@ -48,6 +48,7 @@ public:
timer_ = this->create_wall_timer(20ms, [this]()
{ this->arbitrateAndPublish(); });
RCLCPP_INFO(this->get_logger(), "ArbitrationNode started, waiting for control sources...");
}
private:
@ -60,16 +61,19 @@ private:
if (radio_valid_ && (now - radio_last_time_).nanoseconds() < timeout_ms_ * 1000000)
{
publisher_->publish(radio_msg_);
RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "[ARBITER] Using RADIO control");
return;
}
if (remote_valid_ && (now - remote_last_time_).nanoseconds() < timeout_ms_ * 1000000)
{
publisher_->publish(remote_msg_);
RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "[ARBITER] Using REMOTE control");
return;
}
if (auto_valid_ && (now - auto_last_time_).nanoseconds() < timeout_ms_ * 1000000)
{
publisher_->publish(auto_msg_);
RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "[ARBITER] Using AUTO control");
return;
}
@ -87,6 +91,8 @@ private:
safe_msg.dust_shake = false;
publisher_->publish(safe_msg);
RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
"[ARBITER] All sources timeout, publishing FAILSAFE control");
}
rclcpp::Publisher<sweeperMsg::McCtrl>::SharedPtr publisher_;