Occupancy grid from RTAB-MAP /rtabmap/grid_map topic

[uhroosh]

Hey all,

I'm trying to use the occupancy grid outputted by RTAB-MAP. My robot is running on a Raspberry Pi 4 (using ROS1) and has a Realsense L515 camera (depth + color data) and a T265 camera (pose/odometry data). Is there an easy-to-use .launch file that I could use to get started so I can read from the /rtabmap/grid_map topic using a python script? I'm finding it difficult to make a .launch file that satisfies my needs.

Thanks!

[uhroosh]

Currently using this launch file:

<launch>
 
  <arg name="device_type_camera1" default="t265"/>
  <arg name="device_type_camera2" default="l515"/>

 
  <arg name="serial_no_camera1" default="905312110768"/> 
  <arg name="serial_no_camera2" default="f1230127"/>     

 
  <arg name="camera1" default="t265"/>
  <arg name="camera2" default="l515"/>

 
  <arg name="tf_prefix_camera1" default="$(arg camera1)"/>
  <arg name="tf_prefix_camera2" default="$(arg camera2)"/>

 
  <arg name="initial_reset"      default="false"/>
  <arg name="enable_fisheye"     default="false"/>
  <arg name="color_width"        default="640"/>
  <arg name="color_height"       default="480"/>
  <arg name="color_fps"          default="30"/>
  <arg name="depth_width"        default="640"/>
  <arg name="depth_height"       default="480"/>
  <arg name="depth_fps"          default="30"/>
  <arg name="clip_distance"      default="-2"/>
  <arg name="topic_odom_in"      default="odom_in"/>
  <arg name="calib_odom_file"    default=""/>

 
  <group ns="$(arg camera1)">
    <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
      <arg name="device_type"     value="$(arg device_type_camera1)"/>
      <arg name="serial_no"       value="$(arg serial_no_camera1)"/>
      <arg name="tf_prefix"       value="$(arg tf_prefix_camera1)"/>
      <arg name="initial_reset"   value="$(arg initial_reset)"/>
      <arg name="enable_fisheye1" value="$(arg enable_fisheye)"/>
      <arg name="enable_fisheye2" value="$(arg enable_fisheye)"/>
      <arg name="topic_odom_in"   value="$(arg topic_odom_in)"/>
      <arg name="calib_odom_file" value="$(arg calib_odom_file)"/>
      <arg name="enable_pose"     value="true"/>
    </include>
  </group>

 
  <group ns="$(arg camera2)">
    <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
      <arg name="device_type"   value="$(arg device_type_camera2)"/>
      <arg name="serial_no"     value="$(arg serial_no_camera2)"/>
      <arg name="tf_prefix"     value="$(arg tf_prefix_camera2)"/>
      <arg name="initial_reset" value="$(arg initial_reset)"/>

     
      <arg name="align_depth"   value="true"/>
      <arg name="filters"       value="pointcloud"/>

      <arg name="color_width"   value="$(arg color_width)"/>
      <arg name="color_height"  value="$(arg color_height)"/>
      <arg name="depth_width"   value="$(arg depth_width)"/>
      <arg name="depth_height"  value="$(arg depth_height)"/>
      <arg name="clip_distance" value="$(arg clip_distance)"/>
    </include>
  </group>

 
  <node pkg="tf" type="static_transform_publisher" name="t265_to_l515"
        args="0 0 0 0 0 0 /$(arg tf_prefix_camera1)_link /$(arg tf_prefix_camera2)_link 100"/>
</launch>

And getting these topics from "rostopic list"

/diagnostics
/l515/align_to_color/parameter_descriptions
/l515/align_to_color/parameter_updates
/l515/aligned_depth_to_color/camera_info
/l515/aligned_depth_to_color/image_raw
/l515/aligned_depth_to_color/image_raw/compressed
/l515/aligned_depth_to_color/image_raw/compressed/parameter_descriptions
/l515/aligned_depth_to_color/image_raw/compressed/parameter_updates
/l515/aligned_depth_to_color/image_raw/compressedDepth
/l515/aligned_depth_to_color/image_raw/compressedDepth/parameter_descriptions
/l515/aligned_depth_to_color/image_raw/compressedDepth/parameter_updates
/l515/aligned_depth_to_color/image_raw/theora
/l515/aligned_depth_to_color/image_raw/theora/parameter_descriptions
/l515/aligned_depth_to_color/image_raw/theora/parameter_updates
/l515/color/camera_info
/l515/color/image_raw
/l515/color/image_raw/compressed
/l515/color/image_raw/compressed/parameter_descriptions
/l515/color/image_raw/compressed/parameter_updates
/l515/color/image_raw/compressedDepth
/l515/color/image_raw/compressedDepth/parameter_descriptions
/l515/color/image_raw/compressedDepth/parameter_updates
/l515/color/image_raw/theora
/l515/color/image_raw/theora/parameter_descriptions
/l515/color/image_raw/theora/parameter_updates
/l515/color/metadata
/l515/confidence/camera_info
/l515/confidence/image_rect_raw
/l515/confidence/image_rect_raw/compressed
/l515/confidence/image_rect_raw/compressed/parameter_descriptions
/l515/confidence/image_rect_raw/compressed/parameter_updates
/l515/confidence/image_rect_raw/compressedDepth
/l515/confidence/image_rect_raw/compressedDepth/parameter_descriptions
/l515/confidence/image_rect_raw/compressedDepth/parameter_updates
/l515/confidence/image_rect_raw/theora
/l515/confidence/image_rect_raw/theora/parameter_descriptions
/l515/confidence/image_rect_raw/theora/parameter_updates
/l515/confidence/metadata
/l515/depth/camera_info
/l515/depth/color/points
/l515/depth/image_rect_raw
/l515/depth/image_rect_raw/compressed
/l515/depth/image_rect_raw/compressed/parameter_descriptions
/l515/depth/image_rect_raw/compressed/parameter_updates
/l515/depth/image_rect_raw/compressedDepth
/l515/depth/image_rect_raw/compressedDepth/parameter_descriptions
/l515/depth/image_rect_raw/compressedDepth/parameter_updates
/l515/depth/image_rect_raw/theora
/l515/depth/image_rect_raw/theora/parameter_descriptions
/l515/depth/image_rect_raw/theora/parameter_updates
/l515/depth/metadata
/l515/extrinsics/depth_to_color
/l515/l500_depth_sensor/parameter_descriptions
/l515/l500_depth_sensor/parameter_updates
/l515/motion_module/parameter_descriptions
/l515/motion_module/parameter_updates
/l515/pointcloud/parameter_descriptions
/l515/pointcloud/parameter_updates
/l515/realsense2_camera_manager/bond
/l515/rgb_camera/parameter_descriptions
/l515/rgb_camera/parameter_updates
/rosout
/rosout_agg
/t265/odom/metadata
/t265/odom/sample
/t265/realsense2_camera_manager/bond
/t265/tracking_module/parameter_descriptions
/t265/tracking_module/parameter_updates
/tf
/tf_static

[uhroosh]

On a separate note, this message is getting spammed in the terminal when I run the above launch file:

 23/01 14:39:40,524 WARNING [546962383168] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11

Not sure what the problem is, but I'm choosing to ignore it for now because I see that some data is being published to the /l515/depth/image_rect_raw topic, even though there are some blocks of zeros.

[matlabbe]

23/01 14:39:40,524 WARNING [546962383168] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11

This looks like a realsense driver related issue.

For an example of T265+L515+rtabmap, maybe you could start from https://github.com/IntelRealSense/realsense-ros/blob/ros1-legacy/realsense2_camera/launch/rs_rtabmap.launch, by replacing D400 topics by the ones from L515.

[uhroosh]

Hi,

Thanks for your answer! I tried using the launch file you sent and changing d400 to l515 in the file. I see that RTABMAP seems to be updating the map, I'm getting a lot of messages like:

[ INFO] [1739239669.363752617]: rtabmap (4): Rate=1.00s, Limit=0.000s, Conversion=0.0024s, RTAB-Map=0.2028s, Maps update=0.0002s pub=0.0000s (local map=1, WM=1)

But, when I try to see what's being published to the /rtabmap/grid_map topic in a separate terminal using rostopic hz /rtabmap/grid_map, it just says "no new messages". If I kill the roslaunch command, I see:

rtabmap: Saving database/long-term memory... (located at /home/pi/.ros/rtabmap.db)
rtabmap: 2D occupancy grid map saved.
rtabmap: Saving database/long-term memory...done! (located at /home/pi/.ros/rtabmap.db, 6 MB)

I tried changing <arg name="rtabmap_args"       value="--delete_db_on_start"/> in the original .launch file to

<arg name="rtabmap_args" value="--delete_db_on_start
                                        --Params:Grid/FromDepth=true
                                        --Params:Grid/RayTracing=true
                                        --Params:Grid/RangeMax=5.0" />

but this didn't change anything.

Two questions: How can I get the launch file to start publishing to /rtabmap/grid_map topic, and also where is the final occupancy grid map saved so that I can see if it's working?

[uhroosh]

Sorry about the above message, I figured out how to get the map - it seems like it has values of 100 and -1. I was wondering whether the map will grow in size if the camera is moved to the boundary of the grid? What is the default size of the grid and can we change that as a parameter?