ROS中Navigation功能包里路径规划A*算法详解

一、下载编译功能包

cd ~/catkin_ws/src
sudo apt-get install https://github.com/ros-planning/navigation
cd ..
catkin_make

二、功能包里涵盖的文件

功能包	功能
acml	定位算法
move_base	navigation中最主要的框架
base_local_planner	局部路径规划器
dwa_local_planner	dwa算法局部路径规划实现
global_planner	全局路径规划
navfn	全局路径规划，旧版本的，有bug
carrot_planner	一个简单的全局路径规划器
clear_costmap_recovery	清除代价地图的恢复行为
costmap_2d	实现2d代价地图
fake_localization	acml的API接口
map_server	提供地图数据，yaml或者是image
move_slow_and_clear	缓慢移动修复机制，会限制机器人速度
nav_core	路径规划接口类
rotate_recovery	旋转恢复
voxel_grid	三维代价地图

全局规划器有 3 个：
（1）carrot_planner

carrot_planner 检查需要到达的目标是不是一个障碍物，如果是一个障碍物，它就将目标点替换成一个附近可接近的点。因此，这个模块其实并没有做任何全局规划的工作。在复杂的室内环境中，这个模块并不实用。

（2）navfn

navfn使用 Dijkstra 算法找到最短路径。

（3）global planner

global planner是navfn的升级版。
它相对于navfn增加了更多的选项：支持 A* 算法；可以切换二次近似；切换网格路径；

三、Global Planner 全局路径规划

该文件下的内容：10个头文件，8个源文件

看其中A*算法的文件

先做一个算例，结合算例理解

以下文件会包含其他文件，需要整体看，这里先整理三个文件，其他的慢慢来

1、astar.h

#ifndef _ASTAR_H
#define _ASTAR_H

#include <global_planner/planner_core.h>
#include <global_planner/expander.h>
#include <vector>
#include <algorithm>

namespace global_planner {
   
class Index {
   
    public:
        Index(int a, float b) {
   
            i = a;
            cost = b;
        }
        int i;
        float cost;
};
//Index(i,cost)对应的点及代价

struct greater1 {
   
        bool operator()(const Index& a, const Index& b) const {
   
            return a.cost > b.cost;
        }
};

class AStarExpansion : public Expander {
   
    public:
        AStarExpansion(PotentialCalculator* p_calc, int nx, int ny);
        bool calculatePotentials