slam: added rtabmap example

[matlabbe]

Hi,

Just came across the turtlebot3_slam package. There are many examples with some SLAM approaches already added. I just did one for rtabmap. With the "waffle" model, the RGB+depth images will be also used for 3D mapping. For the other models, only scan is used.

cheers,
Mathieu

EDIT: to just copy the files in binary installed turtbot3 packages:

wget https://raw.githubusercontent.com/ROBOTIS-GIT/turtlebot3/aa6b0df8da395c7dbc34b70a21bb820731f20231/turtlebot3_slam/launch/turtlebot3_rtabmap.launch
wget hhttps://raw.githubusercontent.com/ROBOTIS-GIT/turtlebot3/aa6b0df8da395c7dbc34b70a21bb820731f20231/turtlebot3_slam/rviz/turtlebot3_rtabmap.rviz
sudo mv turtlebot3_rtabmap.launch /opt/ros/noetic/share/turtlebot3_slam/launch/.
sudo mv turtlebot3_rtabmap.rviz /opt/ros/noetic/share/turtlebot3_slam/rviz/.

[ViniciusAbrao]

@matlabbe Hello... one question: did not you face the message bellow?

/rtabmap/rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=10).
/rtabmap/rtabmap subscribed to (approx sync):
/odom,
/camera/rgb/image_raw,
/camera/depth/image_raw,
/camera/rgb/camera_info,
/scan

[matlabbe]

Make sure those topics are published:

$ rostopic hz /odom /camera/rgb/image_raw /camera/depth/image_raw /camera/rgb/camera_info /scan

subscribed to [/odom]
subscribed to [/camera/rgb/image_raw]
subscribed to [/camera/depth/image_raw]
subscribed to [/camera/rgb/camera_info]
subscribed to [/scan]
WARNING: may be using simulated time
         topic             rate   min_delta   max_delta   std_dev    window
===========================================================================
/odom                     29.69   0.0         0.048       0.01438    29    
/camera/rgb/image_raw     46.54   0.019       0.033       0.002928   29    
/camera/depth/image_raw   46.39   0.016       0.035       0.003762   44    
/camera/rgb/camera_info   23.58   0.031       0.064       0.005836   44    
/scan                     5.0     0.198       0.204       0.002345   44

Here is a usage example of this pull request (tested with latest turtlebot3's master branch at the date of this post).

Terminal 1:

$ export TURTLEBOT3_MODEL=waffle
$ roslaunch turtlebot3_gazebo turtlebot3_world.launch

Terminal 2:

$ export TURTLEBOT3_MODEL=waffle
$ roslaunch turtlebot3_slam turtlebot3_slam.launch slam_methods:=rtabmap

Terminal 3 (optional, start move_base):

$ export TURTLEBOT3_MODEL=waffle
$ roslaunch turtlebot3_navigation move_base.launch

Here is a result after sending some goals (2D Nav Goal button in RVIZ):

[ViniciusAbrao]

@matlabbe following your usage example it works correctly. Tks !!

[muratkoc503]

Hi. How i use rtabmap navigation without LaserScan massage or depth to scan conversion? i want to use navigation stack as 3D costmap. Thanks

[matlabbe]

Set subscribe_scan to false.

[muratkoc503]

thanks. also, i see your answer in https://answers.ros.org/question/328855/localising-using-rtabmap-without-lidar/

[shrinivas96]

Dear @matlabbe,
thanks very much for making this example launch file and such good explanations. I followed every one of your instructions only to have the same error as @ViniciusAbrao posted. I even tried to replicate these instructions with the master branch commit closest to the time of your comment (~ Oct 29, 2020). But I still face the same problem.

Just launching turtlebot3_gazebo and checking the topics list shows that all these topics /odom /camera/rgb/image_raw /camera/depth/image_raw /camera/rgb/camera_info /scan are all published.

BUT, the problem goes away when I add parameter queue_size in turtlebot3_rtabmap.launch and set it to a high number, like so:

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

(inside the rtabmap node invoking)

BUT, this stops working again (i.e. giving the same warning as mentioned above) when you launch turtlebot3_gazebo and turtlebot3_slam quickly one after the other, not leaving enough time in between each other for gazebo to launch completely. This is exactly the case when, for example you want to launch both of them through one launch file, like so:

<launch>
    <arg name="model"    default="waffle" />
    <arg name="slam_methods"    default="rtabmap" />
 
    <!-- roslaunch turtlebot3_gazebo turtlebot3_world.launch -->
    <include file="$(find turtlebot3_gazebo)/launch/turtlebot3_world.launch">
      <arg name="model"    value="$(arg model)" />
    </include>
    
    <!-- roslaunch turtlebot3_slam turtlebot3_slam.launch slam_methods:=rtabmap -->
    <include file="$(find turtlebot3_slam)/launch/turtlebot3_slam.launch">
      <arg name="model"    value="$(arg model)" />
      <arg name="slam_methods"    value="$(arg slam_methods)" />
    </include> 
</launch>  

The problem is solved by setting the queue_size parameter to be something even more higher (like, 70, in my case). Even then it publishes the "Did not receive data since 5 seconds ... " warning message once and then gets back on track.

I am not sure if this behavior is expected, as more often than not, one would be invoking rtabmap along with multiple other things through a single launch file.

Though I do not know enough ROS to make this possible, but do I alternatively need to make sure that "timestamps in their header are set"? This is what I would try next, but in the meanwhile, a help in any direction would be greatly appreciated.

As a side note, here is what happens when "it does not work", with queue_size=10:

1. The terminal output of roslaunch turtlebot3_slam turtlebot3_slam.launch slam_methods:=rtabmap:
   

2. The rviz window opening right after:
   

Cheers,
Shrini

[matlabbe]

Just launching turtlebot3_gazebo and checking the topics list shows that all these topics /odom /camera/rgb/image_raw /camera/depth/image_raw /camera/rgb/camera_info /scan are all published.

Do rostopic hz, not rostopic list. Just tried again and the instructions above still work.

[shrinivas96]

@matlabbe, thanks a lot for taking out the time to re-run the example and comment on my issue.

You are right, when I tried to run your example on a different machine, it worked properly. My machine (Ubuntu 20.04 running on WSL2) somehow does not have all 5 topics when doing a rostopic hz at all times. Some times there are two, sometimes five.

Many thanks once again. Cheers,
Shrini

[jnd4i-aj]

Make sure those topics are published:

$ rostopic hz /odom /camera/rgb/image_raw /camera/depth/image_raw /camera/rgb/camera_info /scan

subscribed to [/odom]
subscribed to [/camera/rgb/image_raw]
subscribed to [/camera/depth/image_raw]
subscribed to [/camera/rgb/camera_info]
subscribed to [/scan]
WARNING: may be using simulated time
         topic             rate   min_delta   max_delta   std_dev    window
===========================================================================
/odom                     29.69   0.0         0.048       0.01438    29    
/camera/rgb/image_raw     46.54   0.019       0.033       0.002928   29    
/camera/depth/image_raw   46.39   0.016       0.035       0.003762   44    
/camera/rgb/camera_info   23.58   0.031       0.064       0.005836   44    
/scan                     5.0     0.198       0.204       0.002345   44

Here is a usage example of this pull request (tested with latest turtlebot3's master branch at the date of this post).

Terminal 1:

$ export TURTLEBOT3_MODEL=waffle
$ roslaunch turtlebot3_gazebo turtlebot3_world.launch

Terminal 2:

$ export TURTLEBOT3_MODEL=waffle
$ roslaunch turtlebot3_slam turtlebot3_slam.launch slam_methods:=rtabmap

Terminal 3 (optional, start move_base):

$ export TURTLEBOT3_MODEL=waffle
$ roslaunch turtlebot3_navigation move_base.launch

Here is a result after sending some goals (2D Nav Goal button in RVIZ):

But there is no any method listed for rtabmap SLAM.
I checked the turtlebot3_slam.launch file and in slam methods there is no rtabmap mentioned.
I want to learn RTABMAP SLAM but did not find resources. I don't have a actual robot but I want to learn it using simulation.
Do you have any resources?

[matlabbe]

@AMAN021jin this pull request has never been merged, so rtabmap option is not released in binaries. You can do it manually following this tutorial (section Melodic/Noetic) instead.

[shrinivas96]

Update for anyone who is facing problems with the following error:

ERROR: cannot launch node of type [rtabmap_ros/rtabmap]: Cannot locate node of type [rtabmap] in package [rtabmap_ros]. Make sure file exists in package path and permission is set to executable (chmod +x)

They changed the package that contained the executable rtabmap from rtabmap_ros to rtabmap_slam. So all you have to do is to change a line in the file turtlebot3/turtlebot3_slam/launch/turtlebot3_rtabmap.launch from the old:

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

to the new:

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

You might still get the following two errors:

[ERROR] [1682937967.926884730, 150.614000000]: PluginlibFactory: The plugin for class 'rtabmap_ros/MapCloud' failed to load.  Error: According to the loaded plugin descriptions the class rtabmap_ros/MapCloud with base class type rviz::Display does not exist. Declared types are  rtabmap_rviz_plugins/Info rtabmap_rviz_plugins/MapCloud rtabmap_rviz_plugins/MapGraph rviz/AccelStamped rviz/Axes rviz/Camera rviz/DepthCloud rviz/Effort rviz/FluidPressure rviz/Grid rviz/GridCells rviz/Illuminance rviz/Image rviz/InteractiveMarkers rviz/LaserScan rviz/Map rviz/Marker rviz/MarkerArray rviz/Odometry rviz/Path rviz/PointCloud rviz/PointCloud2 rviz/PointStamped rviz/Polygon rviz/Pose rviz/PoseArray rviz/PoseWithCovariance rviz/Range rviz/RelativeHumidity rviz/RobotModel rviz/TF rviz/Temperature rviz/TwistStamped rviz/WrenchStamped rviz_imu_plugin/Imu rviz_plugin_tutorials/Imu
[ERROR] [1682937967.927239511, 150.614000000]: PluginlibFactory: The plugin for class 'rtabmap_ros/MapGraph' failed to load.  Error: According to the loaded plugin descriptions the class rtabmap_ros/MapGraph with base class type rviz::Display does not exist. Declared types are  rtabmap_rviz_plugins/Info rtabmap_rviz_plugins/MapCloud rtabmap_rviz_plugins/MapGraph rviz/AccelStamped rviz/Axes rviz/Camera rviz/DepthCloud rviz/Effort rviz/FluidPressure rviz/Grid rviz/GridCells rviz/Illuminance rviz/Image rviz/InteractiveMarkers rviz/LaserScan rviz/Map rviz/Marker rviz/MarkerArray rviz/Odometry rviz/Path rviz/PointCloud rviz/PointCloud2 rviz/PointStamped rviz/Polygon rviz/Pose rviz/PoseArray rviz/PoseWithCovariance rviz/Range rviz/RelativeHumidity rviz/RobotModel rviz/TF rviz/Temperature rviz/TwistStamped rviz/WrenchStamped rviz_imu_plugin/Imu rviz_plugin_tutorials/Imu

I do not know how to fix these two, but in the rviz display panel on the left I just removed the MapCloud and MapGraph plugins and added them again from the "Create visualisation" dialog box.

[matlabbe]

@shrinivas96 I updated the pull request with your changes, thx!