pl debug

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

@@ -73,5 +73,6 @@ extern int d_file;
 extern Direction drive_mode;
 extern std::atomic<TaskStatus> task_status;
 extern std::atomic<int> thread_exit_flag;  // 线程退出标志
+extern std::atomic<int> path_finished_published;  // PATH_FINISHED是否已发布标志
 
 #endif

src/autonomy/pl/src/pl.cpp

@@ -41,6 +41,7 @@ int last_mode = 0;
 Direction drive_mode = Direction::STRAIGHT_D;
 std::atomic<TaskStatus> task_status{TaskStatus::PENDING};
 std::atomic<int> thread_exit_flag{0};  // 线程退出标志，0=继续运行，1=需要退出
+std::atomic<int> path_finished_published{0};  // PATH_FINISHED是否已发布标志
 
 // 返回单位：m
 double ntzx_GPS_length(double lonti1, double lati1, double lonti2, double lati2)
@@ -350,6 +351,7 @@ void PL_ProcThread()
                 direction_pos = 20;
                 g_last_road_pos = -1;  // 重置单调性约束，允许在新路径上自由搜索
                 task_status = TaskStatus::RUNNING;
+                path_finished_published = 0;  // 重置发布标志
                 need_reload_path = false;
                 LOG_INFO("#################### 开始执行新路径! ####################");
             }
@@ -409,6 +411,7 @@ void PL_ProcThread()
             y = 1.0;
             pl_speed = 0;
             task_status = TaskStatus::PATH_FINISHED;  // 单路径完成
+            path_finished_published = 0;  // 允许发布一次PATH_FINISHED
             LOG_INFO("设置任务状态为 PATH_FINISHED");
 
             // 短暂休眠，确保车辆停稳，同时检查退出标志

src/autonomy/pl/src/pl_node.cpp

@@ -105,6 +105,9 @@ class pl_node : public rclcpp::Node
 
         if (is_start == 0 && msg->task_status == 1)
         {
+            // 重置退出标志，允许新线程运行
+            thread_exit_flag = 0;
+            path_finished_published = 0;
             pthread_create(&pl_thread_t, NULL, pl_thread, NULL);
             LOG_INFO("started");
             is_start = 1;
@@ -239,6 +242,18 @@ class pl_node : public rclcpp::Node
             return;
         }
 
+        // PATH_FINISHED 只发布一次，避免重复发布导致 task_manager 误判
+        if (task_status == TaskStatus::PATH_FINISHED && path_finished_published == 1)
+        {
+            return;
+        }
+
+        // 标记 PATH_FINISHED 已发布
+        if (task_status == TaskStatus::PATH_FINISHED)
+        {
+            path_finished_published = 1;
+        }
+
         // 创建消息
         sweeper_interfaces::msg::Task message;
         message.task_status = task_status;