多路径任务

src/autonomy/pl/src/pl.cpp

@@ -2,6 +2,7 @@
 
 #include <math.h>
 #include <string.h>
+#include <sys/stat.h>
 #include <unistd.h>
 
 #include <iostream>
@@ -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));
+    if (!load_path_file())
-
-    FILE* fp_gps = fopen("gps_load_now.txt", "r");
-    if (NULL == fp_gps)
     {
-        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! ####################");
 

src/autonomy/pl/src/pl_node.cpp

@@ -1,6 +1,7 @@
 #include <arpa/inet.h>
 #include <pthread.h>
 #include <sys/socket.h>
+#include <sys/stat.h>
 #include <unistd.h>
 
 #include <chrono>  // 新增：用于时间戳处理
@@ -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);
     }

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<ROUTE_INFO>& routeInfos = std::vector<ROUTE_INFO>());
 
     // 获取当前任务名称
     std::string getCurrentTaskName();
@@ -51,9 +51,24 @@ class TaskManager
     // 获取当前任务次数
     int getCurrentTaskCount();
 
-    // 获取当前路径信息
+    // 获取所有路径信息
+    const std::vector<ROUTE_INFO>& getRouteInfos();
+
+    // 获取当前路径索引
+    int getCurrentRouteIndex();
+
+    // 获取当前正在执行的路径信息
     ROUTE_INFO getCurrentRouteInfo();
 
+    // 获取路径总数
+    int getRouteCount();
+
+    // 切换到下一条路径
+    bool switchToNextRoute();
+
+    // 判断是否还有下一条路径
+    bool hasNextRoute();
+
     // 获取任务队列大小
     int getQueueSize();
 

src/communication/task_manager/include/task_manager/task_types.hpp

@@ -31,12 +31,14 @@ using namespace std;
             "name": "任务11", //任务名称
             "mode": 0, //清扫模式
             "count": 1, //清扫次数
-            "routeInfo": { //路径信息
+            "routeInfos": [ //路径信息（支持多条路径）
-                "routeName": "测试路径1", //路径名称
+                {
-                "fileName": "gps_load_1736931111771.txt", //文件名称
+                    "routeName": "测试路径1", //路径名称
-                "url": "http://192.168.4.117:9600/file/gps_load_1736931111771.txt", //路径地址
+                    "fileName": "gps_load_1736931111771.txt", //文件名称
-                "md5": "93c635ea6251f650dfcf0f9a8a72351c" //md5值
+                    "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<ROUTE_INFO> routeInfos;  // 支持多条路径（路径数组）
 };
 struct JSON_DATA
 {
@@ -89,10 +91,11 @@ struct CurrentTask
     int mode;              // 清扫模式: 0=标准模式(刷子), 1=混合模式(刷子+洒水)
     int duration;          // 作业时长，单位：分钟
     int count;             // 清扫次数
-    ROUTE_INFO routeInfo;  // 路径信息
+    std::vector<ROUTE_INFO> routeInfos;  // 所有路径列表
+    int currentRouteIndex;               // 当前正在执行的路径索引
     std::chrono::time_point<std::chrono::steady_clock> 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

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<ROUTE_INFO>& routeInfos)
 {
     std::lock_guard<std::mutex> 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",
+    LOG_INFO("Task updated: ID=%d, Status=%d, Name=%s, Mode=%d, Duration=%d, Routes=%zu",
-             taskId, status, name.c_str(), mode, duration);
+             taskId, status, name.c_str(), mode, duration, current_task_.routeInfos.size());
+}
+
+const std::vector<ROUTE_INFO>& TaskManager::getRouteInfos()
+{
+    std::lock_guard<std::mutex> lock(task_mutex_);
+    return current_task_.routeInfos;
+}
+
+int TaskManager::getCurrentRouteIndex()
+{
+    std::lock_guard<std::mutex> lock(task_mutex_);
+    return current_task_.currentRouteIndex;
+}
+
+int TaskManager::getRouteCount()
+{
+    std::lock_guard<std::mutex> lock(task_mutex_);
+    return current_task_.routeInfos.size();
+}
+
+bool TaskManager::switchToNextRoute()
+{
+    std::lock_guard<std::mutex> 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<std::mutex> lock(task_mutex_);
+    return current_task_.currentRouteIndex < current_task_.routeInfos.size() - 1;
+}
+
+ROUTE_INFO TaskManager::getCurrentRouteInfo()
+{
+    std::lock_guard<std::mutex> 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<std::mutex> 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("路径数量: %zu", json_data.value.routeInfos.size());
-    LOG_INFO("MD5: %s", json_data.value.routeInfo.md5.c_str());
 
     // 更新当前任务（保存完整信息）
     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;
+    bool downloadAllSuccess = true;
-
+    for (size_t i = 0; i < json_data.value.routeInfos.size(); ++i)
-    // 检查文件是否已存在
-    if (access(filepath.c_str(), F_OK) == -1)
     {
-        LOG_INFO("File not found, downloading: %s", downFileName.c_str());
+        const ROUTE_INFO& route = json_data.value.routeInfos[i];
-        if (!downloadFile(json_data.value.routeInfo.url, json_data.value.routeInfo.md5, downFileName))
+        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.");
+            LOG_INFO("文件不存在，开始下载: %s", downFileName.c_str());
-            updateCurrentTask(json_data.value.id, TaskStatus::FAILED);
+            if (!downloadFile(route.url, route.md5, downFileName))
-            if (send_response_callback_)
             {
-                // 发送响应到状态主题（而不是task_response）
+                LOG_ERROR("路径 %zu 下载失败: %s", i + 1, downFileName.c_str());
-                send_response_callback_("task_response", seqNo, 500, "Download failed");
+                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");

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())
+        // 支持多条路径：先尝试解析 routeInfos（复数，新协议），如果没有找到，再尝试 routeInfo（单数，旧协议）
-            task_value.routeInfo = get_route_info(root["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) {
+            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);
+                LOG_INFO("Task published: ID=%d, Status=%d, Name=%s, Mode=%d, Duration=%d", msg.task_id,
-            } else {
+                         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<TaskStatus>(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
         {