一、下載編譯功能包

?

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 
#include 
#include 
#include 

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(unsigned char* costs, double start_x, double start_y, double end_x, double end_y, int cycles, float* potential);
    private:
        void add(unsigned char* costs, float* potential, float prev_potential, int next_i, int end_x, int end_y);
        std::vector queue_;
};

} 

?

2、astar.cpp

?

#include
#include

namespace global_planner {

AStarExpansion::AStarExpansion(PotentialCalculator* p_calc, int xs, int ys) :
        Expander(p_calc, xs, ys) {
}  //命名空間名::標識符名?


/*	
	costs： 地圖指針
	cycles：循環次數
*/
bool AStarExpansion::calculatePotentials(unsigned char* costs, double start_x, double start_y, double end_x, double end_y,int cycles, float* potential)      //傳參
 {
    
    //queue_為啟發式搜索到的向量隊列：
     queue_.clear();  //清空函數：將隊列清空
    
    //將起點放入隊列
    int start_i = toIndex(start_x, start_y);
    //(1,2)
  
    //push_back：向數據結構中添加元素
    queue_.push_back(Index(start_i, 0));  
    //首先將start點以及其代價加入，即（13，0） 
	
	//std::fill函數的作用是：將一個區間的元素都賦予指定的值，即在（first, last)范圍內填充指定值
	//將所有點的potential都設為一個極大值,potential就是估計值g,f=g+h
    std::fill(potential, potential + ns_, POT_HIGH);   //?
     
    //將起點的potential設為0
    potential[start_i] = 0;

    //終點對應的序號
    int goal_i = toIndex(end_x, end_y);//(4,4)
    int cycle = 0;
    
    //進入循環，繼續循環的判斷條件為只要隊列大小大于0且循環次數小于cycles 
	//代碼中cycles = 2 *nx * ny, 即為所有格子數的2倍  //?
	//目的：得到最小cost的索引，并刪除它，如果索引指向goal(目的地)則退出算法，返回true
    while (queue_.size() > 0 && cycle < cycles) 
    {
        Index top = queue_[0];
        
        ///將首元素放到最后，其他元素按照Cost值從小到大排列
		//pop_heap() 是將堆頂元素與最后一個元素交換位置，之后用pop_back將最后一個元素刪除
		//greater1（）是按小頂堆
        std::pop_heap(queue_.begin(), queue_.end(), greater1());
        queue_.pop_back();

        //如果到了目標點，就結束了
        int i = top.i;
        if (i == goal_i) 
            return true;

       // 對前后左右四個點執行add函數，將代價最小點i周圍點加入搜索隊里并更新代價值
        add(costs, potential, potential[i], i + 1, end_x, end_y);
        add(costs, potential, potential[i], i - 1, end_x, end_y);
        add(costs, potential, potential[i], i + nx_, end_x, end_y);
        add(costs, potential, potential[i], i - nx_, end_x, end_y);

        cycle++;
    }

    return false;
}

/*add函數：添加點并更新代價函數；
如果是已經添加的點則忽略，根據costmap的值如果是障礙物的點也忽略。
potential[next_i]是起點到當前點的cost即g(n), 
distance * neutral_cost_是當前點到目的點的cost即h(n)。
f(n) = g(n) + h(n)
*/

// potential 存儲所有點的g(n),即從初始點到節點n的實際代價
// prev_potentia 當前點的f
void AStarExpansion::add(unsigned char* costs, float* potential, float prev_potential, int next_i, int end_x,int end_y) 
{

    //next_i 點不在網格內，忽略
    if (next_i < 0 || next_i >= ns_)  //ns_?
        return;

    //未搜索的點cost為POT_HIGH，如小于該值，則為已搜索點，跳過；
    //potential[next_i]是起點到當前點的cost即g(n)
    if (potential[next_i] < POT_HIGH)   //POT_HIGH?
        return;
        
    //障礙物點,忽略?
    if(costs[next_i]>=lethal_cost_ && !(unknown_ && costs[next_i]==costmap_2d::NO_INFORMATION)) 
        return;
        
    // potential[next_i]是起點到當前點的cost即g(n)
    // potential 存儲所有點的g(n),即從初始點到節點n的實際代價
    // prev_potentia   當前點的f
    // =====見下方potential_calculate.cpp文件=====
    potential[next_i] = p_calc_->calculatePotential(potential, costs[next_i] + neutral_cost_, next_i, prev_potential);

    // 得到在柵格地圖中的（x,y）
    int x = next_i % nx_, y = next_i / nx_;  
    //x=14%5=4  y=14/5=2 ?

    //啟發式函數：即h(n) 從節點n到目標點最佳路徑的估計代價，這里選用了曼哈頓距離
    float distance = abs(end_x - x) + abs(end_y - y);
    // A的i值是13，則add中的next_i分別是12，14，7，19。
    // 以14為例，則x=2, y=2。而B為(4,4)。因此distance = 2+2 =4
    // 由于這只是格子的個數，還有乘上每個格子的真實距離或者是分辨率，所以最后的H = distance *neutral_cost_
    // 因此最后的F = potential[next_i] + distance *neutral_cost_  (F=g+h)
    // 將當前點next_i以及對應的cost加入
    queue_.push_back(Index(next_i, potential[next_i] + distance * neutral_cost_));
    //potential[next_i]：是起點到當前點的cost即g(n)
    //distance * neutral_cost_：是當前點到目的點的cost即h(n)。
    //f(n)=g(n)+h(n)：計算完這兩個cost后，加起來即為f(n)，將其存入隊列中。

    //對加入的再進行堆排序， 把最小代價點放到front隊頭queue_[0]
    //數據結構？
    std::push_heap(queue_.begin(), queue_.end(), greater1());
}

} 

?

3、potential_calculate.h

neutral_cost_ 設定的一個默認值，為50

calculatePotential()計算根據use_quadratic的值有下面兩個選擇:
若為TRUE， 則使用二次曲線計算
若為False， 則采用簡單方法計算， return prev_potential + cost。即：costs[next_i] + neutral_cost_+ prev_potential
地圖代價+單格距離代價(初始化為50)+之前路徑代價為G
?

#ifndef _POTENTIAL_CALCULATOR_H
#define _POTENTIAL_CALCULATOR_H

#include 

namespace global_planner {

class PotentialCalculator 
{
    public:
        PotentialCalculator(int nx, int ny) 
        {
            setSize(nx, ny);
        }

        virtual float calculatePotential(float* potential, unsigned char cost, int n, float prev_potential=-1)
        {
            if(prev_potential < 0)
            {
                float min_h = std::min( potential[n - 1], potential[n + 1] ),
                      min_v = std::min( potential[n - nx_], potential[n + nx_]);
                prev_potential = std::min(min_h, min_v);
            }

            return prev_potential + cost;
        }
//在prev_potentia當前點的f不小于0的時候，返回的是prev_potential + cost
//以start_cost為例，最開始給其賦值是0，因此返回就是cost，可以理解為到下一個柵格的距離，或者是分辨率。
//在計算完potential=g值后開始計算h值，即distance,利用的就是移動多少格子，只能上下左右移動.

        virtual void setSize(int nx, int ny) 
        {
            nx_ = nx;
            ny_ = ny;
            ns_ = nx * ny;
        }

    protected:
        inline int toIndex(int x, int y) 
        {
            return x + nx_ * y;
        }

        int nx_, ny_, ns_;
};
} 

?