From 4198b785ad2089bf3a29a9a330028abef725b7ca Mon Sep 17 00:00:00 2001 From: Alvin-lyq Date: Mon, 27 Apr 2026 10:59:59 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A4=9A=E8=B7=AF=E5=BE=84fix=20bug=2002?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/autonomy/pl/include/pl/pl.hpp | 3 +- src/autonomy/pl/src/pl.cpp | 48 +++++++++++++------ .../include/task_manager/task_types.hpp | 3 +- .../task_manager/src/task_manager.cpp | 27 +++++------ .../task_manager/src/task_manager_main.cpp | 29 +++++++---- 5 files changed, 69 insertions(+), 41 deletions(-) diff --git a/src/autonomy/pl/include/pl/pl.hpp b/src/autonomy/pl/include/pl/pl.hpp index 61f151c..e0e4c41 100644 --- a/src/autonomy/pl/include/pl/pl.hpp +++ b/src/autonomy/pl/include/pl/pl.hpp @@ -15,8 +15,9 @@ enum TaskStatus { PENDING = 0, // 待执行 RUNNING = 1, // 执行中 - COMPLETED = 2, // 执行完成 + COMPLETED = 2, // 全部路径执行完成（任务结束） FAILED = 3, // 执行异常 + PATH_FINISHED = 4, // 单条路径执行完成（等待切换下一条路径） }; #pragma pack(push) diff --git a/src/autonomy/pl/src/pl.cpp b/src/autonomy/pl/src/pl.cpp index 16eb381..a029100 100644 --- a/src/autonomy/pl/src/pl.cpp +++ b/src/autonomy/pl/src/pl.cpp @@ -204,15 +204,16 @@ Direction straight_or_turn(double cur_direction, double des_direction, int thres // 加载路径文件的函数 bool load_path_file() { + LOG_INFO("开始加载路径文件: gps_load_now.txt"); memset(GPSPointQueue, 0, sizeof(GPSPointQueue)); FILE* fp_gps = fopen("gps_load_now.txt", "r"); if (NULL == fp_gps) { - LOG_ERROR("open route file error"); + LOG_ERROR("打开路径文件失败: gps_load_now.txt"); return false; } - LOG_INFO("have opened gps_load_now.txt"); + LOG_INFO("成功打开路径文件"); char mystring[200]; memset(mystring, '\0', 200); @@ -240,7 +241,7 @@ bool load_path_file() } GpsPointNum = i / 4; - LOG_INFO("point_num:%d", GpsPointNum); + LOG_INFO("路径文件加载完成，共 %d 个GPS点", GpsPointNum); fclose(fp_gps); return true; @@ -250,6 +251,7 @@ void PL_ProcThread() { usleep(70000); thread_exit_flag = 0; // 重置退出标志 + LOG_INFO("PL_ProcThread 线程启动"); // 用于检测路径文件更新的变量 struct stat file_stat; @@ -265,6 +267,11 @@ void PL_ProcThread() if (stat("gps_load_now.txt", &file_stat) == 0) { last_file_mtime = file_stat.st_mtime; + LOG_INFO("初始获取路径文件mtime: %ld", last_file_mtime); + } + else + { + LOG_WARN("无法获取路径文件状态，文件可能不存在"); } while (!thread_exit_flag) // 检查线程退出标志 @@ -276,23 +283,30 @@ void PL_ProcThread() { if (file_stat.st_mtime != last_file_mtime) { - LOG_INFO("检测到路径文件更新，重新加载..."); + LOG_INFO("检测到路径文件更新 (mtime: %ld -> %ld)，准备重新加载...", last_file_mtime, + file_stat.st_mtime); last_file_mtime = file_stat.st_mtime; need_reload_path = true; } } + else + { + LOG_WARN_THROTTLE(5000, "检查路径文件状态失败"); + } // 如果需要重新加载路径 if (need_reload_path) { + LOG_INFO("开始重新加载路径文件..."); if (load_path_file()) { LOG_INFO("路径文件重新加载成功，重置路径索引"); road_pos = 0; des_pos = 3; direction_pos = 20; + task_status = TaskStatus::RUNNING; need_reload_path = false; - LOG_INFO("#################### continue new path! ####################"); + LOG_INFO("#################### 开始执行新路径! ####################"); } else { @@ -333,12 +347,9 @@ void PL_ProcThread() double target_lat = GPSPointQueue[des_pos].LatitudeInfo; double target_head = GPSPointQueue[des_pos].Heading; - LOG_INFO_THROTTLE(1000, "当前位置: %d 经度: %.6f 纬度: %.6f 朝向: %.1f", road_pos, current_lon, current_lat, - current_head); - LOG_INFO_THROTTLE(1000, "目标位置: %d 经度: %.6f 纬度: %.6f 朝向: %.1f", des_pos, target_lon, target_lat, - target_head); - LOG_INFO_THROTTLE(1000, "总点数: %d，当前点数: %d，目标点: %d", GpsPointNum, road_pos, des_pos); - LOG_INFO_THROTTLE(1000, "############################################################"); + LOG_INFO_THROTTLE(1000, "循迹状态: 当前点=%d/%d, 目标点=%d", road_pos, GpsPointNum - 1, des_pos); + LOG_INFO_THROTTLE(1000, "当前位置: %.6f, %.6f, 朝向: %.1f", current_lon, current_lat, current_head); + LOG_INFO_THROTTLE(1000, "目标位置: %.6f, %.6f, 朝向: %.1f", target_lon, target_lat, target_head); // 计算当前定位点与目标点距离 double distance = ntzx_GPS_length(current_lon, current_lat, target_lon, target_lat); @@ -348,12 +359,15 @@ void PL_ProcThread() if (road_pos >= (GpsPointNum - 1)) { + LOG_INFO("到达路径终点: 当前点=%d, 总点数=%d", road_pos, GpsPointNum); x = 0.0; y = 1.0; pl_speed = 0; - task_status = TaskStatus::COMPLETED; + task_status = TaskStatus::PATH_FINISHED; // 单路径完成 + LOG_INFO("设置任务状态为 PATH_FINISHED"); // 短暂休眠，确保车辆停稳 + LOG_INFO("车辆停稳中..."); sleep(3); continue; } @@ -368,16 +382,20 @@ void PL_ProcThread() x = x_cm_tmp; y = y_cm_tmp; pl_speed = 800; - task_status = TaskStatus::RUNNING; + if (task_status != TaskStatus::RUNNING) + { + task_status = TaskStatus::RUNNING; + LOG_INFO("设置任务状态为 RUNNING"); + } } // 避免忙等待，增加延迟让 CPU 能够处理其他任务 auto now = std::chrono::steady_clock::now(); - if (now > next_time) - next_time = now; + if (now > next_time) next_time = now; std::this_thread::sleep_until(next_time); } + LOG_INFO("PL_ProcThread 线程退出"); return; } diff --git a/src/communication/task_manager/include/task_manager/task_types.hpp b/src/communication/task_manager/include/task_manager/task_types.hpp index d349621..c5af872 100644 --- a/src/communication/task_manager/include/task_manager/task_types.hpp +++ b/src/communication/task_manager/include/task_manager/task_types.hpp @@ -78,8 +78,9 @@ enum TaskStatus { PENDING = 0, // 待执行 RUNNING = 1, // 执行中 - COMPLETED = 2, // 执行完成 + COMPLETED = 2, // 全部路径执行完成（任务结束） FAILED = 3, // 执行异常 + PATH_FINISHED = 4, // 单条路径执行完成（等待切换下一条路径） }; // 当前任务信息 diff --git a/src/communication/task_manager/src/task_manager.cpp b/src/communication/task_manager/src/task_manager.cpp index 630eff0..c0354f9 100644 --- a/src/communication/task_manager/src/task_manager.cpp +++ b/src/communication/task_manager/src/task_manager.cpp @@ -75,9 +75,8 @@ int TaskManager::getCurrentTaskId() return current_task_.id; } -void TaskManager::updateCurrentTask(int taskId, TaskStatus status, const std::string& name, - int mode, int duration, int count, - const std::vector& routeInfos) +void TaskManager::updateCurrentTask(int taskId, TaskStatus status, const std::string& name, int mode, int duration, + int count, const std::vector& routeInfos) { std::lock_guard lock(task_mutex_); current_task_.id = taskId; @@ -92,8 +91,8 @@ void TaskManager::updateCurrentTask(int taskId, TaskStatus status, const std::st current_task_.currentRouteIndex = 0; // 默认从第一条路径开始 } current_task_.lastUpdateTime = std::chrono::steady_clock::now(); - LOG_INFO("Task updated: ID=%d, Status=%d, Name=%s, Mode=%d, Duration=%d, Routes=%zu", - taskId, status, name.c_str(), mode, duration, current_task_.routeInfos.size()); + LOG_INFO("Task updated: ID=%d, Status=%d, Name=%s, Mode=%d, Duration=%d, Routes=%zu", taskId, status, name.c_str(), + mode, duration, current_task_.routeInfos.size()); } const std::vector& TaskManager::getRouteInfos() @@ -120,13 +119,16 @@ bool TaskManager::switchToNextRoute() if (current_task_.currentRouteIndex < current_task_.routeInfos.size() - 1) { current_task_.currentRouteIndex++; - LOG_INFO("切换到下一条路径，索引: %d/%zu", current_task_.currentRouteIndex, - current_task_.routeInfos.size()); + LOG_INFO("路径切换成功: 从索引 %d 切换到 %d/%zu", current_task_.currentRouteIndex - 1, + current_task_.currentRouteIndex, current_task_.routeInfos.size()); + LOG_INFO("新路径信息: %s (%s)", current_task_.routeInfos[current_task_.currentRouteIndex].routeName.c_str(), + current_task_.routeInfos[current_task_.currentRouteIndex].fileName.c_str()); return true; } else { - LOG_INFO("已到达最后一条路径，任务完成"); + LOG_INFO("已到达最后一条路径（索引: %d/%zu），任务完成", current_task_.currentRouteIndex, + current_task_.routeInfos.size()); return false; } } @@ -259,10 +261,8 @@ bool TaskManager::processStartTask(const JSON_DATA& json_data, long seqNo) LOG_INFO("路径数量: %zu", json_data.value.routeInfos.size()); // 更新当前任务（保存完整信息） - updateCurrentTask(json_data.value.id, TaskStatus::RUNNING, - json_data.value.name, json_data.value.mode, - json_data.value.duration, json_data.value.count, - json_data.value.routeInfos); + updateCurrentTask(json_data.value.id, TaskStatus::RUNNING, json_data.value.name, json_data.value.mode, + json_data.value.duration, json_data.value.count, json_data.value.routeInfos); // 下载所有路径文件 bool downloadAllSuccess = true; @@ -272,8 +272,7 @@ bool TaskManager::processStartTask(const JSON_DATA& json_data, long seqNo) string downFileName = route.fileName; string filepath = "routes/" + downFileName; - LOG_INFO("处理路径 %zu/%zu: %s", i + 1, json_data.value.routeInfos.size(), - downFileName.c_str()); + LOG_INFO("处理路径 %zu/%zu: %s", i + 1, json_data.value.routeInfos.size(), downFileName.c_str()); // 检查文件是否已存在 if (access(filepath.c_str(), F_OK) == -1) diff --git a/src/communication/task_manager/src/task_manager_main.cpp b/src/communication/task_manager/src/task_manager_main.cpp index 3805e9e..333b053 100644 --- a/src/communication/task_manager/src/task_manager_main.cpp +++ b/src/communication/task_manager/src/task_manager_main.cpp @@ -562,23 +562,27 @@ class TaskManagerNode : public rclcpp::Node void task_callback(const sweeper_interfaces::msg::Task::SharedPtr msg) { - if (msg->task_status >= TaskStatus::PENDING && msg->task_status <= TaskStatus::FAILED) + LOG_INFO_THROTTLE(1000, "收到任务状态更新: ID=%d, Status=%d, Name=%s", msg->task_id, msg->task_status, + msg->task_name.c_str()); + + if (msg->task_status >= TaskStatus::PENDING && msg->task_status <= TaskStatus::PATH_FINISHED) { status_up = static_cast(msg->task_status); // 如果当前路径已完成，检查是否有下一条路径 - if (status_up == TaskStatus::COMPLETED) + if (status_up == TaskStatus::PATH_FINISHED) { + LOG_INFO("当前路径执行完成，检查下一条路径..."); if (task_manager.hasNextRoute()) { - LOG_INFO("当前路径完成，切换到下一条..."); + LOG_INFO("发现下一条路径，准备切换..."); // 切换到下一条路径 if (task_manager.switchToNextRoute()) { // 获取新路径并复制 ROUTE_INFO nextRoute = task_manager.getCurrentRouteInfo(); string filepath = "routes/" + nextRoute.fileName; - LOG_INFO("加载新路径: %s (%s)", nextRoute.routeName.c_str(), nextRoute.fileName.c_str()); + LOG_INFO("切换到新路径: %s (%s)", nextRoute.routeName.c_str(), nextRoute.fileName.c_str()); // 复制文件到 gps_load_now.txt std::ifstream src(filepath, std::ios::binary); @@ -586,37 +590,42 @@ class TaskManagerNode : public rclcpp::Node if (src && dst) { dst << src.rdbuf(); - LOG_INFO("路径文件复制成功"); + LOG_INFO("路径文件复制成功: %s -> gps_load_now.txt", nextRoute.fileName.c_str()); // 同时更新两个状态，保持任务继续运行 task_manager.setTaskStatus(1); task_manager.updateCurrentTaskStatus(TaskStatus::RUNNING); - // 定时器会自动发布任务消息，PL 层会重新加载路径 + LOG_INFO("任务状态已更新为 RUNNING，等待PL层重载新路径"); } else { - LOG_ERROR("无法复制路径文件"); + LOG_ERROR("无法复制路径文件: %s", filepath.c_str()); } } + else + { + LOG_ERROR("路径切换失败"); + } } else { // 最后一条路径完成，更新任务管理器内部状态为 COMPLETED - LOG_INFO("已完成全部路径，任务结束"); + LOG_INFO("所有 %d 条路径执行完成，任务结束", task_manager.getRouteCount()); task_manager.setTaskStatus(TaskStatus::COMPLETED); task_manager.updateCurrentTaskStatus(TaskStatus::COMPLETED); } } else { - // 如果不是 COMPLETED 状态，直接同步更新任务管理器的状态 + // 如果不是 PATH_FINISHED 状态，直接同步更新任务管理器的状态 + LOG_INFO_THROTTLE(5000, "更新任务状态: %d", msg->task_status); task_manager.setTaskStatus(msg->task_status); task_manager.updateCurrentTaskStatus(status_up); } } else { - LOG_WARN("Invalid task status: %d", msg->task_status); + LOG_WARN("收到无效的任务状态: %d", msg->task_status); } }