引言
在这个-SLAM建图和导航仿真实例-项目中，主要分为三个部分，分别是
- （一）模型构建
- （二）根据已知地图进行定位和导航
- （三）使用RTAB-MAP进行建图和导航
该项目的slam_bot已经上传我的Github。 由于之前的虚拟机性能限制，我在这个项目中使用了新的ubantu 16.04环境，虚拟机配置
- 内存 8G
- CPU 四核
- 禁用硬件加速（重要：否则gazebo会打开失败）
一、模型构建
1、使用Solidworks建立slam_bot包
使用Solidworks建立模型如下图，其中有很多部分值得注意。
- 定义底盘底面为base_link坐标系的xoy平面，底面中点为坐标原点，小车正方向为x轴正方向建立右手系；
- 各个车轮坐标系原点为各个车轮内侧平面圆心，x轴与base_link坐标系x轴对齐，y轴与车轴平齐（与base_link坐标系y轴对齐或相向）建立右手系；
- 车轮的旋转轴分别为对应的转轴轴线。
值得注意的是，在下图中有两个坐标系，分别是坐标系1和coordinate0。在一开始，我没有意识到使用coordinate0作为坐标原点是一个多么错误的选择。在后来使用skid_steer_drive_controllergazebo插件时，模型一直在原地转圈，直到我在rviz中查看tf转换关系时，才发现是错误的tf转换，导致了插件的控制错误。所以一定要严格遵循上述所说的坐标系建立规则。
图1-1 solidworks模型
关于更多urdf_exporter安装和使用可以参考我的博客-【从零开始的ROS四轴机械臂控制】（一）- 实际模型制作、Solidworks文件转urdf与rviz仿真.。
图1-2 urdf-exporter导出界面
我在Github中提供了最初始的slam_bot package文件。
2.更改slam_bot包
（1）urdf文件夹
导航到urdf文件夹
$ ~/catkin_ws/src/slam_bot/urdf
将slam_bot.urdf 更名成slam_bot.xacro，并新建slam_bot.gazebo.xacro文件。
更改slam_bot.xacro
添加xacro namespace。
<robot name="slam_bot" xmlns:xacro="http://www.ros.org/wiki/xacro">
添加slam_bot.gazebo.xacro，导入gazebo插件
<xacro:include filename="$(find slam_bot)/urdf/slam_bot.gazebo.xacro" />
在base_link节点之前添加base_footprint节点
<!--base_footprint link-->
<link name="base_footprint"> </link>
<joint name="base_link_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0" />
<parent link="base_footprint"/>
<child link="base_link" />
</joint>
在camera节点之后添加laser节点
<!--laser_link-->
<link name="hokuyo">
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://slam_bot/meshes/hokuyo.dae"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://slam_bot/meshes/hokuyo.dae"/>
</geometry>
<material name="red">
<color rgba="1.0 0 0 1.0"/>
</material>
</visual>
<inertial>
<mass value="1e-5" />
<origin xyz="0 0 0" rpy="0 1.57 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
</inertial>
</link>
<joint name="hokuyo_joint" type="fixed">
<axis xyz="0 0 0" />
<origin xyz="0 0 0" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="hokuyo"/>
</joint>
更改各个joint设置，如joint_wheel_FR。注意<axis>的设置，否则也会导致错误的控制。
<joint
name="joint_wheel_FR"
type="continuous">
<origin
xyz="0.0800000000000013 -0.0799999999999997 0.04"
rpy="0 0 0" />
<parent
link="base_link" />
<child
link="link_wheel_FR" />
<axis
xyz="0 1 0" />
</joint>
joint type设置为“continuous”，类似于转动关节，但对其旋转没有限制。它可以绕一个轴连续旋转。关节会有自己的旋转轴axis，一些特定的关节动力学dynamics与关节的物理特性(如“摩擦”)相对应，以及对该关节施加最大“努力”和“速度”的某些限制limits。这些限制对于物理机器人是有用的约束，并且可以帮助在仿真中创建更健壮的机器人模型。
点击这里来更好地理解这些限制。
RGB-D点云是默认朝上,需向slam_bot.xacro中添加如下的link和joint
<link name="camera_depth_optical_frame" />
<joint name="camera_depth_optical_joint" type="fixed">
<origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 0 0" />
<parent link="camera"/>
<child link="camera_depth_optical_frame" />
</joint>
更改slam_bot.gazebo.xacro
添加如下插件
- 四轮机器人控制驱动：
skid_steer_drive_controller - RGBD 深度摄像机:
libgazebo_ros_openni_kinect - 激光传感器:
head_hokuyo_sensor
<?xml version="1.0"?>
<robot>
<gazebo>
<plugin name="skid_steer_drive_controller" filename="libgazebo_ros_skid_steer_drive.so">
<alwaysOn>true</alwaysOn>
<updateRate>100.0</updateRate>
<robotNamespace>/</robotNamespace>
<leftFrontJoint>joint_wheel_FL</leftFrontJoint>
<rightFrontJoint>joint_wheel_FR</rightFrontJoint>
<leftRearJoint>joint_wheel_BL</leftRearJoint>
<rightRearJoint>joint_wheel_BR</rightRearJoint>
<wheelSeparation>0.16</wheelSeparation>
<wheelDiameter>0.08</wheelDiameter>
<robotBaseFrame>base_footprint</robotBaseFrame>
<torque>20</torque>
<commandTopic>cmd_vel</commandTopic>
<odometryTopic>odom</odometryTopic>
<odometryFrame>odom</odometryFrame>
<broadcastTF>1</broadcastTF>
</plugin>
</gazebo>
<!--RGBD camera -->
<gazebo reference="camera">
<sensor type="depth" name="camera">
<always_on>true</always_on>
<visualize>false</visualize>
<update_rate>15.0</update_rate>
<camera name="front">
<horizontal_fov>1.047197</horizontal_fov>
<image>
<!-- openni_kinect plugin works only with BGR8 -->
<format>B8G8R8</format>
<width>400</width>
<height>300</height>
</image>
<clip>
<near>0.01</near>
<far>8</far>
</clip>
</camera>
<plugin name="camera_controller" filename="libgazebo_ros_openni_kinect.so">
<baseline>0.1</baseline>
<alwaysOn>true</alwaysOn>
<updateRate>15.0</updateRate>
<cameraName>camera</cameraName>
<imageTopicName>/camera/rgb/image_raw</imageTopicName>
<cameraInfoTopicName>/camera/rgb/camera_info</cameraInfoTopicName>
<depthImageTopicName>/camera/depth/image_raw</depthImageTopicName>
<depthImageCameraInfoTopicName>/camera/depth_registered/camera_info</depthImageCameraInfoTopicName>
<pointCloudTopicName>/camera/depth_registered/points</pointCloudTopicName>
<frameName>camera_depth_optical_frame</frameName>
<pointCloudCutoff>0.35</pointCloudCutoff>
<pointCloudCutoffMax>4.5</pointCloudCutoffMax>
<CxPrime>0</CxPrime>
<Cx>0</Cx>
<Cy>0</Cy>
<focalLength>0</focalLength>
<hackBaseline>0</hackBaseline>
</plugin>
</sensor>
</gazebo>
<!-- hokuyo -->
<gazebo reference="hokuyo">
<sensor type="ray" name="head_hokuyo_sensor">
<pose>0 0 0 0 0 0</pose>
<visualize>false</visualize>
<update_rate>40</update_rate>
<ray>
<scan>
<horizontal>
<samples>720</samples>
<resolution>1</resolution>
<min_angle>-1.570796</min_angle>
<max_angle>1.570796</max_angle>
</horizontal>
</scan>
<range>
<min>0.10</min>
<max>30.0</max>
<resolution>0.01</resolution>
</range>
<noise>
<type>gaussian</type>
<!-- Noise parameters based on published spec for Hokuyo laser
achieving "+-30mm" accuracy at range < 10m. A mean of 0.0m and
stddev of 0.01m will put 99.7% of samples within 0.03m of the true
reading. -->
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>
<plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_laser.so">
<topicName>/slam_bot/laser/scan</topicName>
<frameName>hokuyo</frameName>
</plugin>
</sensor>
</gazebo>
</robot>
（2）worlds文件夹
在worlds中储存gazebo world。world是模型的集合，还可以定义此world特定的其他几个物理属性。 让我们创建一个简单的世界，其中没有将在以后的gazebo中启动的对象或模型。
$ cd worlds
$ nano slam.world
将以下内容添加到 slam.world
<?xml version="1.0" ?>
<sdf version="1.4">
<world name="default">
<include>
<uri>model://ground_plane</uri>
</include>
<!-- Light source -->
<include>
<uri>model://sun</uri>
</include>
<!-- World camera -->
<gui fullscreen='0'>
<camera name='world_camera'>
<pose>4.927360 -4.376610 3.740080 0.000000 0.275643 2.356190</pose>
<view_controller>orbit</view_controller>
</camera>
</gui>
</world>
</sdf>
.world 文件使用 XML 文件格式来描述所有被定义为 Gazeboeboard 环境的元素。在上面创建的简单世界有以下元素 <sdf>: 基本元素，封装了整个文件结构和内容。 <world>：世界元素定义了世界描述和与世界相关的几个属性。每个模型或属性可以有更多的元素来描述它。例如，camera有一个pose元素，定义了它的位置和方向。 <include>：include元素和<uri>元素一起，提供了通往特定模型的路径。在Gazebo中，有几个模型是默认包含的，可以在创建环境时包含它们。
（3）launch文件夹
创建一个新的启动文件，这将有助于加载URDF文件。
$ cd ~/catkin_ws/src/slam/launch/
$ nano robot_description.launch
将以下内容复制到上述文件中。
<launch>
<!-- send urdf to param server -->
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find slam_bot)/urdf/slam_bot.xacro'" />
<!-- Send fake joint values-->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
<param name="use_gui" value="false"/>
</node>
<!-- Send robot states to tf -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="false" output="screen"/>
</launch>
上面的代码定义了一个参数robot_description，该参数用于设置单个命令，以使用xacro软件包从xacro文件生成URDF。 借助robot_description生成的URDF文件，gazebo_ros包生成对应模型。 接下来创建一个launch文件。ROS中的启动文件使我们可以同时执行多个节点，这有助于避免在单独的shell或终端中定义和启动多个节点的繁琐工作。
$ nano slam.launch
将以下内容添加到启动文件中。
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<include file="$(find slam_bot)/launch/robot_description.xml"/>
<arg name="world" default="empty"/>
<arg name="paused" default="false"/>
<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 slam_bot)/worlds/kitchen_dining.world"/>
<arg name="paused" value="$(arg paused)"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="gui" value="$(arg gui)"/>
<arg name="headless" value="$(arg headless)"/>
<arg name="debug" value="$(arg debug)"/>
</include>
<!--spawn a robot in gazebo world-->
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false"
output="screen" args="-urdf -param robot_description -model slam_bot"/>
<!--launch rviz-->
<node name="rviz" pkg="rviz" type="rviz" respawn="false"/>
</launch>
和.world文件一样，.launch文件也是基于XML的。 首先，使用 <arg>元素定义某些参数。每个这样的元素都会有一个name属性和一个default值。 然后，在gazebo_ros 包中包含了empty_world.launch文件。empty_world文件包含了一组重要的定义，这些定义被我们创建的world所继承。使用 world_name 参数和传递给该参数的value的.world文件的路径，所以我们能够在 Gazebo 中启动world。
3.运行slam_bot
现在可以使用启动文件来启动Gazebo环境。
$ cd ~/catkin_ws/
$ catkin_make
$ source devel/setup.bash
$ roslaunch slam slam.launch
若gazebo长时间打不开，解决办法如下
cd ~/
hg clone https://bitbucket.org/osrf/gazebo_models
下载完成后将gazebo_models复制到~/.gazebo文件夹中，重命名为models。 若模型正常运行，则如图3-1所示。
图3-1 模型在gazebo中运行
（1）测试RGBD相机和激光雷达
这次，凉亭和RViz都应该启动。加载后 选择RViz窗口，然后在左侧的Displays中：
- 选择“ odom”作为fixed frame
点击“Add”按钮，然后
- 添加“ RobotModel”
- 添加“pointcloud2”并选择在gazebo插件中定义的/camera/depth_registered/points主题
- 添加“ LaserScan”并选择在gazebo插件中定义的hokuyo主题。
机器人模型应在RViz中加载。 在凉亭中，单击“插入”，然后从列表中添加机器人前面世界中的任何物品。 至此应该能够在Rviz中的“pointcloud2”查看器中看到该项目，并且也可以对该对象进行激光扫描。
图3-2 测试RGBD相机和激光雷达
（2）测试四轮驱动插件
在上述所有内容仍在运行时，打开一个新的终端窗口，然后输入
$ rostopic pub /cmd_vel geometry_msgs/Twist "linear:
x: 0.1
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.0"
上面的命令会将消息发布到cmd_vel，这是在驱动器控制器插件中定义的主题。
图3-3 测试四轮驱动插件
skid_steer_drive_controller定义- urdf中各个joint定义
- 检查tf变换，使用
rosrun tf view_frames以查看tf信息
创建和查看tf-tree是确保所有链接顺序正确的好方法。 也可以在RViz中绘制不同的框架并在那里进行图形上的检查。
