globalplanner源码解读

1.globalplanner的主要计算路径的接口函数

1. makePlan 路径规划接口函数

2. initialize 初始化规划器接口函数

3. 在GlobalPlanner中的主要接口函数：

4. makePlan 重载了路径规划接口函数

5. initialize 重载了初始化规划器接口函数

6. computePotential 新定义了计算规划地图势场的接口函数 (已经不使用了)

7. getPlanFromPotential 新定义了得到规划路径的接口函数(提供给makePlan使用)

8. 在GlobalPlanner中的主要成员变量

9. Expander* planner_ 成员变量图搜索算法指针,如: (a星、dijkstra)，该成员变量为图搜索算法的基类，定义在navigator/global_planner/expender.h中，提供了一个纯虚汗数接口定义

10. calculatePotentials 图搜索算法计算势场接口，这个接口会被(a星、dijkstra)图搜索算法集成并提供给GlobalPlanner的makePlan使用

11. GlobalPlanner中原定义的computePotential接口不使用了，应该也是为了更方便的扩展不同的图搜索算法，而将计算规划地图势场的功能交由独立的图搜索算法实现。

2. 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)

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

初始化global planner的过程，在MoveBase的构造函数中创建和初始化。如下:

  boost::shared_ptr 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

2.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);

​ [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-CBkulxmd-1655471919747)(C:\Users\lanth211108\AppData\Roaming\Typora\typora-user-images\1649762836229.png)]

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

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

       Expander* planner_;

​ [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-QMDmNtGU-1655471919748)(C:\Users\lanth211108\AppData\Roaming\Typora\typora-user-images\1649762853182.png)]

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

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

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

         Traceback* path_maker_;

​ [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-GzCk77Pv-1655471919749)(C:\Users\lanth211108\AppData\Roaming\Typora\typora-user-images\1649762893799.png)]

  7）发布topic和service

       //发布topic /move_base/GlobalPlanner/plan

       plan_pub_ = private_nh.advertise("plan", 1);

       //发布topic /move_base/GlobalPlanner/potential

       potential_pub_ = private_nh.advertise("potential", 1);

       //发布service /move_base/GlobalPlanner/make_plan

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

​

2.2 makeplan的过程

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

GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
double tolerance, std::vector& plan)

具体的工作流程为：

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

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

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

       怎么理解呢？

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

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

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

       这一步就是做这个转换工作。
       //将map上的坐标（mx,my）转化成world上的坐标（wx，wy）
      void mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy)

		//将world上的坐标（wx，wy）转化成map上的坐标（mx，my）
       bool worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my)const; 

/*******************************************************************/
        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)

3.costmap与globalplanner的关系

规划的路径都是建立在costmap之上的，所以先讲一下必须知道的costmap的知识，实际代价地图存储在这里unsigned char* costmap_;每个字符八位0~255即cost值。把实际地图根据分辨率映射成栅格地图，依次给栅格编号，如下图所示,

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-95uL0y0E-1655471919749)(C:\Users\lanth211108\AppData\Roaming\Typora\typora-user-images\1649763308970.png)]

维护工作是通过以下几个函数进行：

voidmapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy)将map上的坐标（mx,my）转化成world上的坐标（wx，wy）

boolworldToMap(double wx, double wy, unsigned int& mx, unsigned int& my)const; 将world上的坐标（wx，wy）转化成map上的坐标（mx，my）

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

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

       怎么理解呢？

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

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

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

       这一步就是做这个转换工作。
       /*****************************************************************/
       如上图表格的行列对应地图的坐标，如（1,2）的地图坐标数组对应栅格地图5的序号，计算距离要先换算
       
       

costmap的网格代价计算

(1) Lethal（致命的）:单元中存在实际的障碍物，机器人的中心（center cell）在如果在该网格中，此时机器人必然与障碍物冲突。（代价值为254）

(2) Inscribed（内切）：网格中心位于机器人的内切圆轮廓内，此时机器人也必然与障碍物冲突。（代价值为253）

(3) Possibly circumscribed（可能受限）：网格中心位于机器人的外切圆与内切圆轮廓之间，此时机器人相当于位于障碍物附近，所以不一定冲突，取决于机器人的方位或者说姿态。 使用”可能“一词是因为它可能不是真正的障碍单元，而是一些用户偏好（用户自己规定一些区域，不想让机器人通过），将特定的成本值放入地图中。（代价值为128）

(4) Freespace（自由空间）：网格中心位于机器人外切圆之外，属于没有障碍物的空间。 （代价值为1）

(5) Unknown（未知）：未知的空间。（代价值为0）

（1）致命障碍:栅格值为254：此时障碍物与机器人中心重叠，必然发生碰撞；
（2）内切障碍:栅格值为253：此时障碍物处于机器人的内切圆内，必然发生碰撞；
（3）外切障碍:栅格值为[128,252]：【防盗标记–盒子君hzj】此时障碍物处于其机器人的外切圆内，处于碰撞临界，不一定发生碰撞
（4）非自由空间:栅格值为(0,127]：此时机器人处于障碍物附近，属于危险警戒区，进入此区域，将来可能会发生碰撞；
（5）自由区域:栅格值为0：此处机器人可以自由通过；
（6）未知区域:栅格值为255：还没探明是否有障碍物

inline unsignedint getIndex(unsigned int mx, unsigned int my) const根据map上的坐标得到栅格编号index。
源码解读这篇文章讲的比较好 http://blog.csdn.net/u013158492/article/details/50504963大家可以参考，我就不再重复，主要讲一下我看源码总结的框架，如果大家跟着这个框架看源码的话可能会看的比较快。其中use_dijkstra,use_quadratic ,use_grid_path是global_planner的关键配置参数。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-Q9zcFVLH-1655471919750)(C:\Users\lanth211108\Desktop\20170703211054658.png)]

A*与DijKstra算法的讲解http://blog.csdn.net/kongbu0622/article/details/1871520

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-VYdRGN2U-1655471919750)(C:\Users\lanth211108\Desktop\20170706202359469.png)]

A这个函数很重要

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-sh2urOsM-1655471919751)(C:\Users\lanth211108\AppData\Roaming\Typora\typora-user-images\1649814811388.png)]

• 其中f ( n ) 是从初始状态经由状态n到目标状态的代价估计，g ( n ) 是在状态空间中从初始状态到状态n的实际代价（看上面costmap的网格计算），h*(n)是从状态n*到目标状态的最佳路径的估计代价。对于路径搜索问题，状态就是图中的节点，代价就是距离。

  h(n)的选取： 保证找到最短路径（最优解的）条件，关键在于估价函数f*(n)的选取（或者说h*(n)的选取）。 我们以d*(n)表达状态n到目标状态的距离，那么h(n)的选取大致有如下三种情况：

  • （1）如果h ( n ) ≤ d ( n ) ，到目标状态的实际距离，这种情况下，搜索的点数多，搜索范围大，效率低。但能得到最优解。
  • （2）如果h ( n ) = d ( n ) ，，即距离估计h*(n)等于最短距离，那么搜索将严格沿着最短路径进行， 此时的搜索效率是最高的。
  • （3）如果 h ( n ) > d ( n )，搜索的点数少，搜索范围小，效率高，但不能保证得到最优解。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-k6lnNpV3-1655471919751)(C:\Users\lanth211108\Desktop\20170706201109027.png)]

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-Pz4svlOf-1655471919751)(C:\Users\lanth211108\Desktop\20170703213124850.png)]

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-uaxsw5UZ-1655471919752)(C:\Users\lanth211108\Desktop\微信图片_20220225175015.png)]