1　源代码编译

　　混合A*算法实现采用的是Github上 karlkurzer 的版本，下载和编译过程如下。新建并运行脚本即可实现傻瓜式一键安装，非常简单。

#!/bin/bash
mkdir -p ~/HybridAStar/src
cd ~/HybridAStar/src
git clone https://github.com/karlkurzer/path_planner.git
cd .. 
catkin_make
source devel/setup.bash
rospack profile
roslaunch hybrid_astar manual.launch

2　代码修改

　　原文件使用的全局坐标系名称是path，我们改成更常用的map。在path.h文件里修改，如下：

path.header.frame_id = "map";

　发布的路径话题名称是/path，路径在rviz中显示没有问题，但是如果打印出来就会发现消息头多了一个，如下图所示。原因是 path.cpp 文件中的Path::clear()函数里的path.poses.clear()语句导致，把它放到下面的Path::updatePath函数中即可。

3　前进后退

　　混合A*算法输出的是一条路径，该路径由一连串离散的坐标点组成。如果你想在路径基础上添加速度构成一条轨迹，可以这么做：前进时设置速度为正值，后退时设置速度为负值，在前进和后退相接的点设置速度为0。这样做需要判断什么时候是前进，什么时候是后退。判断方法是计算机器人航向角θ r 与相邻路径点构成的向量θ p 的夹角Δ θ。涉及到角度要小心，需要保证角度的范围。为此定义正则函数modpipi，其功能是将角度转换为(−π,π)的范围内。计算相邻路径点向量与x 轴的角度需要利用atan2函数，使用这个函数也要小心。注意，atan2(0,0)=0。原程序有一个小BUG，即可能存在两个坐标完全一样的点，原因是在做Dubins曲线拼接时没有考虑重叠。为此，先对路径预处理，剔除相邻路径点xy坐标完全相同的其中一个点。这也是

4　话题发布path.cpp

　　规划出来的路径通过path.cpp这个文件发布出来。这个文件中的命名雷同，看起来非常累，比如文件名叫path，类的名称叫Path，话题名也叫path，坐标系也叫path，这是一种混乱的命名方式。
　　这里我们最感兴趣的是发布路径的消息：

nav_msgs::Path path;

　　负责向里填充数据的是addSegment函数

void Path::addSegment(const Node3D& node)

　　planner.h里定义了碰撞检测

CollisionDetection configurationSpace

然后把加载的地图赋值给它，

configurationSpace.updateGrid(map);