【SLAM建图和导航仿真实例】（三）- 使用RTAB-MAP进行SLAM建图和导航_人工智能

引言

在这个-SLAM建图和导航仿真实例-项目中，主要分为三个部分，分别是

（一）模型构建
（二）根据已知地图进行定位和导航
（三）使用RTAB-MAP进行建图和导航

该项目的slam_bot已经上传我的Github。这是第三部分，完成效果如下
在这里插入图片描述

图1 建图和导航

三、使用RTAB-Map进行建图和导航

1. rtabmap_ros介绍

之前在GraphSLAM博客中我提到了RTAB-Map。 RTAB-Map是具有实时约束的RGB-D SLAM方法，它是一种基于增量基于外观的闭环检测器的RGB-D，基于立体声和激光雷达图的SLAM方法。闭环检测器使用词袋方法来确定新图像来自先前位置或新位置的可能性。当接受循环闭合假设时，新约束将添加到地图的图形中，然后图形优化器将地图中的错误最小化。使用内存管理方法来限制用于闭环检测和图形优化的位置数量，以便始终遵守对大型环境的实时约束。 代码库： https://github.com/introlab/rtabmap_ros.git http://introlab.github.io/rtabmap 节点： 所有sensor_msgs/Image话题使用image_transport. rtabmap 这是核心节点，是RTAB-Map核心库的封装，这是在检测到循环闭合时增量构建和优化地图的图形。节点的在线输出是具有地图上最新添加数据的本地图。默认RTAB-Map 数据库的位置是 “~/.ros/rtabmap.db”，工作空间也设置为 “~/.ros” 通过订阅cloud_map话题获取3D点云图, grid_map or proj_map话题获取2D网格图 rtabmapviz RTAB-Map的可视化接口，是 RTAB-Map GUI图形库的封装，类似rviz但有针对RTAB-Map的可选项

2.rtabmap_ros安装

安装kinetic版本：

<code class="has-numbering">$ sudo apt-get install ros-kinetic-rtabmap-ros
</code>

必要的依赖安装 (Qt, PCL, VTK, OpenCV, …)：

<code class="has-numbering">$ sudo apt-get install ros-kinetic-rtabmap ros-kinetic-rtabmap-ros
$ sudo apt-get remove ros-kinetic-rtabmap ros-kinetic-rtabmap-ros
</code>

下载安装RTAB-Map

源码安装rtabmap

<code class="has-numbering">$ cd ~
$ git clone https://github.com/introlab/rtabmap.git rtabmap
$ cd rtabmap/build
$ cmake -DCMAKE_INSTALL_PREFIX=~/catkin_ws/devel ..
$ make -j4
$ make install
</code>

源码安装rtabmap_ros

<code class="has-numbering">$ cd ~/catkin_ws
$ git clone https://github.com/introlab/rtabmap_ros.git src/rtabmap_ros
$ catkin_make -j1 
</code>

内存太小的话，使用-j1 ，内存大可以去掉。

3.创建启动文件

根据ROS wiki的建议，我们可以用类似以下的方式搭建mapping的启动文件。
在这里插入图片描述

图3-1 Kinect + Odometry + 2D laser

<code class="prism language-xml has-numbering"><span class="token prolog"><?xml version="1.0" encoding="UTF-8"?></span>

<span class="token tag"><span class="token punctuation"><</span>launch<span class="token punctuation">></span></span>

  <span class="token comment"><!-- Arguments for launch file with defaults provided --></span>
  <span class="token tag"><span class="token punctuation"><</span>arg <span class="token attr-name">name</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>database_path<span class="token punctuation">"</span></span>     <span class="token attr-name">default</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>rtabmap.db<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span>
  <span class="token tag"><span class="token punctuation"><</span>arg <span class="token attr-name">name</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>rgb_topic<span class="token punctuation">"</span></span>   <span class="token attr-name">default</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>/camera/rgb/image_raw<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span>
  <span class="token tag"><span class="token punctuation"><</span>arg <span class="token attr-name">name</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>depth_topic<span class="token punctuation">"</span></span> <span class="token attr-name">default</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>/camera/depth/image_raw<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span>
  <span class="token tag"><span class="token punctuation"><</span>arg <span class="token attr-name">name</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>camera_info_topic<span class="token punctuation">"</span></span> <span class="token attr-name">default</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>/camera/rgb/camera_info<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span>  


  <span class="token comment"><!-- Mapping Node --></span>
  <span class="token tag"><span class="token punctuation"><</span>group <span class="token attr-name">ns</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>rtabmap<span class="token punctuation">"</span></span><span class="token punctuation">></span></span>
    <span class="token tag"><span class="token punctuation"><</span>node <span class="token attr-name">name</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>rtabmap<span class="token punctuation">"</span></span> <span class="token attr-name">pkg</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>rtabmap_ros<span class="token punctuation">"</span></span> <span class="token attr-name">type</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>rtabmap<span class="token punctuation">"</span></span> <span class="token attr-name">output</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>screen<span class="token punctuation">"</span></span> <span class="token attr-name">args</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>--delete_db_on_start<span class="token punctuation">"</span></span><span class="token punctuation">></span></span>

      <span class="token comment"><!-- Basic RTAB-Map Parameters --></span>
      <span class="token tag"><span class="token punctuation"><</span>param <span class="token attr-name">name</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>database_path<span class="token punctuation">"</span></span>       <span class="token attr-name">type</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>string<span class="token punctuation">"</span></span> <span class="token attr-name">value</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>$(arg database_path)<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span>
      <span class="token tag"><span class="token punctuation"><</span>param <span class="token attr-name">name</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>frame_id<span class="token punctuation">"</span></span>            <span class="token attr-name">type</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>string<span class="token punctuation">"</span></span> <span class="token attr-name">value</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>base_footprint<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span>
      <span class="token tag"><span class="token punctuation"><</span>param <span class="token attr-name">name</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>odom_frame_id<span class="token punctuation">"</span></span>       <span class="token attr-name">type</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>string<span class="token punctuation">"</span></span> <span class="token attr-name">value</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>/odom<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span>
      <span class="token tag"><span class="token punctuation"><</span>param <span class="token attr-name">name</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>subscribe_depth<span class="token punctuation">"</span></span>     <span class="token attr-name">type</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>bool<span class="token punctuation">"</span></span>   <span class="token attr-name">value</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>true<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span>
      <span class="token tag"><span class="token punctuation"><</span>param <span class="token attr-name">name</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>subscribe_scan<span class="token punctuation">"</span></span>      <span class="token attr-name">type</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>bool<span class="token punctuation">"</span></span>   <span class="token attr-name">value</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>true<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span>

      <span class="token comment"><!-- RTAB-Map Inputs --></span>
      <span class="token tag"><span class="token punctuation"><</span>remap <span class="token attr-name">from</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>scan<span class="token punctuation">"</span></span>            <span class="token attr-name">to</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>/slam_bot/laser/scan<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span>
      <span class="token tag"><span class="token punctuation"><</span>remap <span class="token attr-name">from</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>rgb/image<span class="token punctuation">"</span></span>       <span class="token attr-name">to</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>$(arg rgb_topic)<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span>
      <span class="token tag"><span class="token punctuation"><</span>remap <span class="token attr-name">from</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>depth/image<span class="token punctuation">"</span></span>     <span class="token attr-name">to</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>$(arg depth_topic)<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span>
      <span class="token tag"><span class="token punctuation"><</span>remap <span class="token attr-name">from</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>rgb/camera_info<span class="token punctuation">"</span></span> <span class="token attr-name">to</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>$(arg camera_info_topic)<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span>

      <span class="token comment"><!-- RTAB-Map Output --></span>
      <span class="token tag"><span class="token punctuation"><</span>remap <span class="token attr-name">from</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>grid_map<span class="token punctuation">"</span></span> <span class="token attr-name">to</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>/map<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span>

      <span class="token comment"><!-- Rate (Hz) at which new nodes are added to map --></span>
      <span class="token tag"><span class="token punctuation"><</span>param <span class="token attr-name">name</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>Rtabmap/DetectionRate<span class="token punctuation">"</span></span> <span class="token attr-name">type</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>string<span class="token punctuation">"</span></span> <span class="token attr-name">value</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>1<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span> 

      <span class="token comment"><!-- 2D SLAM --></span>
      <span class="token tag"><span class="token punctuation"><</span>param <span class="token attr-name">name</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>Reg/Force3DoF<span class="token punctuation">"</span></span> <span class="token attr-name">type</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>string<span class="token punctuation">"</span></span> <span class="token attr-name">value</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>true<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span>  

      <span class="token comment"><!-- Loop Closure Constraint --></span>
      <span class="token comment"><!-- 0=Visual, 1=ICP (1 requires scan)--></span>
      <span class="token tag"><span class="token punctuation"><</span>param <span class="token attr-name">name</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>Reg/Strategy<span class="token punctuation">"</span></span> <span class="token attr-name">type</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>string<span class="token punctuation">"</span></span> <span class="token attr-name">value</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>0<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span>  

      <span class="token comment"><!-- Loop Closure Detection --></span>
      <span class="token comment"><!-- 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE--></span>
      <span class="token tag"><span class="token punctuation"><</span>param <span class="token attr-name">name</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>Kp/DetectorStrategy<span class="token punctuation">"</span></span> <span class="token attr-name">type</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>string<span class="token punctuation">"</span></span> <span class="token attr-name">value</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>0<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span> 

      <span class="token comment"><!-- Maximum visual words per image (bag-of-words) --></span>
      <span class="token tag"><span class="token punctuation"><</span>param <span class="token attr-name">name</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>Kp/MaxFeatures<span class="token punctuation">"</span></span> <span class="token attr-name">type</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>string<span class="token punctuation">"</span></span> <span class="token attr-name">value</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>400<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span>  

      <span class="token comment"><!-- Used to extract more or less SURF features --></span>
      <span class="token tag"><span class="token punctuation"><</span>param <span class="token attr-name">name</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>SURF/HessianThreshold<span class="token punctuation">"</span></span> <span class="token attr-name">type</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>string<span class="token punctuation">"</span></span> <span class="token attr-name">value</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>100<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span>


      <span class="token comment"><!-- Minimum visual inliers to accept loop closure --></span>
      <span class="token tag"><span class="token punctuation"><</span>param <span class="token attr-name">name</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>Vis/MinInliers<span class="token punctuation">"</span></span> <span class="token attr-name">type</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>string<span class="token punctuation">"</span></span> <span class="token attr-name">value</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>15<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span> 

      <span class="token comment"><!-- Set to false to avoid saving data when robot is not moving --></span>
      <span class="token tag"><span class="token punctuation"><</span>param <span class="token attr-name">name</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>Mem/NotLinkedNodesKept<span class="token punctuation">"</span></span> <span class="token attr-name">type</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>string<span class="token punctuation">"</span></span> <span class="token attr-name">value</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>false<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span>

    <span class="token tag"><span class="token punctuation"></</span>node<span class="token punctuation">></span></span> 
  <span class="token tag"><span class="token punctuation"></</span>group<span class="token punctuation">></span></span>
<span class="token tag"><span class="token punctuation"></</span>launch<span class="token punctuation">></span></span>
</code>

让我们分解一下这个启动文件：

<code class="prism language-xml has-numbering">      <span class="token tag"><span class="token punctuation"><</span>param <span class="token attr-name">name</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>database_path<span class="token punctuation">"</span></span>       <span class="token attr-name">type</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>string<span class="token punctuation">"</span></span> <span class="token attr-name">value</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>$(arg database_path)<span class="token punctuation">"</span></span><span class="token punctuation">/>
</span></span>
</code>

参数---delete_db_on_start将使rtabmap在启动时删除数据库（默认位于~/.ros/rtabmap.db）。如果想让机器人继续从之前的映射会话中进行映射，应该删除 --delete_db_on_start。

<code class="prism language-xml has-numbering">      <span class="token tag"><span class="token punctuation"><</span>param <span class="token attr-name">name</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>database_path<span class="token punctuation">"</span></span>       <span class="token attr-name">type</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>string<span class="token punctuation">"</span></span> <span class="token attr-name">value</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>$(arg database_path)<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span>
      <span class="token tag"><span class="token punctuation"><</span>param <span class="token attr-name">name</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>frame_id<span class="token punctuation">"</span></span>            <span class="token attr-name">type</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>string<span class="token punctuation">"</span></span> <span class="token attr-name">value</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>base_footprint<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span>
      <span class="token tag"><span class="token punctuation"><</span>param <span class="token attr-name">name</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>odom_frame_id<span class="token punctuation">"</span></span>       <span class="token attr-name">type</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>string<span class="token punctuation">"</span></span> <span class="token attr-name">value</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>/odom<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span>
</code>

设置database_path,fixed frame和odom。

<code class="prism language-xml has-numbering">      <span class="token tag"><span class="token punctuation"><</span>param <span class="token attr-name">name</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>subscribe_depth<span class="token punctuation">"</span></span>     <span class="token attr-name">type</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>bool<span class="token punctuation">"</span></span>   <span class="token attr-name">value</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>true<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span>
      <span class="token tag"><span class="token punctuation"><</span>param <span class="token attr-name">name</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>subscribe_scan<span class="token punctuation">"</span></span>      <span class="token attr-name">type</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>bool<span class="token punctuation">"</span></span>   <span class="token attr-name">value</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>true<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span>
</code>

默认情况下， subscribe_depth为true。但是，在这个设置中，我们将使用RGB-D图像输入，所以将subscribe_depth设置为false，将subscribe_rgbd设置为true。因为我们有一个2D lidar，所以将 subscribe_scan 设置为 true。如果我们有一个3D lidar发布sensor_msgs/PointCloud2消息，则将sensemble_scan_cloud设置为true，并重映射相应的scan_cloud主题而不是scan。

当 subscribe_rgbd=true时，应设置 rgbd_image 输入主题。
当 subscribe_scan=true时，需要设置扫描输入主题。

<code class="prism language-xml has-numbering">      <span class="token tag"><span class="token punctuation"><</span>remap <span class="token attr-name">from</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>scan<span class="token punctuation">"</span></span>            <span class="token attr-name">to</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>/slam_bot/laser/scan<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span>
      <span class="token tag"><span class="token punctuation"><</span>remap <span class="token attr-name">from</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>rgb/image<span class="token punctuation">"</span></span>       <span class="token attr-name">to</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>$(arg rgb_topic)<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span>
      <span class="token tag"><span class="token punctuation"><</span>remap <span class="token attr-name">from</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>depth/image<span class="token punctuation">"</span></span>     <span class="token attr-name">to</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>$(arg depth_topic)<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span>
      <span class="token tag"><span class="token punctuation"><</span>remap <span class="token attr-name">from</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>rgb/camera_info<span class="token punctuation">"</span></span> <span class="token attr-name">to</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>$(arg camera_info_topic)<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span>
</code>

设置所需的输入主题。若话题没有/，则意味着在其命名空间中订阅了相应话题，例如“rgbd_image” 订阅/rtabmap/rgbd_image。

<code class="prism language-xml has-numbering">      <span class="token comment"><!-- 2D SLAM --></span>
      <span class="token tag"><span class="token punctuation"><</span>param <span class="token attr-name">name</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>Reg/Force3DoF<span class="token punctuation">"</span></span> <span class="token attr-name">type</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>string<span class="token punctuation">"</span></span> <span class="token attr-name">value</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>true<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span>  

      <span class="token comment"><!-- Loop Closure Constraint --></span>
      <span class="token comment"><!-- 0=Visual, 1=ICP (1 requires scan)--></span>
      <span class="token tag"><span class="token punctuation"><</span>param <span class="token attr-name">name</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>Reg/Strategy<span class="token punctuation">"</span></span> <span class="token attr-name">type</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>string<span class="token punctuation">"</span></span> <span class="token attr-name">value</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>0<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span>
</code>

Reg/Force3DoF：强制3DoF配准，不会估计roll、pitch和z。
Reg/Strategy：使用ICP来细化使用激光扫描发现的ICP的全局闭环。

以下是mapping.launch中没有提到的参数的简要概述。

<code class="prism language-xml has-numbering"><span class="token comment"><!-- RTAB-Map's parameters --></span>
<span class="token tag"><span class="token punctuation"><</span>param <span class="token attr-name">name</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>RGBD/NeighborLinkRefining<span class="token punctuation">"</span></span> <span class="token attr-name">type</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>string<span class="token punctuation">"</span></span> <span class="token attr-name">value</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>true<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span>
<span class="token tag"><span class="token punctuation"><</span>param <span class="token attr-name">name</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>RGBD/ProximityBySpace<span class="token punctuation">"</span></span>     <span class="token attr-name">type</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>string<span class="token punctuation">"</span></span> <span class="token attr-name">value</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>true<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span>
<span class="token tag"><span class="token punctuation"><</span>param <span class="token attr-name">name</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>RGBD/AngularUpdate<span class="token punctuation">"</span></span>        <span class="token attr-name">type</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>string<span class="token punctuation">"</span></span> <span class="token attr-name">value</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>0.01<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span>
<span class="token tag"><span class="token punctuation"><</span>param <span class="token attr-name">name</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>RGBD/LinearUpdate<span class="token punctuation">"</span></span>         <span class="token attr-name">type</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>string<span class="token punctuation">"</span></span> <span class="token attr-name">value</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>0.01<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span>
<span class="token tag"><span class="token punctuation"><</span>param <span class="token attr-name">name</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>RGBD/OptimizeFromGraphEnd<span class="token punctuation">"</span></span> <span class="token attr-name">type</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>string<span class="token punctuation">"</span></span> <span class="token attr-name">value</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>false<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span>
<span class="token tag"><span class="token punctuation"><</span>param <span class="token attr-name">name</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>Grid/FromDepth<span class="token punctuation">"</span></span>            <span class="token attr-name">type</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>string<span class="token punctuation">"</span></span> <span class="token attr-name">value</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>false<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span> 

<span class="token comment"><!-- ICP parameters --></span>
<span class="token tag"><span class="token punctuation"><</span>param <span class="token attr-name">name</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>Icp/VoxelSize<span class="token punctuation">"</span></span>                 <span class="token attr-name">type</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>string<span class="token punctuation">"</span></span> <span class="token attr-name">value</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>0.05<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span>
<span class="token tag"><span class="token punctuation"><</span>param <span class="token attr-name">name</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>Icp/MaxCorrespondenceDistance<span class="token punctuation">"</span></span> <span class="token attr-name">type</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>string<span class="token punctuation">"</span></span> <span class="token attr-name">value</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>0.1<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span>
</code>

RGBD/NeighborLinkRefining：使用ICP对输入lidar主题进行正确的里程测量。
RGBD/ProximityBySpace：根据机器人在地图中的位置，寻找局部环形闭合。这在机器人朝反方向回来时非常有用。由于摄像头朝后，无法找到全局环形闭合点。所以利用位置和之前添加的激光扫描到地图上，我们用ICP找到变换。
RGBD/AngularUpdate：机器人应该移动更新地图（如果不是0，则为0）。
RGBD/LinearUpdate：机器人应该移动来更新地图（如果不是0的话）。
RGBD/OptimizeFromGraphEnd：设置为false（默认为false），在循环关闭时，图形将从地图中的第一个姿势开始优化。TF /map -> /odom会在这种情况下改变。当设置为false时，图形将从添加到地图中的最新节点开始优化，而不是第一个节点。通过从最后一个节点开始优化，最后一个姿势保持它的值，而所有之前的姿势都会根据它来修正（所以/odom和/map总是匹配在一起）。
Grid/FromDepth: 如果为true，则从深度相机生成的云层中创建占用网格。如果为false，则从激光扫描中生成占领网格。
Icp/VoxelSize：扫描在做ICP之前被过滤到5厘米的体元。
Icp/MaxCorrespondenceDistance：ICP注册时点之间的最大距离。

更多可以查看SetupOnYourRobot。

4.配置控制程序teleop

使用以下python程序来控制模型在gazebo中的移动。

<code class="has-numbering">$ cd ~/catkin_ws/slam_bot/launch
$ nano teleop
$ sudo chmod 777 teleop
</code>

<code class="prism language-python has-numbering"><span class="token comment">#!/usr/bin/env python</span>
<span class="token keyword">import</span> rospy
<span class="token keyword">from</span> geometry_msgs<span class="token punctuation">.</span>msg <span class="token keyword">import</span> Twist
<span class="token keyword">import</span> sys<span class="token punctuation">,</span> select<span class="token punctuation">,</span> termios<span class="token punctuation">,</span> tty
msg <span class="token operator">=</span> <span class="token triple-quoted-string string">"""
Control Your  SLAM-Bot!
---------------------------
Moving around:
   u    i    o
   j    k    l
   m    ,    .

q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%
space key, k : force stop
anything else : stop smoothly

CTRL-C to quit
"""</span>

moveBindings <span class="token operator">=</span> <span class="token punctuation">{</span>
        <span class="token string">'i'</span><span class="token punctuation">:</span><span class="token punctuation">(</span><span class="token number">1</span><span class="token punctuation">,</span><span class="token number">0</span><span class="token punctuation">)</span><span class="token punctuation">,</span>
        <span class="token string">'o'</span><span class="token punctuation">:</span><span class="token punctuation">(</span><span class="token number">1</span><span class="token punctuation">,</span><span class="token operator">-</span><span class="token number">1</span><span class="token punctuation">)</span><span class="token punctuation">,</span>
        <span class="token string">'j'</span><span class="token punctuation">:</span><span class="token punctuation">(</span><span class="token number">0</span><span class="token punctuation">,</span><span class="token number">1</span><span class="token punctuation">)</span><span class="token punctuation">,</span>
        <span class="token string">'l'</span><span class="token punctuation">:</span><span class="token punctuation">(</span><span class="token number">0</span><span class="token punctuation">,</span><span class="token operator">-</span><span class="token number">1</span><span class="token punctuation">)</span><span class="token punctuation">,</span>
        <span class="token string">'u'</span><span class="token punctuation">:</span><span class="token punctuation">(</span><span class="token number">1</span><span class="token punctuation">,</span><span class="token number">1</span><span class="token punctuation">)</span><span class="token punctuation">,</span>
        <span class="token string">','</span><span class="token punctuation">:</span><span class="token punctuation">(</span><span class="token operator">-</span><span class="token number">1</span><span class="token punctuation">,</span><span class="token number">0</span><span class="token punctuation">)</span><span class="token punctuation">,</span>
        <span class="token string">'.'</span><span class="token punctuation">:</span><span class="token punctuation">(</span><span class="token operator">-</span><span class="token number">1</span><span class="token punctuation">,</span><span class="token number">1</span><span class="token punctuation">)</span><span class="token punctuation">,</span>
        <span class="token string">'m'</span><span class="token punctuation">:</span><span class="token punctuation">(</span><span class="token operator">-</span><span class="token number">1</span><span class="token punctuation">,</span><span class="token operator">-</span><span class="token number">1</span><span class="token punctuation">)</span><span class="token punctuation">,</span>
           <span class="token punctuation">}</span>

speedBindings<span class="token operator">=</span><span class="token punctuation">{</span>
        <span class="token string">'q'</span><span class="token punctuation">:</span><span class="token punctuation">(</span><span class="token number">1.1</span><span class="token punctuation">,</span><span class="token number">1.1</span><span class="token punctuation">)</span><span class="token punctuation">,</span>
        <span class="token string">'z'</span><span class="token punctuation">:</span><span class="token punctuation">(</span><span class="token number">.9</span><span class="token punctuation">,</span><span class="token number">.9</span><span class="token punctuation">)</span><span class="token punctuation">,</span>
        <span class="token string">'w'</span><span class="token punctuation">:</span><span class="token punctuation">(</span><span class="token number">1.1</span><span class="token punctuation">,</span><span class="token number">1</span><span class="token punctuation">)</span><span class="token punctuation">,</span>
        <span class="token string">'x'</span><span class="token punctuation">:</span><span class="token punctuation">(</span><span class="token number">.9</span><span class="token punctuation">,</span><span class="token number">1</span><span class="token punctuation">)</span><span class="token punctuation">,</span>
        <span class="token string">'e'</span><span class="token punctuation">:</span><span class="token punctuation">(</span><span class="token number">1</span><span class="token punctuation">,</span><span class="token number">1.1</span><span class="token punctuation">)</span><span class="token punctuation">,</span>
        <span class="token string">'c'</span><span class="token punctuation">:</span><span class="token punctuation">(</span><span class="token number">1</span><span class="token punctuation">,</span><span class="token number">.9</span><span class="token punctuation">)</span><span class="token punctuation">,</span>
          <span class="token punctuation">}</span>

<span class="token keyword">def</span> <span class="token function">getKey</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">:</span>
    tty<span class="token punctuation">.</span>setraw<span class="token punctuation">(</span>sys<span class="token punctuation">.</span>stdin<span class="token punctuation">.</span>fileno<span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">)</span>
    rlist<span class="token punctuation">,</span> _<span class="token punctuation">,</span> _ <span class="token operator">=</span> select<span class="token punctuation">.</span>select<span class="token punctuation">(</span><span class="token punctuation">[</span>sys<span class="token punctuation">.</span>stdin<span class="token punctuation">]</span><span class="token punctuation">,</span> <span class="token punctuation">[</span><span class="token punctuation">]</span><span class="token punctuation">,</span> <span class="token punctuation">[</span><span class="token punctuation">]</span><span class="token punctuation">,</span> <span class="token number">0.1</span><span class="token punctuation">)</span>
    <span class="token keyword">if</span> rlist<span class="token punctuation">:</span>
        key <span class="token operator">=</span> sys<span class="token punctuation">.</span>stdin<span class="token punctuation">.</span>read<span class="token punctuation">(</span><span class="token number">1</span><span class="token punctuation">)</span>
    <span class="token keyword">else</span><span class="token punctuation">:</span>
        key <span class="token operator">=</span> <span class="token string">''</span>

    termios<span class="token punctuation">.</span>tcsetattr<span class="token punctuation">(</span>sys<span class="token punctuation">.</span>stdin<span class="token punctuation">,</span> termios<span class="token punctuation">.</span>TCSADRAIN<span class="token punctuation">,</span> settings<span class="token punctuation">)</span>
    <span class="token keyword">return</span> key

speed <span class="token operator">=</span> <span class="token number">.2</span>
turn <span class="token operator">=</span> <span class="token number">1</span>

<span class="token keyword">def</span> <span class="token function">vels</span><span class="token punctuation">(</span>speed<span class="token punctuation">,</span>turn<span class="token punctuation">)</span><span class="token punctuation">:</span>
    <span class="token keyword">return</span> <span class="token string">"currently:\tspeed %s\tturn %s "</span> <span class="token operator">%</span> <span class="token punctuation">(</span>speed<span class="token punctuation">,</span>turn<span class="token punctuation">)</span>

<span class="token keyword">if</span> __name__<span class="token operator">==</span><span class="token string">"__main__"</span><span class="token punctuation">:</span>
    settings <span class="token operator">=</span> termios<span class="token punctuation">.</span>tcgetattr<span class="token punctuation">(</span>sys<span class="token punctuation">.</span>stdin<span class="token punctuation">)</span>
    
    rospy<span class="token punctuation">.</span>init_node<span class="token punctuation">(</span><span class="token string">'teleop'</span><span class="token punctuation">)</span>
    pub <span class="token operator">=</span> rospy<span class="token punctuation">.</span>Publisher<span class="token punctuation">(</span><span class="token string">'~/cmd_vel'</span><span class="token punctuation">,</span> Twist<span class="token punctuation">,</span> queue_size<span class="token operator">=</span><span class="token number">5</span><span class="token punctuation">)</span>

    x <span class="token operator">=</span> <span class="token number">0</span>
    th <span class="token operator">=</span> <span class="token number">0</span>
    status <span class="token operator">=</span> <span class="token number">0</span>
    count <span class="token operator">=</span> <span class="token number">0</span>
    acc <span class="token operator">=</span> <span class="token number">0.1</span>
    target_speed <span class="token operator">=</span> <span class="token number">0</span>
    target_turn <span class="token operator">=</span> <span class="token number">0</span>
    control_speed <span class="token operator">=</span> <span class="token number">0</span>
    control_turn <span class="token operator">=</span> <span class="token number">0</span>
    <span class="token keyword">try</span><span class="token punctuation">:</span>
        <span class="token keyword">print</span><span class="token punctuation">(</span>msg<span class="token punctuation">)</span>
        <span class="token keyword">print</span><span class="token punctuation">(</span>vels<span class="token punctuation">(</span>speed<span class="token punctuation">,</span>turn<span class="token punctuation">)</span><span class="token punctuation">)</span>
        <span class="token keyword">while</span><span class="token punctuation">(</span><span class="token number">1</span><span class="token punctuation">)</span><span class="token punctuation">:</span>
            key <span class="token operator">=</span> getKey<span class="token punctuation">(</span><span class="token punctuation">)</span>
            <span class="token keyword">if</span> key <span class="token keyword">in</span> moveBindings<span class="token punctuation">.</span>keys<span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">:</span>
                x <span class="token operator">=</span> moveBindings<span class="token punctuation">[</span>key<span class="token punctuation">]</span><span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">]</span>
                th <span class="token operator">=</span> moveBindings<span class="token punctuation">[</span>key<span class="token punctuation">]</span><span class="token punctuation">[</span><span class="token number">1</span><span class="token punctuation">]</span>
                count <span class="token operator">=</span> <span class="token number">0</span>
            <span class="token keyword">elif</span> key <span class="token keyword">in</span> speedBindings<span class="token punctuation">.</span>keys<span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">:</span>
                speed <span class="token operator">=</span> speed <span class="token operator">*</span> speedBindings<span class="token punctuation">[</span>key<span class="token punctuation">]</span><span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">]</span>
                turn <span class="token operator">=</span> turn <span class="token operator">*</span> speedBindings<span class="token punctuation">[</span>key<span class="token punctuation">]</span><span class="token punctuation">[</span><span class="token number">1</span><span class="token punctuation">]</span>
                count <span class="token operator">=</span> <span class="token number">0</span>

                <span class="token keyword">print</span><span class="token punctuation">(</span>vels<span class="token punctuation">(</span>speed<span class="token punctuation">,</span>turn<span class="token punctuation">)</span><span class="token punctuation">)</span>
                <span class="token keyword">if</span> <span class="token punctuation">(</span>status <span class="token operator">==</span> <span class="token number">14</span><span class="token punctuation">)</span><span class="token punctuation">:</span>
                    <span class="token keyword">print</span><span class="token punctuation">(</span>msg<span class="token punctuation">)</span>
                status <span class="token operator">=</span> <span class="token punctuation">(</span>status <span class="token operator">+</span> <span class="token number">1</span><span class="token punctuation">)</span> <span class="token operator">%</span> <span class="token number">15</span>
            <span class="token keyword">elif</span> key <span class="token operator">==</span> <span class="token string">' '</span> <span class="token operator">or</span> key <span class="token operator">==</span> <span class="token string">'k'</span> <span class="token punctuation">:</span>
                x <span class="token operator">=</span> <span class="token number">0</span>
                th <span class="token operator">=</span> <span class="token number">0</span>
                control_speed <span class="token operator">=</span> <span class="token number">0</span>
                control_turn <span class="token operator">=</span> <span class="token number">0</span>
            <span class="token keyword">else</span><span class="token punctuation">:</span>
                count <span class="token operator">=</span> count <span class="token operator">+</span> <span class="token number">1</span>
                <span class="token keyword">if</span> count <span class="token operator">></span> <span class="token number">4</span><span class="token punctuation">:</span>
                    x <span class="token operator">=</span> <span class="token number">0</span>
                    th <span class="token operator">=</span> <span class="token number">0</span>
                <span class="token keyword">if</span> <span class="token punctuation">(</span>key <span class="token operator">==</span> <span class="token string">'\x03'</span><span class="token punctuation">)</span><span class="token punctuation">:</span>
                    <span class="token keyword">break</span>

            target_speed <span class="token operator">=</span> speed <span class="token operator">*</span> x
            target_turn <span class="token operator">=</span> turn <span class="token operator">*</span> th

            <span class="token keyword">if</span> target_speed <span class="token operator">></span> control_speed<span class="token punctuation">:</span>
                control_speed <span class="token operator">=</span> <span class="token builtin">min</span><span class="token punctuation">(</span> target_speed<span class="token punctuation">,</span> control_speed <span class="token operator">+</span> <span class="token number">0.02</span> <span class="token punctuation">)</span>
            <span class="token keyword">elif</span> target_speed <span class="token operator"><</span> control_speed<span class="token punctuation">:</span>
                control_speed <span class="token operator">=</span> <span class="token builtin">max</span><span class="token punctuation">(</span> target_speed<span class="token punctuation">,</span> control_speed <span class="token operator">-</span> <span class="token number">0.02</span> <span class="token punctuation">)</span>
            <span class="token keyword">else</span><span class="token punctuation">:</span>
                control_speed <span class="token operator">=</span> target_speed

            <span class="token keyword">if</span> target_turn <span class="token operator">></span> control_turn<span class="token punctuation">:</span>
                control_turn <span class="token operator">=</span> <span class="token builtin">min</span><span class="token punctuation">(</span> target_turn<span class="token punctuation">,</span> control_turn <span class="token operator">+</span> <span class="token number">0.1</span> <span class="token punctuation">)</span>
            <span class="token keyword">elif</span> target_turn <span class="token operator"><</span> control_turn<span class="token punctuation">:</span>
                control_turn <span class="token operator">=</span> <span class="token builtin">max</span><span class="token punctuation">(</span> target_turn<span class="token punctuation">,</span> control_turn <span class="token operator">-</span> <span class="token number">0.1</span> <span class="token punctuation">)</span>
            <span class="token keyword">else</span><span class="token punctuation">:</span>
                control_turn <span class="token operator">=</span> target_turn

            twist <span class="token operator">=</span> Twist<span class="token punctuation">(</span><span class="token punctuation">)</span>
            twist<span class="token punctuation">.</span>linear<span class="token punctuation">.</span>x <span class="token operator">=</span> control_speed<span class="token punctuation">;</span> twist<span class="token punctuation">.</span>linear<span class="token punctuation">.</span>y <span class="token operator">=</span> <span class="token number">0</span><span class="token punctuation">;</span> twist<span class="token punctuation">.</span>linear<span class="token punctuation">.</span>z <span class="token operator">=</span> <span class="token number">0</span>
            twist<span class="token punctuation">.</span>angular<span class="token punctuation">.</span>x <span class="token operator">=</span> <span class="token number">0</span><span class="token punctuation">;</span> twist<span class="token punctuation">.</span>angular<span class="token punctuation">.</span>y <span class="token operator">=</span> <span class="token number">0</span><span class="token punctuation">;</span> twist<span class="token punctuation">.</span>angular<span class="token punctuation">.</span>z <span class="token operator">=</span> control_turn
            pub<span class="token punctuation">.</span>publish<span class="token punctuation">(</span>twist<span class="token punctuation">)</span>

    <span class="token keyword">except</span> Exception <span class="token keyword">as</span> e<span class="token punctuation">:</span>
        <span class="token keyword">print</span><span class="token punctuation">(</span>e<span class="token punctuation">)</span>

    <span class="token keyword">finally</span><span class="token punctuation">:</span>
        twist <span class="token operator">=</span> Twist<span class="token punctuation">(</span><span class="token punctuation">)</span>
        twist<span class="token punctuation">.</span>linear<span class="token punctuation">.</span>x <span class="token operator">=</span> <span class="token number">0</span><span class="token punctuation">;</span> twist<span class="token punctuation">.</span>linear<span class="token punctuation">.</span>y <span class="token operator">=</span> <span class="token number">0</span><span class="token punctuation">;</span> twist<span class="token punctuation">.</span>linear<span class="token punctuation">.</span>z <span class="token operator">=</span> <span class="token number">0</span>
        twist<span class="token punctuation">.</span>angular<span class="token punctuation">.</span>x <span class="token operator">=</span> <span class="token number">0</span><span class="token punctuation">;</span> twist<span class="token punctuation">.</span>angular<span class="token punctuation">.</span>y <span class="token operator">=</span> <span class="token number">0</span><span class="token punctuation">;</span> twist<span class="token punctuation">.</span>angular<span class="token punctuation">.</span>z <span class="token operator">=</span> <span class="token number">0</span>
        pub<span class="token punctuation">.</span>publish<span class="token punctuation">(</span>twist<span class="token punctuation">)</span>

    termios<span class="token punctuation">.</span>tcsetattr<span class="token punctuation">(</span>sys<span class="token punctuation">.</span>stdin<span class="token punctuation">,</span> termios<span class="token punctuation">.</span>TCSADRAIN<span class="token punctuation">,</span> settings<span class="token punctuation">)</span>
</code>

5.运行和测试

从Github中获取环境将其置于worlds文件夹中。更改slam.launch

<code class="prism language-xml has-numbering">    <span class="token tag"><span class="token punctuation"><</span>arg <span class="token attr-name">name</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>world_name<span class="token punctuation">"</span></span> <span class="token attr-name">value</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>$(find slam_bot)/worlds/kitchen_dining.world<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span>
</code>

运行程序

<code class="has-numbering">$ cd ~/catkin_ws
$ source devel/setup.bash
$ roslaunch slam_bot slam.luaunch
</code>

启动Teleop节点

<code class="has-numbering">$ rosrun slam_bot teleop
</code>

启动建图节点

<code class="has-numbering">roslaunch slam_project mapping.launch
</code>

使用teleop控制模型四处移动，使用ctrl+c停止建图节点。使用rtabmap-databaseViewer打开建图数据库

<code class="has-numbering">rtabmap-databaseViewer ~/.ros/rtabmap.db
</code>

打开后，我们将需要添加一些窗口以更好地查看相关信息，因此：

同意使用数据库参数
View -> Constraint View
View -> Graph View

在这里插入图片描述

图3-2 rtabmap-databaseViewer

可以在左下方看到循环关闭的数量。这些代码代表以下各项：邻居(Neighbor)、邻居合并(Neighbor Merged)、全局环路闭合、按空间划分的局部环路闭合、按时间划分的局部环路闭合、用户环路闭合、优先链接。
在这里插入图片描述

图3-3 3d地图

可以使用的另一个工具是rtabmapviz，它是用于实时可视化特征映射，循环闭合等功能的附加节点。由于计算量大，不建议在仿真中进行映射时使用此工具。rtabmapviz非常适合在实时映射期间部署在真实的机器人上，以确保获得完成循环闭合所必需的功能。要启动，将以下代码添加到mapping.launch文件中：

<code class="has-numbering"><!-- visualization with rtabmapviz -->
    <node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
        <param name="subscribe_depth"             type="bool" value="true"/>
      <param name="subscribe_scan"              type="bool" value="true"/>
      <param name="frame_id"                    type="string" value="base_footprint"/>

      <remap from="rgb/image"       to="$(arg rgb_topic)"/>
      <remap from="depth/image"     to="$(arg depth_topic)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
      <remap from="scan"            to="/scan"/>
    </node>
</code>

6. 导航

在导航中我们重新使用jackal_race.world。首先对整个地图经行建图。
在这里插入图片描述

图3-4 jackal_race.world的3d地图

localization.launch定位节点

复制mapping.launch文件并更名localization.launch。还需要对localization.launch文件进行以下更改：

args=”–delete_db_on_start”从节点启动器中删除，因为您还将需要数据库进行本地化。
删除Mem / NotLinkedNodesKept参数
最后，添加字符串类型的Mem / IncrementalMemory参数并将其设置为false，以完成使机器人进入本地化模式所需的更改。

move.launch导航节点

如之前一样，使用move_base包

<code class="has-numbering"><?xml version="1.0"?>
<launch>

  <!-- Move base -->
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <rosparam file="$(find slam_bot)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find slam_bot)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find slam_bot)/config/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find slam_bot)/config/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find slam_bot)/config/base_local_planner_params.yaml" command="load" />

    <remap from="cmd_vel" to="/cmd_vel"/>
    <remap from="odom" to="/odom"/>
    <remap from="scan" to="/slam_bot/laser/scan"/>

    <param name="base_global_planner" type="string" value="navfn/NavfnROS" />
    <param name="base_local_planner" value="base_local_planner/TrajectoryPlannerROS"/>

  </node>
</launch>
</code>

rivz节点

我们将rviz节点从slam.launch中移除，等各个节点建立完成后我们再打开rviz，这样需要为rviz专门编写一个launch文件。

<code class="prism language-xml has-numbering"><span class="token tag"><span class="token punctuation"><</span>launch<span class="token punctuation">></span></span>
  <span class="token comment"><!--launch rviz--></span>
  <span class="token tag"><span class="token punctuation"><</span>node <span class="token attr-name">name</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>rviz<span class="token punctuation">"</span></span> <span class="token attr-name">pkg</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>rviz<span class="token punctuation">"</span></span> <span class="token attr-name">type</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>rviz<span class="token punctuation">"</span></span> <span class="token attr-name">args</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>-d $(find slam_bot)/launch/config/robot_slam.rviz<span class="token punctuation">"</span></span><span class="token punctuation">/></span></span>
<span class="token tag"><span class="token punctuation"></</span>launch<span class="token punctuation">></span></span>
</code>

其中args="-d $(find slam_bot)/launch/config/robot_slam.rviz"引入了rviz的设置文件，这样就不需要一一添加各个节点了。

批量运行

由于需要使用的运行命令太多，提供如下的程序

<code class="has-numbering">#! /bin/bash

gnome-terminal -x bash -c "killall gzserver" &
sleep 1 &&

echo ' '
read -p 'Would you like to clear the previous map database? (y/n): ' ansinput

if [ “$ansinput” = “y” ]
then
 printf '\n Map deleted \n'
 rm -f ~/.ros/rtabmap.db

elif [ “$ansinput” = “n” ]
then
 printf '\n Map kept \n'

else
 echo 'Warning: Not an acceptable option. Choose (y/n).
         '
fi

echo ' '

read -p 'Enter target world destination or d for default: ' input_choice

if [ “$input_choice” = “d” ]
then
gnome-terminal -x bash -c " roslaunch slam_bot slam.launch "&

else
gnome-terminal -x bash -c "roslaunch slam_bot slam.launch world_file:=$input_choice"  &
fi

sleep 3 &&

gnome-terminal -x bash -c " rosrun slam_bot teleop" &

sleep 3 &&

echo ' '
read -p 'mapping or localization (m/l): ' input

if [ “$input” = “m” ]
then
gnome-terminal -x bash -c "  roslaunch slam_bot mapping.launch simulation:=true" 


elif [ “$input” = “l” ]
then
gnome-terminal -x bash -c "  roslaunch slam_bot localization.launch" 
gnome-terminal -x bash -c "  roslaunch slam_bot move.launch" 


else
 echo 'Warning: Not an acceptable option. Choose (m/l).'
fi
sleep 3 &&

gnome-terminal -x bash -c "  roslaunch slam_bot rviz.launch"

echo ' '
echo 'Script Completed'
echo ' '
</code>

在【SLAM建图和导航仿真实例】（二）中提供了一个测试程序navigation_goal，我们在这里还是借助这个程序。打开一个新的terminal

<code class="has-numbering">$ cd ~/catkin_ws
$ rosrun slam_bot navigation_goal
</code>