ROS naviagtion analysis: GlobalPlanner

最新推荐文章于 2024-04-24 10:06:39 发布

惜缘865

最新推荐文章于 2024-04-24 10:06:39 发布

阅读量1.5k

点赞数 3

CC 4.0 BY-SA版权

本文链接：https://blog.youkuaiyun.com/u011608180/article/details/94002838

在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;