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
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.
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!