From 0e13aaef1726ec2de8dc65790496fecf897f04bb Mon Sep 17 00:00:00 2001 From: Alvin-lyq Date: Wed, 1 Apr 2026 14:56:04 +0800 Subject: [PATCH] =?UTF-8?q?fix=20bug=20=E8=BD=A6=E8=BE=86"=E8=A1=8C?= =?UTF-8?q?=E9=A9=B6=20=E2=86=92=20=E5=81=9C=E6=AD=A2=20=E2=86=92=20?= =?UTF-8?q?=E8=A1=8C=E9=A9=B6=20=E2=86=92=20=E5=81=9C=E6=AD=A2"=E7=9A=84?= =?UTF-8?q?=E5=BE=AA=E7=8E=AF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../remote_ctrl/src/remote_ctrl_node.cpp | 42 +++++++++++++------ 1 file changed, 30 insertions(+), 12 deletions(-) diff --git a/src/control/remote_ctrl/src/remote_ctrl_node.cpp b/src/control/remote_ctrl/src/remote_ctrl_node.cpp index 491a612..bfcf4c9 100644 --- a/src/control/remote_ctrl/src/remote_ctrl_node.cpp +++ b/src/control/remote_ctrl/src/remote_ctrl_node.cpp @@ -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( "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 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, - msg.brake, msg.rpm, msg.angle, msg.angle_speed, msg.sweep); + 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(throttle) * 6000U) / 65535U; + desired_.rpm = (static_cast(throttle) * 1500U) / 65535U; float target_angle = (static_cast(steer_raw) * 100.0f / 65535.0f) - 50.0f; float delta = target_angle - current_feedback_angle_; @@ -408,7 +426,7 @@ class RemoteCtrlNode : public rclcpp::Node std::atomic remote_authorized_{false}; // 是否允许远控（锁存） std::atomic remote_alive_{false}; // 是否活跃（心跳） 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};