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

多路径任务

Browse Source
This commit is contained in:
Alvin-lyq 2026-04-23 16:27:08 +08:00
parent 18b2da5450
commit fa779cb512
6 changed files with 322 additions and 88 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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));
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! ####################");

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

@ -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);
}

19
src/communication/task_manager/include/task_manager/task_manager.hpp
View File

@ -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();

21
src/communication/task_manager/include/task_manager/task_types.hpp
View File

@ -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<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

133
src/communication/task_manager/src/task_manager.cpp
View File

@ -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",
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<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("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");

93
src/communication/task_manager/src/task_manager_main.cpp
View File

@ -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<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
{