ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Is it possible to use 2d Lidar for Occupancy Map Building while using Stereo Odometry in RTAB-Map?

asked 2023-03-21 12:32:59 -0600

Devin1126 gravatar image

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"/> ...
(more)
edit retag flag offensive close merge delete

Comments

1

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.

matlabbe gravatar image matlabbe  ( 2023-03-24 14:34:51 -0600 )edit

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?

Devin1126 gravatar image Devin1126  ( 2023-03-25 12:39:24 -0600 )edit
1

add approx_sync to true, it is unlikely that all those topics have exactly the same timestamp.

matlabbe gravatar image matlabbe  ( 2023-03-25 16:58:45 -0600 )edit

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 a tf 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)

Devin1126 gravatar image Devin1126  ( 2023-03-25 18:42:33 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
2

answered 2023-03-27 13:03:27 -0600

matlabbe gravatar image

(from comments above)

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.

Add also approx_sync to true, it is unlikely that all those topics have exactly the same timestamp.

For the Lookup would require extrapolation 0.064568610s into the future, you may use rqt_console to know from which node it is logged.

edit flag offensive delete link more

Question Tools

2 followers

Stats

Asked: 2023-03-21 12:32:59 -0600

Seen: 552 times

Last updated: Mar 27 '23