RTAB-Map not working with subscribe_scan

[b.emery94]

Hi Mathieu,

I'm currently trying to run RTAB-Map by passing in wheel odometry and correcting it with 2d a hokuyo 2D laser scanner within RTAB-Map. However, when I set the parameter subscribe_scan to true, the occupancy grid doesn't get published and the pose does not get properly updated. When I use the exact same settings with subscribe_scan set to false everything works as expected.

This is my launch file:
https://gist.github.com/anonymous/878c52597f6e1209be2e8afc21461660

When subscribe_scan is true, the TF tree is completed, however the transform published by rtab-map between wheel_odom and map is static with no translation and rotation. TF tree is here:



The laser scan appears to be subscribed to by rtab-map so it's not an issue with remapping the topic name:

$ rostopic info /scan_lsl

Type: sensor_msgs/LaserScan

Publishers:
 * /urg_node_horizontal (http://brendan-robot:36177/)

Subscribers:
* /rtabmap/rtabmap (http://brendan-robot:49783/)


I also tried it with your exact launch file (with adjusted frames/topics) from the tutorial page, “Setup RTAB-Map on Your Robot!” under the “Kinect + Odometry + 2D laser” with the same result i.e. works with subscribe_scan off and doesn't work with subscribe_scan on.

Do you have any ideas what the issue could be?

Thanks in advance for the help!
Brendan

[matlabbe]

Hi Brendan,

The launch file looks ok. Can you see rtabmap logging stuff on the terminal (at ~1Hz)? Similar to:

[ INFO] [1473362385.305401432]: rtabmap (1): Rate=1.00s, Limit=0.000s, RTAB-Map=0.2441s, Maps update=0.0046s pub=0.0001s (local map=1, WM=1)
[ INFO] [1473362386.312885154]: rtabmap (2): Rate=1.00s, Limit=0.000s, RTAB-Map=0.2171s, Maps update=0.0046s pub=0.0000s (local map=1, WM=1)
[...]

If no, it is maybe a message synchronization problem, generally because of a timestamp not set in the header of a subscribed topic.

If yes, the occupancy grid created from the laser scan is called /rtabmap/grid_map. Are you using the rtabmap binaries (0.11.8) or latest from source (0.11.10)?. What do you mean with "pose does not get properly updated"? You have "RGBD/NeighborLinkRefining" enabled, so laser scans are used to refine the odometry message. Make sure the TF /base_footprint -> /laser is accurate and that odometry refers to /base_footprint frame. Setting "RGBD/NeighborLinkRefining" to false would disable odometry refining, so it would be similar to when "subscribe_scan" is false (to debug if it is a wrong laser TF problem).

With "RGBD/OptimizeFromGraphEnd" = true, the transform /map -> /wheel_odom will be always "Identity" even if there are loop closures or links refined because it is the map that is corrected instead of odometry.

cheers

[b.emery94]

Hi Mathieu,

Thanks again for your help! I managed to solve the problem, so I'll go through what I did for future users that may have the same issue.

I wasn't seeing the rtabmap logging information. So as per your suggestion, I had a look to see if there was a message synchronization problem. The camera/laser scan topics all had the correct headers and timestamps and the odometry/base frames were all set correctly. However, I noticed that the depth image was only being published at ~5Hz. As it turns out, I was having issues with OpenCl and so the kinect drivers were running the depth processing with the cpu rather than OpenCl. After fixing my OpenCl installation and reinstalling libfreenect2 and iai_kinect2 (kinect drivers), OpenCl was working correctly and the depth image was being published at ~30Hz. After that, Rtab-Map began to work as expected when setting /subsribe_scans to true.

I'm not 100% sure if fixing OpenCl fixed the issue or there was something else that was fixed by the reinstallation, but it's working now. Thanks Mathieu!