Multiple D435 Cameras with LIDAR from bag

[kevinshchang]

I'm trying to do multiple camera mapping with 3 D435s in addition to a 2D lidar to get data. We were able to use the scan data when only using one the camera, but once we switch to multiple cameras, the scan data fails to appear when visualizing rtabmap and there are errors printed to the terminal. All the data is from the same bag when we launched our 1 camera and 3 camera launch file for rtabmap, and I can echo the scan topic.




I attached the launch file we've been trying to use. Sorry if this is an easy fix, but we're not familiar with rtabmap. Thanks!

3D435_rtabmap.launch

[matlabbe]

Is the lidar data valid? If you do:

$ rostopic echo /scan

are all values zeros or infs?

EDIT
If you can share the rosbag, it could be easier to debug.

[kevinshchang]

Thanks for the response! I've uploaded a compressed portion of a bag to take a look at.

https://drive.google.com/open?id=1JE_KLBUSq-b8UqaJYimvpRpq5lUzPlgB

The rostopic echo is valid, and I can map the bag with one camera and a LIDAR.

[matlabbe]

Hi,

The problem is a typo in the launch file making the node not subscribing to scan:

...
[ INFO] [1556637243.446678168]: Setup rgbd3 callback
[ INFO] [1556637243.460488756]: 
/rtabmap/rtabmap subscribed to (approx sync):
   /odometry/filtered,
   /left_front/rgbd_image,
   /right_front/rgbd_image,
   /top_front/rgbd_image
[ INFO] [1556637243.469096558]: rtabmap 0.19.2 started...
...

Also the frame_id is not correct, it should be the base frame of the robot. Change all

<param name="suscribe_scan"    type="bool"   value="true"/>
<param name="frame_id"            type="string" value="front_link"/>

by

<param name="subscribe_scan"    type="bool"   value="true"/>
<param name="frame_id"            type="string" value="base_link"/>

So you get on launch:

...
[ INFO] [1556638018.308607568]: Setup rgbd3 callback
[ INFO] [1556638018.332895398]: 
/rtabmap/rtabmap subscribed to (approx sync):
   /odometry/filtered,
   /left_front/rgbd_image,
   /right_front/rgbd_image,
   /top_front/rgbd_image,
   /scan
[ INFO] [1556638018.346058442]: rtabmap 0.19.2 started...
...

Other notes:
1) "rtabmap_args" argument is not used anywhere, you may add it to args="$(arg rtabmap_args)" of rtabmap node.
2) do you use robot_localization package? set "two_d_mode" because the input odometry poses are not 2D, causing occupancy grids not correctly generated.
3) you may have to update the TF between cameras / lidar and the base as the walls seen by the left camera doesn't match the lidar scan. Also, you may offset in Z the whole robot so that the ground in the clouds match the xy plane. We can do it by adding a TF frame base_footprint -> base_link, modify odometry node's base frame id to base_footprint instead of base_link then on rtabmap side:

<param name="frame_id" type="string" value="base_footprint"/>

with

<node pkg="tf" type="static_transform_publisher" name="footprint_to_base" args="0 0 0.4 0 0 0 base_footprint base_link 100"/>

cheers,
Mathieu

[kevinshchang]

Thanks so much! I can't believe we didn't catch the typo earlier. And we do plan on republishing the TFs once we get those found correctly. I'll look into the other notes as well!