ROS navigation分析

在關於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>
      <nav_core plugin="${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<geometry_msgs::PoseStamped>& plan) 
主要的邏輯部分在於class GlobalPlanner： 

makePlan 函數體的主要內容：

bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, double tolerance, std::vector<geometry_msgs::PoseStamped>& 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，所以這是個梯度下降的方法。