【Navigation】global_planner 源码解析

全局规划器 global_planner 功能包

全局规划大都基于静态地图进行规划，产生路径点，然后给到局部规划器进行局部规划，ROS 中常见的全局规划器功能包有 navfn、global_planner(Dijkstra、A*）、asr_navfn、Movelt! (常用于机械臂)等。

global_planner 功能包结构

• plan_node.cpp 文件是全局规划的入口；
• planner_core.cpp 是 global_planner 的核心，进行初始化，调用A*或者Dijkstra进行全局规划，生成搜索路径；
  • astar.cpp
  • dijkstra.cpp
• quadratic_calculator.cpp（二次逼近方式，常用） 和 potential_calculator（直接返回当前点代价最小值，累加前面的代价值）在生成搜索路径中用到；
• 搜索到路径后使用回溯 grid_path.cpp（栅格路径）、gradient_path.cpp（梯度路径）获得最终路径；
  • 栅格路径：从终点开始找上下左右四个点中最小的栅格直到起点
  • 梯度路径：从周围八个栅格中找到下降梯度最大的点
• 之后 orientation_filter.cpp 进行方向滤波。

根据nav_core提供的BaseGlobalPlanner接口：

• initialize(name, costmap) ——算法实例的选取
• makePlan(start, goal, plan)——两个步骤完成路径的生成（①计算可行点矩阵potential_array (planner_->calculatePotentials) → ②从可行点矩阵中提取路径plan (path_maker_->getPath)）

主要是以下三个实例:

1. 计算“一个点”的可行性 —— p_calc_：PotentialCalculator::calculatePotential()、 QuadraticCalculator::calculatePotential()（quadratic_calculator.cpp）
2. 计算“所有”的可行点 —— planner_：DijkstraExpansion::calculatePotentials()、 AStarExpansion::calculatePotentials()（astar.cpp、dijkstra.cpp）
3. 从可行点中“提取路径” —— path_maker_：GridPath::getPath()、 GradientPath::getPath()（grid_path.cpp、gradient_path.cpp）

涉及到四个算法程序：A*, Dijkstra；gradient_path, grid_path

可以总结出global_planner框架：

1、plan_node.cpp

/*********************************************************************
 * 这段代码是一个ROS节点，实现了一个基于代价地图（costmap）的全局路径规划器。
 * 节点初始化了一个全局规划器对象 PlannerWithCostmap，并提供了ROS服务和订阅者，
 * 允许外部调用路径规划服务或通过订阅目标位姿来触发路径规划。
 * 其中使用了ROS中的TransformListener来获取机器人位姿的变换信息。整个节点在main函数中被初始化并启动。
 *********************************************************************/
#include <a_global_planner/planner_core.h>
#include <navfn/MakeNavPlan.h>
#include <boost/shared_ptr.hpp>
#include <costmap_2d/costmap_2d_ros.h>
#include <tf2_ros/transform_listener.h>

namespace cm = costmap_2d;
namespace rm = geometry_msgs;

using std::vector;
using rm::PoseStamped;
using std::string;
using cm::Costmap2D;
using cm::Costmap2DROS;

namespace a_global_planner {
   

// 创建一个继承自AGlobalPlanner的类，名为PlannerWithCostmap
class PlannerWithCostmap : public AGlobalPlanner {
   
    public:
        PlannerWithCostmap(string name, Costmap2DROS* cmap);
        bool makePlanService(navfn::MakeNavPlan::Request& req, navfn::MakeNavPlan::Response& resp);

    private:
        void poseCallback(const rm::PoseStamped::ConstPtr& goal);
        Costmap2DROS* cmap_;
        ros::ServiceServer make_plan_service_;
        ros::Subscriber pose_sub_;
};

// 2、实现makePlanService服务
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";

    // 调用makePlan函数进行路径规划
    bool success = makePlan(req.start, req.goal, path);
    resp.plan_found = success;
    if (success) {
   
        resp.path = path;
    }

    return true;
}

// 3、处理目标位姿的回调函数
void PlannerWithCostmap::poseCallback(const rm::PoseStamped::ConstPtr& goal) {
   
    geometry_msgs::PoseStamped global_pose;
    // 通过Costmap2DROS对象获取机器人当前的全局位姿信息
    cmap_->getRobotPose(global_pose);
    // 创建一个vector<PoseStamped>类型的变量path，用于存储规划得到的路径
    vector<PoseStamped> path;
    // 调用makePlan函数进行路径规划，传递起始位姿为当前机器人位姿(global_pose)，目标位姿为传入的目标位姿(*goal)，规划结果存储在path中
    makePlan(global_pose, *goal, path);
}

// 1、构造函数
PlannerWithCostmap::PlannerWithCostmap(string name, Costmap2DROS* cmap) :
        AGlobalPlanner(name, cmap->getCostmap(), cmap->getGlobalFrameID()) {
   
    ros::NodeHandle private_nh("~");
    // 将传入的Costmap2DROS指针赋值给成员变量cmap_
    cmap_ = cmap;
    // 创建ROS服务，服务名为 "make_plan"，回调函数为 &PlannerWithCostmap::makePlanService，this指向当前对象
    make_plan_service_ = private_nh.advertiseService("make_plan", &PlannerWithCostmap::makePlanService, this);
    // 创建ROS订阅者，订阅名为 "goal" 的消息，消息类型为 rm::PoseStamped，回调函数为 &PlannerWithCostmap::poseCallback，this指向当前对象
    pose_sub_ = private_nh.subscribe<rm::PoseStamped>("goal", 1, &PlannerWithCostmap::poseCallback, this);
}

} // namespace

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

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

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

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

    ros::spin();
    return 0;
}

2、planner_core.cpp

//register this planner as a BaseGlobalPlanner plugin
// 首先，注册全局路径插件，使其成为ros插件，在ros中使用
PLUGINLIB_EXPORT_CLASS(a_global_planner::AGlobalPlanner, nav_core::BaseGlobalPlanner)

initialize()

// 1、
void AGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) {
   
    initialize(name, costmap_ros->getCostmap(), costmap_ros->getGlobalFrameID());
}

// 2、进行初始化
void AGlobalPlanner::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;      // 是否使用dijkstra全局规划
        private_nh.param("use_dijkstra", use_dijkstra, true);
        if (use_dijkstra)
        {
   
            ROS_INFO("use_dijkstra");
            DijkstraExpansion* de = new DijkstraExpansion(p_calc_, cx, cy);
            if(!old_navfn_behavior_)
                de->setPreciseStart(true);
            planner_ = de;
        }
        else{
   
            ROS_INFO("use_A_star");
            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_);        // new 出 path_maker_ 实例，从可行点中提取路径
        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);

        // 是否探索未知区域，flase--不可到达
        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);
        private_nh.param("outline_map", outline_map_, true);

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

        dsrv_ = new dynamic_reconfigure::Server<a_global_planner::GlobalPlannerConfig>(ros::NodeHandle("~/" + name));
        dynamic_reconfigure::Server<a_global_planner::GlobalPlannerConfig>::CallbackType cb =
                [this](auto& config, auto level){
    reconfigureCB(config, level); };
        dsrv_->setCallback(cb);

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

}

makePlan 函数 —— 主要函数
通过输入起始位姿、目标点，返回 plan 路径结果，调用 makePlan 函数
关键方法是：

• calculatePotentials()
  • 若全局规划器选择A* ，就去 astar.cpp 中找；选择 Dijkstra 就去 dijkstra.cpp 中找
• getPlanFromPotential()

// 3、
bool AGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
                           std::vector<geometry_msgs::PoseStamped>& plan) {