Is it possible to use 2d Lidar for Occupancy Map Building while using Stereo Odometry in RTAB-Map?
Hello,
I have a differential-drive robot that is equipped with a Realsense D435 RGBD camera and 2D Lidar scanner. I am using RTAB-Map to perform SLAM and I wanted to know if there is a way to incorporate Lidar readings from the /scan
topic into the RTAB-Map SLAM node rtabmap
. I have chosen to use the stereo mode of the camera to receive stereo images that will be input to the stereo_odometry
and rtabmap
nodes. When I run the launch files that I use, the map cloud and odometry work fine along with the /scan
topic producing messages. However, when I try to view the produced occupancy grid from the Lidar, I get the follow error message continously:
[ WARN] [1679412963.381777182]: Grid map is empty! (local maps=1)
Through further investigation, I realized that the rtabmap
SLAM node is not subscribed to the scan topic even though I set subscribe_scan=true
. The launch file that receives stereo images and launches the standard rtabmap launch file rtabmap.launch
will be placed below:
<launch>
<arg name="rviz" default="true" />
<include file="$(find rtabmap_ros)/launch/rtabmap.launch">
<arg name="rtabmap_args" value="--delete_db_on_start"/>
<arg name="frame_id" value="base_link"/>
<arg name="stereo" value="true"/>
<arg name="subscribe_rgbd" value="false"/>
<arg name="subscribe_scan" value="true"/>
<!-- stereo related topics -->
<arg name="left_image_topic" default="/camera/infra1/image_rect_raw" />
<arg name="right_image_topic" default="/camera/infra2/image_rect_raw" /> <!-- using grayscale image for efficiency -->
<arg name="left_camera_info_topic" default="/camera/infra1/camera_info" />
<arg name="right_camera_info_topic" default="/camera/infra2/camera_info" />
<arg name="rgbd_topic" value="/rgbd_image"/>
<arg name="compressed" value="true"/>
<arg name="rviz" value="$(arg rviz)"/>
<arg if="$(arg rviz)" name="rtabmapviz" value="false"/>
</include>
<!--Launch static transform between base_link and camera_link-->
<node name="base_to_camera_static_publisher" pkg="tf2_ros" type="static_transform_publisher" args="0.2531 0 0.0859 0 0 0 base_link camera_link"/></launch>
Please note that this launch file is being used on a remote computer and the robot is using a separate launch file that initializes the stereo_sync
node along with the driver for the 2d Lidar. This launch file will also be displayed below:
<launch>
<!-- Launch Stereo Camera Node-->
<include file="$(find realsense2_camera)/launch/rs_camera.launch">
<arg name="enable_infra1" value="true" />
<arg name="enable_infra2" value="true" />
</include>
<!-- Launch LD14 Lidar Sensor Node-->
<include file="$(find ldlidar_sl_ros)/launch/ld14.launch"/>
<!-- Launch RTAB-Map Stereo Sync Nodelet-->
<arg name="rate" default="5"/>
<arg name="approx_sync" default="false" />
<arg name="stereo_sync" default="true"/>
<node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager"/>
<node if="$(arg stereo_sync)" pkg="nodelet" type="nodelet" name="stereo_sync" args="load rtabmap_ros/stereo_sync nodelet_manager" output="screen">
<param name="compressed_rate" type="double" value="$(arg rate)"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
<remap from="left/image_rect" to="/camera/infra1/image_rect_raw"/>
<remap from="right/image_rect" to="/camera/infra2/image_rect_raw"/>
<remap from="left/camera_info" to="/camera/infra1/camera_info"/> ...
To use stereo with lidar, you should use
subscribe_rgbd=true
, otherwise the node won't subscribe to scan. When the node is subscribed to scan, by default it should use the scan for occupancy grid.I changed the parameter
subscribe_rgbd=true
. I can confirm that it now shows that the RTAB-Map SLAM node is now subscribed to scan messages, however after doing that, I receive another error that states the following continuously:**[ WARN] [1679763995.026228053]: /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"). Parameter "approx_sync" is false, which means that input topics should have all the exact timestamp for the callback to be called. /rtabmap/rtabmap subscribed to (exact sync):
/rtabmap/odom \
/rgbd_image_relay \
/scan**
I checked if each of these topics above were publishing messages and they all were. Do you have any idea as to why this may be happening?
add
approx_sync
to true, it is unlikely that all those topics have exactly the same timestamp.Thank you for your help so far. Using
approx_sync=true
fixed that error and I now see a stereo pointcloud and an occupancy grid being produced by the Lidar in Rviz. There is only one remaining error that persists even though this may be minor. While rtabmap is running, I am continuously receiving some form of atf
error that states the following:[ WARN] [1679786410.105168036]: Could not get transform from odom to base_link after 0.200000 seconds (for stamp=1679786409.920569)! Error="Lookup would require extrapolation 0.064568610s into the future. Requested time 1679786409.920568705 but the latest data is at time 1679786409.856000185, when looking up transform from frame [base_link] to frame [odom]. canTransform returned after 0.200711 timeout was 0.2.".
If you could provide some type of solution for this as well, I would really appreciate it. Also, please add your previous comment as an answer ...(more)