EDNIL导航系统概述

节点point_n_click_target_client

• 源文件orunav_vehicle_execution/src/point_n_click_target_client.cpp
• 接收消息ros类型目标点topic：/goal <geometry_msgs::PoseStampedConstPtr>,调用process_goal，流程为：

1. 调用服务"/compute_task"<orunav_msgs::ComputeTask>得到任务(位于节点orunav_vehicle_execution_node)。
2. 调用服务"/set_task"<orunav_msgs::SetTask>发送任务到协调器（位于coordinator_fake_node）。

节点orunav_vehicle_execution_node

• 源文件orunav_vehicle_execution/src/orunav_vehicle_execution_node.cpp
• 提供服务"/compute_task"，处理函数为computeTaskCB，流程为：

1. 解析任务请求中的装卸货标记、起始点、目标点、地图、障碍物等信息。
2. 调用服务"get_path"，基于A*（ARA*）的lattice planner生成路径,位于节点"orunav_motion_planner"。
3. 路径生成结果后处理。

   a. 路径过短导致路径生成失败，则返回失败。 b. 路径点为空导致路径生成失败，可能是起始点=目标点，则尝试使用上个任务的最后一小段路径(小于1.0),后退1.0m左右，再次运行到目标点，在停止精度异常时候可以使用。 c. 将起始点与目标点插入路径中。 d. 移除无用路径点（间距小于0.00002)；需要插点的地方插入路径点（具体见get_path）。 e. 移除间距过小的点（小于某个距离或者小于路径长度/最大路径点个数，以两者中小的为准）。 

4. 调用服务"/polygonconstraint_service"，多边形约束提取，位于节点"polygonconstraint_service"。
5. 调用服务"/get_smoothed_path"，路径平滑，位于节点"smoothed_path_service"。
6. 路径平滑后处理,判断最终路径是否满足车辆约束条件（最大舵轮速度、最大距离误差、最大航向角误差）。
7. 装卸货处理。
9. 调用服务"/deltatvec_service"，时间分割，位于节点"deltatvec_service"。

• 提供服务"/execute_task"，处理函数为executeTaskCB，异常判断后发送信号，通知轨迹跟踪线程开始运行。（todo）
• 轨迹跟踪线程处于等待状态，接受到信号，开始执行（todo）。

节点coordinator_fake_node

• 源文件orunav_coordinator_fake/src/coordinator_fake_node.cpp
• 提供服务"/set_task"，调度车辆执行任务，发送信号，通知执行任务线程开始执行任务。
• 执行任务线程处于等待任务状态，接受到信号，调用服务"/execute_task"（位于orunav_vehicle_execution_node）。

节点orunav_motion_planner

• 源文件orunav_motion_planner/src/get_path_service.cpp
• 提供服务"/get_path"
• 参数
  motion_prim_dir_: ./Primitives/ 运动基存放目录
  lookup_tables_dir_:./LookupTables/ 查找表存放目录
  maps_dir_:./Maps 地图目录
  model:车辆模型
  min_incr_path_dist_:路径点的最小间距
  save_paths_：是否保存路径log
• 预加载的文件内容
  motion_prim_dir_:运动基存放目录，对应数据结构为:std::map<std::pair<uint8_t, uint8_t>, std::vector<MotionPrimitiveData*> > modelMotionPrimitivesLT_，包含两类文件：

   1. .mprim:运动基文件，对应数据结构为`std::vector<MotionPrimitiveData*> ` a. 基公共参数 car_width：0.75 车辆宽度 car_length_front: 1. 车辆中心到车头的距离 car_length_back: 0. 车辆中心到车尾的距离 max_steering_radians: 1. 舵轮最大弧度 distance_between_axes: 1. 轴距 resolution_m: 0. 空间分辨率 numberofangles: 16 车辆角度分割个数 steeringanglepartitions: 1 舵轮角度分割个数 steeringanglecardinality: 1 舵轮角度分割间隔 startsteer_c: 0 基的舵轮起始角度 b. 基独立参数 primID: 0 ～49、63、149 基ID startangle_c:0x00～0x0f 基的车辆起始角度标号，仅方便显示，实际由基位姿点的第一个点确定 additionalactioncostmult: 1.2 由于某种动作引起的额外的代价系数 motiondirection: 1 基方向（1正；-1负） intermediateposes: 100 基的位姿点个数 100个基位姿点（位姿+舵轮方向） 2. .adat:运动基附加文件 若没有，则计算后保存，内容为：每行为一个基的附件数据，每行的意义为： orientID：车辆角度ID steeringID:舵轮角度ID primitiveID:基ID distance:基覆盖距离 sweptCells:n个x、y坐标，为车辆沿着基运动是覆盖的所有坐标 9999 9999:车辆运动坐标与末端占用坐标的分界线 occCells:n个x、y坐标，为车辆位于基末端时占用的所有坐标 根据orientID和steeringID匹配相同起始角度的一组运动基，在根据primitiveID匹配对应基， 将distance、sweptCells、occCells附件到对应基上。 

  lookup_tables_dir_:查找表存放目录，包含 .hst启发值查找表，对应数据结构为std::map<uint64_t, double> modelHeuristicLT_

   每行两个数据，第一个为64bit的key值，第二个为启发值 其中64bit的key值的构成为： 

  63 ～ 48	47 ～ 32	31 ～ 24	23 ~ 16	15 ～ 8	7~0
  dx	dy	startOrientID	goalOrientID	startSteeringID	goalSteeringID
  x方向长度	y方向长度	车辆起始角度	车辆终点角度	舵轮起始角度	舵轮终点角度

• 初始化

1. 加载车辆模型
2. 加载基文件
3. 加载启发值文件

• 服务"/get_path"处理流程为:

5. 将接受到的ros格式(rviz)的起始点和目标点转换为oru的数据格式。
6. 加载地图，地图偏移处理，保证不同地图的坐标系与默认的坐标系相同。
7. VehicleMission():根据起始点和目标点离散化车辆，坐标离散粒度等于地图分辨率，离散点位于网格的顶点，采用四舍五入理论；角度的离散粒度为2π/numberofangles,同样采用四舍五入理论。
8. addMission():将任务放入规划的队列。
9. solve():求解路径，可用A或ARA求解，ARA为A的变种，多了Heuristics的系数，初始系数比较大，能快速找到非最优路径，然后在时间要求范围内，逐渐缩小系数，进而优化路径，这种算法保证了速度，但不保证是最优解。

    a. 求解中与常规A*或ARA*算法的不同之处为：由当前点进行扩展时，扩展点为基的终点，而不是相邻网格点，这样大大加快了搜索速度。 b. 其中搜索算法的基本素是路径点PathNode，每个路径点对应一个基（路径点为基的终点）。 c. expandedList_：存放已扩展节点 openList_：存放待扩展的节点，数据结构使用二叉堆binary heap的最小堆(已弃用，改为uniqueNodes_) uniqueNodes_：升序unordered_map inconsistentList_(不一致节点离链表，用于ARA*) 

10. 路径后处理

    1、删除间隔过小的点（小于min_incr_path_dist_），min_incr_path_dist_不应小于分辨率 2、方向仅改变一次时需插入一个点，可能是为了路径平滑使用，如方向为1,1,1,0,1,1在0之后需插入一个点）。 forwardDirection():判断所有路径点的运行方向 方法为：计算当前路径点到下个路径点的方向，依据为相对位姿的X值，若x>=0则返回true，为向前；若x<0则返回false，为向后。 若路径点的方向中出现以下情况，则需要插点： a. 正向中出现一个反向点，需要在反向点之后的中间位置插入一个反向点 b. 反向中出现一个正向点，需要在正向点之后的中间位置插入一个正向点 

节点polygonconstraint_service

• 源文件orunav_constraint_extract/src/polygonconstraint_service.cpp
• 提供服务"/polygonconstraint_service"多边形约束提取流程,包括初始化和服务调用两部分：
• 初始化流程

1. 加载车辆模型（本处为(1.9, 0.3)，(1.9, -0.3)，(-0.2, -0.3)，(-0.2, 0.3)不同模型的区别仅为车辆尺寸不同）。
2. 加载托盘类型（无托盘）。
3. 创建查找表lookup。

    其中params_（）：约束条件，六个数组，表示左右前后四个方向和角度，共7*7*6*6*4*4=28224个约束 x_left(左侧位置范围)：-1.2～~0.01 x_right(右侧位置范围)：0.01～1.2 y_up(前方位置范围)：0.01~0.9 y_down(后方位置范围)：-0.9~-0.01 th_left(左侧角度范围)：-π/6~-π/180 th_right(右侧角度范围)：π/180~π/6 th_bounds(角度范围)：（-π/6，0）～（0，π/6） model(车辆模型):RobotModel2dCiTiTruck，尺寸为fl(1.9,0.3),fr(1.9,-0.3),rr(-0.2,-0.3),rl(-0.2,0.3) loadType(是否携带货物)_:NO_LOAD computed_(是否需要重新计算):false) 其他默认参数resolution = 0.1 th_sweep_incr = M_PI/36. use_th_bounds = false skip_overlap = false---->true 

4. 加载多边形约束查找表（service_lookup_model2load1.dat或创建）

   本文件内容为：28224个栅格地图，每个地图为约束多边形组成的对应的6m*6m的栅格地图，权重从小到大排序，对应数据为occupancyMaps_（栅格地图数组）

5. 设置每个约束多边形对应的栅格地图，若文件不存在，则后续自动生成后保存以供下次使用（生成较耗时）
6. 开始计算约束compute（），流程为7~10：
7. 创建28224个约束多边形PolygonConstraint，每个约束多边形包含一个由顶点组成的多边形和其权重(多边形面积×角度范围,使用boost库计算面积)，并按其权重降序排列
8. 构造std::map<int, PolygonConstraint&>类型的valid_，key值越小，约束多边形越大，方便后续操作
9. 创建6m*6m的空栅格地图模板（resolution=0.1,偏移量（-3.0,-3.0,0），原点位于栅格地图的中心）
10. 结合车辆模型的尺寸计算最大约束外接多边形largestFootPrint_（即左右前后和角度均为边界，车辆的最大膨胀范围）computeLargestConstraintOuterRegion，所需参数为（最大约束多边形，车辆模型，角度递增值）具体方法为：

    遍历约束多边形顶点（左上-->右上-->右下-->左下共4次） 角度从最小值（-π/6）按照递增值（π/36）依次递增至最大值（π/6）（共12次） 车辆旋转（角度值）平移（顶点值），得到新位姿多边形PosePolygon 累加新位姿（需要有相交部分才能累加），得到最大约束多边形并计算最大面积 将约束多边形优化为凸多边形 累加4个顶点构成的多边形并最终优化为一个凸多边形，即为最大的约束多边形，返回结果 将得到的多边形(origin为车体中心，)附加到创建的6*6空栅格地图，设置为已占据，返回多边形占据的栅格地图的索引（索引原点为左下角，网格是否占据是以网格中间位置为准） 若computed_==false;则需要重新创建多边形约束查找表，特别耗时。 

• 服务polygonconstraint_service流程

11. 接收“get_path”服务生成的路径path
12. 遍历路径点,对每个路径点执行查找约束findConstraint（）：所需参数（全局地图，路径点），输出结果为包含下个路径点的最佳约束和最佳约束对应的像素¹集合（最终路径点的最佳约束仅需包含自身）

    findConstraint（）： occ_patch = getOccupiedPixelsInLocalPixelCoords（）：对于当前路径点，结合全局地图返回最大约束多边形在栅格地图模板中，处于未越界但被占据状态的像素集 occ_patch_local = getOccupiedGridPatch(localMap)：返回栅格地图模板中langestFootPrint_的像素集 pts = getPoint2dVecFromGridPatchIdx(localMap, occ_patch_local)：由栅格地图模板中像素集得到坐标点集（米制） movePoint2dContainer(pts, pose)：将坐标点转换到全局地图，得到pts---全局地图中，当前路径点下的最大约束多边形的坐标集 loop,遍历上述坐标点集pts，得到被占据点在最大约束多边形中的像素集 metricToPixelOccupancyGrid（map, pts[i], pixel）：由坐标点得到像素 isValidAndOccupied(map, pixel)：全局地图下，对于有碰撞的像素点，则保存此像素点在栅格地图模板的像素集并返回 sort()：对occ_patch按照索引升序排列（平方L2范数） loop，遍历valid_（权重降序的约束多边形） 对所有occ_patch进行处理，在栅格地图模板中，若occ_patch与当前约束多边形有碰撞，说明约束多边形过大，继续遍历，直到找到无碰撞的最大的约束多边形（耗时大） 根据需要选择是否检测可行性 contain_pose_relative = orunav_generic::subPose2d(pose, containPose)：结果是下个路径点以当前位姿为坐标系的相对位姿 isFeasible(contain_pose_relative):检测下个路径点角度和位置是否可行，不可行则返回失败，可行则继续执行。其函数逻辑见部分函数说明3 得到result:包含得到的约束多边形、当前路径点、约束多边形的索引 use_th_bounds：是否融合角度？？--未使用 将约束多边形转换到全局地图中（包括点和角度） validConstraint():约束的kexing有效性检查，使用isFeasible（pose）,其中pose为当前路径点 computeConstraintOuterRegion():计算约束的外接多边形outer_constraint，放入约束集constraints，方法同初始化中10 至此，约束查找结束 

13. getInvalidConstraintPathOverlap(constraints, path):约束多边形constraints的index和路径点path的index相同，若当前点和下个点不同时在当前约束多边形，也不在下个约束多边形，说明两点不包含于同一多边形，则返回当前点的index。使用isFeasible（），用于再次判断路径点是否可行。
14. 使用isFeasible（）判断起始点和目标点是否可行。
15. save_constraints_and_path:保存log,包含此次任务的路径、约束多边形、外接多边形、需要显示的矩形和角度信息，根据此功能可优化代码，提升效率，或保存平滑后的路径。
16. assert校验:路径点个数与约束多边形个数是否依然相相等。
17. createRobotConstraintsFromPolygonConstraintsVec():创建service的返回结果res.constraints

结束

• 部分函数说明：

1. Pose2d addPose2d(const Pose2d &origin, const Pose2d &incr)：

   $\left[\begin{array}{c}{x}_{ret}\\ y_{ret}\\ {\theta }_{ret}\end{array}\right]$ = $[\begin{array}{ccc}cos{\theta }_0& -sin{\theta }_0& 0\\ sin{\theta }_0& cos{\theta }_0& 0\\ 0& 0& 1\end{array}$