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