oru导航系统概述

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.900000 车辆中心到车头的距离
    		car_length_back: 0.300000 车辆中心到车尾的距离
    		max_steering_radians: 1.220000 舵轮最大弧度
    		distance_between_axes: 1.190000 轴距
    		resolution_m: 0.200000 空间分辨率
    		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}$