ROS naviagtion analysis: GlobalPlanner

在ROS navigation stack中，关于机器人全局路径规划BaseGlobalPlanner的plugin有三种实现，分别是：

"navfn/NavfnROS","global_planner/GlobalPlanner","carrot_planner/CarrotPlanner"

其中常用的是global_planner/GlobalPlanner，它是navfn/NavfnROS的改进版本。关于两者是什么关系，以及为什么都存在于ROS navigation中，可以参考该链接：http://answers.ros.org/question/98511/global_planner-package-with-a-planner-question/

本文分析其中的一种实现"global_planner/GlobalPlanner" 。源码位于navigation stack中的global_planner源码包，包含了A*算法和Dijkstra算法的实现。

首先，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>

global_planner main函数的入口，核心代码行是：global_planner::PlannerWithCostmap pppp("planner", &lcr);

int main(int argc, char** argv) {
    ros::init(argc, argv, "global_planner");

    tf2_ros::Buffer buffer(ros::Duration(10));
    tf2_ros::TransformListener tf(buffer);

    costmap_2d::Costmap2DROS lcr("costmap", buffer);

    global_planner::PlannerWithCostmap pppp("planner", &lcr);

    ros::spin();
    return 0;
}

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);
}

第一：发布一个make_plan的service，一旦有请求，就回调PlannerWithCostmap::makePlanService方法，执行makePlan(req.start, req.goal, path); 如果makePlan成功，将vector<PoseStamped> path 赋值到resp.path中，并返回true。

bool PlannerWithCostmap::makePlanService(navfn::MakeNavPlan::Request& req, navfn::MakeNavPlan::Response& resp) {
    vector<PoseStamped> path;

    req.start.header.frame_id = "map";
    req.goal.header.frame_id = "map";
    bool success = makePlan(req.start, req.goal, path);
    resp.plan_found = success;
    if (success) {
        resp.path = path;
    }

    return true;
}

第二: 订阅一个goal的topic，一旦收到goal的发布，就回调PlannerWithCostmap::poseCallback方法，根据机器人当前所在地图的位置和目标goal的位置，执行makePlan(global_pose, *goal, path)，计算出vector<PoseStamped> path数据。

void PlannerWithCostmap::poseCallback(const rm::PoseStamped::ConstPtr& goal) {
    geometry_msgs::PoseStamped global_pose;
    cmap_->getRobotPose(global_pose);
    vector<PoseStamped> path;
    makePlan(global_pose, *goal, path);
}

第三：主要的逻辑部分在于class GlobalPlanner中初始化函数initialize和路径规划函数makePlan。

GlobalPlanner(name, cmap->getCostmap(), cmap->getGlobalFrameID())

GlobalPlanner的构造方法有：

GlobalPlanner::GlobalPlanner() :
        costmap_(NULL), initialized_(false), allow_unknown_(true) {
}

或者：

GlobalPlanner::GlobalPlanner(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id) :
        costmap_(NULL), initialized_(false), allow_unknown_(true) {
    //initialize the planner
    initialize(name, costmap, frame_id);
}

初始化函数initialize

void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id) {
    if (!initialized_) {
        ros::NodeHandle private_nh("~/" + name);
        costmap_ = costmap;
        frame_id_ = frame_id;

        unsigned int cx = costmap->getSizeInCellsX(), cy = costmap->getSizeInCellsY();

        private_nh.param("old_navfn_behavior", old_navfn_behavior_, false);
        if(!old_navfn_behavior_)
            convert_offset_ = 0.5;
        else
            convert_offset_ = 0.0;

        bool use_quadratic;
        private_nh.param("use_quadratic", use_quadratic, true);
        if (use_quadratic)
            p_calc_ = new QuadraticCalculator(cx, cy);
        else
            p_calc_ = new PotentialCalculator(cx, cy);

        bool use_dijkstra;
        private_nh.param("use_dijkstra", use_dijkstra, true);
        if (use_dijkstra)
        {
            DijkstraExpansion* de = new DijkstraExpansion(p_calc_, cx, cy);
            if(!old_navfn_behavior_)
                de->setPreciseStart(true);
            planner_ = de;
        }
        else
            planner_ = new AStarExpansion(p_calc_, cx, cy);

        bool use_grid_path;
        private_nh.param("use_grid_path", use_grid_path, false);
        if (use_grid_path)
            path_maker_ = new GridPath(p_calc_);
        else
            path_maker_ = new GradientPath(p_calc_);

        orientation_filter_ = new OrientationFilter();

        plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
        potential_pub_ = private_nh.advertise<nav_msgs::OccupancyGrid>("potential", 1);

        private_nh.param("allow_unknown", allow_unknown_, true);
        planner_->setHasUnknown(allow_unknown_);
        private_nh.param("planner_window_x", planner_window_x_, 0.0);
        private_nh.param("planner_window_y", planner_window_y_, 0.0);
        private_nh.param("default_tolerance", default_tolerance_, 0.0);
        private_nh.param("publish_scale", publish_scale_, 100);

        make_plan_srv_ = private_nh.advertiseService("make_plan", &GlobalPlanner::makePlanService, this);

        dsrv_ = new dynamic_reconfigure::Server<global_planner::GlobalPlannerConfig>(ros::NodeHandle("~/" + name));
        dynamic_reconfigure::Server<global_planner::GlobalPlannerConfig>::CallbackType cb = boost::bind(
                &GlobalPlanner::reconfigureCB, this, _1, _2);
        dsrv_->setCallback(cb);

        initialized_ = true;
    } else
        ROS_WARN("This planner has already been initialized, you can't call it twice, doing nothing");

}

参数配置：

参数一：old_navfn_behavior （若在某些情况下,想让global_planner完全复制navfn的功能,那就设置为true,但是需要注意navfn是非常旧的ROS系统中使用的,现在已经都用global_planner代替navfn了,所以不建议设置为true.）

private_nh.param("old_navfn_behavior", old_navfn_behavior_, false);
if(!old_navfn_behavior_)
  convert_offset_ = 0.5;
else
  convert_offset_ = 0.0;

参数二：use_quadratic （设置为true,将使用二次函数近似函数,否则使用更加简单的计算方式,这样节省硬件计算资源.）

bool use_quadratic;
private_nh.param("use_quadratic", use_quadratic, true);
if (use_quadratic)
   p_calc_ = new QuadraticCalculator(cx, cy);
else
   p_calc_ = new PotentialCalculator(cx, cy);

参数三：use_dijkstra （设置为true,将使用dijkstra算法,否则使用A*算法.）

bool use_dijkstra;
private_nh.param("use_dijkstra", use_dijkstra, true);
if (use_dijkstra)
{
    DijkstraExpansion* de = new DijkstraExpansion(p_calc_, cx, cy);
    if(!old_navfn_behavior_)
        de->setPreciseStart(true);
    planner_ = de;
}
else
    planner_ = new AStarExpansion(p_calc_, cx, cy);

参数四：use_grid_path （如果设置为true,则会规划一条沿着网格边界的路径,偏向于直线穿越网格,否则将使用梯度下降算法,路径更为光滑点.）

bool use_grid_path;
private_nh.param("use_grid_path", use_grid_path, false);
if (use_grid_path)
    path_maker_ = new GridPath(p_calc_);
else
    path_maker_ = new GradientPath(p_calc_);

 参数五：allow_unknown（是否允许规划器规划穿过未知区域的路径,只设计该参数为true还不行,还要在costmap_commons_params.yaml中设置track_unknown_space参数也为true才行。）

参数六：planner_window_x，planner_window_y

参数七：default_tolerance（当设置的目的地被障碍物占据时,需要以该参数为半径寻找到最近的点作为新目的地点.）

参数八：publish_scale

参数九：lethal_cost  致命代价值,默认是设置为253,可以动态来配置该参数.

参数十：neutral_cost  中等代价值,默认设置是50,可以动态配置该参数.

参数十一：cost_factor  代价地图与每个代价值相乘的因子.

参数十二：publish_potential  是否发布costmap的势函数.

参数十三：orientation_mode  如何设置每个点的方向（None = 0,Forward = 1,Interpolate = 2,

                  ForwardThenInterpolate = 3,Backward = 4,Leftward = 5,Rightward = 6）（可动态重新配置）

参数十四：orientation_window_size  根据orientation_mode指定的位置积分来得到使用窗口的方向.默认值1,可以动态重新配置.

发布两个topic： plan , potential ，一个service ：make_plan，并且回调makePlanService方法，执行makePlan函数。

plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
potential_pub_ = private_nh.advertise<nav_msgs::OccupancyGrid>("potential", 1);

make_plan_srv_ = private_nh.advertiseService("make_plan", &GlobalPlanner::makePlanService, this);

 路径规划函数makePlan()

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) {
        ROS_ERROR(
                "The goal pose passed to this planner must be in the %s frame.  It is instead in the %s frame.", global_frame.c_str(), goal.header.frame_id.c_str());
        return false;
    }

    if (start.header.frame_id != global_frame) {
        ROS_ERROR(
                "The start pose passed to this planner must be in the %s frame.  It is instead in the %s frame.", global_frame.c_str(), start.header.frame_id.c_str());
        return false;
    }

    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)) {
        ROS_WARN(
                "The robot's start position is off the global costmap. Planning will always fail, are you sure the robot has been properly localized?");
        return false;
    }
    if(old_navfn_behavior_){
        start_x = start_x_i;
        start_y = start_y_i;
    }else{
        worldToMap(wx, wy, start_x, start_y);
    }

    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);
    }

    //clear the starting cell within the costmap because we know it can't be an obstacle
    clearRobotCell(start, start_x_i, start_y_i);

    int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY();

    //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];

    outlineMap(costmap_->getCharMap(), nx, ny, costmap_2d::LETHAL_OBSTACLE);

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

    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);
        } 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);

    //publish the plan for visualization purposes
    publishPlan(plan);
    delete potential_array_;
    return !plan.empty();
}

worldToMap 将start坐标点和goal的坐标点转换到map上；

clearRobotCell 清除Costmap中的起始单元格，因为我们知道它不会成为障碍。

outlineMap 将costmap的四个边的全部cell都设置为LETHAL_OBSTACLE

calculatePotentials 计算potential，调用的是：

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

getPlanFromPotential 提取plan数据

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;

 

参考文章：

http://answers.ros.org/question/98511/global_planner-package-with-a-planner-question/

http://www.voidcn.com/article/p-fhhnkxfi-kk.html

http://answers.ros.org/question/11388/navfn-algorism/?answer=16891#answer-container-16891

http://answers.ros.org/question/28366/why-navfn-is-using-dijkstra/