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版本：

1
$ sudo apt-get install ros-kinetic-rtabmap-ros

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

1
2
$ sudo apt-get install ros-kinetic-rtabmap ros-kinetic-rtabmap-ros
$ sudo apt-get remove ros-kinetic-rtabmap ros-kinetic-rtabmap-ros

下载安装RTAB-Map

源码安装rtabmap

1
2
3
4
5
6
$ cd ~
$ git clone https://github.com/introlab/rtabmap.git rtabmap
$ cd rtabmap/build
$ cmake -DCMAKE_INSTALL_PREFIX=~/catkin_ws/devel ..
$ make -j4
$ make install

源码安装rtabmap_ros

1
2
3
$ cd ~/catkin_ws
$ git clone https://github.com/introlab/rtabmap_ros.git src/rtabmap_ros
$ catkin_make -j1

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

3.创建启动文件

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

图3-1 Kinect + Odometry + 2D laser

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
<?xml version="1.0" encoding="UTF-8"?>

<launch>

  <!-- Arguments for launch file with defaults provided -->
  <arg name="database_path"     default="rtabmap.db"/>
  <arg name="rgb_topic"   default="/camera/rgb/image_raw"/>
  <arg name="depth_topic" default="/camera/depth/image_raw"/>
  <arg name="camera_info_topic" default="/camera/rgb/camera_info"/>  


  <!-- Mapping Node -->
  <group ns="rtabmap">
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">

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

      <!-- RTAB-Map Inputs -->
      <remap from="scan"            to="/slam_bot/laser/scan"/>
      <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)"/>

      <!-- RTAB-Map Output -->
      <remap from="grid_map" to="/map"/>

      <!-- Rate (Hz) at which new nodes are added to map -->
      <param name="Rtabmap/DetectionRate" type="string" value="1"/>

      <!-- 2D SLAM -->
      <param name="Reg/Force3DoF" type="string" value="true"/>  

      <!-- Loop Closure Constraint -->
      <!-- 0=Visual, 1=ICP (1 requires scan)-->
      <param name="Reg/Strategy" type="string" value="0"/>  

      <!-- Loop Closure Detection -->
      <!-- 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-->
      <param name="Kp/DetectorStrategy" type="string" value="0"/>

      <!-- Maximum visual words per image (bag-of-words) -->
      <param name="Kp/MaxFeatures" type="string" value="400"/>  

      <!-- Used to extract more or less SURF features -->
      <param name="SURF/HessianThreshold" type="string" value="100"/>


      <!-- Minimum visual inliers to accept loop closure -->
      <param name="Vis/MinInliers" type="string" value="15"/>

      <!-- Set to false to avoid saving data when robot is not moving -->
      <param name="Mem/NotLinkedNodesKept" type="string" value="false"/>

    </node>
  </group>
</launch>

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

1
      <param name="database_path"       type="string" value="$(arg database_path)"/>

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

1
2
3
      <param name="database_path"       type="string" value="$(arg database_path)"/>
      <param name="frame_id"            type="string" value="base_footprint"/>
      <param name="odom_frame_id"       type="string" value="/odom"/>

设置database_path,fixed frame和odom。

1
2
      <param name="subscribe_depth"     type="bool"   value="true"/>
      <param name="subscribe_scan"      type="bool"   value="true"/>

默认情况下， 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时，需要设置扫描输入主题。
1
2
3
4
      <remap from="scan"            to="/slam_bot/laser/scan"/>
      <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)"/>

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

1
2
3
4
5
6
      <!-- 2D SLAM -->
      <param name="Reg/Force3DoF" type="string" value="true"/>  

      <!-- Loop Closure Constraint -->
      <!-- 0=Visual, 1=ICP (1 requires scan)-->
      <param name="Reg/Strategy" type="string" value="0"/>

  • Reg/Force3DoF：强制3DoF配准，不会估计roll、pitch和z。

  • Reg/Strategy：使用ICP来细化使用激光扫描发现的ICP的全局闭环。

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

1
2
3
4
5
6
7
8
9
10
11
<!-- RTAB-Map's parameters -->
<param name="RGBD/NeighborLinkRefining" type="string" value="true"/>
<param name="RGBD/ProximityBySpace"     type="string" value="true"/>
<param name="RGBD/AngularUpdate"        type="string" value="0.01"/>
<param name="RGBD/LinearUpdate"         type="string" value="0.01"/>
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
<param name="Grid/FromDepth"            type="string" value="false"/>

<!-- ICP parameters -->
<param name="Icp/VoxelSize"                 type="string" value="0.05"/>
<param name="Icp/MaxCorrespondenceDistance" type="string" value="0.1"/>

  • 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中的移动。

1
2
3
$ cd ~/catkin_ws/slam_bot/launch
$ nano teleop
$ sudo chmod 777 teleop
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
import sys, select, termios, tty
msg = """
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
"""

moveBindings = {
        'i':(1,0),
        'o':(1,-1),
        'j':(0,1),
        'l':(0,-1),
        'u':(1,1),
        ',':(-1,0),
        '.':(-1,1),
        'm':(-1,-1),
           }

speedBindings={
        'q':(1.1,1.1),
        'z':(.9,.9),
        'w':(1.1,1),
        'x':(.9,1),
        'e':(1,1.1),
        'c':(1,.9),
          }

def getKey():
    tty.setraw(sys.stdin.fileno())
    rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
    if rlist:
        key = sys.stdin.read(1)
    else:
        key = ''

    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
    return key

speed = .2
turn = 1

def vels(speed,turn):
    return "currently:\tspeed %s\tturn %s " % (speed,turn)

if __name__=="__main__":
    settings = termios.tcgetattr(sys.stdin)
   
    rospy.init_node('teleop')
    pub = rospy.Publisher('~/cmd_vel', Twist, queue_size=5)

    x = 0
    th = 0
    status = 0
    count = 0
    acc = 0.1
    target_speed = 0
    target_turn = 0
    control_speed = 0
    control_turn = 0
    try:
        print(msg)
        print(vels(speed,turn))
        while(1):
            key = getKey()
            if key in moveBindings.keys():
                x = moveBindings[key][0]
                th = moveBindings[key][1]
                count = 0
            elif key in speedBindings.keys():
                speed = speed * speedBindings[key][0]
                turn = turn * speedBindings[key][1]
                count = 0

                print(vels(speed,turn))
                if (status == 14):
                    print(msg)
                status = (status + 1) % 15
            elif key == ' ' or key == 'k' :
                x = 0
                th = 0
                control_speed = 0
                control_turn = 0
            else:
                count = count + 1
                if count > 4:
                    x = 0
                    th = 0
                if (key == '\x03'):
                    break

            target_speed = speed * x
            target_turn = turn * th

            if target_speed > control_speed:
                control_speed = min( target_speed, control_speed + 0.02 )
            elif target_speed < control_speed:
                control_speed = max( target_speed, control_speed - 0.02 )
            else:
                control_speed = target_speed

            if target_turn > control_turn:
                control_turn = min( target_turn, control_turn + 0.1 )
            elif target_turn < control_turn:
                control_turn = max( target_turn, control_turn - 0.1 )
            else:
                control_turn = target_turn

            twist = Twist()
            twist.linear.x = control_speed; twist.linear.y = 0; twist.linear.z = 0
            twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = control_turn
            pub.publish(twist)

    except Exception as e:
        print(e)

    finally:
        twist = Twist()
        twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
        twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
        pub.publish(twist)

    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)

5.运行和测试

从Github中获取环境将其置于worlds文件夹中。

更改slam.launch

1
    <arg name="world_name" value="$(find slam_bot)/worlds/kitchen_dining.world"/>

运行程序

1
2
3
$ cd ~/catkin_ws
$ source devel/setup.bash
$ roslaunch slam_bot slam.luaunch

启动Teleop节点

1
$ roslaunch slam_bot teleop

启动建图节点

1
roslaunch slam_project mapping.launch

使用teleop控制模型四处移动，使用ctrl+c停止建图节点。

使用rtabmap-databaseViewer打开建图数据库

1
rtabmap-databaseViewer ~/.ros/rtabmap.db

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

  • 同意使用数据库参数
  • View -> Constraint View
  • View -> Graph View
    在这里插入图片描述

图3-2 rtabmap-databaseViewer

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

在这里插入图片描述

图3-3 3d地图

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

要启动，将以下代码添加到mapping.launch文件中：

1
2
3
4
5
6
7
8
9
10
11
<!-- 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>

6. 导航

在导航中我们重新使用jackal_race.world。

首先对整个地图经行建图。
在这里插入图片描述

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

localization.launch定位节点

复制mapping.launch文件并更名localization.launch。

还需要对localization.launch文件进行以下更改：

  1. args="–delete_db_on_start"从节点启动器中删除，因为您还将需要数据库进行本地化。

  2. 删除Mem / NotLinkedNodesKept参数

  3. 最后，添加字符串类型的Mem / IncrementalMemory参数并将其设置为false，以完成使机器人进入本地化模式所需的更改。

move.launch导航节点

如之前一样，使用move_base包

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
<?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>

rivz节点

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

1
2
3
4
<launch>
  <!--launch rviz-->
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find slam_bot)/launch/config/robot_slam.rviz"/>
</launch>

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

批量运行

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
#! /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 ' '

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

打开一个新的terminal

1
2
$ cd ~/catkin_ws
$ rosrun slam_bot navigation_goal

在这里插入图片描述

图3-5 建图和导航