move base全解

0. 简介

之前我们专门有一节讲到了《move_base源码学习》。主要介绍了MoveBase基类中函数的大概意思以及调用的方式。move_base是ROS下关于机器人路径规划的中心枢纽。它通过订阅激光雷达、map地图、amcl的定位等数据，然后规划出全局和局部路径，再将路径转化为机器人的速度信息，最终实现机器人导航。下面是move_base的整个框架。

1. 代码详解

1.1 代码结构

$ tree .
.
├── cfg
│   └── MoveBase.cfg
├── CHANGELOG.rst
├── CMakeLists.txt
├── include
│   └── move_base
│       └── move_base.h
├── package.xml
├── planner_test.xml
└── src
    ├── move_base.cpp
    └── move_base_node.cpp

4 directories, 8 files

如右侧代码所示，我们通过Git工具把navigation从github上拉下来，并查看包move_base的目录结构。涉及到的文件和目录并没有多少。

其中子目录cfg中只有一个MoveBase.cfg的文件，实际上它是一个python的脚本，用于动态的修改运行节点的参数。与导航控制的实现无关，这里不再赘述。

子目录include中的move_base.h和src目录下的move_base.cpp一起定义和实现了我们要研究的导航框架类MoveBase。而源文件move_base_node.cpp是ROS系统的节点实现，它实例化了MoveBase，并开启了ROS的消息循环。

CHANGELOG.rst是更新日志，记录了move_base的历次版本的修改内容。package.xml是ROS系统用于描述包的基本信息的文件，其中记录了包的名称、作者信息、以及依赖关系。

文件planner_test.xml实际上是一个launch文件，是一个以PR2机器人为平台测试规划器的demo，对于我们而言没有什么用处。

CMakeLists.txt是CMake的编译指导文件，描述了如何把源文件编译成实际运行的节点move_base。下面是从中截取的一段代码片段，我们可以看到move_base.cpp被编译成为一个库，而实际的可执行文件move_base则是由move_base_node.cpp生成的。

        add_library(move_base src/move_base.cpp)
        target_link_libraries(move_base ${Boost_LIBRARIES} ${catkin_LIBRARIES})
        add_dependencies(move_base ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
        
        add_executable(move_base_node src/move_base_node.cpp)
        add_dependencies(move_base_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
        target_link_libraries(move_base_node move_base)
        set_target_properties(move_base_node PROPERTIES OUTPUT_NAME move_base)

1.2 代码解析

以下是MoveBase的构造函数，为了方便本文的写作，我将它的语句顺序做了适当的调整，不影响整个系统的运行。如下面的代码片段所示，在一开始对一系列的成员变量赋予了初值：

tf_是坐标变换TF2的接口对象;
as_则是Action服务器;
planner_costmap_ros_是用于全局规划器的代价地图对象;
controller_costmap_ros_则是局部规划器所用的代价地图对象;
bgp_loader_是装载全局规划器插件的工具;
blp_loader_是装载局部规划器插件的工具;
planner_plan_, latest_plan_ 和controller_plan_是三个记录规划结果的缓存;
runPlanner_, setup_, p_freq_change_, c_freq_change_, new_global_plan_则是一些控制和反映MoveBase系统状态的布尔变量。

        MoveBase::MoveBase(tf2_ros::Buffer& tf) :
            tf_(tf), as_(NULL), planner_costmap_ros_(NULL), controller_costmap_ros_(NULL),
            bgp_loader_("nav_core", "nav_core::BaseGlobalPlanner"),
            blp_loader_("nav_core", "nav_core::BaseLocalPlanner"), 
            recovery_loader_("nav_core", "nav_core::RecoveryBehavior"),
            planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),
            runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false)
        {

在构造函数的一开始，定义了两个ROS的句柄，用于获取节点参数，订阅和发布主题。以下面的第13和14行为例，MoveBase从参数服务器中获取了全局规划器和局部规划器的名称，如果系统中没有定义这些参数，将以默认值"navfn/NavfnROS"和"base_local_planner/TrajectoryPlannerROS"完成初始化工作。还有很多其它参数需要配置，这里不再一一介绍。

            ros::NodeHandle private_nh("~");
            ros::NodeHandle nh;
            std::string global_planner, local_planner;

            private_nh.param("base_global_planner", global_planner, std::string("navfn/NavfnROS"));
            private_nh.param("base_local_planner", local_planner, std::string("base_local_planner/TrajectoryPlannerROS"));
            // 省略其它加载参数的语句

通过在一开始获取的全局规划器名称global_planner构造全局规划器，并用刚刚构建的全局代价地图完成对其的初始化操作。整个过程在一个try-catch语句块中完成，如果出现异常将退出整个系统。

            // 全局规划器
            try {
                planner_ = bgp_loader_.createInstance(global_planner);
                planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);
            } catch (const pluginlib::PluginlibException& ex) {
                ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", global_planner.c_str(), ex.what());
                exit(1);
            }

以类似的套路，MoveBase还构建了局部代价地图和局部规划器的对象。

            // 局部代价地图和局部规划器
            controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
            controller_costmap_ros_->pause();
            try {
                tc_ = blp_loader_.createInstance(local_planner);
                tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_);
            } catch (const pluginlib::PluginlibException& ex) {
                ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", local_planner.c_str(), ex.what());
                exit(1);
            }

该规划方法具体实现在 navigation/navfn中，标志在文件 navigation/navfn/bgp_plugin.xml中。这也是每一个插件必须要有的

  <class name="navfn/NavfnROS" type="navfn::NavfnROS" base_class_type="nav_core::BaseGlobalPlanner">

该方法的核心调用在move_base.cpp下

// 函数 MoveBase::makePlan()下
planner_->makePlan(start, goal, plan)

详细的内容和结构可以参考：《move_base源码学习》以及《ROS DWA局部路径规划原理详解+源码分析》

2. 参数配置

启动move_base的launch，包括解析map，move_base和amcl定位三个部分，这构成了一个完整的框架，下面我们主要来看move_base.launch里的配置。

<launch>
 
  <param name="use_sim_time" value="false" />
  
  <!-- EDIT THIS LINE TO REFLECT THE NAME OF YOUR OWN MAP FILE 
       Can also be overridden on the command line -->
  <arg name="map" default="test_map.yaml" />
 
  <!-- Run the map server with the desired map -->
  <node name="map_server" pkg="map_server" type="map_server" args="$(find dart_nav)/maps/dart.yaml"/> 
 
  <!-- Start move_base  -->
  <include file="$(find dart_nav)/launch/tb_move_base_test.launch" />
 
  <!-- Fire up AMCL -->
  <include file="$(find dart_nav)/launch/tb_amcl.launch" /> 
 
 
</launch>

下面是move_base.launch内的配置：

在这里插入图片描述

<launch>
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
    <rosparam file="$(find dart_nav)/config1/move_base_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find dart_nav)/config1/move_base_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find dart_nav)/config1/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find dart_nav)/config1/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find dart_nav)/config1/teb_local_planner_params.yaml" command="load" />
 
     <param name="base_global_planner" value="global_planner/GlobalPlanner"/> 
     <param name="planner_frequency" value="1.0" />
     <param name="planner_patience" value="5.0" />
 
     <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
     <param name="controller_frequency" value="15.0" />
     <param name="controller_patience" value="15.0" />
 <rosparam file="$(find dart_nav)/config1/costmap_conversion_params.yaml" command="load" />
 
  </node>
  
</launch>

如上所示，我使用的是global_planner这个包，它默认使用的是dijkstra，当然也可以使用A*全局路径规划，局部路径规划我使用的是teb，同样需要配置上面第3行到第7行的一些yaml，这些yaml是costmap和planner的一些配置文件。上图已经展示了对应部分的配置代表的含义，下面我们来主要看一下local_costmap_params.yaml和global_costmap_params.yaml。这里配置了上面提到的各个层（layers）的使用。

2.1 move_base_params.yaml

配置文件内容如下：

#FileName: move_base_params.yaml 
#Copyright: 2016-2018 ROS小课堂www.corvin.cn
#Author: corvin
#Description:
# move_base软件包的通用配置参数,现在依次解释每个参数意义:
#   shutdown_costmaps:当move_base在不活动状态时,是否关掉costmap.
#   controller_frequency:向底盘控制移动话题cmd_vel发送命令的频率.
#   controller_patience:在空间清理操作执行前,控制器花多长时间等有效控制下发.
#   planner_frequency:全局规划操作的执行频率.如果设置为0.0,则全局规划器仅
#       在接收到新的目标点或者局部规划器报告路径堵塞时才会重新执行规划操作.
#   planner_patience:在空间清理操作执行前,留给规划器多长时间来找出一条有效规划.
#   oscillation_timeout:执行修复机制前,允许振荡的时长.
#   oscillation_distance:来回运动在多大距离以上不会被认为是振荡.
#   base_local_planner:指定用于move_base的局部规划器插件名称.
#   base_global_planner:指定用于move_base的全局规划器插件名称.
#History:
# 20180726: initial this comment.
#

shutdown_costmaps: false

controller_frequency: 5.0
controller_patience: 3.0

planner_frequency: 1.0
planner_patience: 5.0

oscillation_timeout: 8.0
oscillation_distance: 0.3

base_local_planner: "dwa_local_planner/DWAPlannerROS"
base_global_planner: "global_planner/GlobalPlanner"

下面来依次解释下各参数的意义，其中base_local_planner和base_global_planner我们可以替换自己的算法：

shutdown_costmaps:当move_base在不活动状态时,是否关掉costmap.
controller_frequency:向底盘控制移动话题cmd_vel发送命令的频率.
controller_patience:在空间清理操作执行前,控制器花多长时间等有效控制下发.
planner_frequency:全局规划操作的执行频率.如果设置为0.0,则全局规划器仅在接收到新的目标点或者局部规划器报告路径堵塞时才会重新执行规划操作.
planner_patience:在空间清理操作执行前,留给规划器多长时间来找出一条有效规划.
oscillation_timeout:执行修复机制前,允许振荡的时长.
oscillation_distance:来回运动在多大距离以上不会被认为是振荡.
base_local_planner:指定用于move_base的局部规划器名称.
base_global_planner:指定用于move_base的全局规划器插件名称.