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