Far planner 代码系列（33） 关于real_world_contour和contour_graph

        我们知道far planner前部分的流程是这样子的：

        对于特定的地图，如下：

        我们要从surround_obs_cloud里抽取生成realworld_contour_，如下图所示：

        在这时候，每个node的z坐标都是以机器人的高度定的。如图所示：

        然后我们用real_world_contour去更新我们的contour_graph，对于PILLAR的点，它用取均值去替代。

        如图所示，深紫色的点为通过real_world_contour去更新得到的contour_graph，其中淡蓝色的点是之前real_world_contour内的点，他们被标记为pillar类型，所以通过mean_p去合成标记一个紫色的点。

        对于有完整的一组polygon的点，我们还要把它的第一个点放入poly_ctnodes列表里，也就是图中淡紫色的两个点，我们可以通过该node的front方向和back方向去遍历该polygon里的每一个点。

         之后，我们得到contour_graph，我们还需要对它每个node里的z坐标进行还原。我们取机器人的z值，在加上一个辅助的限高值kTolerZ，得到h_min和h_max：

        1、首先判断该点是不是属于terrain_cloud里的点，若用kdtree查找返回的点的个数大于0，则表示该点是属于terrain_cloud的点，标识符is_ground_associate为true，并更新返回平均高度

        2、若不属于terrain_cloud，则直接返回该点的z值

        3、根据标识符is_ground_associate进行判断

                (1)is_ground_associate为true，按kdtree查找的点云的minH和小车的高度去更新z值，

        并判断该高度在不在h_min和h_max的范围内

               (2)is_ground_associate为false，把该点变成(x,y,0)，然后在terrain_height_grid_下进

        行一个从pos2sub的变换，得到sub，判断sub在不在terrain_height_grid_的subs范围内，如

        果在的换，就用sub2ind转换成对应的ind。判断该ind在terrain_grid_traverse_list_上是否被探

        索过，如果被探索过，就根据cell[ind]取出里面存放的第一个z值，并把is_ground_associate

        标记为true。

                如果该ind不在terrain_height_grid_范围内，但是is_search标记为true，那么我们就在

        kdtree_terrain_clould里找离该ctnode最近的一个node，并用它的高度去代替该ctnode的高

        度，完事以后我们要加上小车本身的高度，得到该ctnode的最终高度，并且要判断该高度在

        不在h_min和h_max的范围内

开发者涨薪指南 48位大咖的思考法则、工作方式、逻辑体系