diff --git a/src/autonomy/pl/include/pl/pl.hpp b/src/autonomy/pl/include/pl/pl.hpp index 246e35b..c501670 100644 --- a/src/autonomy/pl/include/pl/pl.hpp +++ b/src/autonomy/pl/include/pl/pl.hpp @@ -73,5 +73,6 @@ extern int d_file; extern Direction drive_mode; extern std::atomic task_status; extern std::atomic thread_exit_flag; // 线程退出标志 +extern std::atomic path_finished_published; // PATH_FINISHED是否已发布标志 #endif \ No newline at end of file diff --git a/src/autonomy/pl/src/pl.cpp b/src/autonomy/pl/src/pl.cpp index 67851e8..605121b 100644 --- a/src/autonomy/pl/src/pl.cpp +++ b/src/autonomy/pl/src/pl.cpp @@ -41,6 +41,7 @@ int last_mode = 0; Direction drive_mode = Direction::STRAIGHT_D; std::atomic task_status{TaskStatus::PENDING}; std::atomic thread_exit_flag{0}; // 线程退出标志,0=继续运行,1=需要退出 +std::atomic 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"); // 短暂休眠,确保车辆停稳,同时检查退出标志 diff --git a/src/autonomy/pl/src/pl_node.cpp b/src/autonomy/pl/src/pl_node.cpp index 654dd80..ae3dad7 100644 --- a/src/autonomy/pl/src/pl_node.cpp +++ b/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;