RTAB-Map with only scan and IMU odometry

[esutton]

I'm attempting to implement RTAB-Map on my RC car, and I could really use some help. It has a lidar and IMU, no camera. I attempted to use the launch file in this answer: https://answers.ros.org/question/321879/is-it-possible-to-use-rtabmap_ros-with-only-scan-data/, but it looks like it's not reading the scan data from my bag file. I've tried two checks:
1) rostopic echo /tf_static returns the transforms for /base_link to /base_imu_link, /base_link to /laser, and base_link to /base_footprint.
2) rostopic echo /scan shows the scan data as expected.

I built rtabmap from source with libpointmatcher. Is there something obvious that I'm doing wrong? Thanks for your help.


SUMMARY
========

PARAMETERS
 * /rosdistro: melodic
 * /rosversion: 1.14.5
 * /rtabmap/rtabmap/RGBD/NeighborLinkRefining: True
 * /rtabmap/rtabmap/Reg/Force3DoF: True
 * /rtabmap/rtabmap/Reg/Strategy: 1
 * /rtabmap/rtabmap/frame_id: base_link
 * /rtabmap/rtabmap/odom_frame_id: odom
 * /rtabmap/rtabmap/odom_tf_angular_variance: 0.05
 * /rtabmap/rtabmap/odom_tf_linear_variance: 0.01
 * /rtabmap/rtabmap/subscribe_depth: False
 * /rtabmap/rtabmap/subscribe_rgb: False
 * /rtabmap/rtabmap/subscribe_scan: True
 * /rtabmap/rtabmap/wait_for_transform_duration: 1
 * /use_sim_time: True

NODES
  /rtabmap/
    rtabmap (rtabmap_ros/rtabmap)
    rtabmapviz (rtabmap_ros/rtabmapviz)

auto-starting new master
process[master]: started with pid [6448]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 25cee56c-7aa3-11ea-8ff7-dcf505efc641
process[rosout-1]: started with pid [6459]
started core service [/rosout]
process[rtabmap/rtabmap-2]: started with pid [6466]
process[rtabmap/rtabmapviz-3]: started with pid [6467]
[ INFO] [1586465172.975675497]: Starting node...
[ INFO] [1586465172.997145357]: Initializing nodelet with 8 worker threads.
[ INFO] [1586465173.076381581]: Starting node...
[ INFO] [1586465173.124373462]: /rtabmap/rtabmap(maps): map_filter_radius          = 0.000000
[ INFO] [1586465173.124411713]: /rtabmap/rtabmap(maps): map_filter_angle           = 30.000000
[ INFO] [1586465173.124427903]: /rtabmap/rtabmap(maps): map_cleanup                = true
[ INFO] [1586465173.124443051]: /rtabmap/rtabmap(maps): map_always_update          = false
[ INFO] [1586465173.124457198]: /rtabmap/rtabmap(maps): map_empty_ray_tracing      = true
[ INFO] [1586465173.124468438]: /rtabmap/rtabmap(maps): cloud_output_voxelized     = true
[ INFO] [1586465173.124480701]: /rtabmap/rtabmap(maps): cloud_subtract_filtering   = false
[ INFO] [1586465173.124494717]: /rtabmap/rtabmap(maps): cloud_subtract_filtering_min_neighbors = 2
[ INFO] [1586465173.127673093]: /rtabmap/rtabmap(maps): octomap_tree_depth         = 16
[ INFO] [1586465173.143784067]: rtabmap: frame_id      = base_link
[ INFO] [1586465173.143825154]: rtabmap: odom_frame_id = odom
[ INFO] [1586465173.143838278]: rtabmap: map_frame_id  = map
[ INFO] [1586465173.143850351]: rtabmap: use_action_for_goal  = false
[ INFO] [1586465173.143868775]: rtabmap: tf_delay      = 0.050000
[ INFO] [1586465173.143879485]: rtabmap: tf_tolerance  = 0.100000
[ INFO] [1586465173.143889303]: rtabmap: odom_sensor_sync   = false
[ INFO] [1586465173.206838485]: rtabmapviz: Using configuration from "/home/offset/.ros/rtabmapGUI.ini"
[ INFO] [1586465173.324961001]: Setting RTAB-Map parameter "RGBD/NeighborLinkRefining"="true"
[ INFO] [1586465173.336221884]: Setting RTAB-Map parameter "Reg/Force3DoF"="true"
[ INFO] [1586465173.337755974]: Setting RTAB-Map parameter "Reg/Strategy"="1"
[ WARN] [1586465173.506830430]: Setting "Grid/FromDepth" parameter to false (default true) as "subscribe_scan" or "subscribe_scan_cloud" is true. The occupancy grid map will be constructed from laser scans. To get occupancy grid map from cloud projection, set "Grid/FromDepth" to true. To suppress this warning, add
[ INFO] [1586465173.507042695]: Setting "Grid/RangeMax" parameter to 0 (default 5.000000) as "subscribe_scan" or "subscribe_scan_cloud" is true.
[ WARN] [1586465173.507201540]: Setting "RGBD/ProximityPathMaxNeighbors" parameter to 10 (default 0) as "subscribe_scan" is true and "Reg/Strategy" uses ICP. Proximity detection by space will be also done by merging close scans. To disable, set "RGBD/ProximityPathMaxNeighbors" to 0. To suppress this warning, add
[ INFO] [1586465173.508605538]: RTAB-Map detection rate = 1.000000 Hz
[ INFO] [1586465173.508919502]: rtabmap: Deleted database "/home/offset/.ros/rtabmap.db" (--delete_db_on_start or -d are set).
[ INFO] [1586465173.508994722]: rtabmap: Using database from "/home/offset/.ros/rtabmap.db" (0 MB).
[ INFO] [1586465173.572830260]: rtabmap: Database version = "0.19.6".
[ INFO] [1586465173.604950941]: /rtabmap/rtabmap: subscribe_depth = false
[ INFO] [1586465173.605018597]: /rtabmap/rtabmap: subscribe_rgb = false
[ INFO] [1586465173.605047441]: /rtabmap/rtabmap: subscribe_stereo = false
[ INFO] [1586465173.605088317]: /rtabmap/rtabmap: subscribe_rgbd = false (rgbd_cameras=1)
[ INFO] [1586465173.605122250]: /rtabmap/rtabmap: subscribe_odom_info = false
[ INFO] [1586465173.605155031]: /rtabmap/rtabmap: subscribe_user_data = false
[ INFO] [1586465173.605189615]: /rtabmap/rtabmap: subscribe_scan = true
[ INFO] [1586465173.605220001]: /rtabmap/rtabmap: subscribe_scan_cloud = false
[ INFO] [1586465173.605257641]: /rtabmap/rtabmap: queue_size    = 10
[ INFO] [1586465173.605295241]: /rtabmap/rtabmap: approx_sync   = false
[ INFO] [1586465173.605400807]: Setup scan callback
[ INFO] [1586465173.609202531]:
/rtabmap/rtabmap subscribed to:
   /scan
[ WARN] [1586465173.609305222]: There is no image subscription, bag-of-words loop closure detection will be disabled...
[ WARN] [1586465173.609355776]: Setting Kp/MaxFeatures=-1 (bag-of-words disabled)
[ INFO] [1586465173.692464886]: rtabmap 0.19.6 started...
libpng warning: iCCP: known incorrect sRGB profile
libpng warning: iCCP: known incorrect sRGB profile
libpng warning: iCCP: known incorrect sRGB profile
[ INFO] [1586465174.248309976]: Reading parameters from the ROS server...
[ INFO] [1586465174.480078831]: Parameters read = 318
[ INFO] [1586465174.480138872]: Parameters successfully read.
[ INFO] [1586465174.646604689]: /rtabmap/rtabmapviz: subscribe_depth = false
[ INFO] [1586465174.646635827]: /rtabmap/rtabmapviz: subscribe_rgb = false
[ INFO] [1586465174.646646266]: /rtabmap/rtabmapviz: subscribe_stereo = false
[ INFO] [1586465174.646655794]: /rtabmap/rtabmapviz: subscribe_rgbd = false (rgbd_cameras=1)
[ INFO] [1586465174.646665091]: /rtabmap/rtabmapviz: subscribe_odom_info = false
[ INFO] [1586465174.646675380]: /rtabmap/rtabmapviz: subscribe_user_data = false
[ INFO] [1586465174.646688024]: /rtabmap/rtabmapviz: subscribe_scan = false
[ INFO] [1586465174.646703112]: /rtabmap/rtabmapviz: subscribe_scan_cloud = false
[ INFO] [1586465174.646718420]: /rtabmap/rtabmapviz: queue_size    = 10
[ INFO] [1586465174.646736293]: /rtabmap/rtabmapviz: approx_sync   = true
[ INFO] [1586465174.646780375]: Setup scan callback
[ INFO] [1586465174.649749202]:
/rtabmap/rtabmapviz subscribed to:
   /rtabmap/odom
[ INFO] [1586465174.649894542]: rtabmapviz started.
[ WARN] [1586465178.936559694, 1584814422.207412331]: Could not get transform from base_link to laser after 1.000000 seconds (for stamp=1584814421.254500)! Error="canTransform: source_frame laser does not exist.. canTransform returned after 1.006 timeout was 1.".
[ERROR] [1586465178.936667064, 1584814422.207412331]: Could not convert laser scan msg! Aborting rtabmap update...
[ WARN] [1586465179.942588634, 1584814423.213351253]: Could not get transform from base_link to laser after 1.000000 seconds (for stamp=1584814422.261556)! Error="canTransform: source_frame laser does not exist.. canTransform returned after 1.00594 timeout was 1.".

[matlabbe]

Hi,

The error tells us that it cannot find the TF between base_link frame and laser frame. It is strange because you said that tf_static does contain /base_link to /laser. Which node is publishing /tf_static? Can you show the tf tree?

$ rosrun tf view_frames

cheers,
Mathieu

[esutton]

$ rostopic echo /tf_static
WARNING: no messages received and simulated time is active.
Is /clock being published?
transforms:
  -
    header:
      seq: 0
      stamp:
        secs: 1584814372
        nsecs: 227364584
      frame_id: "/base_link"
    child_frame_id: "/base_imu_link"
    transform:
      translation:
        x: 0.245
        y: 0.0
        z: 0.117
      rotation:
        x: 0.707106781187
        y: 0.707106781187
        z: 0.0
        w: 0.0
---
transforms:
  -
    header:
      seq: 0
      stamp:
        secs: 1584814372
        nsecs: 247168926
      frame_id: "/base_link"
    child_frame_id: "/laser"
    transform:
      translation:
        x: 0.285
        y: 0.0
        z: 0.127
      rotation:
        x: 0.0
        y: 0.0
        z: 0.0
        w: 1.0
---
transforms:
  -
    header:
      seq: 0
      stamp:
        secs: 1584814372
        nsecs: 297804879
      frame_id: "/base_link"
    child_frame_id: "/base_footprint"
    transform:
      translation:
        x: 0.0
        y: 0.0
        z: 0.0
      rotation:
        x: 0.0
        y: 0.0
        z: 0.0
        w: 1.0
---

[matlabbe]

Hi,

There is an issue with tf_static and rosbags. The tf_statiic topic is not latched, and published only one time at the beginning of the bag. If other nodes needing that transform are not started before the bag is played, they won't be able to use frames in it.

Workarounds:
1) Start all other nodes before the bag. This may not work for some nodes.
2) Republish tf_static into a latched topic. To do that, we can create a script like this, called republish_tf_static.py:

#!/usr/bin/env python
import rospy
from tf2_msgs.msg import TFMessage
        
msg=TFMessage()
        
def callback(data):
    global msg
    if len(msg.transforms) == 0:
        msg = data
    else:
        msg.transforms = msg.transforms+ data.transforms
    rospy.loginfo("Received /tf_static_old and republishing latched /tf_static")
    pub.publish(msg)

if __name__ == '__main__':
    rospy.init_node('listener', anonymous=True)
    rospy.Subscriber("tf_static_old", TFMessage, callback)
    pub = rospy.Publisher('tf_static', TFMessage, queue_size=10, latch=True)
    rospy.spin()

Usage:

$ roscore
$ rosparam set use_sim_time true
$ python republish_tf_static.py
$ rosbag play --clock recorded.bag tf_static:=tf_static_old
...
every nodes launched after this point will see correctly the new tf_static latched.

cheers,
Mathieu