ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | Q&A answers.ros.org
Ask Your Question
0

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

asked 2017-09-07 03:31:21 -0600

lealem gravatar image

updated 2017-09-08 10:45:13 -0600

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/y9ocshgq8ye...

RTABMAP start log

https://www.dropbox.com/s/wdwivai4snq...

ros graph

https://www.dropbox.com/s/zr0wa3n0gtz...

Thank you in advance!

Lealem

edit retag flag offensive close merge delete

Comments

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.

matlabbe gravatar image matlabbe  ( 2017-09-07 09:21:13 -0600 )edit

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!

lealem gravatar image lealem  ( 2017-09-08 10:51:50 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2017-09-08 12:35:21 -0600

matlabbe gravatar image

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

edit flag offensive delete link more

Comments

Works perfect! Thanks for the help!

lealem gravatar image lealem  ( 2017-09-08 13:11:51 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2017-09-07 03:31:21 -0600

Seen: 757 times

Last updated: Sep 08 '17