ROS Navigation源代码剖析(5)-move_base 全局规划器GlobalPlanner流程

4.2.3 GlobalPlanner工作过程

MoveBase的全局规划线程通过调用配置的实际全局规划器的makePlan方法来计算全局路径规划。基类是class BaseGlobalPlanner（navigation-kinetic\nav_core\include\nav_core\ base_global_planner.h）， 具体的全局规划器需要继承此基类。 例如，GlobalPlanner就继承此基类，并实现相应的方法。

class GlobalPlanner : public nav_core::BaseGlobalPlanner

(navigation-kinetic\global_planner\include\global_planner\planner_core.h)

ROS可用的全局规划器有navfn/NavfnROS，global_planner/GlobalPlanner 和carrot_planner。类图如下图所示：

这里主要分析GlobalPlanner全局规划器。

在4.2.0节中已经介绍了创建和初始化global planner的过程。它是在MoveBase的构造函数中创建和初始化。如下:

      boost::shared_ptr<nav_core::BaseGlobalPlanner> planner_;

      planner_ = bgp_loader_.createInstance(global_planner);

                                             //使用的planner为 global_planner/GlobalPlanner

      planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);

                                            //加载和初始化相应的动态库/opt/ros/kinetic/lib/libglobal_planner.so

4.2.3.1 GlobalPlanner initialize过程

实现是在void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) 函数中完成，需要costmap作为入参。

initialize(name, costmap_ros->getCostmap(), costmap_ros->getGlobalFrameID());

 

   1）保存costmap对象的指针和frameid

        costmap_ = costmap;

        frame_id_ = frame_id;

   2）获取costmap的CellsX, CellsY即地图的长和宽

      unsigned int cx = costmap->getSizeInCellsX(),

                          cy = costmap->getSizeInCellsY();

    3） 根据old_navfn_behavior的值设定convert_offset_

                   如果old_navfn_behavior为true，convert_offset_ = 0.0

                   如果old_navfn_behavior为false，convert_offset_ = 0.5

          我们这里设置为false，所以convert_offset_的值为0.5

    4） 根据use_quadratic的值分配计算器

              PotentialCalculator* p_calc_；

           我们设置为true，所以p_calc_ = new QuadraticCalculator(cx, cy);

            

   5） 根据use_dijkstra设置最短路径算法。

           如果为true，则使用地杰斯特拉算法，否则使用A Star算法

           Expander* planner_;

            

     6）  根据use_grid_path设置路径生成器

             若为true，使用网格边界，否则，使用梯度下降法。

             我们设置为false，所以使用梯度下降法。 

             Traceback* path_maker_;

              

      7）发布topic和service

           //发布topic /move_base/GlobalPlanner/plan

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

           //发布topic /move_base/GlobalPlanner/potential

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

           //发布service /move_base/GlobalPlanner/make_plan

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

      

4.2.3.2 makeplan的过程

Global planner线程收到action server的请求后，会调用makePlan()函数做全局路径规划。最终调用的函数为：

GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,

                                           double tolerance, std::vector<geometry_msgs::PoseStamped>& plan)

具体的工作流程为：

     1)  将起点坐标和终点坐标从世界坐标系转到地图坐标系。

           这里的世界坐标指的是坐标使用米为单位

           地图坐标指的是将米转换为数组的下标。

           怎么理解呢？

           实际存储地图时，使用的是二维数组。根据地图的长和宽以及分辨率计算出二维数据的大小。

           举例：地图的大小为3.0X4.0米，分辨率为0.1米/像素。则实际存储地图的二维数组大小为

                      30X40. 如果一个点的世界坐标为（1.0, 2.0）, 在实际二维数组中的下标就为（10, 20）

            这一步就是做这个转换工作。

            costmap_->worldToMap(wx, wy, start_x_i, start_y_i)

             worldToMap(wx, wy, start_x, start_y)

             costmap_->worldToMap(wx, wy, goal_x_i, goal_y_i)

              worldToMap(wx, wy, goal_x, goal_y)

      2)   将起始点的costmap标为空闲。因为机器人就在起始点，所以此点肯定没有障碍物。

            clearRobotCell(start_pose, start_x_i, start_y_i);

      3）重新设置相关数组的size

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

     4）将costmap的所有点都标为有障碍物

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

     5)   计算可用路径

         bool found_legal = planner_->calculatePotentials(costmap_->getCharMap(), start_x, start_y,

                                                                                        goal_x, goal_y, nx * ny * 2, potential_array_);

     6)   生成路径规划

          getPlanFromPotential(start_x, start_y, goal_x, goal_y, goal, plan)