全局路径规划(一) global_planner

概述

ROS 的navigation官方功能包提供了三种全局路径规划器：carrot_planner、global_planner、navfn，默认使用的是navfn。
继承关系 1.png

carrot_planner检查需要到达的目标是不是一个障碍物，如果是一个障碍物，它就将目标点替换成一个附近可接近的点。因此，这个模块其实并没有做任何全局规划的工作。在复杂的室内环境中，这个模块并不实用。
navfn使用Dijkstra算法找到最短路径。
global planner是navfn的升级版。它支持A*算法；可以切换二次近似；切换网格路径；

目前常用的是global_planner，需要先设定move_base的参数： base_global_planner: "global_planner/GlobalPlanner"。global_planner根据给定的目标位置进行总体路径的规划，只处理全局代价地图中的数据。提供快速的、内插值的全局规划，目前已经取代navfn。遵循navcore::navcore 包中指定的BaseGlobalPlanner接口。它接受costmap生成的全局代价地图规划出从起始点到目标点的路径，为local_planner规划路径作出参考。

global_planner没有提供类似D*这样的动态方法，而是用了定时规划路径，ROS是启动了一个线程，在移动过程中对路径不断的重新规划。这个feature是可以去掉的，特别是当你的运算负载很高，处理器又有限的情况下。还有重新规划(当找不到路径，也就是走着走着新扫描到未知区域的障碍或者动态增加的障碍)两种办法。加上了定时规划和重新规划之后的A*和D*几乎是一模一样的。

配置

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的配置中，加入如下行：

1
2
3

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

参数

allow_unknown: true 是否允许规划器规划在未知区域创建规划，只设置该参数为true还不行,还要在costmap_commons_params.yaml中设置 track_unknown_space 参数也为true才行，
default_tolerance: 0.0 当设置的目的地被障碍物占据时，需要以该参数为半径寻找到最近的点作为新目的地点.
visualize_potential: false 是否显示从PointCloud2计算得到的势区域. 这个参数可以让你看见potential array的图像，看计算出的cost是怎么样子(颜色深浅代表距离起始点的远近)
use_dijkstra: true 设置为true,将使用dijkstra算法, 否则使用A*算法
use_quadratic: true 设置为true,将使用二次函数近似函数计算potential，否则使用更加简单的计算方式，这样节省硬件资源
use_grid_path: false 默认使用梯度下降法，路径更为光滑，从周围八个栅格中找到下降梯度最大的点。如果为true，使用栅格路径，从终点开始找上下或左右4个中最小的栅格直到起点，会规划一条沿着网格边界的路径,偏向于直线穿越网格
old_navfn_behavior: false navfn是非常旧的ROS系统中使用的，现在已经都用global_planner代替navfn了，所以不建议设置为true.
lethal_cost: 253 致命代价值,默认是设置为253,可以动态来配置该参数.
neutral_cost: 50 中等代价值,默认设置是50,可以动态配置该参数.
cost_factor: 3.0 代价地图与每个代价值相乘的因子.
publish_potential: true 是否发布costmap的势函数.
orientation_mode: 0 如何设置每个点的方向（None = 0,Forward = 1,Interpolate = 2,ForwardThenInterpolate = 3,Backward = 4,Leftward = 5,Rightward = 6）（可动态重新配置）
orientation_window_size: 1 根据orientation_mode指定的位置积分来得到使用窗口的方向.默认值1,可以动态重新配置.

A*和Dijkstra两种算法

两种算法的效果对比

A*比Dijkstra少计算很多，但可能不会产生相同路径。另外，在global_planner的A*里，the potentials are computed using 4-connected grid squares, while the path found by tracing the potential gradient from the goal back to the start uses the same grid in an 8-connected fashion. Thus, the actual path found may not be fully optimal in an 8-connected sense. (Also, no visited-state set is tracked while computing potentials, as in a more typical A* implementation, because such is unnecessary for 4-connected grids).

话题

发布的话题是~<name>/plan(nav_msgs/Path)，即最新规划出的路径，每次规划出新路径就要发布一次，主要用于观测。

GlobalPlanner::initialize

if (!initialized_)
{
    ros::NodeHandle private_nh("~/" + name);
    costmap_ = costmap;
    frame_id_ = frame_id;   //costmap_ros->getGlobalFrameID()

    unsigned int cx = costmap->getSizeInCellsX(), cy = costmap->getSizeInCellsY();
    // 参数赋值, 默认false
    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);

    // p_calc_声明为PotentialCalculator指针
    // QuadraticCalculator是它的派生类, 唯一区别是覆盖了calculatePotential函数
    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);
    //DijkstraExpansion 和 AStarExpansion 都继承 Expander类
    if (use_dijkstra)
    {
        DijkstraExpansion* de = new DijkstraExpansion(p_calc_, cx, cy);
        if(!old_navfn_behavior_)
            de->setPreciseStart(true);
        planner_ = de;      // Expander* planner_;
    }
    else   // 如果不用dijkstra,就用A星算法.
        planner_ = new AStarExpansion(p_calc_, cx, cy);

    bool use_grid_path;
    private_nh.param("use_grid_path", use_grid_path, false);
    // GradientPath 和 GridPath都继承了Traceback
    if (use_grid_path)
        path_maker_ = new GridPath(p_calc_);
    else
        path_maker_ = new GradientPath(p_calc_);
        
    orientation_filter_ = new OrientationFilter();
    // NavfnROS::initialize中也注册此话题
    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);

    double costmap_pub_freq;
    private_nh.param("planner_costmap_publish_frequency", costmap_pub_freq, 0.0);

    //get the tf prefix
    ros::NodeHandle prefix_nh;
    tf_prefix_ = tf::getPrefixParam(prefix_nh);

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

参考：
ROS- global_panner
global_planner历史背景和概述