Far planner 代码系列（22）
DynamicGraph::UpdateGlobalNearNodes( ) （二）
经过之前的过程，我们得到了extend_match_nodes_、wide_near_nodes_、near_nav_nodes_、internav_near_nodes_、surround_internav_nodes_和margin_near_nodes_
我们要接着完善 wide_near_nodes_，这里也很好理解，把odom里连接到的每个node都取出来，如果这个node中的is_wide_near为false，那么我们就把它加入到wide_near_nodes_中。再来一个循环，把和node连接的其他contour的node也取出来，同样通过is_wide_near判断该点要不要放进wide_near_nodes_
说白了，之前从globalGraphNodes_里得到一部分，现在要通过odom把漏掉的一部分也补进去，就这么简单。
接着，如果internav_near_nodes_不为空，那就从里面找一个连接到odom的最短的inter_nav node
首先，若internav_near_nodes_不为空，我们按intensity从小到大给里面的node进行排序，所以intensity小的就可能是最接近odom的点。
当然我们还要通过循环每一个node来判断它到底是不是最接近odom的点，我们可以通过取出每一个点，令其为temp_internav_ptr。
我们知道每一个odom都有一组potential_edges(这个之后再说，先知道有就行了)来存放可能的点，或者说离odom一段范围内的点。那我们就先要判断temp_internav_ptr在不在这个potential_edges里，如果不在，那直接排除掉，换下一个temp_internav_ptr。
除了满足上面的要求，这个temp_internav_ptr还要满足它在InternavInRange的范围里。
这个函数给了一个TRAJ_DIST 也就是trajectory的距离为5，高度范围的阈值为1.3，它判断当前传入的node的fgscore 和TRAJ_DIST的关系与该node 的position和height的关系，只要一个不满足要求就返回false。（fgscore我还没看到，感觉可能是和路径规划有关的内容？）
如果上面两个要求都满足，那接下来继续看
这个判断蛮长的，其中的 cur_internav_ptr_ 在初始化的时候为NULL，所以一开始是满足条件的，那么他就去更新cur_internav_ptr_
这个更新也蛮好理解的，先把temp_internav_ptr赋给cur_internav_ptr_
然后last_internav_ptr_ 初始是NULL ，开始设置last_internav_ptr_，更新terrain_grid的中心点，重置grid范围内的点，重置is_occupied，更新last_internav_ptr_ = cur_internav_ptr_
接着，若last_internav_ptr_ 不为null了，（第一次更新让两者相等了，第二次更新 cur_internav_ptr就变了，而last_internav_ptr_还是之前的cur_internav_ptr），那这时候last_internav_ptr_ 和 cur_internav_ptr_两者就不相等了。这里同样更新terrain_grid的中心点，重置grid范围内的点，不同的是，增加了一个AddTrajectoryConnect的函数。
最后依然还是把当前cur_internav_ptr的点赋给last_internav_ptr
通过UpdateCurInterNavNode更新完，我们就要把函数给break了，跳出循环，那么整个UpdateGlobalNearNodes()就结束了。
如果我们在循环internav_near_nodes的时候，存在一个node，它之前的条件都满足，但是到了UpdateCurInterNavNode的时候不满足条件，我们就认为该node它是一个bridge，将这个node的is_bridge_internav_设置为true（判断是否需要设置inter_nav_point的时候会用到），然后将函数break，跳出循环。
到这里整个UpdateGlobalNearNodes()就结束了。