Fast-planner代码阅读2-TopoReplan (path searchiing部分)

最近由于多个比较实验的原故，把ego-planner及Fast-planner中TOPO-REPLAN的代码都分析了一遍。EGO主要是为了看gridmap的使用。TOPO-Replan主要是分析了topo-path searching部分。现记录于下，以供交流。

1.欢迎参看关于kinoreplan部分的完整分析：Fast-Planner代码阅读-1. Robust and Efficient Quadrotor Trajectory Generation for Fast Autonomous Flight
2.欢迎参看我关于ego-planner地图部分的代码分析：EGO-PLANNER代码阅读1-（地图部分）

1. 总体流程

1. Toporoad Map generation生成一系列PATH
2. 对充满了redundant 的path进行short Prune,并只保留属于不同UVD class 前 K m a x K_{max} Kmax​个最短Path
3. 首先利用PGO对这 K m a x K_{max} Kmax​个最短Path进行优化，最后再利用GTO对这些PATH进行二次优化，选取COST最小的PATH.

2.Topo Path Finding Algorithm主要算法及图例

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-h3SMEotr-1641962066019)(https://raw.githubusercontent.com/Joeyyuenjoyslife/Joeyyu-imgaebed/main/image-20220105131040549.png)]

3. topoPath路径搜索代码流程

Topopath 路径搜索算法的调用入口是在topo_prm.cpp中TopologyPRM类的成员函数findTopoPaths(). 而findtTopoPaths()这一函数又包括1.createGraph；

2.searchPaths； 3.shortcutPaths(); 4.pruneEquivalent；5.selectShortPaths这五个主要二级（我自己定的叫法）函数。

3.1 createGraph（）

这一函数的目的是为了创建空间中的采样节点图。主要又包括getSample(), findVisibGUard, needConnection，pruneGraph这几个三级函数

首先根据起始点的位置信息和载入的参数来设置采样点的范围。在时间小于最大采样时间，点数小于最大采样点数时，保持采样循环。

首先利用getSample函数采样后，再利用ESDF地图判定采样点是不是障碍物，如果是，则continue，如果不是，则执行循环内的之后代码。

3.1.1 findVisbGUard

得到一个合法的采样点后，就要判定它有几个可见的guard, findvibGUard函数是利用linVisb这一函数来判断两个guard之间是否可见的。lineVisb这一函数也很简单，即把两个节点连起来，利用raycast步进和ESDF地图来逐步检验连线上的每一个点是否是障碍物。如果当前采样点一个可见guard都没有，则把它当做新的guard添加到gragh中，如果有且只有两个可见guard，则要利用needConnection函数判断是否要添加新的connector.

3.1.2 needConection

needConnection也很直观。即判断当前路径（可见guard1,当前采样点，可见guard2）是否与（可见guard1,可见guard2,…）形成的其他路径同伦，如果同伦，则直接返回true.将当前采样点添加为新的connector至graph中。如果不同伦，则要判断新的路径是否比之前的路径短，如果短，则替换之前两个guard的connector的位置并返回false。如果更长，则直接返回fase.

3.1.3 pruneGraph()

这一函数的作用就是删去那些只有一个neighbor的guard节点。（connector不可能只有一个邻居节点）。

3.2 searchPaths()

这一二级函数的主要作用是在节点图的基础上找到多条可用的路径。主要流程为先利用depthFirstSearch()函数找到所有PATH,然后再对各个path的节点数多少进行排序，只留下若干个最短的path。

这一二级函数中需要注意理解的为深度有限搜索寻找路径的depthFirstSearch函数，其他都较为直观。

3.3 shortcurPaths()

得到经过排序筛选的原始路径后，还需要对这些路径进行缩短。shortcutPaths这一函数利用利用了std::thread进行并行操作。对每一条路径都执行shortcutPath函数，直到所有路径完成。

3.3.1 shortcutPath

对于一条待缩短的路径，首先利用discretizePath函数将其从几个节点变为更稠密的一系列路径点。然后创建一个空的vectorEigen::Vector3d short_path来存储新的缩短后的路径。对于原始上的每一个点，都与short_path的最后一个点（初始化时为原始路径的起点）连线，并利用lineVisb来衡量可见性。若不可见，则将线上不可见的点往外推至一个新的位置。推的方向与连线垂直并和不可见点处的距离梯度方向面。并把新的位置点push_back进short_path中。直到结束循环把终点Push_back进short_path中。最后判断当前short_path是否比原来的路径短，若短，则取代原来的路径，若不是，则保持不变。

3.4 pruneEquivalent（）

得到缩短后的路径集后，还需要将同伦的路径删去。这一二级函数的逻辑也很简单，即新建立一个已存在路径集，然后将原有路径集中的每一条路径与路径集中的路径进行比较，若同伦，则不加入已存在路径集且删去当前路径。若不同伦，则加入已存在路径集。

3.5 selectShortPaths

最后，同伦剪枝后的函数还需要再进行一次筛选。首先筛出所有路径中的最短路径。其次，循环地将除去最短路径的剩余的路径中的最短路径与真正的最短路径进行比较，若长度比值小于一定阈值，则可以将其加入新的路径集。直到达到最大路径数量或者循环结束。

并再做一遍shortcutPath和pruneEquivalent得到最终路径。

5. 多条PATH在RVIZ中的显示

1. traj_utils 中的planning_visualization.cpp形成了traj_utils/planning_visualization.h的头文件
2. 这一头文件被包含在planner_manager.h中和ego_replan_fsm.h中。 plan_manager.h也包含在ego_replan_fsm.h中
3. \

5.1 PlanningVisualization::displayAStarList

路径的可视化最终由PlanningVisualization调用displayAstarList来进行发布。这一函数在planning_visualization.cpp中

而具体的发布位置在plan_manager.cpp的reboundReplan（）中。