混合A*的ROS程序讲解演示

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 \theta_r θr​与相邻路径点构成的向量 θ p \theta_p θp​的夹角 Δ θ \Delta\theta Δθ。涉及到角度要小心，需要保证角度的范围。为此定义正则函数modpipi，其功能是将角度转换为 ( − π , π ) (-\pi,\pi) (−π,π)的范围内。计算相邻路径点向量与 x x x轴的角度需要利用atan2函数，这个函数也要小心。注意，atan2(0,0)=0。为此，先对路径预处理，剔除相邻路径点 x y xy xy坐标完全相同的其中一个点。这也是源程序的一个小BUG。

标签：演示,函数,后退,路径,讲解,path,theta,ROS,前进
来源： https://blog.csdn.net/robinvista/article/details/115485953