修改远控逻辑

src/control/remote_ctrl/src/remote_ctrl_node.cpp

@@ -180,32 +180,31 @@ class RemoteCtrlNode : public rclcpp::Node
 
     void publishMcCtrlMsg()
     {
+        // 调试日志：显示当前授权状态
+        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 (!remote_authorized_.load(std::memory_order_acquire))
+        if (!authorized)
         {
             LOG_WARN("[REMOTE] NOT AUTHORIZED - will NOT publish message");
             return;
         }
 
+        // 已授权但超时：完全不发
+        if (!alive)
+        {
+            LOG_WARN("[REMOTE] REMOTE ALIVE=FALSE - 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;
@@ -214,18 +213,16 @@ class RemoteCtrlNode : public rclcpp::Node
             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);
+        LOG_DEBUG("[REMOTE] Message published successfully");
     }
 
     void onMqttMessage(const std::string& payload)
     {
-        std::lock_guard<std::mutex> lock(state_mtx_);
-
         try
         {
             auto j = json::parse(payload);
@@ -254,6 +251,8 @@ class RemoteCtrlNode : public rclcpp::Node
             {
                 int mode = value.get<int>();
 
+                {
+                    std::lock_guard<std::mutex> lock(state_mtx_);
                     if (mode == 3)
                     {
                         remote_authorized_.store(true, std::memory_order_release);
@@ -265,6 +264,7 @@ class RemoteCtrlNode : public rclcpp::Node
                         remote_alive_.store(false, std::memory_order_relaxed);
                         desired_ = {};
                     }
+                }
 
                 publishMcCtrlMsg();
                 return;
@@ -290,6 +290,11 @@ class RemoteCtrlNode : public rclcpp::Node
 
             LOG_DEBUG("[REMOTE] Processing command: %s", cmd.c_str());
 
+            // =====================================================
+            // 处理具体指令
+            // =====================================================
+            {
+                std::lock_guard<std::mutex> lock(state_mtx_);
                 // =====================================================
                 // drive 指令
                 // =====================================================
@@ -369,6 +374,7 @@ class RemoteCtrlNode : public rclcpp::Node
                     LOG_WARN("[REMOTE] Unknown command: %s", cmd.c_str());
                     return;
                 }
+            }
 
             publishMcCtrlMsg();
         }