一、下載下傳編譯功能包
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(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<Index> queue_;
};
}
2、astar.cpp
#include<global_planner/astar.h>
#include<costmap_2d/cost_values.h>
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_為啟發式搜尋到的向量隊列：<i , cost>
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 <algorithm>
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_;
};
}