混合A星--关键代码、逻辑个人通俗理解

混合A核心代码在hybrid_a_star.cpp中：
首先介绍算法整体逻辑：
1：使用两种搜索方式，一种是可以传统的通过节点权重拓展的，和普通A搜索方式一致。另一种是基于RS曲线搜索路径，它考虑了动力学但没有避障功能。如下所示：左边两张图是传统A*搜索，可避障没考虑运动学。右边两个是RS曲线生成的路，不能避障但符合车辆运动

2：混合A 整体路径由两部分组成，传统A搜索出的路（左边）+RS曲线生成的路（右边）

什么时候开始以RS搜路？ 当 当前点和目标点距离小于设置参数 shot_distance时候 以RS曲线生成路。

3：迪杰斯特拉和A算法最小单位都是node。混合A 相邻两个node之间的Vec3d 才是最小单位，每个node都存着到下一个node之间的最小点的序列，如下所示。

node->intermediate_states_ 存的是当前点 向外离散搜索采样 到下一个neighbor的小段路。

4：state_map和点对应关系：每个node指针都有一个父节点，所以依次回溯遍历就可以找到起点，所以，每个加入openlist里的指针都相当于一条备选的路。
Map是一个三维数组，存node用的，目的快速查找node减小开销。Map索引是x,y,航向，这样想搜索某个node节点不用变量所有链表，直接根据x,y,heading查找是否为空即可。