修改远控逻辑

src/control/remote_ctrl/src/remote_ctrl_node.cpp

@@ -91,8 +91,7 @@ class RemoteCtrlNode : public rclcpp::Node
                 try_subscribe_ctrl_topic();
             });
 
-        // 主动连一次，后面内部 loop 线程也会自动尝试重连
+        // MQTTClientWrapper内部已有reconnectLoop线程自动处理连接和重连，无需主动调用connect()
-        mqtt_->connect();
 
         identity_sub_ = this->create_subscription<sweeper_interfaces::msg::VehicleIdentity>(
             "/vehicle/identity", rclcpp::QoS(1).transient_local().reliable(),
@@ -119,7 +118,7 @@ class RemoteCtrlNode : public rclcpp::Node
 
         LOG_INFO("RemoteCtrlNode started mqtt=%s client_id=%s", uri.str().c_str(), client_id.c_str());
 
-        // ===== 看门狗 & 定时发布 =====
+        // ===== 看门狗 & 消息发布 =====
         last_msg_time_ = std::chrono::steady_clock::now();
 
         // 看门狗：800ms 内没收到新 remote 指令则停控
@@ -142,43 +141,6 @@ class RemoteCtrlNode : public rclcpp::Node
                                                 }
                                             });
 
-        // 周期发布到 /remote_mc_ctrl
-        timer_ = this->create_wall_timer(20ms,  // 50Hz
-                                         [this]()
-                                         {
-                                             // 未授权远控：完全不发
-                                             if (!remote_authorized_.load(std::memory_order_acquire)) return;
-
-                                             sweeperMsg::McCtrl msg;
-
-                                             {
-                                                 std::lock_guard<std::mutex> lock(state_mtx_);
-
-                                                 // 已授权但超时：安全停车态
-                                                 if (!remote_alive_.load(std::memory_order_relaxed))
-                                                 {
-                                                     msg.gear = desired_.gear;  // 通常保持挡位
-                                                     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);
-                                         });
-
         // CAN 反馈订阅，用于计算当前转向角
         can_sub_ = this->create_subscription<sweeperMsg::CanFrame>(
             "can_data", 10,
@@ -216,36 +178,77 @@ class RemoteCtrlNode : public rclcpp::Node
         mqtt_->subscribe(ctrl_topic_, 1);
     }
 
+    void publishMcCtrlMsg()
+    {
+        // 未授权时：完全不发
+        if (!remote_authorized_.load(std::memory_order_acquire))
+        {
+            LOG_WARN("[REMOTE] NOT AUTHORIZED - will NOT publish message");
+            return;
+        }
+
+        sweeperMsg::McCtrl msg;
+
+        {
+            std::lock_guard<std::mutex> lock(state_mtx_);
+
+            // 已授权但超时：安全停车状态
+            if (!remote_alive_.load(std::memory_order_relaxed))
+            {
+                LOG_WARN("[REMOTE] REMOTE ALIVE=FALSE - sending safe-stop message");
+                msg.gear = desired_.gear;
+                msg.brake = 1;
+                msg.rpm = 0;
+                msg.angle = 0.0f;
+                msg.angle_speed = 120;
+                msg.sweep = false;
+            }
+            // 正常远控
+            else
+            {
+                LOG_INFO("[REMOTE] REMOTE ALIVE=TRUE - sending normal control message");
+                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;
+            }
+        }
+
+        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);
+
+        pub_->publish(msg);
+    }
+
     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)
-        {
-            LOG_INFO("[REMOTE] control recovered");
-        }
-
-        last_msg_time_ = std::chrono::steady_clock::now();
-
-        LOG_DEBUG("[REMOTE] recv MQTT payload: %s", payload.c_str());
-
         try
         {
             auto j = json::parse(payload);
-            if (!j.contains("data")) return;
+            if (!j.contains("data"))
+            {
+                LOG_WARN("[REMOTE] Invalid JSON: missing 'data' field, payload=%s", payload.c_str());
+                return;
+            }
 
             const auto& data = j["data"];
-            if (!data.contains("command") || !data.contains("value")) return;
+            if (!data.contains("command") || !data.contains("value"))
+            {
+                LOG_WARN("[REMOTE] Invalid JSON: missing 'command' or 'value' field");
+                return;
+            }
 
             const std::string cmd = data["command"].get<std::string>();
             const auto& value = data["value"];
 
+            LOG_INFO("[REMOTE] Received command: %s", cmd.c_str());
+
             // =====================================================
-            // 2. mode 指令：唯一的“授权 / 取消授权”入口
+            // mode 指令：唯一的授权入口
             // =====================================================
             if (cmd == "mode")
             {
@@ -253,35 +256,42 @@ class RemoteCtrlNode : public rclcpp::Node
 
                 if (mode == 3)
                 {
-                    // 进入远控授权态（锁存）
                     remote_authorized_.store(true, std::memory_order_release);
                     remote_alive_.store(true, std::memory_order_relaxed);
-
-                    LOG_INFO("[REMOTE] authorized (mode=3)");
                 }
                 else if (mode == 0)
                 {
-                    // 退出远控授权态（锁存）
                     remote_authorized_.store(false, std::memory_order_release);
                     remote_alive_.store(false, std::memory_order_relaxed);
-
-                    // 明确清空目标状态
                     desired_ = {};
-
-                    LOG_INFO("[REMOTE] deauthorized (mode=0)");
                 }
 
-                // mode 指令处理完直接返回
+                publishMcCtrlMsg();
                 return;
             }
 
             // =====================================================
-            // 3. 非 mode 指令：只有在“已授权”状态下才处理
+            // 非 mode 指令：检查授权
             // =====================================================
-            if (!remote_authorized_.load(std::memory_order_acquire)) return;
+            if (!remote_authorized_.load(std::memory_order_acquire))
+            {
+                LOG_WARN("[REMOTE] Command '%s' received but NOT AUTHORIZED - will not process", cmd.c_str());
+                return;
+            }
+
+            // 刷新活性
+            bool was_dead = !remote_alive_.exchange(true, std::memory_order_relaxed);
+            if (was_dead)
+            {
+                LOG_INFO("[REMOTE] ✓ control recovered");
+            }
+
+            last_msg_time_ = std::chrono::steady_clock::now();
+
+            LOG_DEBUG("[REMOTE] Processing command: %s", cmd.c_str());
 
             // =====================================================
-            // 4. drive：油门 / 刹车 / 转向
+            // drive 指令
             // =====================================================
             if (cmd == "drive")
             {
@@ -295,12 +305,14 @@ class RemoteCtrlNode : public rclcpp::Node
                 }
                 else
                 {
+                    LOG_WARN("[REMOTE] drive value is not string");
                     return;
                 }
 
+                LOG_DEBUG("[REMOTE] drive: throttle=%d brake=%d steer=%d", throttle, brake, steer_raw);
+
                 if (brake > 0)
                 {
-                    // 刹车优先
                     desired_.brake = 1;
                     desired_.rpm = 0;
                     desired_.angle = 0.0f;
@@ -308,28 +320,25 @@ class RemoteCtrlNode : public rclcpp::Node
                 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;
                 }
             }
             // =====================================================
-            // 5. gear：挡位
+            // gear 指令
             // =====================================================
             else if (cmd == "gear")
             {
                 int gear = value.get<int>();
+                LOG_DEBUG("[REMOTE] gear command: %d", gear);
+
                 switch (gear)
                 {
                     case 0:
@@ -347,16 +356,26 @@ class RemoteCtrlNode : public rclcpp::Node
                 }
             }
             // =====================================================
-            // 6. sweepCtrl：清扫开关
+            // sweepCtrl 指令
             // =====================================================
             else if (cmd == "sweepCtrl")
             {
-                desired_.sweep = (value.get<int>() != 0);
+                int val = value.get<int>();
+                desired_.sweep = (val != 0);
+                LOG_DEBUG("[REMOTE] sweepCtrl: %d", val);
             }
+            else
+            {
+                LOG_WARN("[REMOTE] Unknown command: %s", cmd.c_str());
+                return;
+            }
+
+            publishMcCtrlMsg();
         }
         catch (const std::exception& e)
         {
-            LOG_WARN("[REMOTE] JSON parse error: %s", e.what());
+            LOG_ERROR("[REMOTE] Exception in onMqttMessage: %s, payload=%s", e.what(), payload.c_str());
+            return;
         }
     }