lyq/sweeper_200
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

pl debug

Browse Source
This commit is contained in:
Alvin-lyq 2026-05-07 16:01:25 +08:00
parent 3a4c1191c0
commit d963cff057
3 changed files with 19 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/autonomy/pl/include/pl/pl.hpp
View File

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

3
src/autonomy/pl/src/pl.cpp
View File

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

15
src/autonomy/pl/src/pl_node.cpp
View File

@ -105,6 +105,9 @@ class pl_node : public rclcpp::Node
if (is_start == 0 && msg->task_status == 1) 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); pthread_create(&pl_thread_t, NULL, pl_thread, NULL);
LOG_INFO("started"); LOG_INFO("started");
is_start = 1; is_start = 1;
@ -239,6 +242,18 @@ class pl_node : public rclcpp::Node
return; 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; sweeper_interfaces::msg::Task message;
message.task_status = task_status; message.task_status = task_status;