pfu pl task_manager debug

src/autonomy/fu/src/fu_node.cpp

@@ -541,7 +541,7 @@ class fu_node : public rclcpp::Node
                 state_machine_->publish_speed = 0;
                 state_machine_->publish_angle = 0.0f;
                 state_machine_->setState(StateMachine::WAITING);
-                publishControlCommand(true);  // 立即发布带刹车的停车指令
+                publishControlCommand(true, true);  // 立即发布带刹车的停车指令，关闭清扫
                 LOG_WARN("[紧急停车] 栅格回调中检测到前方障碍物，立即停车！");
             }
         }
@@ -1136,7 +1136,7 @@ class fu_node : public rclcpp::Node
                         state_machine_->setState(StateMachine::WAITING);
                         LOG_WARN("[超声波停车] 前方检测到障碍物！左:%ucm 右:%ucm 阈值:%dcm，立即停车！",
                                  front_left, front_right, uss_stop_distance_);
-                        publishControlCommand(true);
+                        publishControlCommand(true, true);  // 紧急停车并关闭清扫
                         return;
                     }
                 }
@@ -1145,7 +1145,7 @@ class fu_node : public rclcpp::Node
             // 紧急停车优先级最高
             if (executeEmergencyStop())
             {
-                publishControlCommand(true);  // 紧急停车使用刹车
+                publishControlCommand(true, true);  // 紧急停车使用刹车并关闭清扫
                 return;
             }
 
@@ -1155,7 +1155,7 @@ class fu_node : public rclcpp::Node
                 state_machine_->publish_speed = 0;
                 state_machine_->publish_angle = 0;
                 LOG_INFO("PL速度为0，已停车");
-                publishControlCommand();
+                publishControlCommand(false, true);  // 停车时关闭清扫部件
                 return;
             }
 
@@ -1203,21 +1203,28 @@ class fu_node : public rclcpp::Node
             // 发布控制指令
             publishControlCommand();
         }
+        else
+        {
+            // 任务未运行时，发送停止命令，确保车辆和清扫部件都停止
+            state_machine_->publish_speed = 0;
+            state_machine_->publish_angle = 0;
+            publishControlCommand(false, true);  // emergency_brake=false, disable_sweep=true
+        }
     }
 
     // ======== 发布控制指令 ========
-    void publishControlCommand(bool emergency_brake = false)
+    void publishControlCommand(bool emergency_brake = false, bool disable_sweep = false)
     {
         auto message = sweeper_interfaces::msg::McCtrl();
 
-        message.enable_main_brush = true;
+        message.enable_main_brush = !disable_sweep;
-        message.enable_vacuum = true;
+        message.enable_vacuum = !disable_sweep;
         message.enable_dust_shake = false;
-        message.enable_main_brush_pole = true;
+        message.enable_main_brush_pole = !disable_sweep;
-        message.enable_flap_pole = true;
+        message.enable_flap_pole = !disable_sweep;
-        message.enable_side_brush = true;
+        message.enable_side_brush = !disable_sweep;
         message.sweep_mode = current_task_mode;  // 清扫模式: 0=标准模式(刷子), 1=混合模式(刷子+洒水)
-        message.enable_water_pump = (message.sweep_mode == 1);
+        message.enable_water_pump = (!disable_sweep && message.sweep_mode == 1);
 
         message.brake = emergency_brake ? 1 : 0;  // 紧急停车时启用刹车
         message.gear = 2;

src/autonomy/pl/include/pl/pl.hpp

@@ -1,6 +1,7 @@
 #ifndef __PL_H__
 #define __PL_H__
 
+#include <atomic>
 #include <string>
 
 enum Direction
@@ -70,7 +71,7 @@ extern int last_mode;
 extern std::string filename;
 extern int d_file;
 extern Direction drive_mode;
-extern TaskStatus task_status;
+extern std::atomic<TaskStatus> task_status;
-extern int thread_exit_flag;  // 线程退出标志
+extern std::atomic<int> thread_exit_flag;  // 线程退出标志
 
 #endif

src/autonomy/pl/src/pl.cpp

@@ -39,8 +39,8 @@ int mode = 0;
 int last_mode = 0;
 
 Direction drive_mode = Direction::STRAIGHT_D;
-TaskStatus task_status = TaskStatus::PENDING;
+std::atomic<TaskStatus> task_status{TaskStatus::PENDING};
-int thread_exit_flag = 0;  // 线程退出标志，0=继续运行，1=需要退出
+std::atomic<int> thread_exit_flag{0};  // 线程退出标志，0=继续运行，1=需要退出
 
 // 返回单位：m
 double ntzx_GPS_length(double lonti1, double lati1, double lonti2, double lati2)

src/autonomy/pl/src/pl_node.cpp

@@ -4,6 +4,7 @@
 #include <sys/stat.h>
 #include <unistd.h>
 
+#include <atomic>
 #include <chrono>  // 新增：用于时间戳处理
 #include <cstring>
 #include <fstream>
@@ -21,7 +22,7 @@
 using namespace std;
 using namespace std::chrono;  // 新增：时间戳命名空间
 pthread_t pl_thread_t;
-int is_start = 0;
+std::atomic<int> is_start{0};
 
 // 保存完整的任务信息
 int current_task_id = 0;
@@ -181,7 +182,7 @@ class pl_node : public rclcpp::Node
             {
                 close(sock);
                 sock = -1;
-                return timer_callback_pl();  // 重试
+                return;  // 下次定时器回调会重试连接
             }
         }
         else if (sock >= 0)

src/communication/task_manager/include/task_manager/task_manager.hpp

@@ -1,6 +1,7 @@
 #ifndef __TASK_MANAGER_H__
 #define __TASK_MANAGER_H__
 
+#include <atomic>
 #include <boost/any.hpp>
 #include <condition_variable>
 #include <functional>
@@ -104,7 +105,7 @@ class TaskManager
     std::mutex task_mutex_;
 
     string destination_file_path_;
-    int task_status_;
+    std::atomic<int> task_status_;
 
     // 回调函数用于发送MQTT响应
     std::function<void(const string& topic, int seqNo, int code, const string& msg)> send_response_callback_;

src/communication/task_manager/src/task_manager_main.cpp

@@ -373,8 +373,6 @@ int handleMqttMessage(char* topicName, int topicLen, MQTTClient_message* message
 
                     LOG_INFO("Stop task: %d", stop_id);
                     task_manager.addTask(mqtt_json);
-
-                    sendTaskResponse(seqNo, 200, "Task stop command received");
                 }
                 catch (const std::exception& e)
                 {
@@ -609,9 +607,9 @@ class TaskManagerNode : public rclcpp::Node
                 }
                 else
                 {
-                    // 最后一条路径完成，更新任务管理器内部状态为 COMPLETED
+                    // 最后一条路径完成，发送 Status=0 触发 PL 停止线程
                     LOG_INFO("所有 %d 条路径执行完成，任务结束", task_manager.getRouteCount());
-                    task_manager.setTaskStatus(TaskStatus::COMPLETED);
+                    task_manager.setTaskStatus(0);  // 设为0触发PL停止
                     task_manager.updateCurrentTaskStatus(TaskStatus::COMPLETED);
                 }
             }