rtabmap_ros won't subscribe to 3D point cloud from velodyne

Question

Hi

I am using rtabmap_ros version 0.13.1 with the ZED stereo camera and the VLP16 laser scanner. Even though I enabled the "subscribe to scan cloud" option and used the correct cloud topic name "velodyne_points" rtabmap seems to ignore the subscription.

My objective is simply to use the pose graph and loop closure optimization from rtabmap to display point clouds from VLP16. Can rtabmap store the 3d laser scan at the graph nodes and display them later?

I am using the sample launch file stereo_mapping.launch

https://www.dropbox.com/s/y9ocshgq8yed3kq/stereo_mapping.launch?dl=0

RTABMAP start log

https://www.dropbox.com/s/wdwivai4snqzyf6/rtabmap%20start%20log?dl=0

ros graph

https://www.dropbox.com/s/zr0wa3n0gtzpqec/rosgraph.png?dl=0

Thank you in advance!

Lealem

---

Originally posted by lealem on ROS Answers with karma: 1 on 2017-09-07

Post score: 0

---

Original comments

Comment by matlabbe on 2017-09-07:
Can you add the launch file (rtabmap node part)? You can also copy paste the log output when rtabmap is initialized to see to which topics it is subscribed.

Comment by lealem on 2017-09-08:
Also checked on rostopic info, it shows there is no subscription to the topic /velodyne_points. I also attached the computation graph for your reference.

Thanks again!

Answer 1

Thx for the update, I can see the problem. rtabmap_ros cannot subscribe to scan topics when using stereo topics (for synchronization issue). We should use rgb/depth interface instead.

To make sure we are using exactly synchronized camera topics while we are subscribing to a scan topic (approximately synchronized), we need to use the rtabmap_ros/rgbd_sync nodelet prior to rtabmap node. Here is an example using direclty rgb/depth images from zed_wrapper (zed_rgbd_mode:=true) or using stereo_image_proc to generate depth image from left/right images of zed_wrapper (zed_rgbd_mode:=false). You will need latest version of rtabmap.launch and this is tested with this version of zed_wrapper (we should apply an optical rotation for the stereo mode).

<launch>
   
   <arg name="zed_rgbd_mode"        default="false"/>
   <arg name="subscribe_scan_cloud" default="true"/>
   <arg name="frame_id"             default="base_link"/>
   <arg name="rtabmap_args"         default="--delete_db_on_start"/>

   <arg name="pi/2" value="1.5707963267948966" />
   <arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
   <node     if="$(arg zed_rgbd_mode)" pkg="tf" type="static_transform_publisher" name="camera_base_link" 
       args="0 0 0 0 0 0 $(arg frame_id) zed_center 100" /> 
   <node unless="$(arg zed_rgbd_mode)" pkg="tf" type="static_transform_publisher" name="camera_base_link" 
       args="$(arg optical_rotate) $(arg frame_id) zed_center 100" /> 

   <group ns="/zed" >

      <include file="$(find zed_wrapper)/launch/zed_camera.launch">
         <arg name="publish_tf" value="false"/>
      </include> 

      <node unless="$(arg zed_rgbd_mode)" pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc">
         <remap from="left/image_raw"    to="left/image_rect_color"/>
         <remap from="left/camera_info"  to="left/camera_info"/>
         <remap from="right/image_raw"   to="right/image_rect_color"/>
         <remap from="right/camera_info" to="right/camera_info"/>
         <param name="disparity_range" value="200"/>
      </node>

      <node pkg="nodelet" type="nodelet" name="camera_nodelet_manager"  args="manager"/>
      <node unless="$(arg zed_rgbd_mode)" pkg="nodelet" type="nodelet" name="disparity2depth" 
         args="load rtabmap_ros/disparity_to_depth camera_nodelet_manager"/>
      
      <node     if="$(arg zed_rgbd_mode)" pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync camera_nodelet_manager">
         <remap from="rgb/image"       to="rgb/image_rect_color"/>
         <remap from="depth/image"     to="depth/depth_registered"/>
         <remap from="rgb/camera_info" to="rgb/camera_info"/>
         <param name="approx_sync"     value="false"/>
      </node>
      <node unless="$(arg zed_rgbd_mode)" pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync camera_nodelet_manager">
         <remap from="rgb/image"       to="left/image_rect_color"/>
         <remap from="depth/image"     to="depth"/>
         <remap from="rgb/camera_info" to="left/camera_info"/>
         <param name="approx_sync"     value="false"/>
      </node>
   </group>
              
   <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
      <arg name="frame_id"                value="$(arg frame_id)"/>
      <arg name="subscribe_rgbd"          value="true"/>
      <arg name="subscribe_scan_cloud"    value="$(arg subscribe_scan_cloud)" />
      <arg name="approx_sync"             value="true"/>
      <arg name="rgbd_topic"              value="/zed/rgbd_image" />
      <arg name="scan_cloud_topic"        value="/velodyne_points" />
      <arg name="rtabmap_args"            value="$(arg rtabmap_args)"/>

      <arg name="rtabmapviz"              value="false"/>
      <arg name="rviz"                    value="true"/>
   </include>

</launch>

Another optimization would be to use the zed nodelet inside the camera nodelet manager to avoid some serialization/deserialization.

cheers

---

Originally posted by matlabbe with karma: 6409 on 2017-09-08

This answer was ACCEPTED on the original site

Post score: 1

---

Original comments

Comment by lealem on 2017-09-08:
Works perfect! Thanks for the help!