rtabmap not subscribing to scan

[profmartin]

Hello,

I am using a point grey xb3 stereo camera for rtabmap and I want it to use my laser scanner as well. Here are my relevant launch file excerpts:

<arg name="use_scan" default="true" />

<param name="subscribe_stereo" type="bool" value="true" />
<param name="subscribe_depth" type="bool" value="false" />
<param if="$(arg use_scan)" name="subscribe_scan" value="true"/>
<param unless="$(arg use_scan)" name="subscribe_scan" value="false"/>

<remap from="scan" to="/scan" />
...
(Several camera topic remap calls are made)
...
      <param name="Rtabmap/TimeThr" type="string" value="700" />
      <param name="Rtabmap/DetectionRate" type="string" value="1" />

      <param name="Vis/MinInliers" type="string" value="10" />
      <param name="Vis/InlierDistance" type="string" value="0.1" />
      <param name="Kp/DetectorStrategy" type="string" value="2" />

      <param name="Optimizer/Robust" type="bool" value="true" />
      <param name="RGBD/OptimizeMaxError" type="string" value="0" />
      <param name="Optimizer/Strategy" type="string" value="1" />

      <param name="StereoBM/MinDisparities" type="string" value="0" />
      <param name="StereoBM/NumDisparities" type="string" value="128" />
      <param name="StereoBM/SpeckleWindowSize" type="string" value="500" />

      <param name="Grid/3D" value="true" />
      <param name="Grid/FromDepth" value="true" />
      <param if="$(arg use_scan)" name="Grid/FromDepth" value="false" />
      <param name="Grid/MaxObstacleHeight" value="2.0"/> <!-- Set so that we filter out overpasses / ceiling -->
      <param name="Grid/MaxGroundHeight" value="0"/>
      <param name="Grid/MaxGroundAngle" value="45"/>
      <param name="Grid/FootprintHeight" value = "1.1" />
      <param name="Grid/FootprintLength" value = "0.99" />
      <param name="Grid/FootprintWidth" value = "0.67" />
      <param name="Grid/DepthMin" value="0.0" />
      <param name="Grid/DepthMax" value="8.0" />

      <!-- Output mapping for move_base integration -->
      <remap from="grid_map" to="/map" />

When I use this launch file, the stereo loads and rtabmap is using it correctly. However, it does not subscribe to my /scan topic. (command line output attached)


Do I have some parameters interfering to keep rtabmap from subscribing to /scan? I based my launch file on the rtabmap Turtlebot tutorials online.

Thanks for your help!

[matlabbe]

Hi,

stereo + scan interface is not available for synchronization issues. Stereo topics should be exactly synchronized together before synchronizing [approximately] with laser scan (note that this also applies to RGB-D cameras). To do so, convert stereo to RGB-D format with stereo_img_proc, use rgbd_sync to exactly synchronize RGB-D topics together, then use rgbd_image + scan subscriptions for rtabmap node. Similar to this example:

  <group ns="stereo">
    <node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc"/>

    <!-- Disparity to depth -->
    <node pkg="nodelet" type="nodelet" name="disparity2depth" args="standalone rtabmap_ros/disparity_to_depth"/>

    <!-- Synchronize RGB-D topics -->
    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="screen">
      <remap from="rgb/image"       to="left/image_rect"/>
      <remap from="depth/image"     to="depth"/>
      <remap from="rgb/camera_info" to="left/camera_info"/>
      <remap from="rgbd_image"      to="rgbd_image"/> <!-- output -->
      <param name="approx_sync"     type="bool"   value="false"/>
    </node>
  </group>

  <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
      <param name="subscribe_rgbd" type="bool" value="true"/>
      <param name="subscribe_scan" type="bool" value="true"/>
      <param name="approx_sync" type="bool" value="true"/>

      <remap from="rgbd_image"       to="/stereo/rgbd_image"/>
      <remap from="scan"             to="/scan"/>
      ...

cheers,
Mathieu

[zlacelle]

Hello,

Is there any way to add 3D pointclouds to the RTABMap mapping, while using stereo, without having to do the dense stereo before RTABMap?  One of the really nice things about using RTABMap is that if we feed it stereo, it will do the raw feature extraction and localization using structure from motion, and then only run dense stereo at 1Hz or so.  This keeps CPU usage very significantly lower than running full dense stereo at full frame rate.

Is it possible to configure the pipeline as so (assuming I understand the pipeline fully...):

stereo camera -> RTABMap (structure from motion / feature detection at high rate) -> stereoBM (1Hz-ish) (ADD IN 3D POINTCLOUD HERE) -> mapping algorithms (ground plane, etc)

We can do approx sync of 3D pointcloud here, since it's coming in at high frequency (20Hz or so) and is timestamped.  Should be relatively close in time to the dense stereo cloud.

[matlabbe]

Hi,

This is something I had in mind after I added RGBDImage msg. I updated the code (in devel branch for now) so that RGBDImage can be also used to synchronize stereo topics like this:

    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync">
      <remap from="rgb/image"         to="/stereo_camera/left/image_rect_color"/>
      <remap from="depth/image"       to="/stereo_camera/right/image_rect"/>
      <remap from="rgb/camera_info"   to="/stereo_camera/left/camera_info"/>
      <remap from="depth/camera_info" to="/stereo_camera/right/camera_info"/>
      <remap from="rgbd_image"      to="/rgbd_image"/>
      <param name="approx_sync"     type="bool"   value="false"/>
    </node>
  
    <!-- Stereo Odometry -->
      <node pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen">
        <param name="frame_id"        type="string" value="base_link"/>
        <param name="subscribe_rgbd"  type="bool"   value="true"/>
      </node>
  
    <!-- RTAB-Map -->
    <node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" args:="-d" output="screen">
      <param name="frame_id"        type="string" value="base_link"/>
      <param name="subscribe_rgbd"  type="bool"   value="true"/>
      <param name="subscribe_scan"  type="bool"   value="true"/>
      <param name="approx_sync"     type="bool"   value="true"/>
      <remap from="rgbd_image"      to="/rgbd_image"/>
      <remap from="scan"                to="/scan"/>
    </node>
  
    <!-- Visualisation -->
    <node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" output="screen">
      <param name="frame_id" type="string" value="$(arg frame_id)"/>
      <param name="subscribe_rgbd"  type="bool"   value="true"/>
      <param name="subscribe_scan"  type="bool"   value="true"/>
      <param name="approx_sync"     type="bool"   value="true"/>
      <remap from="rgbd_image"      to="/rgbd_image"/>
      <remap from="scan"                to="/scan"/>
    </node>

In this example, the stereo topics are exactly synchronized before sending the resulting topic (rgbd_image) to rtabmap node where it is approximately synchronized with a laser scan.

I didn't change the name of RGBDImage msg yet, for compatibility and usage reason.

cheers,
Mathieu