ROS自定义路径规划器的原理及实现

虽然我还没有对象，但已不知面向了多少对象

——沃.兹基硕德

全局规划器的理论实现

上次写到宕开一笔去看move_base源码，死磕后也勉强是理解了，但是效率低效果差，属于是筋疲力尽且无效果。所以最开始如果想先看move_base的话，只需要了解一下类的继承关系和主要函数功能及执行顺序即可。

1. move_base实现流程

首先，move_base是什么？可以从几个不同的观点来看：

1. 从最后结果来看，它是ROS中的一个节点：作为ROS中的一个节点，提供各种话题和服务；
2. 从代码上来看，他是一个C++C类及其实例化：move_base功能的实现都是基于Movebase类的实例化来完成的，理解这个类的性质和功能也就理解了move_base的性能；
3. 从实现原理来看：它是ROS框架下一个actionServer，在回调函数中执行各项供能；

因此对move_base的分析也可以从三点，实际上是两点即供能和实现。功能上篇已经分析过了，而实现则要从源码开始
该部分的源码大概分如下几部分：

• 加载几个yaml中的参数并初始化节点和各个功能的发布者、订阅者；
• 初始化ActionSsrver和局部、全局规划器及代价地图；
• 初始化action回调函数线程
• 等待接受goal，接收到后进入回调函数；
• 开启全局路径规划线程，并在该线程中执行全局路径规划函数；
• 执行完全局路径规划线程后再次回到action回调函数；
• 将规划好的全局路径传入局部规划函数，完成局部避障、结束判断等功能，并在该函数中输出cmd_vel指令；

大致来说就是以上几步，当然我做了一些简化省略了规则检查、坐标变换、抢占机制、和局部规划函数中繁杂的判断实现。如果想理解规划器的调用流程，理解到这里就够了，如果想深入了解：
ROS Navigation之move_base完全详解
源码分析系列
源码分析（侧重流程）

2. move_base与规划器联系

实现上述流程涉及到一些关键的类成员函数，具体如下：

• MoveBase::executeCb()：整个move_base节点的回调函数，接收到目标后再该函数内循环执行，直到到达目标点或者出现意外中止；
• MoveBase::planThread()：路径规划线程（本质也是一个函数），嵌套在executeCb函数中，当 runPlanner_ = true时该线程开始执行；
• MoveBase::makePlan()：路径规划函数，嵌套在planThread()中；在该函数内调用全局路径规划器，最后输出一条全局规划路径；
• MoveBase::executeCycle()：局部规划函数，该函数的逻辑最为复杂我一时也没太明白，只能大概能理解其在executeCb()函数中，在路径规划线程结束后开始循环执行，传入参数为全局路径和目标点坐标，在函数内调用局部路径规划器实现局部避障、速度指令生成和结束判断功能；
• MoveBase::reconfigureCB()：动态参数配置；

1. 创建和初始化global planner
 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_);

2  创建和初始化local planner
 boost::shared_ptr<nav_core::BaseLocalPlanner> tc_;
 tc_ = blp_loader_.createInstance(local_planner);
 
 //使用的planner为teb_local_planner/TebLocalPlannerROS
 tc_->initialize(blp_loader_.getName(local_planner), &tf_,  controller_costmap_ros_);
 4 创建并启动global planner线程
 planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));
 
 5 创建一个action server：MoveBaseActionServer。
 as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base",    boost::bind(&MoveBase::executeCb, this, _1), false);

到现在我们大概已经理解了两个路径规划器在move_base中的调用流程，下一步就是理解规划器的具体功能和实现了；

3. Plugin和nav_core

ROS中采用插件机制来实现路径规划器功能，实现该功能有一套标准的流程：ROS中的插件plugin机制，该部分内容在此博客中有较为详细的描写，总的来说分为以下几点：

• 创建基类，定义了插件应该实现哪些功能；
• 定义功能类，此类继承了基类，面向具体类型实现基类中定义的功能；
• 同时在功能类对应的cpp文件中，我们需要申明注册创建好的插件；
• 之后需要将插件编译成为lib文件，并注册到ROS中，使得catkin系统能够直接查找到对应的动态链接库；
• 最后，在应用的时候，我们需要用到ROS中pluginlib函数库，创建一个ClassLoader，用来加载plugin；

具体到move_base中，基类即nav_core，在该文章中有详细的介绍，该基类中的成员函数对应两个路径规划器的功能；然后基于该基类继承除了两种全局规划器：
global_planner: 一个快速的，内插值的路径规划器，其能更灵活地代替navfn；
navfn: 一个基于栅格的全局路径规划器，利用导航函数来计算路径；
该过程的实现与上文插件流程一样，具体见此：ROS: global_planner 整体解析

4. 两个路径规划器实现的功能

看完move_base源码在看nav_core这个基类，真的可以说是如天堂一样，没有线程、没有逻辑判断甚至没有具体实现，只有简简单单的功能描述，直接泪目啊喂。言归正传，nav_cove这个基类最主要的内容就是提供了插件功能的接口，换种说法来讲它定义了两个规划器和一个恢复插件的基本功能：

• base_global_planner.h：实现参数初始化和全局路径规划的功能；
• base_local_planner.h：参数初始化、局部轨迹规划、判断是否到达目标点、由局部轨迹计算出速度指令；

这篇文章主要分享全局规划器的实现流程，以GlobalPlanner规划器为例

5. 全局规划器的实现流程

功能类较基类相比内容要扩展很多，具体到GlobalPlanner规划器，其核心是planner_core类，除此之外还有一些辅助类，这些类相互继承、调用构成了很复杂的一套代码，具体简要介绍如下：

• planner_node.cpp：规划器的入口，在此实例化了Global_planner类；
• planner_core.h：规划器的功能类，此类继承nav_core，在该类内完成规划器的主要功能；
• expandar.h：相当于另一个插件的基类，A*和D算法的规划部分的类都继承此类来实现；
• astar.h：继承Expandar类，实现A*算法的搜索过程；
• dijkstra.h：继承Expandar类，实现D算法的搜索过程；
• gradient_path.h：梯度下降法检索路径实现的类，实现梯度下降法在启发式算法结束后检索出一条路径；
• grid_path.h：栅格法检索路径实现的类，实现栅格法法在启发式算法结束后检索出一条路径；
• potential_calculator.h：检索器类，在astar.h和dijkstra.h两个类中作为搜索器传入，作为其中的一个成员变量；
• quadratic_calculator.h：作用同上，且和上个类一样，在功能类初始化阶段·就被选择；；

基于以上几个类来实现全局规划的功能，按照源码中的嵌套关系大概分为以下流程（以A*为例）：

1. 合法性检验和预处理，在planner_core.h定义的类中实现，同时也加载ros_param，并通过逻辑判断选择规划器、路径检索方式和路径拟合方式：
2. 节点检索：在像素坐标系内开始搜索（每一个像素为一个开启点），输出一个px*py长度的一维数组，存放最后各个节点的开启状态。该部分在astar.h或者dijkstra.h定义的类（继承Expandar）内完成，入口函数为GlobalPlanner::makePlan函数中：

bool found_legal = planner_->calculatePotentials(costmap_->getCharMap(), start_x, start_y, goal_x, goal_y,
                                                    nx * ny * 2, potential_array_);

3. 路径搜索：调用栅格法或梯度下降法处理上一步中生成的数组，输出为一个数组，其元素为（float，float）分别存放路点的x和y坐标，该部分在两个搜索类中的一个完成（视参数决定），入口在GlobalPlanner::makePlan函数：

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

//更为准确来说，是这个函数里面嵌套的：
path_maker_->getPath(potential_array_, start_x, start_y, goal_x, goal_y, path)

4. 路径发布：将路径转化为geometry_msgs::PoseStamped 格式后结束规划器；

全局规划参数和基本介绍
global_planner源码阅读笔记
基于A在navugation中添加自己的算法
浅谈路径规划算法（转载）
ROS中Navigation功能包里路径规划A算法详解

全局规划器的具体实现

基于之前的分析，我们大概能推断出全局规划器中各个类的调用顺序：
即先启动GlobalPlanner这个核心类；

在完成初始化后再该类内调用AStarExpansion或DijkstraExpansion这个类来实现节点检索的工能，这两个类继承Expander这个类，同时Expander类的拷贝构造函数初始化参数时传入PotentialCalculator这个类作为成员变量；

最后调用两个路径搜索类来实现节点列表数组到路点数组的转换；

各个类之间存在着极为复杂的继承与调用关系，因此如果想基于GlobalPlanner的源码进行更改，最好不要动函数结构比如增改传入参数之类的，建议还是按照原来的流程走哪怕你的算法并不需要这么多步骤；如果实在需要更改传入参数，建议做函数重载，这样不会打乱继承和调用的关系；

规划器更改心得

1. 仿照Astar.h和Astar.cpp建立自己的规划方式类（其实算不上完整的规划，因为这个类只是在像素空间操作的），并更改Cmakelist；
2. 在GlobalPlanner类初始化部分将规划器定向到自定义的规划器；
3. 仿照GridPath类定义相应的路径搜索类，并更改CmakeList；
4. 在GlobalPlanner类初始化部分将搜索器定向到自定义的；

照这个步骤做下来基本是没问题的，反正最起码我这是没问题的，能不能做成就看命运了，另外除了嵌套关系复杂外函数传参和各个类的全局变量定义也都不一样，所以强烈不建议大幅度更改，就做一个扩展就得了

另一种全局规划器的定义方法是参照carrot_planner，从头到尾按照ros::Plugin 的定义流程做，但这样做想避障还需要对Cost_map有深入了解
移动机器人——自定义规划算法

长大后，我们闭口不谈梦想

梦想很贵也很难，就像你对着一道题啃了一下午只收获了焦头烂额；
也像那个价格离谱到让你望而生畏的驱动板或者电机；
或是一次次很累但什么都没有做到的尝试。

后来我们不再谈及梦想，只谈近况；不想太远，只是一步一步向前走。