diff --git a/src/autonomy/pl/src/pl.cpp b/src/autonomy/pl/src/pl.cpp index 40a7ea6..3f2a7ac 100644 --- a/src/autonomy/pl/src/pl.cpp +++ b/src/autonomy/pl/src/pl.cpp @@ -2,6 +2,7 @@ #include #include +#include #include #include @@ -200,20 +201,106 @@ Direction straight_or_turn(double cur_direction, double des_direction, int thres } } +// 加载路径文件的函数 +bool load_path_file() +{ + memset(GPSPointQueue, 0, sizeof(GPSPointQueue)); + + FILE* fp_gps = fopen("gps_load_now.txt", "r"); + if (NULL == fp_gps) + { + LOG_ERROR("open route file error"); + return false; + } + LOG_INFO("have opened gps_load_now.txt"); + + char mystring[200]; + memset(mystring, '\0', 200); + int point_num = 0; + int i = 0; + while (fgets(mystring, 200, fp_gps)) + { + i++; + + string line_string(mystring); + point_num = i / 4; + + if (i % 4 == 1) + { + GPSPointQueue[point_num].LatitudeInfo = atof(line_string.c_str()); + } + else if (i % 4 == 2) + { + GPSPointQueue[point_num].LongitudeInfo = atof(line_string.c_str()); + } + else if (i % 4 == 3) + { + GPSPointQueue[point_num].Heading = atof(line_string.c_str()); + } + } + + GpsPointNum = i / 4; + LOG_INFO("point_num:%d", GpsPointNum); + fclose(fp_gps); + + return true; +} + void PL_ProcThread() { usleep(70000); thread_exit_flag = 0; // 重置退出标志 + // 用于检测路径文件更新的变量 + struct stat file_stat; + static time_t last_file_mtime = 0; + bool need_reload_path = false; + int road_pos = 0; int des_pos = 3; int direction_pos = 20; auto next_time = std::chrono::steady_clock::now(); + // 初始化时获取文件时间 + if (stat("gps_load_now.txt", &file_stat) == 0) + { + last_file_mtime = file_stat.st_mtime; + } + while (!thread_exit_flag) // 检查线程退出标志 { next_time += std::chrono::milliseconds(50); + // 检查路径文件是否更新 + if (stat("gps_load_now.txt", &file_stat) == 0) + { + if (file_stat.st_mtime != last_file_mtime) + { + LOG_INFO("检测到路径文件更新，重新加载..."); + last_file_mtime = file_stat.st_mtime; + need_reload_path = true; + } + } + + // 如果需要重新加载路径 + if (need_reload_path) + { + if (load_path_file()) + { + LOG_INFO("路径文件重新加载成功，重置路径索引"); + road_pos = 0; + des_pos = 3; + direction_pos = 20; + need_reload_path = false; + LOG_INFO("#################### continue new path! ####################"); + } + else + { + LOG_ERROR("路径文件重新加载失败"); + need_reload_path = false; + } + } + g_NavInfo.Latitude_degree = g_rtk.lat; g_NavInfo.Longitude_degree = g_rtk.lon; g_NavInfo.Yaw_rad = g_rtk.direction; @@ -261,6 +348,8 @@ void PL_ProcThread() y = 1.0; pl_speed = 0; task_status = TaskStatus::COMPLETED; + + // 短暂休眠，确保车辆停稳 sleep(3); continue; } @@ -295,44 +384,10 @@ void* pl_thread(void* args) (void)args; sleep(3); - memset(GPSPointQueue, 0, sizeof(GPSPointQueue)); - - FILE* fp_gps = fopen("gps_load_now.txt", "r"); - if (NULL == fp_gps) + if (!load_path_file()) { - LOG_ERROR("open route file error"); return NULL; } - LOG_INFO("have opened gps_load_now.txt"); - - char mystring[200]; - memset(mystring, '\0', 200); - int point_num = 0; - int i = 0; - while (fgets(mystring, 200, fp_gps)) - { - i++; - - string line_string(mystring); - point_num = i / 4; - - if (i % 4 == 1) - { - GPSPointQueue[point_num].LatitudeInfo = atof(line_string.c_str()); - } - else if (i % 4 == 2) - { - GPSPointQueue[point_num].LongitudeInfo = atof(line_string.c_str()); - } - else if (i % 4 == 3) - { - GPSPointQueue[point_num].Heading = atof(line_string.c_str()); - } - } - - GpsPointNum = i / 4; - LOG_INFO("point_num:%d", GpsPointNum); - fclose(fp_gps); LOG_INFO("#################### auto drive on! ####################"); diff --git a/src/autonomy/pl/src/pl_node.cpp b/src/autonomy/pl/src/pl_node.cpp index 6ee1327..d82ba01 100644 --- a/src/autonomy/pl/src/pl_node.cpp +++ b/src/autonomy/pl/src/pl_node.cpp @@ -1,6 +1,7 @@ #include #include #include +#include #include #include // 新增：用于时间戳处理 @@ -97,9 +98,9 @@ class pl_node : public rclcpp::Node current_route_name = msg->route_name; current_file_name = msg->file_name; - LOG_INFO("任务信息更新：ID=%d, 名称=%s, 模式=%d, 时长=%d", + LOG_INFO("任务信息更新：ID=%d, 名称=%s, 模式=%d, 时长=%d, 状态=%d", current_task_id, current_task_name.c_str(), - current_task_mode, current_task_duration); + current_task_mode, current_task_duration, msg->task_status); if (is_start == 0 && msg->task_status == 1) { @@ -120,6 +121,20 @@ class pl_node : public rclcpp::Node is_start = 0; task_status = TaskStatus::COMPLETED; } + else if (is_start == 1 && msg->task_status == 1 && task_status == TaskStatus::COMPLETED) + { + // 任务重新运行，重新启动线程（这样会重新加载路径文件） + LOG_INFO("检测到任务从 COMPLETED 回到 RUNNING，重新加载路径..."); + + // 等待线程正常退出 + thread_exit_flag = 1; + pthread_join(pl_thread_t, NULL); + + // 重新启动线程（会重新加载 gps_load_now.txt） + pthread_create(&pl_thread_t, NULL, pl_thread, NULL); + LOG_INFO("路径重新加载成功，继续运行"); + task_status = TaskStatus::RUNNING; + } // LOG_INFO(" ==================================== is_start : %d", is_start); // LOG_INFO(" ==================================== task_status : %d", msg->task_status); } diff --git a/src/communication/task_manager/include/task_manager/task_manager.hpp b/src/communication/task_manager/include/task_manager/task_manager.hpp index 1e44787..c3fb6c5 100644 --- a/src/communication/task_manager/include/task_manager/task_manager.hpp +++ b/src/communication/task_manager/include/task_manager/task_manager.hpp @@ -37,7 +37,7 @@ class TaskManager // 更新当前任务（完整信息） void updateCurrentTask(int taskId, TaskStatus status, const std::string& name = "", int mode = 0, int duration = 0, int count = 0, - const ROUTE_INFO& routeInfo = ROUTE_INFO()); + const std::vector& routeInfos = std::vector()); // 获取当前任务名称 std::string getCurrentTaskName(); @@ -51,9 +51,24 @@ class TaskManager // 获取当前任务次数 int getCurrentTaskCount(); - // 获取当前路径信息 + // 获取所有路径信息 + const std::vector& getRouteInfos(); + + // 获取当前路径索引 + int getCurrentRouteIndex(); + + // 获取当前正在执行的路径信息 ROUTE_INFO getCurrentRouteInfo(); + // 获取路径总数 + int getRouteCount(); + + // 切换到下一条路径 + bool switchToNextRoute(); + + // 判断是否还有下一条路径 + bool hasNextRoute(); + // 获取任务队列大小 int getQueueSize(); 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 73f590b..d349621 100644 --- a/src/communication/task_manager/include/task_manager/task_types.hpp +++ b/src/communication/task_manager/include/task_manager/task_types.hpp @@ -31,12 +31,14 @@ using namespace std; "name": "任务11", //任务名称 "mode": 0, //清扫模式 "count": 1, //清扫次数 - "routeInfo": { //路径信息 - "routeName": "测试路径1", //路径名称 - "fileName": "gps_load_1736931111771.txt", //文件名称 - "url": "http://192.168.4.117:9600/file/gps_load_1736931111771.txt", //路径地址 - "md5": "93c635ea6251f650dfcf0f9a8a72351c" //md5值 - } + "routeInfos": [ //路径信息（支持多条路径） + { + "routeName": "测试路径1", //路径名称 + "fileName": "gps_load_1736931111771.txt", //文件名称 + "url": "http://192.168.4.117:9600/file/gps_load_1736931111771.txt", //路径地址 + "md5": "93c635ea6251f650dfcf0f9a8a72351c" //md5值 + } + ] } } }*/ @@ -56,7 +58,7 @@ struct TASK_VALUE int mode; // 清扫模式: 0=标准模式(刷子), 1=混合模式(刷子+洒水) int duration; // 作业时长，单位：分钟 int count; // 清扫次数（保留兼容） - ROUTE_INFO routeInfo; + std::vector routeInfos; // 支持多条路径（路径数组） }; struct JSON_DATA { @@ -89,10 +91,11 @@ struct CurrentTask int mode; // 清扫模式: 0=标准模式(刷子), 1=混合模式(刷子+洒水) int duration; // 作业时长，单位：分钟 int count; // 清扫次数 - ROUTE_INFO routeInfo; // 路径信息 + std::vector routeInfos; // 所有路径列表 + int currentRouteIndex; // 当前正在执行的路径索引 std::chrono::time_point lastUpdateTime; - CurrentTask() : id(0), status(PENDING), mode(0), duration(0), count(0) {} + CurrentTask() : id(0), status(PENDING), mode(0), duration(0), count(0), currentRouteIndex(0) {} }; #endif \ No newline at end of file diff --git a/src/communication/task_manager/src/task_manager.cpp b/src/communication/task_manager/src/task_manager.cpp index cf3db20..9a7b818 100644 --- a/src/communication/task_manager/src/task_manager.cpp +++ b/src/communication/task_manager/src/task_manager.cpp @@ -77,7 +77,7 @@ int TaskManager::getCurrentTaskId() void TaskManager::updateCurrentTask(int taskId, TaskStatus status, const std::string& name, int mode, int duration, int count, - const ROUTE_INFO& routeInfo) + const std::vector& routeInfos) { std::lock_guard lock(task_mutex_); current_task_.id = taskId; @@ -86,10 +86,65 @@ void TaskManager::updateCurrentTask(int taskId, TaskStatus status, const std::st if (mode >= 0) current_task_.mode = mode; if (duration >= 0) current_task_.duration = duration; if (count >= 0) current_task_.count = count; - if (!routeInfo.fileName.empty()) current_task_.routeInfo = routeInfo; + if (!routeInfos.empty()) + { + current_task_.routeInfos = routeInfos; + 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", - taskId, status, name.c_str(), mode, duration); + 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() +{ + std::lock_guard lock(task_mutex_); + return current_task_.routeInfos; +} + +int TaskManager::getCurrentRouteIndex() +{ + std::lock_guard lock(task_mutex_); + return current_task_.currentRouteIndex; +} + +int TaskManager::getRouteCount() +{ + std::lock_guard lock(task_mutex_); + return current_task_.routeInfos.size(); +} + +bool TaskManager::switchToNextRoute() +{ + std::lock_guard lock(task_mutex_); + if (current_task_.currentRouteIndex < current_task_.routeInfos.size() - 1) + { + current_task_.currentRouteIndex++; + LOG_INFO("切换到下一条路径，索引: %d/%zu", current_task_.currentRouteIndex, + current_task_.routeInfos.size()); + return true; + } + else + { + LOG_INFO("已到达最后一条路径，任务完成"); + return false; + } +} + +bool TaskManager::hasNextRoute() +{ + std::lock_guard lock(task_mutex_); + return current_task_.currentRouteIndex < current_task_.routeInfos.size() - 1; +} + +ROUTE_INFO TaskManager::getCurrentRouteInfo() +{ + std::lock_guard lock(task_mutex_); + if (current_task_.currentRouteIndex < current_task_.routeInfos.size()) + { + return current_task_.routeInfos[current_task_.currentRouteIndex]; + } + return ROUTE_INFO(); } std::string TaskManager::getCurrentTaskName() @@ -116,12 +171,6 @@ int TaskManager::getCurrentTaskCount() return current_task_.count; } -ROUTE_INFO TaskManager::getCurrentRouteInfo() -{ - std::lock_guard lock(task_mutex_); - return current_task_.routeInfo; -} - string TaskManager::calculate_md5(const string& filename) { MD5 md5; @@ -200,48 +249,68 @@ bool TaskManager::processStartTask(const JSON_DATA& json_data, long seqNo) LOG_INFO("Mode: %d", json_data.value.mode); LOG_INFO("Duration: %d minutes", json_data.value.duration); LOG_INFO("Count: %d", json_data.value.count); - LOG_INFO("URL: %s", json_data.value.routeInfo.url.c_str()); - LOG_INFO("MD5: %s", json_data.value.routeInfo.md5.c_str()); + 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.routeInfo); + json_data.value.routeInfos); - string downFileName = json_data.value.routeInfo.fileName; - string filepath = "routes/" + downFileName; - - // 检查文件是否已存在 - if (access(filepath.c_str(), F_OK) == -1) + // 下载所有路径文件 + bool downloadAllSuccess = true; + for (size_t i = 0; i < json_data.value.routeInfos.size(); ++i) { - LOG_INFO("File not found, downloading: %s", downFileName.c_str()); - if (!downloadFile(json_data.value.routeInfo.url, json_data.value.routeInfo.md5, downFileName)) + const ROUTE_INFO& route = json_data.value.routeInfos[i]; + string downFileName = route.fileName; + string filepath = "routes/" + downFileName; + + LOG_INFO("处理路径 %zu/%zu: %s", i + 1, json_data.value.routeInfos.size(), + downFileName.c_str()); + + // 检查文件是否已存在 + if (access(filepath.c_str(), F_OK) == -1) { - LOG_WARN("Download failed, skipping task."); - updateCurrentTask(json_data.value.id, TaskStatus::FAILED); - if (send_response_callback_) + LOG_INFO("文件不存在，开始下载: %s", downFileName.c_str()); + if (!downloadFile(route.url, route.md5, downFileName)) { - // 发送响应到状态主题（而不是task_response） - send_response_callback_("task_response", seqNo, 500, "Download failed"); + LOG_ERROR("路径 %zu 下载失败: %s", i + 1, downFileName.c_str()); + downloadAllSuccess = false; + break; } - return false; + } + else + { + LOG_INFO("文件已存在，跳过下载: %s", filepath.c_str()); } } - else + + if (!downloadAllSuccess) { - LOG_INFO("File already exists, skipping download: %s", filepath.c_str()); + LOG_WARN("任务下载失败，跳过执行"); + updateCurrentTask(json_data.value.id, TaskStatus::FAILED); + if (send_response_callback_) + { + send_response_callback_("task_response", seqNo, 500, "Download failed"); + } + return false; } - // 复制文件 - copyFileAndOverwrite(filepath, destination_file_path_); + // 处理第一条路径 + if (!json_data.value.routeInfos.empty()) + { + const ROUTE_INFO& firstRoute = json_data.value.routeInfos[0]; + string filepath = "routes/" + firstRoute.fileName; + copyFileAndOverwrite(filepath, destination_file_path_); + LOG_INFO("已加载第一条路径: %s", firstRoute.routeName.c_str()); + } // 启动任务 task_status_ = 1; updateCurrentTask(json_data.value.id, TaskStatus::RUNNING); - LOG_INFO("Task started successfully: %d", json_data.value.id); + LOG_INFO("任务启动成功: %d，路径数: %zu", json_data.value.id, json_data.value.routeInfos.size()); - // 发送响应（响应主题由调用方决定） + // 发送响应 if (send_response_callback_) { send_response_callback_("task_response", seqNo, 200, "Task started successfully"); diff --git a/src/communication/task_manager/src/task_manager_main.cpp b/src/communication/task_manager/src/task_manager_main.cpp index feca6ee..dd21072 100644 --- a/src/communication/task_manager/src/task_manager_main.cpp +++ b/src/communication/task_manager/src/task_manager_main.cpp @@ -107,7 +107,7 @@ ROUTE_INFO get_route_info(const Json::Value& root) } else { - LOG_ERROR("routeInfo is not a valid JSON object"); + LOG_ERROR("routeInfos is not a valid JSON object"); } return route_info; @@ -151,10 +151,49 @@ TASK_VALUE get_task_value(const Json::Value& root) else LOG_WARN("Missing 'count' field, using default 0"); - if (root.isMember("routeInfo") && root["routeInfo"].isObject()) - task_value.routeInfo = get_route_info(root["routeInfo"]); + // 支持多条路径：先尝试解析 routeInfos（复数，新协议），如果没有找到，再尝试 routeInfo（单数，旧协议） + if (root.isMember("routeInfos")) + { + if (root["routeInfos"].isArray()) + { + const Json::Value& routeArray = root["routeInfos"]; + for (Json::ArrayIndex i = 0; i < routeArray.size(); ++i) + { + ROUTE_INFO route = get_route_info(routeArray[i]); + task_value.routeInfos.push_back(route); + } + LOG_INFO("解析到 %zu 条路径", task_value.routeInfos.size()); + } + else if (root["routeInfos"].isObject()) + { + // 兼容写法：单一路径 + ROUTE_INFO route = get_route_info(root["routeInfos"]); + task_value.routeInfos.push_back(route); + LOG_INFO("兼容旧协议：解析到 1 条路径"); + } + else + { + LOG_ERROR("'routeInfos' field is neither array nor object"); + } + } + else if (root.isMember("routeInfo")) + { + // 兼容旧协议：单数形式 routeInfo + if (root["routeInfo"].isObject()) + { + ROUTE_INFO route = get_route_info(root["routeInfo"]); + task_value.routeInfos.push_back(route); + LOG_INFO("兼容旧协议（routeInfo 单数）：解析到 1 条路径"); + } + else + { + LOG_ERROR("'routeInfo' field is not an object"); + } + } else - LOG_ERROR("Missing or invalid 'routeInfo' field"); + { + LOG_ERROR("Missing 'routeInfos' or 'routeInfo' field"); + } } else { @@ -507,10 +546,13 @@ class TaskManagerNode : public rclcpp::Node if (msg.task_status != last_status || msg.task_id > 0) { msg_publisher_->publish(msg); - if (msg.task_id > 0) { - LOG_INFO("Task published: ID=%d, Status=%d, Name=%s, Mode=%d, Duration=%d", - msg.task_id, msg.task_status, msg.task_name.c_str(), msg.mode, msg.duration); - } else { + if (msg.task_id > 0) + { + LOG_INFO("Task published: ID=%d, Status=%d, Name=%s, Mode=%d, Duration=%d", msg.task_id, + msg.task_status, msg.task_name.c_str(), msg.mode, msg.duration); + } + else + { LOG_INFO("Task status published: %d", msg.task_status); } } @@ -523,6 +565,41 @@ class TaskManagerNode : public rclcpp::Node if (msg->task_status >= TaskStatus::PENDING && msg->task_status <= TaskStatus::FAILED) { status_up = static_cast(msg->task_status); + + // 如果当前路径已完成，检查是否有下一条路径 + if (status_up == TaskStatus::COMPLETED) + { + if (task_manager.hasNextRoute()) + { + 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()); + + // 复制文件到 gps_load_now.txt + std::ifstream src(filepath, std::ios::binary); + std::ofstream dst("gps_load_now.txt", std::ios::binary); + if (src && dst) + { + dst << src.rdbuf(); + LOG_INFO("路径文件复制成功"); + + // 重新设置状态为 RUNNING，让 PL 层继续工作 + task_manager.setTaskStatus(1); + // 定时器会自动发布任务消息，PL 层会收到 task_status=1 并重新加载路径 + } + else + { + LOG_ERROR("无法复制路径文件"); + } + } + } + // 如果没有下一条路径，保持 COMPLETED 状态 + } } else {