三维 A* 搜索算法（Rviz可视化）

本项目使用的 A* 源码 来自：HKUST-Aerial-Robotics/Fast-Planner

本项目开源在：https://github.com/bhu619/3D-AStar-Planner（如果项目对你有帮助，希望能帮我点个 Star~！感谢您！）

起因是我在复现 Fast-Planner 的时候发现代码中的 Geometric path planning (几何 A* algorithm)部分不完整，无法直接使用，于是我把项目中的 AStar 源码拿出来自己写了一个模拟 A*搜索的代码。

算法效果

遇到的问题

1. 为什么搜索的距离稍微给大，就无法搜索到路径了？

哪怕 z 轴无变化，同样找不到路径。在A*搜索过程中，邻居节点扩张范围主要受到 搜索分辨率resolution_、节点内存池的大小path_node_pool_[allocate_num_]以及启发式函数权重lambda_heu_ 等多重因素的限制。这些限制综合作用，使得搜索过程不至于无限制地向整个空间扩张，从而在可用资源（时间和内存）内找到可行路径或者在无法到达目标时及时结束搜索。

起点：[1.0, 1.0, 1.0]

终点：[1.8, 1.8, 4.0]

起点：[1.0, 1.0, 1.0]

终点：[1.8, 1.8, 4.5]

起点：[1.0, 1.0, 1.0]

终点：[1.8, 1.8, 5.0]

已经找不到点了

为了找到路径无法搜索的原因，我把 A* 搜索过程中的扩张的邻居节点在 Rviz 中可视化了。

可以看到，当目标点设为 [2.9, 1.1, 8.4] 的时候，节点池耗尽了，且邻居节点的扩张远没有到终点位置。

造成此现象有两个原因：

1. 节点池分配的大小不够。
2. 搜索分辨率过低。

增大节点池大小

增大节点池大小为10000000 时，终端显示分配的节点数为 110680，貌似最大的节点数就是 100000 左右了。

增大搜索分辨率

search_resolution 从 0.1 变为 0.2 时：

search_resolution 变为 0.3 时，0.3 的时候已经能够搜索整个地图空间了 （10 * 10 * 10）：

search_resolution 变为 0.4 时：

目前的地图大小，搜索分辨率设置为0.3就足够使用了。