amcl_demo之导航控制器——move_base

move_base应该说是navigation技术栈的核心，它通过Action机制实现了机器人的导航控制。 给定地图中的一个点，move_base通过全局规划器为机器人提供一条可行的路径。与此同时，它使用局部规划器根据周围的障碍物实时修正这条轨迹，进行避障。 下面右图描述了move_base的基本结构和接口。

move_base是一个导航控制框架，它尽可能的以一种松耦合的形式，组合各个功能模块。我们可以设计自己的全局规划器和局部规划器， 但需要分别继承自nav_core中的虚类nav_core::BaseGlobalPlanner和nav_core::BaseLocalPlanner。这两个虚类就是一种接口， 我们需要提供虚类中的各个抽象函数的实现。具例化的规划器将以插件的形式加载到move_base中运行。

全局和局部的规划器进行路径规划的依据是全局和局部两个代价地图(costmap)，它们由costmap_2d提供。它在地图数据的基础上， 融合了传感器探测到的障碍信息。

1. move_base.launch.xml

如下面的代码片段所示，在amcl_demo的launch文件中，通过引入一个launch文件加载了move_base节点。

    <include file="$(find turtlebot_navigation)/launch/includes/move_base.launch.xml"/>

在move_base.launch.xml的一开始导入了两个launch文件，分别用平滑速度以及安全驾驶，我们将在后续的文章中予以详细介绍。

        <launch>
            <include file="$(find turtlebot_navigation)/launch/includes/velocity_smoother.launch.xml"/>
            <include file="$(find turtlebot_navigation)/launch/includes/safety_controller.launch.xml"/>

然后定义了一堆输入参数arg，用于指定运行move_base节点时所需的里程计(odom_frame_id)、底盘(base_frame_id)和全局坐标系(global_frame_id)的名称。 主题"odom_topic"和"laser_topic"分别是里程计和激光传感器的数据主题，它们是计算代价地图和当前位姿的数据输入。最后的"custom_param_file"是move_base的配置参数文件。

            <arg name="odom_frame_id"   default="odom"/>
            <arg name="base_frame_id"   default="base_footprint"/>
            <arg name="global_frame_id" default="map"/>
            <arg name="odom_topic" default="odom"/>
            <arg name="laser_topic" default="scan"/>
            <arg name="custom_param_file" default="$(find turtlebot_navigation)/param/dummy.yaml"/>

接下来，加载运行move_base节点。在加载过程中，通过一系列的*.yaml文件描述了move_base运行时所需的各种参数，这些参数文件的作用如下:

• costmap_common_parms.yaml:描述了代价地图的通用设置，主要涉及到地图类型、障碍物特征、障碍膨胀特性等设置。
• local_costmap_params.yaml:具体描述了局部代价地图的配置，该文件中通过plugin字段，指示计算代价地图中障碍物和膨胀属性的插件。
• global_costmap_params.yaml:具体描述了全局代价地图的配置，也使用plugin字段指示static_layer, obstacle_layer, inflation_layer三层代价地图的计算插件。
• dwa_local_planner_params.yaml:是ROS的navigation技术栈提供的动态窗口局部规划器(Dynamic Window Approach local planner)的配置文件， 记录了dwa局部规划器运行所需的配置参数。如果我们使用了其它局部规划器，那么就需要将该文件替换为对应的参数文件。
• move_base_params.yaml:详细描述了节点move_base的运行配置。其中base_local_planner字段指示了实际使用的局部规划器，没有修改的情况下是dwa_local_planner/DWAPlannerROS。 base_global_planner字段描述了全局规划器，默认是navfn/NavfnROS。
• global_planner_params.yaml:是全局规划器GlobalPlanner的配置参数。
• navfn_global_planner_params.yaml:是全局规划器NavfnROS的配置参数。
• $(arg custom_param_file):是在上面代码片段的第9行中描述的配置参数文件。

            <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
                <rosparam file="$(find turtlebot_navigation)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
                <rosparam file="$(find turtlebot_navigation)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />   
                <rosparam file="$(find turtlebot_navigation)/param/local_costmap_params.yaml" command="load" />   
                <rosparam file="$(find turtlebot_navigation)/param/global_costmap_params.yaml" command="load" />
                <rosparam file="$(find turtlebot_navigation)/param/dwa_local_planner_params.yaml" command="load" />
                <rosparam file="$(find turtlebot_navigation)/param/move_base_params.yaml" command="load" />
                <rosparam file="$(find turtlebot_navigation)/param/global_planner_params.yaml" command="load" />
                <rosparam file="$(find turtlebot_navigation)/param/navfn_global_planner_params.yaml" command="load" />
                <!-- external params file that could be loaded into the move_base namespace -->
                <rosparam file="$(arg custom_param_file)" command="load" />

下面根据各种输入参数配置，设定全局和局部的代价地图的惯性坐标系和机器人坐标系，以及DWAPlannerROS的坐标系。

                <!-- reset frame_id parameters using user input data -->
                <param name="global_costmap/global_frame" value="$(arg global_frame_id)"/>
                <param name="global_costmap/robot_base_frame" value="$(arg base_frame_id)"/>
                <param name="local_costmap/global_frame" value="$(arg odom_frame_id)"/>
                <param name="local_costmap/robot_base_frame" value="$(arg base_frame_id)"/>
                <param name="DWAPlannerROS/global_frame_id" value="$(arg odom_frame_id)"/>

最后，配置move_base的输入主题"odom"和"scan"以及输出主题"cmd_vel"。

                <remap from="cmd_vel" to="navigation_velocity_smoother/raw_cmd_vel"/>
                <remap from="odom" to="$(arg odom_topic)"/>
                <remap from="scan" to="$(arg laser_topic)"/>
            </node>
        </launch>

2. move_base的运行分析

我们分别在三个终端中运行如下的指令，依次开启Gazebo仿真环境，运行amcl_demo，打开可视化界面rviz，详细可以参见Turtlebot你好!。

        $ roslaunch turtlebot_gazebo turtlebot_world.launch
        $ roslaunch turtlebot_gazebo amcl_demo.launch
        $ roslaunch turtlebot_rviz_launchers view_navigation.launch

通过工具rosnode，我们可以查看节点move_base的运行状态。下面的日志片段描述了move_base发布的主题。它们大体上可以分为如下几类:

• 局部规划器DWAPlannerROS发布的主题，它们具有共同的前缀"/move_base/DWAPlannerROS"
• 对应全局代价地图的主题，前缀是"/move_base/global_costmap"
• 局部代价地图的主题，前缀是"/move_base/local_costmap"
• move_base的Action主题:"/move_base/goal"，"/move_base/status"，"/move_base/result", "/move_base/status"
• /navigation_velocity_smoother/raw_cmd_vel是move_base的输出，它给出了控制机器人运动的线速度和角速度指令。
• 其它的一些主题主要是用于输出日志或者配置参数。

        $ rosnode info /move_base 
        --------------------------------------------------------------------------------
        Node [/move_base]
        Publications: 
         * /move_base/DWAPlannerROS/cost_cloud [sensor_msgs/PointCloud2]
         * /move_base/DWAPlannerROS/global_plan [nav_msgs/Path]
         * /move_base/DWAPlannerROS/local_plan [nav_msgs/Path]
         * /move_base/DWAPlannerROS/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
         * /move_base/DWAPlannerROS/parameter_updates [dynamic_reconfigure/Config]
         * /move_base/DWAPlannerROS/trajectory_cloud [sensor_msgs/PointCloud2]
         * /move_base/NavfnROS/plan [nav_msgs/Path]
         * /move_base/current_goal [geometry_msgs/PoseStamped]
         * /move_base/feedback [move_base_msgs/MoveBaseActionFeedback]
         * /move_base/global_costmap/costmap [nav_msgs/OccupancyGrid]
         * /move_base/global_costmap/costmap_updates [map_msgs/OccupancyGridUpdate]
         * /move_base/global_costmap/footprint [geometry_msgs/PolygonStamped]
         * /move_base/global_costmap/inflation_layer/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
         * /move_base/global_costmap/inflation_layer/parameter_updates [dynamic_reconfigure/Config]
         * /move_base/global_costmap/obstacle_layer/clearing_endpoints [sensor_msgs/PointCloud]
         * /move_base/global_costmap/obstacle_layer/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
         * /move_base/global_costmap/obstacle_layer/parameter_updates [dynamic_reconfigure/Config]
         * /move_base/global_costmap/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
         * /move_base/global_costmap/parameter_updates [dynamic_reconfigure/Config]
         * /move_base/global_costmap/static_layer/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
         * /move_base/global_costmap/static_layer/parameter_updates [dynamic_reconfigure/Config]
         * /move_base/goal [move_base_msgs/MoveBaseActionGoal]
         * /move_base/local_costmap/costmap [nav_msgs/OccupancyGrid]
         * /move_base/local_costmap/costmap_updates [map_msgs/OccupancyGridUpdate]
         * /move_base/local_costmap/footprint [geometry_msgs/PolygonStamped]
         * /move_base/local_costmap/inflation_layer/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
         * /move_base/local_costmap/inflation_layer/parameter_updates [dynamic_reconfigure/Config]
         * /move_base/local_costmap/obstacle_layer/clearing_endpoints [sensor_msgs/PointCloud]
         * /move_base/local_costmap/obstacle_layer/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
         * /move_base/local_costmap/obstacle_layer/parameter_updates [dynamic_reconfigure/Config]
         * /move_base/local_costmap/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
         * /move_base/local_costmap/parameter_updates [dynamic_reconfigure/Config]
         * /move_base/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
         * /move_base/parameter_updates [dynamic_reconfigure/Config]
         * /move_base/result [move_base_msgs/MoveBaseActionResult]
         * /move_base/status [actionlib_msgs/GoalStatusArray]
         * /navigation_velocity_smoother/raw_cmd_vel [geometry_msgs/Twist]
         * /rosout [rosgraph_msgs/Log]

下面是move_base运行过程中订阅的主题。其中"/clock"是gazebo仿真的时钟系统，"/tf"和"/tf_static"描述了坐标变换关系，"/scan"则是激光传感器的扫描数据， "odom"是里程计数据，"/map"是地图信息。

此外，主题"/move_base/goal"和"/move_base/cancel"是move_base的Action主题，用于接收和取消导航目标。而主题"/move_base_simple/goal"则是一个非Action的接口， 同样用于接收导航目标点。

其它主题的意义，我们将在详细分析move_base的代码和其它相关功能模块时予以介绍。

        Subscriptions: 
         * /clock [rosgraph_msgs/Clock]
         * /map [nav_msgs/OccupancyGrid]
         * /mobile_base/sensors/bumper_pointcloud [sensor_msgs/PointCloud2]
         * /move_base/cancel [unknown type]
         * /move_base/global_costmap/footprint [geometry_msgs/PolygonStamped]
         * /move_base/goal [move_base_msgs/MoveBaseActionGoal]
         * /move_base/local_costmap/footprint [geometry_msgs/PolygonStamped]
         * /move_base_simple/goal [geometry_msgs/PoseStamped]
         * /odom [nav_msgs/Odometry]
         * /scan [sensor_msgs/LaserScan]
         * /tf [tf2_msgs/TFMessage]
         * /tf_static [tf2_msgs/TFMessage]

在move_base提供的服务中大多是用于在线配置参数用的。只有"/move_base/clear_costmaps"和"/move_base/make_plan"具有比较实际的作用。 "clear_costmaps"用于清空代价地图，"make_plan"用于用户请求执行规划，但不要求机器人按照规划路径移动。

        Services: 
         * /move_base/DWAPlannerROS/set_parameters
         * /move_base/NavfnROS/make_plan
         * /move_base/clear_costmaps
         * /move_base/get_loggers
         * /move_base/global_costmap/inflation_layer/set_parameters
         * /move_base/global_costmap/obstacle_layer/set_parameters
         * /move_base/global_costmap/set_parameters
         * /move_base/global_costmap/static_layer/set_parameters
         * /move_base/local_costmap/inflation_layer/set_parameters
         * /move_base/local_costmap/obstacle_layer/set_parameters
         * /move_base/local_costmap/set_parameters
         * /move_base/make_plan
         * /move_base/set_logger_level
         * /move_base/set_parameters

move_base提供一个Action服务作为其业务入口，其使用的Action消息定义在move_base_msgs包中的MoveBase.action中，其文件内容如下所示。 goal和feedback消息都是geometry_msgs/PoseStamped类型的位姿点，没有定义result消息内容。

        geometry_msgs/PoseStamped target_pose
        ---
        ---
        geometry_msgs/PoseStamped base_position

3. 完

move_base是ROS的导航技术栈的核心，它通过全局和局部规划器进行路径规划，并输出控制指令控制机器人沿着规划出的轨迹运动。 为了提高系统的可扩展性，move_base使用了插件的机制，用户可以设计自己的全局和局部规划器，以及代价地图的计算方式。这些都可以通过配置文件进行说明。