Note: This tutorial assumes that you have completed the previous tutorials: RGB-D Hand-Held Mapping With a Kinect.
(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Setup RTAB-Map on Your Robot!

Description: This tutorial shows multiple RTAB-Map configurations that can be used on your robot.

Tutorial Level: INTERMEDIATE

Next Tutorial: Mapping and Navigation with Turtlebot

Introduction

The robot must be equipped at least with a Kinect-like sensor. I recommend highly to calibrate your Kinect-like sensor following this guide. If you want to use a 2D laser, the Kinect's clouds must be aligned with the laser scans.

I will present in the next sections some possible configurations depending on the robot with example launch files.

Recommended robot configuration:

These examples are based on what I did for AZIMUT3. The robot is equipped with a Kinect, an URG-04LX and odometry is provided using the wheel encoders.

AZIMUT3

Bring-up your robot

  • If your robot has odometry: the robot should publish his odometry in nav_msgs/Odometry format.

  • If you have a laser: launch the laser node, it should publish sensor_msgs/LaserScan messages. Here we use hokuyo_node node.

  • Launch OpenNI with depth_registration:=true:

    •  $ roslaunch openni_launch openni.launch depth_registration:=true
  • TF transforms must be provided (through using robot's URDF or manually in a file using static transform publishers). The main useful transforms are "/odom", "/base_link", "/base_laser_link" and "/camera_link".

Kinect + Odometry + 2D laser

Kinect + Odometry + 2D laser

In this configuration, I assume that you have a wheeled robot constrained to the plane XY with only rotation on yaw (around z-axis).

<launch>
  <group ns="rtabmap">
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
          <param name="frame_id" type="string" value="base_link"/>

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

          <remap from="odom" to="/base_controller/odom"/>
          <remap from="scan" to="/base_scan"/>

          <remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
          <remap from="depth/image" to="/camera/depth_registered/image_raw"/>
          <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>

          <param name="queue_size" type="int" value="10"/>

          <!-- 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="Reg/Strategy"              type="string" value="1"/> <!-- 1=ICP -->
          <param name="Reg/Force3DoF"             type="string" value="true"/>
          <param name="Vis/MinInliers"            type="string" value="12"/>
    </node>
  </group>
</launch>

Let's break down this launch file:

 <group ns="rtabmap">

For convenience, we put rtabmap node in rtabmap namespace. rtabmap node provides services which can conflict with other services from other nodes.

<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">

The "--delete_db_on_start" argument will make rtabmap to delete the database (default located in ~/.ros/rtabmap.db) when starting. If you want the robot to continue mapping from a previous mapping session, you should remove "--delete_db_on_start".

<param name="frame_id" type="string" value="base_link"/>

The "frame_id" should be a fixed frame on the robot.

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

In RGBD-SLAM mode, "subscribe_depth" should always be true. In appearance-based loop closure detection only mode, "subscribe_depth" would be false. Since we have a laser, set "subscribe_laserScan" to true.

  • When "subscribe_depth=true", "depth/image" and "rgb/camera_info" input topics should be set.
  • When "subscribe_scan=true", "scan" input topics should be set.

<remap from="odom" to="/base_controller/odom"/>
<remap from="scan" to="/base_scan"/>

<remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
<remap from="depth/image" to="/camera/depth_registered/image_raw"/>
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>

Set the required input topics.

<param name="queue_size" type="int" value="10"/>

Used for synchronization of the input topics above. The rtabmap node synchronizes /odom, /scan, /rgb/image, /depth/image and /rgb/camera_info in a single callback. Higher the value, more flexible can be the rate used for each topic. On AZIMUT3, /odom is published at 50 Hz, /scan at 10 Hz, and the other at 30 Hz.

<!-- 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="Reg/Strategy"              type="string" value="1"/> <!-- 1=ICP -->
<param name="Reg/Force3DoF"             type="string" value="true"/>
<param name="Vis/MinInliers"            type="string" value="12"/>

This section sets the RTAB-Map's parameters (the same as in Preferences dialog in the standalone version). To know all RTAB-Map's parameters that can be set with some descriptions, execute this command:

  •  $ rosrun rtabmap_ros rtabmap --params

Here is a brief overview of the main parameters set here:

  • RGBD/NeighborLinkRefining: Correct odometry using the input laser topic using ICP.

  • RGBD/ProximityBySpace: Find local loop closures based on the robot position in the map. It is useful when the robot, for example, is coming back in the opposite direction. With camera facing back, global loop closures cannot be found. So using the position and previously added laser scans to the map, we find the transform using ICP. Be aware that on large-scale mapping, this method should be disabled because when the odometry is very erroneous, local ICP could give wrong results (false loop closures).

  • RGBD/AngularUpdate: The robot should move to update the map (if not 0).

  • RGBD/LinearUpdate: The robot should move to update the map (if not 0).

  • RGBD/OptimizeFromGraphEnd: Here we optimized from the latest node added to the map instead of the first. By optimizing from the last, the last pose keeps it's value and all the previous poses are corrected according to it (so /odom and /map will always match together). By optimizing from the first, all the successive nodes are corrected according to the first one (so there will be a transformation between /odom and /map to correct the last pose added). However, by optimizing from the first: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).

  • Reg/Strategy: We chose ICP to refine global loop closures found with ICP using the laser scans.

  • Reg/Force3DoF: Force 3DoF registration: roll, pitch and z won't be estimated.

  • Vis/MinInliers: Minimum visual words inliers (after RANSAC transformation between images of a loop closure) to accept the transformation.

  </node>
</group>

Close the group.

Kinect + Odometry + Fake 2D laser from Kinect

Kinect + Odometry + Fake 2D laser from Kinect

If you don't have a laser and you want to create a 2D occupancy grid map (for which laser scans are required), you can simulate a 2D laser with the depth image from the Kinect using depthimage_to_laserscan ros-pkg. This was the configuration used for the IROS 2014 Kinect Contest. This could be also used with a robot like the TurtleBot. Unlike with a real lidar on example above, I don't recommend setting Reg/Strategy to 1 (for ICP) because the field of view of the camera is too small to have good ICP registrations.

<launch>
  <!-- Kinect cloud to laser scan -->
    <node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan">
      <remap from="image"     to="/camera/depth_registered/image_raw"/>
      <remap from="camera_info" to="/camera/depth_registered/camera_info"/>
      <remap from="scan" to="/kinect_scan"/>
      <param name="range_max" type="double" value="4"/>
    </node>

  <!-- SLAM -->
  <group ns="rtabmap">
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
          <param name="frame_id" type="string" value="base_footprint"/>

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

          <remap from="odom" to="/base_controller/odom"/>
          <remap from="scan" to="/kinect_scan"/>

          <remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
          <remap from="depth/image" to="/camera/depth_registered/image_raw"/>
          <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>

          <param name="queue_size" type="int" value="10"/>

          <!-- RTAB-Map's parameters -->
          <param name="RGBD/ProximityBySpace"     type="string" value="false"/>
          <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="Reg/Force3DoF"             type="string" value="true"/>
          <param name="Vis/MinInliers"            type="string" value="12"/>
    </node>
  </group>
</launch>

Kinect + 2D laser

Kinect + 2D laser

In you don't have odometry, you can create one using the 2D laser scans and ICP. Once you have this new odometry node, you can do the same steps as above (with odometry). To generate odometry from laser scans, take a look at these packages: laser_scan_matcher or hector_slam. NEW RTAB-Map has now its own icp_odometry node.

Here is an example on how hector_slam can be integrated with rtabmap: demo_hector_mapping.launch. you can try it with this bag: demo_mapping.bag (you should remove the "/odom" tf from the bag first) :

$ rosbag filter demo_mapping.bag demo_mapping_no_odom.bag 'topic != "/tf" or topic == "/tf" and m.transforms[0].header.frame_id != "/odom"'
$ roslaunch rtabmap_ros demo_hector_mapping.launch hector:=true
$ rosbag play --clock demo_mapping_no_odom.bag

For an example using rtabmap_ros/icp_odometry, set hector argument to false above.

Kinect + Odometry

Kinect + Odometry

In this configuration, I assume that you have a robot not constrained to a single plane (like UAV), which can move in XYZ and roll, pitch, yaw rotations.

<launch>
  <group ns="rtabmap">
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
          <param name="frame_id" type="string" value="base_link"/>

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

          <remap from="odom" to="/base_controller/odom"/>

          <remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
          <remap from="depth/image" to="/camera/depth_registered/image_raw"/>
          <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>

          <param name="queue_size" type="int" value="10"/>

          <!-- RTAB-Map's parameters -->
          <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"/>
    </node>
  </group>
</launch>

Kinect

Kinect

If you can't have a reliable odometry, you can map using only RTAB-Map at the cost of "lost odometry" (like the RED screens in the standalone version). The robot may detect this "lost" state when a null odometry message is sent by the rgbd_odometry node.

<launch>
  <group ns="rtabmap">

    <!-- Odometry -->
    <node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
      <remap from="rgb/image"       to="/camera/rgb/image_rect_color"/>
      <remap from="depth/image"     to="/camera/depth_registered/image_raw"/>
      <remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>

      <param name="frame_id" type="string" value="base_link"/>
    </node>

    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
          <param name="frame_id" type="string" value="base_link"/>
          <param name="subscribe_depth" type="bool" value="true"/>

          <remap from="odom" to="odom"/>
          <remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
          <remap from="depth/image" to="/camera/depth_registered/image_raw"/>
          <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>

          <param name="queue_size" type="int" value="10"/>

          <!-- RTAB-Map's parameters -->
          <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"/>
    </node>
  </group>
</launch>

Stereo A

Stereo A

RTAB-Map can be also used with a stereo camera. As shown in the picture above, you will need to install viso2_ros and the stereo_image_proc nodes.

<launch>
  <!-- Run the ROS package stereo_image_proc for image rectification and disparity computation -->
  <group ns="stereo">
    <node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc"/>

    <!-- Disparity to depth -->
    <node pkg="nodelet" type="nodelet" name="disparity2depth" args="standalone rtabmap_ros/disparity_to_depth"/>
  </group>

  <!-- Odometry: Run the viso2_ros package -->
  <node pkg="viso2_ros" type="stereo_odometer" name="stereo_odometer" output="screen">
    <remap from="stereo" to="stereo"/>
    <remap from="image" to="image_rect"/>
    <param name="base_link_frame_id" value="/base_link"/>
    <param name="odom_frame_id" value="/odom"/>
    <param name="ref_frame_change_method" value="1"/>
  </node>

  <group ns="rtabmap">
    <!-- Visual SLAM -->
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
      <param name="subscribe_depth" type="bool" value="true"/>
      <param name="subscribe_laserScan" type="bool" value="false"/>

      <remap from="rgb/image"       to="/stereo/left/image_rect"/>
      <remap from="rgb/camera_info" to="/stereo/left/camera_info"/>
      <remap from="depth/image"     to="/stereo/depth"/>

      <remap from="odom" to="/stereo_odometer/odometry"/>

      <param name="frame_id" type="string" value="/base_link"/>
      <param name="queue_size" type="int" value="30"/>

      <param name="Vis/MinInliers" type="string" value="12"/>
    </node>
  </group>
</launch>

Stereo B

Stereo B

The page page StereoOutdoorMapping shows a working demonstration that you can try with the provided rosbag. For actual mapping with your stereo camera, you camera driver should publish image messages like these (the camera namespace can be different than stereo_camera):

$ rostopic list
/stereo_camera/left/image_raw
/stereo_camera/left/camera_info
/stereo_camera/right/image_raw
/stereo_camera/right/camera_info

and assuming that the camera driver provides TF stereo_camera and that left and right images are synchronized (note the approx_sync and stereo_approx_sync), the corresponding launch file could be like this:

<launch>
<arg name="pi/2" value="1.5707963267948966" />
<arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
<node pkg="tf" type="static_transform_publisher" name="camera_base_link"
     args="$(arg optical_rotate) base_link stereo_camera 100" />  

<!-- Run the ROS package stereo_image_proc -->
<group ns="/stereo_camera" >
    <node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc"/>

    <!-- Odometry -->
    <node pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen">
        <remap from="left/image_rect"       to="left/image_rect"/>
        <remap from="right/image_rect"      to="right/image_rect"/>
        <remap from="left/camera_info"      to="left/camera_info"/>
        <remap from="right/camera_info"     to="right/camera_info"/>

        <param name="frame_id" type="string" value="base_link"/>
        <param name="odom_frame_id" type="string" value="odom"/>
        <param name="approx_sync" type="bool" value="false"/>
        <param name="queue_size" type="int" value="5"/>

        <param name="Odom/MinInliers" type="string" value="12"/>
        <param name="Odom/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
    </node>     
</group>

<group ns="rtabmap">   
  <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
     <param name="frame_id" type="string" value="base_link"/>
     <param name="subscribe_stereo" type="bool" value="true"/>
     <param name="subscribe_depth" type="bool" value="false"/>
     <param name="stereo_approx_sync" type="bool" value="false"/>

     <remap from="left/image_rect" to="/stereo_camera/left/image_rect_color"/>
     <remap from="right/image_rect" to="/stereo_camera/right/image_rect"/>
     <remap from="left/camera_info" to="/stereo_camera/left/camera_info"/>
     <remap from="right/camera_info" to="/stereo_camera/right/camera_info"/>

     <remap from="odom" to="/stereo_camera/odom"/>

     <param name="queue_size" type="int" value="30"/>

     <!-- RTAB-Map's parameters -->
     <param name="Vis/MinInliers" type="string" value="12"/>
  </node>
 </group>
</launch>

For visualization, I recommend to try the stereo outdoor mapping tutorial to see what is going on with rviz. To use rtabmapviz, you can add the node under rtabmap namespace above:

<group ns="rtabmap">  
  <!-- Visualisation RTAB-Map -->
  <node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
     <param name="subscribe_stereo" type="bool" value="true"/>
     <param name="subscribe_odom_info" type="bool" value="true"/>
     <param name="queue_size" type="int" value="10"/>
     <param name="frame_id" type="string" value="base_link"/>
     <remap from="left/image_rect" to="/stereo_camera/left/image_rect_color"/>
     <remap from="right/image_rect" to="/stereo_camera/right/image_rect"/>
     <remap from="left/camera_info" to="/stereo_camera/left/camera_info"/>
     <remap from="right/camera_info" to="/stereo_camera/right/camera_info"/>
     <remap from="odom_info" to="/stereo_camera/odom_info"/>
     <remap from="odom" to="/stereo_camera/odom"/>
  </node>
</group>

The rtabmap node uses the laser scans to create a 2D occupancy grid map that can be used by a planner (see grid_map topic). If you don't have laser scans, you can create with rtabmap node with proj_map topic a 2D occupancy grid map from the projection of the Kinect or Stereo point clouds on the ground. Visit the StereoOutdoorNavigation page for an example of creating such maps.

Remote visualization

rtabmapviz

<launch>
  <group ns="rtabmap">
    <node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" output="screen">
      <param name="frame_id" type="string" value="base_link"/>
      <remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
      <param name="rgb/image_transport" type="string" value="compressed"/>
    </node>
  </group>
</launch>

rviz

When RTAB-Map's ros-pkg is built, the rtabmap_ros/MapCloud plugin can be selected in RVIZ for visualization of the constructed 3D map cloud.

$ rosrun rviz rviz

Remote visualization: bandwidth efficiency

The data_throttle node can be used to decrease the framerate of the OpenNI camera, thus limiting the bandwidth used.

On the robot, you can add this (throttling to 5 Hz) and link the outputs to rtabmapviz or rviz :

  <!-- Throttling messages -->
  <group ns="camera">
    <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen">
      <param name="rate" type="double" value="5.0"/>

      <remap from="rgb/image_in"       to="rgb/image_rect_color"/>
      <remap from="depth/image_in"     to="depth_registered/image_raw"/>
      <remap from="rgb/camera_info_in" to="rgb/camera_info"/>

      <remap from="rgb/image_out"       to="data_throttled_image"/>
      <remap from="depth/image_out"     to="data_throttled_image_depth"/>
      <remap from="rgb/camera_info_out" to="data_throttled_camera_info"/>
    </node>
  </group>

Note that this nodelet is created in the "camera_nodelet_manager" manager from OpenNI.

rtabmapviz

rtabmapviz

On the client side, use the following for rtabmapviz (this will make rtabmapviz showing latest camera data and laser data):

<launch>
  <!-- Visualisation (client side) -->
  <group ns="rtabmap">
    <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_laserScan" type="bool" value="true"/> <!-- if you have a laser -->
      <param name="queue_size" type="int" value="10"/>
      <param name="frame_id" type="string" value="base_link"/>

      <remap from="rgb/image" to="/camera/data_throttled_image"/>
      <remap from="depth/image" to="/camera/data_throttled_image_depth"/>
      <remap from="rgb/camera_info" to="/camera/data_throttled_camera_info"/>
      <remap from="scan" to="/base_scan"/>
      <remap from="odom" to="/base_controller/odom"/>

      <param name="rgb/image_transport" type="string" value="compressed"/>
      <param name="depth/image_transport" type="string" value="compressedDepth"/>
    </node>
  </group>
</launch>

rviz

rviz

Use the following for rviz:

<launch>
  <node pkg="rviz" type="rviz" name="rviz"/>

  <!-- Construct and voxelize the point cloud (for fast visualization in rviz) -->
  <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
    <remap from="rgb/image"       to="/camera/data_throttled_image"/>
    <remap from="depth/image"     to="/camera/data_throttled_image_depth"/>
    <remap from="rgb/camera_info" to="/camera/data_throttled_camera_info"/>
    <remap from="cloud"           to="voxel_cloud" />

    <param name="rgb/image_transport" type="string" value="theora"/>
    <param name="depth/image_transport" type="string" value="compressedDepth"/>

    <param name="queue_size" type="int" value="10"/>
    <param name="voxel_size" type="double" value="0.01"/>
  </node>
</launch>

rtabmap_ros/point_cloud_xyzrgb nodelet create a point cloud from the RGB and depth images, with optional voxel size (0=disabled).

Remote mapping

Remote mapping

EDIT (February 4 2016): There is now a simple tutorial about remote mapping with a Kinect here: http://wiki.ros.org/rtabmap_ros/Tutorials/RemoteMapping

In this example, we assume that the robot is started with only sensors and robot controllers ready. The SLAM will be done on the client side. Sensors' data are transferred over Wifi to the client. For efficiency and to not subscribe to same topics multiple time on the robot, we use ros relays to subscribe just once to incoming topics from the robot and relay the messages to multiples nodes on the client side. We use of rtabmap_ros/data_throttle nodelet to throttle the images from OpenNI node at a lower rate (5 Hz). Subscriptions to throttled images use image_transport with theora and compressedDepth encodings for RGB and depth images respectively. This gives a bandwidth around 700 KB/s between the robot and the client.

<launch>
  <!-- Visualization and SLAM nodes use same data, so just subscribe once and relay messages -->
  <node name="odom_relay" type="relay" pkg="topic_tools" args="/base_controller/odom /base_controller/odom_relay" />
  <node name="scan_relay" type="relay" pkg="topic_tools" args="/base_scan /base_scan_relay" />
  <node name="camera_info_relay" type="relay" pkg="topic_tools" args="/camera/data_throttled_camera_info /camera/data_throttled_camera_info_relay" />
  <node name="republish_rgb" type="republish" pkg="image_transport" args="theora in:=/camera/data_throttled_image raw out:=/camera/data_throttled_image_relay" />
  <node name="republish_depth" type="republish" pkg="image_transport" args="compressedDepth in:=/camera/data_throttled_image_depth raw out:=/camera/data_throttled_image_depth_relay" />

  <!-- SLAM client side -->
  <!-- args: "delete_db_on_start" and "udebug" -->
  <group ns="rtabmap">
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
          <param name="frame_id" type="string" value="base_link"/>

          <param name="subscribe_depth" type="bool" value="true"/>
          <param name="subscribe_laserScan" type="bool" value="true"/>

          <remap from="odom" to="/base_controller/odom_relay"/>
          <remap from="scan" to="/base_scan_relay"/>

          <remap from="rgb/image" to="/camera/data_throttled_image_relay"/>
          <remap from="depth/image" to="/camera/data_throttled_image_depth_relay"/>
          <remap from="rgb/camera_info" to="/camera/data_throttled_camera_info_relay"/>

          <param name="queue_size" type="int" value="10"/>

          <!-- RTAB-Map's parameters -->
          <param name="RGBD/NeighborLinkRefining" type="string" value="true"/>
          <param name="RGBD/ProximityBySpace" type="string" value="true"/>
          <param name="LccIcp/Type" type="string" value="1"/>
          <param name="Vis/MinInliers" type="string" value="12"/>
          <param name="Vis/InlierDistance" type="string" value="0.1"/>
          <param name="RGBD/AngularUpdate" type="string" value="0.01"/>
          <param name="RGBD/LinearUpdate" type="string" value="0.01"/>
    </node>

  <node pkg="rviz" type="rviz" name="rviz"/>

  <!-- Construct point cloud of the latest throttled data -->
  <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
    <remap from="rgb/image"       to="/camera/data_throttled_image_relay"/>
    <remap from="depth/image"     to="/camera/data_throttled_image_depth_relay"/>
    <remap from="rgb/camera_info" to="/camera/data_throttled_camera_info_relay"/>
    <remap from="cloud"           to="voxel_cloud" />

    <param name="queue_size" type="int" value="10"/>
    <param name="voxel_size" type="double" value="0.01"/>
  </node>
</launch>

Switching between Mapping and Localization

It can be convenient after mapping an area to put rtabmap in localization mode to avoid increasing the map size in already mapped areas. If you are using rtabmapviz, there are already buttons on the interface:

mapping_localization_buttons

Otherwise, you can call the set_mode_mapping and set_mode_localization services.

 $ rosservice call rtabmap/set_mode_localization
 $ rosservice call rtabmap/set_mode_mapping

Wiki: rtabmap_ros/Tutorials/SetupOnYourRobot (last edited 2018-09-18 22:49:36 by MathieuLabbe)