lyq/sweeper_200
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix bug 车辆"行驶 → 停止 → 行驶 → 停止"的循环

Browse Source
This commit is contained in:
Alvin-lyq 2026-04-01 14:56:04 +08:00
parent c928d5f225
commit 0e13aaef17
1 changed files with 30 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
src/control/remote_ctrl/src/remote_ctrl_node.cpp
View File

@ -121,7 +121,7 @@ class RemoteCtrlNode : public rclcpp::Node
// ===== 看门狗 & 消息发布 ===== // ===== 看门狗 & 消息发布 =====
last_msg_time_ = std::chrono::steady_clock::now(); last_msg_time_ = std::chrono::steady_clock::now();
// 看门狗：800ms 内没收到新 remote 指令则停控 // 看门狗：1000ms 内没收到新 remote 指令则停控
watchdog_ = this->create_wall_timer(100ms, watchdog_ = this->create_wall_timer(100ms,
[this]() [this]()
{ {
@ -137,16 +137,37 @@ class RemoteCtrlNode : public rclcpp::Node
last = last_msg_time_; last = last_msg_time_;
} }
if (now - last > 800ms) if (now - last > 1000ms)
{ {
// 只在第一次进入“失活”时打印日志 // 只在第一次进入“失活”时打印日志并发送刹车指令
if (remote_alive_.exchange(false)) if (remote_alive_.exchange(false))
{ {
LOG_WARN("[REMOTE] control timeout, enter safe-stop"); LOG_WARN("[REMOTE] control timeout, enter safe-stop");
// 主动发送刹车指令
sweeperMsg::McCtrl safe_msg;
safe_msg.brake = 1;
safe_msg.gear = 0;
safe_msg.rpm = 0;
safe_msg.angle = 0.0f;
safe_msg.angle_speed = 120;
safe_msg.sweep = false;
pub_->publish(safe_msg);
} }
} }
}); });
// 添加定时器：定期发送控制指令（50ms 间隔），防止 ctrl_arbiter 超时
timer_ = this->create_wall_timer(
50ms,
[this]()
{
// 只有在授权且活跃状态下才定期发送
if (remote_authorized_.load(std::memory_order_acquire) && remote_alive_.load(std::memory_order_relaxed))
{
publishMcCtrlMsg();
}
});
// CAN 反馈订阅，用于计算当前转向角 // CAN 反馈订阅，用于计算当前转向角
can_sub_ = this->create_subscription<sweeperMsg::CanFrame>( can_sub_ = this->create_subscription<sweeperMsg::CanFrame>(
"can_data", 10, "can_data", 10,
@ -189,19 +210,18 @@ class RemoteCtrlNode : public rclcpp::Node
// 调试日志：显示当前授权状态 // 调试日志：显示当前授权状态
bool authorized = remote_authorized_.load(std::memory_order_acquire); bool authorized = remote_authorized_.load(std::memory_order_acquire);
bool alive = remote_alive_.load(std::memory_order_relaxed); bool alive = remote_alive_.load(std::memory_order_relaxed);
LOG_DEBUG("[REMOTE] publishMcCtrlMsg: authorized=%d, alive=%d", authorized, alive);
// 未授权时：完全不发 // 未授权时：完全不发
if (!authorized) if (!authorized)
{ {
LOG_WARN("[REMOTE] NOT AUTHORIZED - will NOT publish message"); LOG_WARN_ONCE("[REMOTE] NOT AUTHORIZED - will NOT publish message");
return; return;
} }
// 已授权但超时：完全不发 // 已授权但超时：完全不发
if (!alive) if (!alive)
{ {
LOG_WARN("[REMOTE] REMOTE ALIVE=FALSE - will NOT publish message"); LOG_WARN_ONCE("[REMOTE] REMOTE ALIVE=FALSE - will NOT publish message");
return; return;
} }
@ -211,7 +231,6 @@ class RemoteCtrlNode : public rclcpp::Node
std::lock_guard<std::mutex> lock(state_mtx_); std::lock_guard<std::mutex> lock(state_mtx_);
// 正常远控 // 正常远控
LOG_INFO("[REMOTE] REMOTE ALIVE=TRUE - sending normal control message");
msg.gear = desired_.gear; msg.gear = desired_.gear;
msg.brake = desired_.brake; msg.brake = desired_.brake;
msg.rpm = desired_.rpm; msg.rpm = desired_.rpm;
@ -220,11 +239,10 @@ class RemoteCtrlNode : public rclcpp::Node
msg.sweep = desired_.sweep; msg.sweep = desired_.sweep;
} }
LOG_INFO("[REMOTE] Publishing McCtrl: gear=%d brake=%d rpm=%d angle=%.1f angle_speed=%u sweep=%d", msg.gear, LOG_DEBUG("[REMOTE] Publishing McCtrl: gear=%d brake=%d rpm=%d angle=%.1f angle_speed=%u sweep=%d", msg.gear,
msg.brake, msg.rpm, msg.angle, msg.angle_speed, msg.sweep); msg.brake, msg.rpm, msg.angle, msg.angle_speed, msg.sweep);
pub_->publish(msg); pub_->publish(msg);
LOG_DEBUG("[REMOTE] Message published successfully");
} }
void onMqttMessage(const std::string& payload) void onMqttMessage(const std::string& payload)
@ -334,7 +352,7 @@ class RemoteCtrlNode : public rclcpp::Node
else else
{ {
desired_.brake = 0; desired_.brake = 0;
desired_.rpm = (static_cast<uint32_t>(throttle) * 6000U) / 65535U; desired_.rpm = (static_cast<uint32_t>(throttle) * 1500U) / 65535U;
float target_angle = (static_cast<float>(steer_raw) * 100.0f / 65535.0f) - 50.0f; float target_angle = (static_cast<float>(steer_raw) * 100.0f / 65535.0f) - 50.0f;
float delta = target_angle - current_feedback_angle_; float delta = target_angle - current_feedback_angle_;
@ -408,7 +426,7 @@ class RemoteCtrlNode : public rclcpp::Node
std::atomic<bool> remote_authorized_{false}; // 是否允许远控（锁存） std::atomic<bool> remote_authorized_{false}; // 是否允许远控（锁存）
std::atomic<bool> remote_alive_{false}; // 是否活跃（心跳） std::atomic<bool> remote_alive_{false}; // 是否活跃（心跳）
std::chrono::steady_clock::time_point last_msg_time_; std::chrono::steady_clock::time_point last_msg_time_;
std::mutex last_msg_time_mtx_; // 保护 last_msg_time_ std::mutex last_msg_time_mtx_; // 保护 last_msg_time_
float current_feedback_angle_{0.0f}; float current_feedback_angle_{0.0f};