【Navigation】global_planner 源码解析

2024-01-07 18:03:34

全局规划器 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<

文章来源:https://blog.csdn.net/zeroheitao/article/details/135437803
本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。