【ROS】移动机器人导航仿真（2）——SLAM（gmapping）

在前一节中，简单介绍了移动机器人的3D建模，并在gazebo三维仿真环境中实现了简单的移动。这一节采用gmapping包实现机器人的SLAM（同时定位与地图建立）。使用上一节构建的机器人3D模型，在gazebo三维仿真环境中移动，并为此环境构建一个二维地图。

---

概述

移动机器人在未知的环境中进行导航时，接收来自里程计，传感器流的信息，构建工作环境的地图模型，然后再根据地图和目标位姿信息来导航、定位。

ROS导航功能包集是ROS系统最强大的特性之一，可以实现机器人的自主导航和运动，但只能用于双轮差分驱动或者完全轮驱动的机器人。导航功能包集假定机器人以特定方式配置以便运行。下图显示了此配置的概述。白色组件是ROS提供的导航所必需的组件，灰色组件是ROS提供的可选组件，你要做的是为每个机器人平台创建蓝色组件。详见http://wiki.ros.org/navigation/Tutorials/RobotSetup

以下是你需要做的：
1. 让机器人发布关于所有关节和传感器位置关系的信息（tf）；
2. 让机器人发送线速度和角速度信息（odom）；
3. 接收激光雷达的信息来实现完成地图构建和定位（sensor sources）；
4. 创建机器人的基础控制器，负责将线速度和转向角度信息发布给硬件平台（base controller）。

gmapping包采用了粒子滤波算法，提供基于激光的SLAM（同时定位和地图建立），节点名为slam_gmapping。使用slam_gmapping，可以从移动机器人收集的激光和位姿关系数据创建二维栅格地图。即订阅了tf (tf/tfMessage) 和scan (sensor_msgs/LaserScan) 主题，发布map_metadata (nav_msgs/MapMetaData) 和map (nav_msgs/OccupancyGrid) 主题。详见http://wiki.ros.org/gmapping

---

使用gazebo创建变换（tf）

tf是ROS中的一个功能包，它可以让用户随着时间的推移跟踪多个坐标系。tf维护在时间上缓冲的坐标变换树结构中的坐标系之间的关系，并且使得用户在任何时间点可以任意转换两个坐标系之间的点和向量。在一个机器人系统中，需要用tf将各种数据的坐标系串联起来，变成一个树形结构（每个节点只能有一个父节点，可以有多个孩子节点），以便后面通讯和显示。

在上一节机器人的3D建模中，已经配置了机器人本体各组件之间的坐标变换关系，它们之间的变换都为静态变换。在本节中，需要配置odom到机器人基座标系base_footprint之间的坐标变换关系，此变换为动态变化。odom是一个很好的固定坐标系，在机器人的起点位姿上，机器人的姿态相对odom而言是随时间经常变动的。在gazebo环境中可以使用Gazebo 插件（plugins）来配置odom到base_footprint的变换。

Gazebo 插件可以为URDF模型提供更强大的功能，并且可以为传感器输出和电机输出绑定ROS消息和服务。插件可以添加到URDF的任何主要元素 —robot，link或joint，具体取决于插件的范围和目的。要完成向URDF中的特定元素添加插件，必须在gazebo元素中包含plugin标记。
以下是URDF中robots元素的插件示例：

  "differential_drive_controller" filename="libdiffdrive_plugin.so">
    ... plugin parameters ...
  

在上面的例子中，插件被添加到robot元素中，因为与其他gazebo元素和属性类似，如果没有指定reference =“x”（link、joint等），则假设reference是整个robot。name为插件的名称，filename为插件所适用的共享库。详见http://gazebosim.org/tutorials?tut=ros_gzplugins

在上一节中构建的机器人模型为滑移转向（skid-steer）机器人，所以我在模型文件中添加了skid_steer_drive_controller插件，代码如下：

<gazebo>
  <plugin name="skid_steer_drive_controller" filename="libgazebo_ros_skid_steer_drive.so">
    <alwaysOn>truealwaysOn>
    <updateRate>100.0updateRate>
    <robotNamespace>/robotNamespace>
    <leftFrontJoint>base_to_wheel1leftFrontJoint>
    <rightFrontJoint>base_to_wheel3rightFrontJoint>
    <leftRearJoint>base_to_wheel2leftRearJoint>
    <rightRearJoint>base_to_wheel4rightRearJoint>
    <wheelSeparation>0.2wheelSeparation>
    <wheelDiameter>0.1wheelDiameter>
    <torque>2torque>
    <commandTopic>cmd_velcommandTopic>
    <odometryTopic>odomodometryTopic>
    <odometryFrame>odomodometryFrame>
    <robotBaseFrame>base_footprintrobotBaseFrame>
    <broadcastTF>1broadcastTF>
  plugin>
gazebo>

其中torque设置轮子的扭矩力，wheelSeparation设置左右轮子之间的距离，wheelDiameter设置轮子直径，commandTopic设置订阅的控制指令主题，odometryTopic设置机器人发布的里程计主题，odometryFrame设置odom坐标系，robotBaseFrame设置机器人基坐标系，broadcastTF设置是否发布tf变换。该插件的源码见
http://docs.ros.org/hydro/api/gazebo_plugins/html/gazebo__ros__skid__steer__drive_8cpp_source.html

设置完成后，可以在节点运行的时候通过$ rosrun tf view_frames来查看tf坐标变换树的结构。
所生成的tf坐标变换树结构如下图所示：

友情提醒：坚决、千万、绝对不要在已经加载你的机器人模型的情况下保存gazebo环境，不要问我是怎么知道的！

---

仿真实现

首先启动机器人模型：

<launch>
  
  <arg name="paused" default="true"/>
  <arg name="use_sim_time" default="true"/>
  <arg name="gui" default="true"/>
  <arg name="headless" default="false"/>
  <arg name="debug" default="false"/>

  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find nav_sim)/urdf/wall.world"/>
    <arg name="use_sim_time" value="$(arg use_sim_time)"/>
    <arg name="debug" value="$(arg debug)" />
    <arg name="gui" value="$(arg gui)" />
  include>

  
  <arg name="model" default="$(find nav_sim)/urdf/myrobot.xacro" />
  <param name="robot_description" command="$(find xacro)/xacro.py $(arg model)" />

  
  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
    args="-urdf -model robot -param robot_description -z 0.00"/> 
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find nav_sim)/urdf/nav.rviz"/>

  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher"/>

launch>

接着启动gmapping节点：

<launch>
  <node name="slam_gmapping" pkg="gmapping" type="slam_gmapping" >
    <remap  from="scan" to="/robot/laser/scan"/>
    <param name="base_link" value="base_footprint"/>
  node>

launch>

然后启动键盘控制节点移动机器人使其扫描周围环境并建图：

$ rosrun teleop_twist_keyboard teleop_twist_keyboard.py

最后，使用map_server保存地图：

$ rosrun map_server map_saver -f map

生成的地图如下图所示：

整个建图过程的视频见：http://v.youku.com/v_show/id_XMTg2NDkxNDAwOA==.html