Far planner 代码系列 (34)路径规划（一）

        当系统正在运行的时候，每次进行ros spinonce 它都会调用PlanningCallBack函数，在这个函数没有接收到goal目标的时候，它会先进行通行区域的更新，也就是计算fgscore和gscore

        我们进入UpdateGraphTraverability函数，它主要分为两个部分：

        第一个是针对odom，拓展到所有的nav_node，我们计算gscore，并更新当前cur_graph的gscore值和nav_node的父节点parent，这里用到的是广度优先算法，正如作者论文里提到的，我们先选择odom，然后遍历它的所有connect_nodes，更新open_set，在取出connect_nodes的connect_nodes，一直遍历直到所有点都进入close_set即完成。

        第二部分是更新fgscore，这个只针对node属性里is_covered为true的nav_node，所以并不是所有的nav_node都有fgscore。对于有fgscore的nav_node，我们称它的父节点为free_parent

        当我们发送goal命令的时候，ros接收到槽信号，进行ros spinonce的时候，就会跳转到WaypointCallBack函数。

        函数最主要的部分就是UpdateGoal，它将更新我们的goal点。

         我们可以看到，首先遍历现有的graph，判断traj里的node是否有离goal非常近的（这个一般都没有用到~）如果有，那么我们就用该traj_node来进行导航（设为终点），如果没有，我们就新建立一个goal_node，并把该node加入graph中。

        在经过一个ros spinonce，我们再次进入PlanningCallBack函数，这回我们就会往下进行了：

        我们把goal在world_obs_grid下的sub取出，并以该sub为中心建立一个3x3x5的sub体块，将该体块内部的点云提取出来。如下图所示： 

        至于为什么是3x3x5的体块，我想原本单个sub就已经足够了，原因如下：

        在建立world_obs_cloud_grid的时候，我们是按200x200x125这个来建立的，而它的缩放系数为(5,5,0.8)，如果以odom(,,)为准，它建立的一个grid就如下图所示，其中心为odom，起点为Origin，每个sub占5x5x0.8的距离。

         在建立free_terrain_grid的时候，如下图所示，由于该grid大小为27x27x1，缩放比率为(0.15,0.15,0.15)，其实总长度也就4x4，对于world_obs_cloud_grid下的一个sub就有5x5应该是绰绰有余了。

        言归正传，我们获取到了goal_node周围的free和obs点云信息后，就用来更新我们的free_terrain_grid。我们把goal_node当做该grid的中心点，用该中心点去重置grid的origin

         更新完后，示意图如下图所示，其中红色为障碍物的sub，蓝色为goal目标点的sub：

         未完待续~