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

多路径fix bug 02

Browse Source
This commit is contained in:
Alvin-lyq 2026-04-27 10:59:59 +08:00
parent 75394b1226
commit 4198b785ad
5 changed files with 69 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -15,8 +15,9 @@ enum TaskStatus
{ {
PENDING = 0, // 待执行 PENDING = 0, // 待执行
RUNNING = 1, // 执行中 RUNNING = 1, // 执行中
COMPLETED = 2, // 执行完成 COMPLETED = 2, // 全部路径执行完成（任务结束）
FAILED = 3, // 执行异常 FAILED = 3, // 执行异常
PATH_FINISHED = 4, // 单条路径执行完成（等待切换下一条路径）
}; };
#pragma pack(push) #pragma pack(push)

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

@ -204,15 +204,16 @@ Direction straight_or_turn(double cur_direction, double des_direction, int thres
// 加载路径文件的函数 // 加载路径文件的函数
bool load_path_file() bool load_path_file()
{ {
LOG_INFO("开始加载路径文件: gps_load_now.txt");
memset(GPSPointQueue, 0, sizeof(GPSPointQueue)); memset(GPSPointQueue, 0, sizeof(GPSPointQueue));
FILE* fp_gps = fopen("gps_load_now.txt", "r"); FILE* fp_gps = fopen("gps_load_now.txt", "r");
if (NULL == fp_gps) if (NULL == fp_gps)
{ {
LOG_ERROR("open route file error"); LOG_ERROR("打开路径文件失败: gps_load_now.txt");
return false; return false;
} }
LOG_INFO("have opened gps_load_now.txt"); LOG_INFO("成功打开路径文件");
char mystring[200]; char mystring[200];
memset(mystring, '\0', 200); memset(mystring, '\0', 200);
@ -240,7 +241,7 @@ bool load_path_file()
} }
GpsPointNum = i / 4; GpsPointNum = i / 4;
LOG_INFO("point_num:%d", GpsPointNum); LOG_INFO("路径文件加载完成，共 %d 个GPS点", GpsPointNum);
fclose(fp_gps); fclose(fp_gps);
return true; return true;
@ -250,6 +251,7 @@ void PL_ProcThread()
{ {
usleep(70000); usleep(70000);
thread_exit_flag = 0; // 重置退出标志 thread_exit_flag = 0; // 重置退出标志
LOG_INFO("PL_ProcThread 线程启动");
// 用于检测路径文件更新的变量 // 用于检测路径文件更新的变量
struct stat file_stat; struct stat file_stat;
@ -265,6 +267,11 @@ void PL_ProcThread()
if (stat("gps_load_now.txt", &file_stat) == 0) if (stat("gps_load_now.txt", &file_stat) == 0)
{ {
last_file_mtime = file_stat.st_mtime; last_file_mtime = file_stat.st_mtime;
LOG_INFO("初始获取路径文件mtime: %ld", last_file_mtime);
}
else
{
LOG_WARN("无法获取路径文件状态，文件可能不存在");
} }
while (!thread_exit_flag) // 检查线程退出标志 while (!thread_exit_flag) // 检查线程退出标志
@ -276,23 +283,30 @@ void PL_ProcThread()
{ {
if (file_stat.st_mtime != last_file_mtime) 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; last_file_mtime = file_stat.st_mtime;
need_reload_path = true; need_reload_path = true;
} }
} }
else
{
LOG_WARN_THROTTLE(5000, "检查路径文件状态失败");
}
// 如果需要重新加载路径 // 如果需要重新加载路径
if (need_reload_path) if (need_reload_path)
{ {
LOG_INFO("开始重新加载路径文件...");
if (load_path_file()) if (load_path_file())
{ {
LOG_INFO("路径文件重新加载成功，重置路径索引"); LOG_INFO("路径文件重新加载成功，重置路径索引");
road_pos = 0; road_pos = 0;
des_pos = 3; des_pos = 3;
direction_pos = 20; direction_pos = 20;
task_status = TaskStatus::RUNNING;
need_reload_path = false; need_reload_path = false;
LOG_INFO("#################### continue new path! ####################"); LOG_INFO("#################### 开始执行新路径! ####################");
} }
else else
{ {
@ -333,12 +347,9 @@ void PL_ProcThread()
double target_lat = GPSPointQueue[des_pos].LatitudeInfo; double target_lat = GPSPointQueue[des_pos].LatitudeInfo;
double target_head = GPSPointQueue[des_pos].Heading; double target_head = GPSPointQueue[des_pos].Heading;
LOG_INFO_THROTTLE(1000, "当前位置: %d 经度: %.6f 纬度: %.6f 朝向: %.1f", road_pos, current_lon, current_lat, LOG_INFO_THROTTLE(1000, "循迹状态: 当前点=%d/%d, 目标点=%d", road_pos, GpsPointNum - 1, des_pos);
current_head); LOG_INFO_THROTTLE(1000, "当前位置: %.6f, %.6f, 朝向: %.1f", current_lon, current_lat, current_head);
LOG_INFO_THROTTLE(1000, "目标位置: %d 经度: %.6f 纬度: %.6f 朝向: %.1f", des_pos, target_lon, target_lat, LOG_INFO_THROTTLE(1000, "目标位置: %.6f, %.6f, 朝向: %.1f", target_lon, target_lat, target_head);
target_head);
LOG_INFO_THROTTLE(1000, "总点数: %d，当前点数: %d，目标点: %d", GpsPointNum, road_pos, des_pos);
LOG_INFO_THROTTLE(1000, "############################################################");
// 计算当前定位点与目标点距离 // 计算当前定位点与目标点距离
double distance = ntzx_GPS_length(current_lon, current_lat, target_lon, target_lat); 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)) if (road_pos >= (GpsPointNum - 1))
{ {
LOG_INFO("到达路径终点: 当前点=%d, 总点数=%d", road_pos, GpsPointNum);
x = 0.0; x = 0.0;
y = 1.0; y = 1.0;
pl_speed = 0; pl_speed = 0;
task_status = TaskStatus::COMPLETED; task_status = TaskStatus::PATH_FINISHED; // 单路径完成
LOG_INFO("设置任务状态为 PATH_FINISHED");
// 短暂休眠，确保车辆停稳 // 短暂休眠，确保车辆停稳
LOG_INFO("车辆停稳中...");
sleep(3); sleep(3);
continue; continue;
} }
@ -368,16 +382,20 @@ void PL_ProcThread()
x = x_cm_tmp; x = x_cm_tmp;
y = y_cm_tmp; y = y_cm_tmp;
pl_speed = 800; pl_speed = 800;
task_status = TaskStatus::RUNNING; if (task_status != TaskStatus::RUNNING)
{
task_status = TaskStatus::RUNNING;
LOG_INFO("设置任务状态为 RUNNING");
}
} }
// 避免忙等待，增加延迟让 CPU 能够处理其他任务 // 避免忙等待，增加延迟让 CPU 能够处理其他任务
auto now = std::chrono::steady_clock::now(); auto now = std::chrono::steady_clock::now();
if (now > next_time) if (now > next_time) next_time = now;
next_time = now;
std::this_thread::sleep_until(next_time); std::this_thread::sleep_until(next_time);
} }
LOG_INFO("PL_ProcThread 线程退出");
return; return;
} }

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

@ -78,8 +78,9 @@ enum TaskStatus
{ {
PENDING = 0, // 待执行 PENDING = 0, // 待执行
RUNNING = 1, // 执行中 RUNNING = 1, // 执行中
COMPLETED = 2, // 执行完成 COMPLETED = 2, // 全部路径执行完成（任务结束）
FAILED = 3, // 执行异常 FAILED = 3, // 执行异常
PATH_FINISHED = 4, // 单条路径执行完成（等待切换下一条路径）
}; };
// 当前任务信息 // 当前任务信息

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

@ -75,9 +75,8 @@ int TaskManager::getCurrentTaskId()
return current_task_.id; return current_task_.id;
} }
void TaskManager::updateCurrentTask(int taskId, TaskStatus status, const std::string& name, void TaskManager::updateCurrentTask(int taskId, TaskStatus status, const std::string& name, int mode, int duration,
int mode, int duration, int count, int count, const std::vector<ROUTE_INFO>& routeInfos)
const std::vector<ROUTE_INFO>& routeInfos)
{ {
std::lock_guard<std::mutex> lock(task_mutex_); std::lock_guard<std::mutex> lock(task_mutex_);
current_task_.id = taskId; current_task_.id = taskId;
@ -92,8 +91,8 @@ void TaskManager::updateCurrentTask(int taskId, TaskStatus status, const std::st
current_task_.currentRouteIndex = 0; // 默认从第一条路径开始 current_task_.currentRouteIndex = 0; // 默认从第一条路径开始
} }
current_task_.lastUpdateTime = std::chrono::steady_clock::now(); current_task_.lastUpdateTime = std::chrono::steady_clock::now();
LOG_INFO("Task updated: ID=%d, Status=%d, Name=%s, Mode=%d, Duration=%d, Routes=%zu", LOG_INFO("Task updated: ID=%d, Status=%d, Name=%s, Mode=%d, Duration=%d, Routes=%zu", taskId, status, name.c_str(),
taskId, status, name.c_str(), mode, duration, current_task_.routeInfos.size()); mode, duration, current_task_.routeInfos.size());
} }
const std::vector<ROUTE_INFO>& TaskManager::getRouteInfos() const std::vector<ROUTE_INFO>& TaskManager::getRouteInfos()
@ -120,13 +119,16 @@ bool TaskManager::switchToNextRoute()
if (current_task_.currentRouteIndex < current_task_.routeInfos.size() - 1) if (current_task_.currentRouteIndex < current_task_.routeInfos.size() - 1)
{ {
current_task_.currentRouteIndex++; current_task_.currentRouteIndex++;
LOG_INFO("切换到下一条路径，索引: %d/%zu", current_task_.currentRouteIndex, LOG_INFO("路径切换成功: 从索引 %d 切换到 %d/%zu", current_task_.currentRouteIndex - 1,
current_task_.routeInfos.size()); 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; return true;
} }
else else
{ {
LOG_INFO("已到达最后一条路径，任务完成"); LOG_INFO("已到达最后一条路径（索引: %d/%zu），任务完成", current_task_.currentRouteIndex,
current_task_.routeInfos.size());
return false; return false;
} }
} }
@ -259,10 +261,8 @@ bool TaskManager::processStartTask(const JSON_DATA& json_data, long seqNo)
LOG_INFO("路径数量: %zu", json_data.value.routeInfos.size()); LOG_INFO("路径数量: %zu", json_data.value.routeInfos.size());
// 更新当前任务（保存完整信息） // 更新当前任务（保存完整信息）
updateCurrentTask(json_data.value.id, TaskStatus::RUNNING, updateCurrentTask(json_data.value.id, TaskStatus::RUNNING, json_data.value.name, json_data.value.mode,
json_data.value.name, json_data.value.mode, json_data.value.duration, json_data.value.count, json_data.value.routeInfos);
json_data.value.duration, json_data.value.count,
json_data.value.routeInfos);
// 下载所有路径文件 // 下载所有路径文件
bool downloadAllSuccess = true; bool downloadAllSuccess = true;
@ -272,8 +272,7 @@ bool TaskManager::processStartTask(const JSON_DATA& json_data, long seqNo)
string downFileName = route.fileName; string downFileName = route.fileName;
string filepath = "routes/" + downFileName; string filepath = "routes/" + downFileName;
LOG_INFO("处理路径 %zu/%zu: %s", i + 1, json_data.value.routeInfos.size(), LOG_INFO("处理路径 %zu/%zu: %s", i + 1, json_data.value.routeInfos.size(), downFileName.c_str());
downFileName.c_str());
// 检查文件是否已存在 // 检查文件是否已存在
if (access(filepath.c_str(), F_OK) == -1) if (access(filepath.c_str(), F_OK) == -1)

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

@ -562,23 +562,27 @@ class TaskManagerNode : public rclcpp::Node
void task_callback(const sweeper_interfaces::msg::Task::SharedPtr msg) 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<TaskStatus>(msg->task_status); status_up = static_cast<TaskStatus>(msg->task_status);
// 如果当前路径已完成，检查是否有下一条路径 // 如果当前路径已完成，检查是否有下一条路径
if (status_up == TaskStatus::COMPLETED) if (status_up == TaskStatus::PATH_FINISHED)
{ {
LOG_INFO("当前路径执行完成，检查下一条路径...");
if (task_manager.hasNextRoute()) if (task_manager.hasNextRoute())
{ {
LOG_INFO("当前路径完成，切换到下一条..."); LOG_INFO("发现下一条路径，准备切换...");
// 切换到下一条路径 // 切换到下一条路径
if (task_manager.switchToNextRoute()) if (task_manager.switchToNextRoute())
{ {
// 获取新路径并复制 // 获取新路径并复制
ROUTE_INFO nextRoute = task_manager.getCurrentRouteInfo(); ROUTE_INFO nextRoute = task_manager.getCurrentRouteInfo();
string filepath = "routes/" + nextRoute.fileName; 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 // 复制文件到 gps_load_now.txt
std::ifstream src(filepath, std::ios::binary); std::ifstream src(filepath, std::ios::binary);
@ -586,37 +590,42 @@ class TaskManagerNode : public rclcpp::Node
if (src && dst) if (src && dst)
{ {
dst << src.rdbuf(); dst << src.rdbuf();
LOG_INFO("路径文件复制成功"); LOG_INFO("路径文件复制成功: %s -> gps_load_now.txt", nextRoute.fileName.c_str());
// 同时更新两个状态，保持任务继续运行 // 同时更新两个状态，保持任务继续运行
task_manager.setTaskStatus(1); task_manager.setTaskStatus(1);
task_manager.updateCurrentTaskStatus(TaskStatus::RUNNING); task_manager.updateCurrentTaskStatus(TaskStatus::RUNNING);
// 定时器会自动发布任务消息，PL 层会重新加载路径 LOG_INFO("任务状态已更新为 RUNNING，等待PL层重载新路径");
} }
else else
{ {
LOG_ERROR("无法复制路径文件"); LOG_ERROR("无法复制路径文件: %s", filepath.c_str());
} }
} }
else
{
LOG_ERROR("路径切换失败");
}
} }
else else
{ {
// 最后一条路径完成，更新任务管理器内部状态为 COMPLETED // 最后一条路径完成，更新任务管理器内部状态为 COMPLETED
LOG_INFO("已完成全部路径，任务结束"); LOG_INFO("所有 %d 条路径执行完成，任务结束", task_manager.getRouteCount());
task_manager.setTaskStatus(TaskStatus::COMPLETED); task_manager.setTaskStatus(TaskStatus::COMPLETED);
task_manager.updateCurrentTaskStatus(TaskStatus::COMPLETED); task_manager.updateCurrentTaskStatus(TaskStatus::COMPLETED);
} }
} }
else else
{ {
// 如果不是 COMPLETED 状态，直接同步更新任务管理器的状态 // 如果不是 PATH_FINISHED 状态，直接同步更新任务管理器的状态
LOG_INFO_THROTTLE(5000, "更新任务状态: %d", msg->task_status);
task_manager.setTaskStatus(msg->task_status); task_manager.setTaskStatus(msg->task_status);
task_manager.updateCurrentTaskStatus(status_up); task_manager.updateCurrentTaskStatus(status_up);
} }
} }
else else
{ {
LOG_WARN("Invalid task status: %d", msg->task_status); LOG_WARN("收到无效的任务状态: %d", msg->task_status);
} }
} }