- 文中“A*算法”、“grid_path算法”注释转自“移动机器人导航技术笔记（1）-global_planner学习”。
- 网上文章在说如何优化，像“ROS:一种简单的基于global_planner的全局路径优化方法”。
- 成本。在不断遍历栅格时，把两栅格之间衡量距离的那个数值称为成本。没用距离，考虑到距离一般表示两点之间的线段长度。也不用代价，这里专门用代价指代价地图中的栅格值。代码中，成本矩阵potential_array中单元值potential就是start到该点需要的成本。
- [a,b]。表示结点a到结点b的实际最短路径成本。设起始结点为S，当前结点为n，目标结点为G，于是n的实际成本应该是f*(n)=g*(n)+h*(n)，其中g*(n)=[S,n]，h*(n)=[n,G]，对于是g*(n)是比较容易得到的，在搜索的过程中我们可以按搜索的顺序对它进行累积计算。
- 判断路径下一个点a，是要把足迹放在该点a上进行判断吗？从a点出发，向前有三个方向，难道它要把足迹转三个方向都判断一次？——计算全局路径时就没用到足迹结构(footprint)。那如何做到考虑足迹？在判断某栅格是否可衍生时，剔除的不仅仅障碍栅格，还包括内切圆(膨胀)栅格。这意味着，只要该栅格被放入OPEN表，它和障碍栅格间至少距离了内切圆半径，也就达到考虑足迹目的。剔除膨胀栅格参考DijkstraExpansion::getCost，当中lethal_cost_是253(INSCRIBED_INFLATED_OBSTACLE)，“c < lethal_cost_ - 1”表示只有代价小于“INSCRIBED_INFLATED_OBSTACLE - 1”的栅格才会放入OPEN表。
- 起点（start）和终点（goal）距离非常近，makePlan会因为距离太近而失败吗？——不会。即使近到同一个栅格，也不会因距离近而失败。
- makePlan时，会修改地图中3类栅格的代价。1）start所在栅格，改为FREE_SPACE。2）goal和它周围8个栅格，改为FREE_SPACE。3）边界那一圈栅格，改为LETHAL_OBSTACLE。(2)是rose_ros新加的，这么做的原因：导航时，机器人并不是要准确到达goal指定栅格，到达附近就行。加之goal栅格代价“>=lethal_cost_ - 1”时，calculatePotentials会失败，导致makePlan失败。另外，(2)在makePlan后，相关栅格会恢复回原貌，(1)、(3)则不会。
- 同步全局代价地图。1）makePlan用的代价地图就是move_base中的planner_costmap_ros_。planner_costmap_ros_在全局代价地图的upateMap线程更新，makePlan运行在planThread线程，它们之间须要同步。2）在makePlan，首先会申请掉costmap锁，防止updateMap更新地图。同样，updateMap也要申请掉costmap锁，防止makePlan用这地图去规划路径。3）makePlan规完出一条路径需要点时间，这会不会影响其它线程使用到“最新”地图。local_costmap这个Costmap2DROS不用全局地图，因而全局地图的作用可说就是makePlan，所以只要让每次makePlan使用最新地图，makePlan时间长点不会对其它模块造成太多影响。
- 是否可以用全局规划间接实现探索建图？假设机器人站在一个代价已知的栅格start，然后指定一个未知区域栅格goal，makePlan能算出一条从start到goal的全局路径吗。——不能。makePlan要求start到goal的代价地图绝大多数是已知代价。探索时，栅格代价很多是未知的。
一、nav_core::BaseGlobalPlanner
对全局规划（global planner），ros提供的三种实现：navfn/NavfnROS、global_planner/GlobalPlanner、carrot_planner/CarrotPlanner，本文分析其中一种实现：global_planner/GlobalPlanner。它们都从BaseGlobalPlanner派生，主要操作是两个。
1.1 initialize(name, costmap)
初始化，算法实例p_calc_、planner_、path_maker_的选取。主要是以下三个实例。
- 计算起始点s到点n的成本，即g(n)。p_calc_：PotentialCalculator::calculatePotential()、 QuadraticCalculator::calculatePotential()
- 计算成本矩阵potential_array_，内中要使用p_calc_。planner_：DijkstraExpansion::calculatePotentials()、 AStarExpansion::calculatePotentials()
- 从成本矩阵中提取路径path。path_maker_：GridPath::getPath()、 GradientPath::getPath()
1.2 makePlan(start, goal, plan)
生成全局路径。要生成路径时，调用者只要须要调用一次makePlan，它主要两个步骤。
- 计算成本矩阵potential_array(planner_->calculatePotentials)
- 从成本矩阵中提取路径plan(path_maker_->getPath)
二、GlobalPlanner::makePlan
- [IN]start：此刻机器人中心在/map坐标系下的位姿。
- [IN]goal：此次导航要到达的目标。
- [IN]tolerance：默认0。
- [IN]plan：存储计算出的全局路径。
bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
double tolerance, std::vector<geometry_msgs::PoseStamped>& plan) {
boost::mutex::scoped_lock lock(mutex_);
if (!initialized_) {
ROS_ERROR(
"This planner has not been initialized yet, but it is being used, please call initialize() before use");
return false;
}
//clear the plan, just in case
plan.clear();
ros::NodeHandle n;
std::string global_frame = frame_id_;
//until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame
if (goal.header.frame_id != global_frame) {
return false;
}
if (start.header.frame_id != global_frame) {
return false;
}goal、start必须是/map坐标系下的位姿。
double wx = start.pose.position.x;
double wy = start.pose.position.y;
unsigned int start_x_i, start_y_i, goal_x_i, goal_y_i;
double start_x, start_y, goal_x, goal_y;
if (!costmap_->worldToMap(wx, wy, start_x_i, start_y_i)) {用worldToMap把world坐标系下的坐标(wx, wy)转换到map坐标系下坐标。如果(wx, wy)不在代价地图内，它将返回false。
return false;
}
if(old_navfn_behavior_){对libros，old_navfn_behavior_总是false，不会进这入口。
start_x = start_x_i;
start_y = start_y_i;
} else{
worldToMap(wx, wy, start_x, start_y);
}此个worldToMap对应的是GlobalPlanner::worldToMap。它和costmap_->worldToMap有什么区别？——它算出的map坐标系x、y坐标类型不是int，而是double。因为map坐标系是栅格中心，所以它要都要减去convert_offset_。假设convert_offset_=0.5，那以下等式成立。
- start_x = double时的start_x_i - convert_offset_。
- start_y = double时的start_x_y - convert_offset_。
wx = goal.pose.position.x;
wy = goal.pose.position.y;
if (!costmap_->worldToMap(wx, wy, goal_x_i, goal_y_i)) {
ROS_WARN_THROTTLE(1.0,
"The goal sent to the global planner is off the global costmap. Planning will always fail to this goal.");
return false;
}
if(old_navfn_behavior_){
goal_x = goal_x_i;
goal_y = goal_y_i;
}else{
worldToMap(wx, wy, goal_x, goal_y);
}类似start.pose.position，算出goal.pose.position在map坐标系下的坐标。
//clear the starting cell within the costmap because we know it can't be an obstacle
clearRobotCell(start, start_x_i, start_y_i);
// It is not necessary robot to reach the goal-cell, just reach near.
std::vector<SDL_Point> locs;
locs.push_back(SDL_Point{(int)goal_x_i, (int)goal_y_i});
costmap_->getMax8Cells(goal_x_i, goal_y_i, nullptr, &locs);
costmap_2d::tset_cost_restorer FREE_SPACE_cost_restorer(*costmap_, locs, costmap_2d::FREE_SPACE);makePlan时，会修改地图中3类栅格的代价。
- start所在栅格，改为FREE_SPACE。见上面的clearRobotCell(...)。
- goal和它周围8个栅格，改为FREE_SPACE。见上面的FREE_SPACE_cost_restorer。这点是rose_ros新加的，这么做的原因：导航时，机器人并不是要准确到达goal指定栅格，到达附近就行。加之goal栅格代价“>=lethal_cost_ - 1”时，calculatePotentials会失败，导致makePlan失败。
- 边界那一圈栅格，改为LETHAL_OBSTACLE。见下面的outlineMap(...)。
另外，(2)在makePlan后，相关栅格会恢复回原貌，(1)、(3)则不会。
int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY();
在全局代价地图，是静态层决定这个代价代图尺寸，所以(nx, ny)就是栅格图尺寸。
//make sure to resize the underlying array that Navfn uses p_calc_->setSize(nx, ny); planner_->setSize(nx, ny); path_maker_->setSize(nx, ny); potential_array_ = new float[nx * ny]; if(outline_map_) outlineMap(costmap_->getCharMap(), nx, ny, costmap_2d::LETHAL_OBSTACLE);
在libros，outline_map_是true，因而总会执行outlineMap。outlineMap作用是把代价格地图中上、下两行，左、右两行栅格置为costmap_2d::LETHAL_OBSTACLE。
bool found_legal = planner_->calculatePotentials(costmap_->getCharMap(), start_x, start_y, goal_x, goal_y, nx * ny * 2, potential_array_); FREE_SPACE_cost_restorer.manual_restore();
计算成本矩阵，结果放在potential_array_。采用的是狄克斯特拉寻路时，对应DijkstraExpansion::calculatePotentials。
用代价地图计算出成本矩阵后，调用FREE_SPACE_cost_restorer.manual_restore()，将(2)修改的相关栅格恢复成原貌。
if(!old_navfn_behavior_)
planner_->clearEndpoint(costmap_->getCharMap(), potential_array_, goal_x_i, goal_y_i, 2);
if(publish_potential_)
publishPotential(potential_array_);
if (found_legal) {
//extract the plan
if (getPlanFromPotential(start_x, start_y, goal_x, goal_y, goal, plan)) {
//make sure the goal we push on has the same timestamp as the rest of the plan
geometry_msgs::PoseStamped goal_copy = goal;
goal_copy.header.stamp = ros::Time::now();
plan.push_back(goal_copy);found_legal是true，意味着成功计算出成本矩阵。接下是调用getPlanFromPotential，从成本矩阵得到上层须要的、std::vector<geometry_msgs::PoseStamped>类型的plan。在getPlanFromPotential，要根据参数use_grid_path调用不同的提取算法，true时是GridPath::getPath，否则GradientPath::getPath。libros用的是GradientPath::getPath。
} else {
ROS_ERROR("Failed to get a plan from potential when a legal potential was found. This shouldn't happen.");
}
}else{
ROS_ERROR("Failed to get a plan.");
}
// add orientations if needed
orientation_filter_->processPath(start, plan);
processPath用于调整路径中每个点的角度(orientation)。之前生成路径时，每个点已经有个非零角度，这里按规则统一调整角度。对libros，omode_值是FORWARD，在这种模式下，对第i个点的角度等于atan2(y1-y0, x1-x0)，(x0, y0)是i点坐标，(x1, y1)是i+1点坐标。即所有点的角度是指向下一个点。图2显示了omode_==FORWARD调整后的各点角度。
//publish the plan for visualization purposes publishPlan(plan); delete[] potential_array_; return !plan.empty(); }
在图1，rose_ros用的是绿色实例，但非绿色的更容易理解。为尽快知道各实例在makePlan中作用，以及makePlan能做什么、不能做什么，下面让深入A*算法、PotentialCalculator、GridPath。
关于DijkstraExpansion算法，参考“global_planner解析（4）”。
三、计算成本矩阵：A*算法
3.1 calculatePotentials
<libros>/global_planner/src/astar.cpp
------
bool AStarExpansion::calculatePotentials(unsigned char* costs, double start_x, double start_y, double end_x, double end_y,
int cycles, float* potential) {
queue_.clear();
int start_i = toIndex(start_x, start_y);
// 1.将起点放入队列
queue_.push_back(Index(start_i, 0));
// 2.将所有点的potential都设为一个极大值
std::fill(potential, potential + ns_, POT_HIGH);
// 3.将起点的potential设为0
potential[start_i] = 0;
int goal_i = toIndex(end_x, end_y);
int cycle = 0;
// 4.进入循环，继续循环的判断条件为只要队列大小大于0且循环次数小于所有格子数的2倍
while (queue_.size() > 0 && cycle < cycles) {
// 5.得到最小cost的索引，并删除它，如果索引指向goal(目的地)则退出算法，返回true
Index top = queue_[0];
std::pop_heap(queue_.begin(), queue_.end(), greater1());
queue_.pop_back();
int i = top.i;
if (i == goal_i)
return true;
// 6.对右、左、上、下四个点执行add函数
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;
}3.2 AStarExpansion::add
add函数中，如果是已经添加的的点则忽略，根据costmap的值如果是障碍物的点也忽略。
void AStarExpansion::add(unsigned char* costs, float* potential, float prev_potential, int next_i, int end_x,
int end_y) {
if (next_i < 0 || next_i >= ns_)
return;
if (potential[next_i] < POT_HIGH)
return;
if(costs[next_i]>=lethal_cost_ && !(unknown_ && costs[next_i]==costmap_2d::NO_INFORMATION))
return;
// p_calc_->calculatePotential用于计算起点到当前点next_i的potential(成本)
potential[next_i] = p_calc_->calculatePotential(potential, costs[next_i] + neutral_cost_, next_i, prev_potential);
int x = next_i % nx_, y = next_i / nx_;
float distance = abs(end_x - x) + abs(end_y - y);
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)，将其存入队列中。
std::push_heap(queue_.begin(), queue_.end(), greater1());
}A* 算法是策略寻路，不保证一定是最短路径。Dijkstra算法是全局遍历，确保运算结果一定是最短路径。 Dijkstra需要载入全部数据，遍历搜索。
四、计算起点到当前点n的potential(成本)：PotentialCalculator
- neutral_cost_：平均代价。不知道那些未知栅格是什么值，但有时须要“认为”它们有值，那就认为就是这个平均值。举个例子，要计算h(n)，即栅格n到目标栅格成本，算出n到gobal的线段长distance后，认为distance*neutral_cost_就是h(n)。
calculatePotential用于计算起始点s到点n的成本，即g(n)。
<libros>/include/global_planner/potential_calculator.h
------
class PotentialCalculator {
virtual float calculatePotential(float* potential, unsigned char cost, int n, float prev_potential=-1){
if (prev_potential < 0){
// 何时会进这个入口，下面会说。
// get min of neighbors
// 分别求出左、右、下、上最小的代价值
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);
}
// 成本 = 当前已经累计的成本 + 代价地图中栅格n代价
return prev_potential + cost;
}
}g(n) = g(n-1) + (代价地图中栅格n的代价+neutral_cost_)。
prev_potential<0，这值往往就是-1。此时调用者告知calculatePotential，我不知道g(n-1)，即prev_potential，你内部算出一个g(n-1)。如何计算g(n-1)？——PotentialCalculator::calculatePotential的方法是取出该栅格前、后、左、右最小成本的栅格，把它的成本作为做为起点到栅格n-1的g(n-1)
代码中何会出现prev_potential<0？——回看makePath代码，它算出成本矩阵后，会调用planner_->clearEndpoint。
planner_->clearEndpoint(costmap_->getCharMap(), potential_array_, goal_x_i, goal_y_i, 2);
class Expander {
void clearEndpoint(unsigned char* costs, float* potential, int gx, int gy, int s){
int startCell = toIndex(gx, gy);
for(int i=-s;i<=s;i++){
for(int j=-s;j<=s;j++){
int n = startCell+i+nx_*j;
if(potential[n]<POT_HIGH)
continue;
float c = costs[n]+neutral_cost_;
float pot = p_calc_->calculatePotential(potential, c, n);
potential[n] = pot;
}
}
}
}clearEndpoint会调用calculatePotential，调用时不给第4个参数prev_potential，即此时的prev_potential会是-1。
此时(gx, gy)是目标点在map坐标系坐标。可以肯定它所在栅格成本不会是POT_HIGH，clearEndpoint的功能是检查周围8个栅格，如果没有设置过成本的，以“prev_potential=-1”调用calculatePotential。此时如何给这些栅格n设置成本？——上面已说过，PotentialCalculator版本的方法是取出该栅格前、后、左、右最小成本的栅格，把它的成本作为做为起点到栅格n-1的g(n-1)。g(n-1)加上n代价后，就是起点到栅格n的g(n)。因为“8”栅格成本取的是最小g(n-1)，这可能就是函数名中“clear”的语义吧。
顺便说下，在QuadraticCalculator::calculatePotential，计算g(n)时就没使用prev_potential参数。
五、从成本矩阵提取出路径：GridPath算法
<libros>/global_planner/src/grid_path.cpp
------
bool GridPath::getPath(float* potential, double start_x, double start_y, double end_x, double end_y,
std::vector<std::pair<float, float> >& path) {
std::pair<float, float> current;
current.first = end_x;
current.second = end_y;
// 1.将goal（目的地）所在的点的(x,y)作为当前点加入path
int start_index = getIndex(start_x, start_y);
path.push_back(current);
int c = 0;
int ns = xs_ * ys_;
// 2.进入循环，继续循环的条件为当前点的索引不是起始点
while (getIndex(current.first, current.second) != start_index) {
float min_val = 1e10;
int min_x = 0, min_y = 0;
// 3.搜索当前点的四周的四个临近点，
// 选取这四个临近点的potential的值最小的点
for (int xd = -1; xd <= 1; xd++) {
for (int yd = -1; yd <= 1; yd++) {
if (xd == 0 && yd == 0)
continue;
int x = current.first + xd, y = current.second + yd;
int index = getIndex(x, y);
if (potential[index] < min_val) {
min_val = potential[index];
min_x = x;
min_y = y;
}
}
}
if (min_x == 0 && min_y == 0)
return false;
// 4.将potential值最小的点更改为当前点，并加入path
current.first = min_x;
current.second = min_y;
path.push_back(current);
if(c++>ns*4){
return false;
}
} // 返回2，继续循环
return true;
}这个模块负责使用计算好的potential矩阵生成一条全局规划路径。算法很好理解，首先将goal所在的点的(x, y)塞到path，然后搜索当前的点的四周的八个临近点，选取这八个临近点的potential的值最小的点min，然后把这点塞到path，然后再将当前点更新为min这点，由于start点的potential的值是0，所以这是个梯度下降的方法。