ROS navigation planner--GlobalPlanner

在关于move base 的配置文章中，关于planner部分的分析有一句：

对于global planner，可以采用以下三种实现之一：
"navfn/NavfnROS","global_planner/GlobalPlanner","carrot_planner/CarrotPlanner"

本文分析其中的一种实现"global_planner/GlobalPlanner" ：
首先，move base是通过plugin调用它的：
文件bgp_plugin.xml

<library path="lib/libglobal_planner">
  <class name="global_planner/GlobalPlanner" type="global_planner::GlobalPlanner" base_class_type="nav_core::BaseGlobalPlanner">
    <description>
      A implementation of a grid based planner using Dijkstras or A*
    description>
  class>
library>

然后在package.xml的配置中，加入如下行：

  <export>
      "${prefix}/bgp_plugin.xml" />
  export>

planner 部分的入口：

int main(int argc, char** argv) {
    ros::init(argc, argv, "global_planner");
    tf::TransformListener tf(ros::Duration(10));
    costmap_2d::Costmap2DROS lcr("costmap", tf);
    //需要一个costmap，传递给planner初始化
    global_planner::PlannerWithCostmap pppp("planner", &lcr);

    ros::spin();
    return 0;
}

在class PlannerWithCostmap 的构造函数中：

PlannerWithCostmap::PlannerWithCostmap(string name, Costmap2DROS* cmap) :GlobalPlanner(name, cmap->getCostmap(), cmap->getGlobalFrameID()) 
{
    ros::NodeHandle private_nh("~");
    cmap_ = cmap;
    make_plan_service_ = private_nh.advertiseService("make_plan", &PlannerWithCostmap::makePlanService, this);
    pose_sub_ = private_nh.subscribe<rm::PoseStamped>("goal", 1, &PlannerWithCostmap::poseCallback, this);
}

这里开启了两个线程，第一个是提供plan service，一旦由请求，就调用bool success = makePlan(req.start, req.goal, path);；第二个是去订阅goal，拿到goal之后，就调用makePlan(start, *goal, path); 。
而的定义是：bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, double tolerance, std::vector& plan)
主要的逻辑部分在于class GlobalPlanner：

makePlan 函数体的主要内容：

bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, double tolerance, std::vector& plan) 
{
 plan.clear();//清除plan
 double wx = start.pose.position.x;
 double wy = start.pose.position.y;
 costmap_->worldToMap(wx, wy, start_x_i, start_y_i)//将start转到地图的cell表达坐标
 ...
 //同样，将目标点也转到map表达
costmap_->worldToMap(wx, wy, goal_x_i, goal_y_i))
...
//然后将开始点设置为FREE_SPACE
 clearRobotCell(start_pose, start_x_i, start_y_i);

//分配空间，大小和costmap一样大
  p_calc_->setSize(nx, ny);
  planner_->setSize(nx, ny);
  path_maker_->setSize(nx, ny);
  potential_array_ = new float[nx * ny];

//调用以下函数将costmap的四个边的全部cell都设置为LETHAL_OBSTACLE
  outlineMap(costmap_->getCharMap(), nx, ny, costmap_2d::LETHAL_OBSTACLE);

//计算potential
    bool found_legal = planner_->calculatePotentials(costmap_->getCharMap(), start_x, start_y, goal_x, goal_y,  nx * ny * 2, potential_array_);

    planner_->clearEndpoint(costmap_->getCharMap(), potential_array_, goal_x_i, goal_y_i, 2);

   if (found_legal)
      {
      if (getPlanFromPotential(start_x, start_y, goal_x, goal_y, goal, plan))  //extract the 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);
        } 
        }
 //增加orientation 信息      
 orientation_filter_->processPath(start, plan);
}

在上面的分析中，有两个核心的东西：

//计算potential
    bool found_legal = planner_->calculatePotentials(costmap_->getCharMap(), start_x, start_y, goal_x, goal_y,  nx * ny * 2, potential_array_);
//提取plan
    getPlanFromPotential(start_x, start_y, goal_x, goal_y, goal, plan)

计算potential，调用的是：

Expander::virtual bool calculatePotentials(unsigned char* costs, double start_x, double start_y, double end_x, double end_y,int cycles, float* potential) = 0;

其实现有两种：

class AStarExpansion : public Expander 
class DijkstraExpansion : public Expander 

而后面的提取plan，调用的是：

Traceback::virtual bool getPath(float* potential, double start_x, double start_y, double end_x, double end_y, std::vector<std::pair<float, float> >& path) = 0;

这是一个纯虚函数，实现有两种：

class GradientPath : public Traceback
class GridPath : public Traceback 

噢噢，接下来就到算法核心部分了，关于这四种算法的具体分析是怎么实现：
A*, Dijkstra，gradient_path, grid_path 。

A*算法：

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);
    queue_.push_back(Index(start_i, 0));//push the start point into queue_

    std::fill(potential, potential + ns_, POT_HIGH);//initial all the potential as very large value
    potential[start_i] = 0;//initial the start position at potential as 0

    int goal_i = toIndex(end_x, end_y);
    int cycle = 0;

    while (queue_.size() > 0 && cycle < cycles) 
    {
        Index top = queue_[0];//get the Index that with mini cost
        std::pop_heap(queue_.begin(), queue_.end(), greater1());//build the heap sort
        queue_.pop_back();//remove the Index with mini cost

        int i = top.i;//the Index's i from (i,cost)
        if (i == goal_i)//stop condition
            return true;
        //add the neighborhood 4 points into the search scope
        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);
    }
    return false;
}

void AStarExpansion::add(unsigned char* costs, float* potential, float prev_potential, int next_i, int end_x,int end_y)
 {
    if (potential[next_i] < POT_HIGH)//it means the potential cell has been explored
        return;

    if(costs[next_i]>=lethal_cost_ &&
     !(unknown_ && costs[next_i]==costmap_2d::NO_INFORMATION))//it means this cell is obstacle
        return;
    // compute the next_i cell in potential
    // costs[next_i] + neutral_cost_:original cost + extra cost
    // potential[next_i] means the cost from start to current
    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);
    //distance * neutral_cost_ means the current to the target
    //f(n)=g(n)+h(n)
    queue_.push_back(Index(next_i, potential[next_i] + distance * neutral_cost_));
    std::push_heap(queue_.begin(), queue_.end(), greater1());//sort about the previous insert element
}

而这行potential[next_i] = p_calc_->calculatePotential(potential, costs[next_i] + neutral_cost_, next_i, prev_potential);的调用是：

 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);
    }
    return prev_potential + cost;
}

通过设置的代价potential[next_i] + distance * neutral_cost_ 代表了A*的核心思想，寻找距离start和goal代价距离都最少的点。
A* 算法是策略寻路，不保证一定是最短路径。
Dijkstra 算法是全局遍历，确保运算结果一定是最短路径。
Dijkstra 需要载入全部数据，遍历搜索。

ROS的DijkstraExpansion实现，代码明显不如A*的实现优雅，还使用了宏定义函数，额，所以暂时放一放吧。

然后来看grid_path的实现：

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;

    int start_index = getIndex(start_x, start_y);

    path.push_back(current);
    int c = 0;
    int ns = xs_ * ys_;

    while (getIndex(current.first, current.second) != start_index) {
        float min_val = 1e10;
        int min_x = 0, min_y = 0;
        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;
        current.first = min_x;
        current.second = min_y;
        path.push_back(current);

        if(c++>ns*4){
            return false;
        }

    }
    return true;
}

这个算法很好理解，首先将goal所在的点的(x,y)塞到path，然后搜索当前的点的四周的四个临近点，选取这四个临近点的potential的值最小的点min，然后把这点塞到path，然后再将当前点更新为min这点，由于start 点的potential的值是0，所以这是个梯度下降的方法。