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

38
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();
// 看门狗：800ms 内没收到新 remote 指令则停控
// 看门狗：1000ms 内没收到新 remote 指令则停控
watchdog_ = this->create_wall_timer(100ms,
[this]()
{
@ -137,16 +137,37 @@ class RemoteCtrlNode : public rclcpp::Node
last = last_msg_time_;
}
if (now - last > 800ms)
if (now - last > 1000ms)
{
// 只在第一次进入“失活”时打印日志
// 只在第一次进入“失活”时打印日志并发送刹车指令
if (remote_alive_.exchange(false))
{
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_sub_ = this->create_subscription<sweeperMsg::CanFrame>(
"can_data", 10,
@ -189,19 +210,18 @@ class RemoteCtrlNode : public rclcpp::Node
// 调试日志：显示当前授权状态
bool authorized = remote_authorized_.load(std::memory_order_acquire);
bool alive = remote_alive_.load(std::memory_order_relaxed);
LOG_DEBUG("[REMOTE] publishMcCtrlMsg: authorized=%d, alive=%d", authorized, alive);
// 未授权时：完全不发
if (!authorized)
{
LOG_WARN("[REMOTE] NOT AUTHORIZED - will NOT publish message");
LOG_WARN_ONCE("[REMOTE] NOT AUTHORIZED - will NOT publish message");
return;
}
// 已授权但超时：完全不发
if (!alive)
{
LOG_WARN("[REMOTE] REMOTE ALIVE=FALSE - will NOT publish message");
LOG_WARN_ONCE("[REMOTE] REMOTE ALIVE=FALSE - will NOT publish message");
return;
}
@ -211,7 +231,6 @@ class RemoteCtrlNode : public rclcpp::Node
std::lock_guard<std::mutex> lock(state_mtx_);
// 正常远控
LOG_INFO("[REMOTE] REMOTE ALIVE=TRUE - sending normal control message");
msg.gear = desired_.gear;
msg.brake = desired_.brake;
msg.rpm = desired_.rpm;
@ -220,11 +239,10 @@ class RemoteCtrlNode : public rclcpp::Node
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);
pub_->publish(msg);
LOG_DEBUG("[REMOTE] Message published successfully");
}
void onMqttMessage(const std::string& payload)
@ -334,7 +352,7 @@ class RemoteCtrlNode : public rclcpp::Node
else
{
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 delta = target_angle - current_feedback_angle_;