导航模块架构
- TraceNavigate发还给业务模块GTraceList数据结构中，GTraceList/GTraceSeg/finished 标志当前完成的情况; GTraceList/status 标志当前任务完成或者出发停障
- ObstacleNavigate 发还给业务模块GTraceList数据结构中，应当更新GTraceList/GTraceSeg/finished 标示该继续从哪个GTraceSeg走；GTraceList/status标志绕障成功或失败
- & 6. 中，每5s间隔更新GTraceList并发还给业务模块用于任务状态监控，GTraceList/status 等于EXECUTE_STATUS_RUNNING
- 业务模块发送的GTraceList中如果GTraceSeg为空列表，导航模块视为取消所有当前正在执行任务，发布0速度后进入idle状态
数据结构定义
# 路径点、速度
message GTracePoint{
required double x = 1;
required double y = 2;
required double angle =3;
optional int32 speed = 4;
optional int32 starter = 5; # 可选 改点是否可作为起始点
}
# 前后相邻两个点组成的有向段、完成比例
message GTraceSeg {
required GTracePoint first = 1;
required GTracePoint second = 2;
required double finished = 3;
}
# 有向段的列表
message GTraceList {
enum ExecuteStatus{
# 1/2/3都是持续性状态 4是瞬时状态
EXECUTE_STATUS_IDLE = 1;
EXECUTE_STATUS_RUNNING = 2;
EXECUTE_STATUS_BLOCKED = 3;
EXECUTE_STATUS_FINISHED = 4;
}
repeated GTraceSeg lists = 1;
required bytes map = 2;
# 下发给导航模块可以不带status，导航模块上报必须带status
optional ExecuteStatus status = 3;
}
| 消息名 |
消息方向 |
消息定义 |
| msg_robot_laser_data |
激光采集-->TraceNavigate |
NODE.subscribe<GLaserScan>("msg_robot_laser_data",CallBackNewScan); |
| msg_robot_global_pos |
定位-->TraceNavigate |
NODE.subscribe<RobotPose2D>("msg_robot_global_pos",CallBackNewRobotPos); |
| msg_trace_navigate_route |
业务-->TraceNavigate |
NODE.subscribe<GTraceList>("msg_trace_navigate_route",CallBackNavigateRoute); |
| msg_trace_navigate_status |
TraceNavigate-->业务 |
NODE.advertise<GTraceList>("msg_trace_navigate_status"); |
| msg_robot_speed_control |
TraceNavigate-->底盘 |
NODE.advertise<GRobotSpeedControl>("msg_robot_speed_control"); |
| 消息名 |
消息方向 |
消息定义 |
| msg_robot_laser_data |
激光采集-->ObstacleNavigate |
NODE.subscribe<GLaserScan>("msg_robot_laser_data",CallBackNewScan); |
| msg_robot_global_pos |
定位-->ObstacleNavigate |
NODE.subscribe<RobotPose2D>("msg_robot_global_pos",CallBackNewRobotPos); |
| msg_obstacle_navigate_route |
业务-->ObstacleNavigate |
NODE.subscribe<GTraceList>("msg_obstacle_navigate_route",CallBackNavigateRoute); |
| msg_obstacle_navigate_status |
ObstacleNavigate-->业务 |
NODE.advertise<GTraceList>("msg_obstacle_navigate_status"); |
| msg_robot_speed_control |
ObstacleNavigate-->底盘 |
NODE.advertise<GRobotSpeedControl>("msg_robot_speed_control"); |
附
title 导航功能模块间调用
业务节点->TraceNavigate:1.publish message<GTraceList>
note left of 业务节点:调用轨迹跟踪
TraceNavigate->TraceNavigate:2.trace loop
TraceNavigate->TraceNavigate:3.waiting 60\n seconds
note left of 业务节点:停障超时
业务节点<--TraceNavigate:4.publish message<GTraceList>::\nEXECUTE_STATUS_BLOCKED
业务节点->ObstacleNavigate:5.publish message<GTraceList>
note left of 业务节点:调用绕障导航
ObstacleNavigate->ObstacleNavigate:6.navigate Loop
业务节点<--ObstacleNavigate:7.publish message<GTraceList>::\nEXECUTE_STATUS_FINISHED
业务节点->TraceNavigate:8.publish message<GTraceList>
note left of 业务节点:恢复轨迹跟踪
TraceNavigate->TraceNavigate:9.trace loop
业务节点<--TraceNavigate:10.publish message<GTraceList>::\nEXECUTE_STATUS_FINISHED
