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, // 待执行
RUNNING = 1, // 执行中
COMPLETED = 2, // 执行完成
COMPLETED = 2, // 全部路径执行完成（任务结束）
FAILED = 3, // 执行异常
PATH_FINISHED = 4, // 单条路径执行完成（等待切换下一条路径）
};
#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()
{
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;
}

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

@ -78,8 +78,9 @@ enum TaskStatus
{
PENDING = 0, // 待执行
RUNNING = 1, // 执行中
COMPLETED = 2, // 执行完成
COMPLETED = 2, // 全部路径执行完成（任务结束）
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;
}
void TaskManager::updateCurrentTask(int taskId, TaskStatus status, const std::string& name,
int mode, int duration, int count,
const std::vector<ROUTE_INFO>& routeInfos)
void TaskManager::updateCurrentTask(int taskId, TaskStatus status, const std::string& name, int mode, int duration,
int count, const std::vector<ROUTE_INFO>& routeInfos)
{
std::lock_guard<std::mutex> 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<ROUTE_INFO>& 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)

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)
{
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);
// 如果当前路径已完成，检查是否有下一条路径
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);
}
}