一、下載編譯功能包
?
cd ~/catkin_ws/src
sudo apt-get install https://github.com/ros-planning/navigation
cd ..
catkin_make
二、功能包里涵蓋的文件
功能包 功能
acml 定位算法
move_base navigation中最主要的框架
base_local_planner 局部路徑規(guī)劃器
dwa_local_planner dwa算法局部路徑規(guī)劃實(shí)現(xiàn)
global_planner 全局路徑規(guī)劃
navfn 全局路徑規(guī)劃,舊版本的,有bug
carrot_planner 一個簡單的全局路徑規(guī)劃器
clear_costmap_recovery 清除代價(jià)地圖的恢復(fù)行為
costmap_2d 實(shí)現(xiàn)2d代價(jià)地圖
fake_localization acml的API接口
map_server 提供地圖數(shù)據(jù),yaml或者是image
move_slow_and_clear 緩慢移動修復(fù)機(jī)制,會限制機(jī)器人速度
nav_core 路徑規(guī)劃接口類
rotate_recovery 旋轉(zhuǎn)恢復(fù)
voxel_grid 三維代價(jià)地圖
全局規(guī)劃器有 3 個:
(1)carrot_planner
carrot_planner 檢查需要到達(dá)的目標(biāo)是不是一個障礙物,如果是一個障礙物,它就將目標(biāo)點(diǎn)替換成一個附近可接近的點(diǎn)。因此,這個模塊其實(shí)并沒有做任何全局規(guī)劃的工作。在復(fù)雜的室內(nèi)環(huán)境中,這個模塊并不實(shí)用。
(2)navfn
navfn使用 Dijkstra 算法找到最短路徑。
(3)global planner
global planner是navfn的升級版。
它相對于navfn增加了更多的選項(xiàng):支持 A* 算法;可以切換二次近似;切換網(wǎng)格路徑;
三、Global Planner 全局路徑規(guī)劃
該文件下的內(nèi)容:10個頭文件,8個源文件
看其中A*算法的文件
先做一個算例,結(jié)合算例理解
以下文件會包含其他文件,需要整體看,這里先整理三個文件,其他的慢慢來
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)對應(yīng)的點(diǎn)及代價(jià)
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) {
} //命名空間名::標(biāo)識符名?
/*
costs: 地圖指針
cycles:循環(huán)次數(shù)
*/
bool AStarExpansion::calculatePotentials(unsigned char* costs, double start_x, double start_y, double end_x, double end_y,int cycles, float* potential) //傳參
{
//queue_為啟發(fā)式搜索到的向量隊(duì)列:
queue_.clear(); //清空函數(shù):將隊(duì)列清空
//將起點(diǎn)放入隊(duì)列
int start_i = toIndex(start_x, start_y);
//(1,2)
//push_back:向數(shù)據(jù)結(jié)構(gòu)中添加元素
queue_.push_back(Index(start_i, 0));
//首先將start點(diǎn)以及其代價(jià)加入,即(13,0)
//std::fill函數(shù)的作用是:將一個區(qū)間的元素都賦予指定的值,即在(first, last)范圍內(nèi)填充指定值
//將所有點(diǎn)的potential都設(shè)為一個極大值,potential就是估計(jì)值g,f=g+h
std::fill(potential, potential + ns_, POT_HIGH); //?
//將起點(diǎn)的potential設(shè)為0
potential[start_i] = 0;
//終點(diǎn)對應(yīng)的序號
int goal_i = toIndex(end_x, end_y);//(4,4)
int cycle = 0;
//進(jìn)入循環(huán),繼續(xù)循環(huán)的判斷條件為只要隊(duì)列大小大于0且循環(huán)次數(shù)小于cycles
//代碼中cycles = 2 *nx * ny, 即為所有格子數(shù)的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();
//如果到了目標(biāo)點(diǎn),就結(jié)束了
int i = top.i;
if (i == goal_i)
return true;
// 對前后左右四個點(diǎn)執(zhí)行add函數(shù),將代價(jià)最小點(diǎn)i周圍點(diǎn)加入搜索隊(duì)里并更新代價(jià)值
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函數(shù):添加點(diǎn)并更新代價(jià)函數(shù);
如果是已經(jīng)添加的點(diǎn)則忽略,根據(jù)costmap的值如果是障礙物的點(diǎn)也忽略。
potential[next_i]是起點(diǎn)到當(dāng)前點(diǎn)的cost即g(n),
distance * neutral_cost_是當(dāng)前點(diǎn)到目的點(diǎn)的cost即h(n)。
f(n) = g(n) + h(n)
*/
// potential 存儲所有點(diǎn)的g(n),即從初始點(diǎn)到節(jié)點(diǎn)n的實(shí)際代價(jià)
// prev_potentia 當(dāng)前點(diǎn)的f
void AStarExpansion::add(unsigned char* costs, float* potential, float prev_potential, int next_i, int end_x,int end_y)
{
//next_i 點(diǎn)不在網(wǎng)格內(nèi),忽略
if (next_i < 0 || next_i >= ns_) //ns_?
return;
//未搜索的點(diǎn)cost為POT_HIGH,如小于該值,則為已搜索點(diǎn),跳過;
//potential[next_i]是起點(diǎn)到當(dāng)前點(diǎn)的cost即g(n)
if (potential[next_i] < POT_HIGH) //POT_HIGH?
return;
//障礙物點(diǎn),忽略?
if(costs[next_i]>=lethal_cost_ && !(unknown_ && costs[next_i]==costmap_2d::NO_INFORMATION))
return;
// potential[next_i]是起點(diǎn)到當(dāng)前點(diǎn)的cost即g(n)
// potential 存儲所有點(diǎn)的g(n),即從初始點(diǎn)到節(jié)點(diǎn)n的實(shí)際代價(jià)
// prev_potentia 當(dāng)前點(diǎn)的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 ?
//啟發(fā)式函數(shù):即h(n) 從節(jié)點(diǎn)n到目標(biāo)點(diǎn)最佳路徑的估計(jì)代價(jià),這里選用了曼哈頓距離
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
// 由于這只是格子的個數(shù),還有乘上每個格子的真實(shí)距離或者是分辨率,所以最后的H = distance *neutral_cost_
// 因此最后的F = potential[next_i] + distance *neutral_cost_ (F=g+h)
// 將當(dāng)前點(diǎn)next_i以及對應(yīng)的cost加入
queue_.push_back(Index(next_i, potential[next_i] + distance * neutral_cost_));
//potential[next_i]:是起點(diǎn)到當(dāng)前點(diǎn)的cost即g(n)
//distance * neutral_cost_:是當(dāng)前點(diǎn)到目的點(diǎn)的cost即h(n)。
//f(n)=g(n)+h(n):計(jì)算完這兩個cost后,加起來即為f(n),將其存入隊(duì)列中。
//對加入的再進(jìn)行堆排序, 把最小代價(jià)點(diǎn)放到front隊(duì)頭queue_[0]
//數(shù)據(jù)結(jié)構(gòu)?
std::push_heap(queue_.begin(), queue_.end(), greater1());
}
}
?
3、potential_calculate.h
neutral_cost_ 設(shè)定的一個默認(rèn)值,為50
calculatePotential()計(jì)算根據(jù)use_quadratic的值有下面兩個選擇:
若為TRUE, 則使用二次曲線計(jì)算
若為False, 則采用簡單方法計(jì)算, return prev_potential + cost。即:costs[next_i] + neutral_cost_+ prev_potential
地圖代價(jià)+單格距離代價(jià)(初始化為50)+之前路徑代價(jià)為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當(dāng)前點(diǎn)的f不小于0的時候,返回的是prev_potential + cost
//以start_cost為例,最開始給其賦值是0,因此返回就是cost,可以理解為到下一個柵格的距離,或者是分辨率。
//在計(jì)算完potential=g值后開始計(jì)算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_;
};
}
?
評論