修改远控看门狗逻辑

src/remote_ctrl/config/config.ini

@@ -1,6 +1,6 @@
 [MQTT]
-host = 192.168.4.150
+host = 36.153.162.171
-port = 1883
+port = 61883
 username = zxwl
 password = zxwl1234@
 vehicle_id = V090001

src/remote_ctrl/include/remote_ctrl/mqtt_client.hpp

@@ -81,6 +81,7 @@ public:
         msg_cb_ = std::move(cb);
     }
 
+    // 注意：connected_cb_ 在 MQTT 网络线程中调用，禁止阻塞
     void setConnectedCallback(ConnectedCallback cb)
     {
         std::lock_guard<std::mutex> lk(mtx_);
@@ -90,7 +91,10 @@ public:
     bool subscribe(const std::string &topic, int qos = 0)
     {
         std::lock_guard<std::mutex> lk(mtx_);
-        if (!client_ || !connected_)
+
+        if (!client_ ||
+            !connected_.load(std::memory_order_acquire) ||
+            !ready_.load(std::memory_order_acquire))
             return false;
 
         int rc = MQTTClient_subscribe(client_, topic.c_str(), qos);
@@ -109,7 +113,10 @@ public:
                  int qos = 0)
     {
         std::lock_guard<std::mutex> lk(mtx_);
-        if (!client_ || !connected_)
+
+        if (!client_ ||
+            !connected_.load(std::memory_order_acquire) ||
+            !ready_.load(std::memory_order_acquire))
             return false;
 
         MQTTClient_message msg = MQTTClient_message_initializer;
@@ -122,7 +129,8 @@ public:
 
         if (rc != MQTTCLIENT_SUCCESS)
         {
-            connected_ = false;
+            connected_.store(false, std::memory_order_release);
+            ready_.store(false, std::memory_order_release);
             return false;
         }
 
@@ -153,20 +161,27 @@ private:
         int rc = MQTTClient_connect(client_, &opts);
         if (rc == MQTTCLIENT_SUCCESS)
         {
-            connected_ = true;
+            connected_.store(true, std::memory_order_release);
-            if (connected_cb_)
+            ready_.store(false, std::memory_order_release);
-                connected_cb_();
+
+            // ⭐ 标记：需要在 loop 线程中触发 connected_cb_
+            need_callback_.store(true, std::memory_order_release);
+
             return true;
         }
 
-        connected_ = false;
+        connected_.store(false, std::memory_order_release);
+        ready_.store(false, std::memory_order_release);
         return false;
     }
 
     static void connLostCB(void *ctx, char *cause)
     {
         auto self = static_cast<MQTTClientWrapper *>(ctx);
-        self->connected_ = false;
+
+        self->connected_.store(false, std::memory_order_release);
+        self->ready_.store(false, std::memory_order_release);
+
         std::cerr << "[MQTT] lost connection: "
                   << (cause ? cause : "unknown") << std::endl;
     }
@@ -193,19 +208,41 @@ private:
     // 异步线程：负责 reconnect + yield
     void loopFunc()
     {
+        auto last_connect_try = std::chrono::steady_clock::now();
+
         while (!stop_flag_)
         {
-
+            // 1️⃣ 未连接：按重连间隔尝试连接
-            if (!connected_)
+            if (!connected_.load(std::memory_order_acquire))
             {
-                doConnect();
+                auto now = std::chrono::steady_clock::now();
+                if (now - last_connect_try >= std::chrono::milliseconds(reconnect_ms_))
+                {
+                    last_connect_try = now;
+                    doConnect();
+                }
+            }
+            else
+            {
+                // 2️⃣ 已连接：处理网络事件
+                MQTTClient_yield();
+
+                // 3️⃣ 第一次 yield 后，才算真正 ready
+                if (!ready_.load(std::memory_order_acquire))
+                {
+                    ready_.store(true, std::memory_order_release);
+
+                    // ⭐ 在 ready 状态触发 connected callback
+                    if (need_callback_.exchange(false))
+                    {
+                        if (connected_cb_)
+                            connected_cb_();
+                    }
+                }
             }
 
-            // 关键：处理 MQTT 网络事件
+            // ⭐ 正常网络轮询节奏
-            MQTTClient_yield();
+            std::this_thread::sleep_for(std::chrono::milliseconds(50));
-
-            std::this_thread::sleep_for(
-                std::chrono::milliseconds(reconnect_ms_));
         }
     }
 
@@ -223,4 +260,7 @@ private:
 
     MessageCallback msg_cb_;
     ConnectedCallback connected_cb_;
+
+    std::atomic<bool> ready_{false}; // 是否真正进入可用状态
+    std::atomic<bool> need_callback_{false};
 };

src/remote_ctrl/src/remote_ctrl_node.cpp

@@ -118,22 +118,28 @@ public:
                     client_id.c_str());
 
         // ===== 看门狗 & 定时发布 =====
-        remote_enabled_.store(false);
         last_msg_time_ = std::chrono::steady_clock::now();
 
-        // 看门狗：300ms 内没收到新 remote 指令则停控
+        // 看门狗：800ms 内没收到新 remote 指令则停控
         watchdog_ = this->create_wall_timer(
             100ms,
             [this]()
             {
+                // 未授权远控，watchdog 什么都不做
+                if (!remote_authorized_.load(std::memory_order_acquire))
+                    return;
+
                 auto now = std::chrono::steady_clock::now();
-                if (remote_enabled_.load() &&
+
-                    (now - last_msg_time_ > 300ms))
+                // 超时阈值：建议 800ms ~ 1500ms
+                if (now - last_msg_time_ > 800ms)
                 {
-                    RCLCPP_WARN(this->get_logger(),
+                    // 只在第一次进入“失活”时打印日志
-                                "[REMOTE] timeout, disable remote");
+                    if (remote_alive_.exchange(false))
-                    remote_enabled_.store(false);
+                    {
-                    desired_ = {};
+                        RCLCPP_WARN(this->get_logger(),
+                                    "[REMOTE] control timeout, enter safe-stop");
+                    }
                 }
             });
 
@@ -142,18 +148,35 @@ public:
             20ms, // 50Hz
             [this]()
             {
-                if (!remote_enabled_.load())
+                // 未授权远控：完全不发
+                if (!remote_authorized_.load(std::memory_order_acquire))
                     return;
 
                 sweeperMsg::McCtrl msg;
+
                 {
                     std::lock_guard<std::mutex> lock(state_mtx_);
-                    msg.gear = desired_.gear;
+
-                    msg.rpm = desired_.rpm;
+                    // 已授权但超时：安全停车态
-                    msg.brake = desired_.brake;
+                    if (!remote_alive_.load(std::memory_order_relaxed))
-                    msg.angle = desired_.angle;
+                    {
-                    msg.sweep = desired_.sweep;
+                        msg.gear = desired_.gear; // 通常保持挡位
-                    msg.angle_speed = desired_.angle_speed;
+                        msg.brake = 1;
+                        msg.rpm = 0;
+                        msg.angle = 0.0f;
+                        msg.angle_speed = 120;
+                        msg.sweep = false;
+                    }
+                    // 正常远控
+                    else
+                    {
+                        msg.gear = desired_.gear;
+                        msg.brake = desired_.brake;
+                        msg.rpm = desired_.rpm;
+                        msg.angle = desired_.angle;
+                        msg.angle_speed = desired_.angle_speed;
+                        msg.sweep = desired_.sweep;
+                    }
                 }
 
                 pub_->publish(msg);
@@ -187,11 +210,22 @@ private:
     void onMqttMessage(const std::string &payload)
     {
         std::lock_guard<std::mutex> lock(state_mtx_);
+
+        // =====================================================
+        // 1. MQTT 消息到达：刷新活性 & 判断是否从超时中恢复
+        // =====================================================
+        bool was_dead = !remote_alive_.exchange(true, std::memory_order_relaxed);
+        if (was_dead)
+        {
+            RCLCPP_INFO(this->get_logger(),
+                        "[REMOTE] control recovered");
+        }
+
         last_msg_time_ = std::chrono::steady_clock::now();
 
-        RCLCPP_INFO(this->get_logger(),
+        RCLCPP_DEBUG(this->get_logger(),
-                    "[REMOTE] recv MQTT payload: %s",
+                     "[REMOTE] recv MQTT payload: %s",
-                    payload.c_str());
+                     payload.c_str());
 
         try
         {
@@ -199,93 +233,145 @@ private:
             if (!j.contains("data"))
                 return;
 
-            std::string cmd = j["data"]["command"].get<std::string>();
+            const auto &data = j["data"];
-            auto value = j["data"]["value"];
+            if (!data.contains("command") || !data.contains("value"))
+                return;
 
+            const std::string cmd = data["command"].get<std::string>();
+            const auto &value = data["value"];
+
+            // =====================================================
+            // 2. mode 指令：唯一的“授权 / 取消授权”入口
+            // =====================================================
             if (cmd == "mode")
             {
                 int mode = value.get<int>();
+
                 if (mode == 3)
                 {
-                    remote_enabled_.store(true);
+                    // 进入远控授权态（锁存）
-                    RCLCPP_INFO(this->get_logger(), "[REMOTE] enabled");
+                    remote_authorized_.store(true, std::memory_order_release);
+                    remote_alive_.store(true, std::memory_order_relaxed);
+
+                    RCLCPP_INFO(this->get_logger(),
+                                "[REMOTE] authorized (mode=3)");
                 }
                 else if (mode == 0)
                 {
-                    remote_enabled_.store(false);
+                    // 退出远控授权态（锁存）
+                    remote_authorized_.store(false, std::memory_order_release);
+                    remote_alive_.store(false, std::memory_order_relaxed);
+
+                    // 明确清空目标状态
                     desired_ = {};
-                    RCLCPP_INFO(this->get_logger(), "[REMOTE] disabled");
+
+                    RCLCPP_INFO(this->get_logger(),
+                                "[REMOTE] deauthorized (mode=0)");
+                }
+
+                // mode 指令处理完直接返回
+                return;
+            }
+
+            // =====================================================
+            // 3. 非 mode 指令：只有在“已授权”状态下才处理
+            // =====================================================
+            if (!remote_authorized_.load(std::memory_order_acquire))
+                return;
+
+            // =====================================================
+            // 4. drive：油门 / 刹车 / 转向
+            // =====================================================
+            if (cmd == "drive")
+            {
+                int throttle = 0;
+                int brake = 1;
+                int steer_raw = 0;
+
+                if (value.is_string())
+                {
+                    sscanf(value.get<std::string>().c_str(),
+                           "%d,%d,%d",
+                           &throttle,
+                           &brake,
+                           &steer_raw);
+                }
+                else
+                {
+                    return;
+                }
+
+                if (brake > 0)
+                {
+                    // 刹车优先
+                    desired_.brake = 1;
+                    desired_.rpm = 0;
+                    desired_.angle = 0.0f;
+                }
+                else
+                {
+                    desired_.brake = 0;
+
+                    // 油门映射：0~65535 -> 0~6000 rpm
+                    desired_.rpm =
+                        (static_cast<uint32_t>(throttle) * 6000U) / 65535U;
+
+                    // 转向映射：0~65535 -> -50~+50 度
+                    float target_angle =
+                        (static_cast<float>(steer_raw) * 100.0f / 65535.0f) - 50.0f;
+
+                    float delta = target_angle - current_feedback_angle_;
+                    float angle_speed_deg = std::fabs(delta) / DELTA_T;
+                    float motor_rpm =
+                        angle_speed_deg * DEG_PER_SEC_TO_RPM * GEAR_RATIO;
+
+                    desired_.angle_speed = std::clamp<uint16_t>(
+                        static_cast<uint16_t>(motor_rpm),
+                        120,
+                        1500);
+
+                    desired_.angle = target_angle;
                 }
             }
-            else if (remote_enabled_.load())
+            // =====================================================
+            // 5. gear：挡位
+            // =====================================================
+            else if (cmd == "gear")
             {
-                if (cmd == "drive")
+                int gear = value.get<int>();
+                switch (gear)
                 {
-                    int t = 0, b = 1, s = 0;
+                case 0:
-                    sscanf(value.get<std::string>().c_str(),
+                    desired_.gear = 0; // N
-                           "%d,%d,%d", &t, &b, &s);
+                    break;
-
+                case 1:
-                    if (b > 0)
+                    desired_.gear = 2; // D
-                    {
+                    break;
-                        desired_.brake = 1;
+                case 2:
-                        desired_.rpm = 0;
+                    desired_.gear = 1; // R
-                        desired_.angle = 0;
+                    break;
-                    }
+                default:
-                    else
+                    desired_.gear = 0;
-                    {
+                    break;
-                        desired_.brake = 0;
-                        desired_.rpm = (uint32_t(t) * 6000) / 65535;
-
-                        // s: 0~65535 -> -50~+50
-                        float steer = (float(s) * 100.0f / 65535.0f) - 50.0f;
-                        float target_angle = steer;
-
-                        float delta = target_angle - current_feedback_angle_;
-                        float angle_speed_deg = std::fabs(delta) / DELTA_T;
-                        float motor_rpm = angle_speed_deg * DEG_PER_SEC_TO_RPM * GEAR_RATIO;
-
-                        uint16_t can_speed = std::clamp<uint16_t>(
-                            static_cast<uint16_t>(motor_rpm),
-                            120,
-                            1500);
-
-                        desired_.angle = target_angle;
-                        desired_.angle_speed = can_speed;
-                    }
-                }
-                else if (cmd == "gear")
-                {
-                    int gear = value.get<int>();
-                    switch (gear)
-                    {
-                    case 0:
-                        desired_.gear = 0; // N
-                        break;
-                    case 1:
-                        desired_.gear = 2; // D
-                        break;
-                    case 2:
-                        desired_.gear = 1; // R
-                        break;
-                    default:
-                        desired_.gear = 0;
-                        break;
-                    }
-                }
-                else if (cmd == "sweepCtrl")
-                {
-                    desired_.sweep = (value.get<int>() != 0);
                 }
             }
+            // =====================================================
+            // 6. sweepCtrl：清扫开关
+            // =====================================================
+            else if (cmd == "sweepCtrl")
+            {
+                desired_.sweep = (value.get<int>() != 0);
+            }
         }
-        catch (...)
+        catch (const std::exception &e)
         {
-            RCLCPP_WARN(this->get_logger(), "[REMOTE] invalid JSON");
+            RCLCPP_WARN(this->get_logger(),
+                        "[REMOTE] JSON parse error: %s",
+                        e.what());
         }
     }
 
 private:
-    std::atomic<bool> remote_enabled_{false};
     RemoteCtrlState desired_;
     std::mutex state_mtx_;
 
@@ -296,7 +382,10 @@ private:
     rclcpp::TimerBase::SharedPtr timer_;
     rclcpp::Subscription<sweeperMsg::CanFrame>::SharedPtr can_sub_;
 
+    std::atomic<bool> remote_authorized_{false}; // 是否允许远控（锁存）
+    std::atomic<bool> remote_alive_{false};      // 是否活跃（心跳）
     std::chrono::steady_clock::time_point last_msg_time_;
+
     float current_feedback_angle_{0.0f};
 };