Did not receive message for 5 seconds, but message are published.

[cjsurjadi]

I'm trying to use Rtabmap with Ouster OS-1 and Realsense D435 with ROS2 Humble. I can run rtabmap with ICP odometry with only the 3D point cloud from Ouster successfully, and I can also run rtabmap with rgbd odometry with only the Realsense successfully. I've attempted combining the lidar with the rgb and depth topic, and syncing them into an rgbd image first then combining it. But in both cases, when I run rtabmap to subscribe to all the topics (3D lidar, rgb and depth image), I receive the error:

[WARN] [1700552704.314934175] [rtabmap]: rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=10). 
rtabmap subscribed to (approx sync):
   /odom \
   /camera/color/image_raw \
   /camera/aligned_depth_to_color/image_raw \
   /camera/color/camera_info \
   /ouster/points \
   /odom_info

My launch file is as the following. I use the same launch file while only changing the odom node and the subscribe_rgb/depth/scan_cloud parameter during testing for only lidar, only rgbd, or both. The standalone testing for lidar and rgbd works.

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
from launch.substitutions import LaunchConfiguration
from launch.conditions import IfCondition, UnlessCondition
from launch_ros.actions import Node

def generate_launch_description():

    use_sim_time = LaunchConfiguration('use_sim_time')
    qos = LaunchConfiguration('qos')
    localization = LaunchConfiguration('localization')
    icp_odom = 'false'
    vis_odom = 'true'
    rgbd_sync = 'false'
    
    parameters={\
          'frame_id':'base_link',
          'use_sim_time':use_sim_time,
          'subscribe_depth':True,
          'subscribe_rgb':True,
          'subscribe_rgbd':False,
          'subscribe_scan':False,
          'subscribe_scan_cloud':True,
          'subscribe_stereo':False,
          'subscribe_odom_info':True,
          'queue_size':10,
          'rgb_image_topic': '/camera/color/image_raw',
          'depth_image_topic': '/camera/aligned_depth_to_color/image_raw',
          'camera_info_topic': '/camera/color/camera_info',
          'left_image_topic':'/camera/infra1/image_rect_raw',
          'right_image_topic':'/camera/infra2/image_rect_raw',
          'left_camera_info_topic':'/camera/infra1/camera_info',
          'right_camera_info_topic':'/camera/infra2/camera_info',
          'scan_cloud_topic':'/ouster/points',
          'scan_cloud_is_2d':False,
          'imu_topic':'/ouster/imu',
          'approx_sync':True,
          'use_action_for_goal':True,
          'qos_scan':qos,
          'qos_image':qos,
          'qos_imu':qos,
          'qod_odom':qos,
          'Kp/DetectorStrategy':'2',
          'Vis/FeatureType':'2',
          'Kp/MaxFeatures':'500',
          'Kp/MaxDepth':'4.0',
          'Vis/MaxDepth':'4.0',
          'Kp/MinDepth':'0.5',
          'Vis/MinDepth':'0.5',
          'Vis/MinInliers':'15',
          'Reg/Strategy':'1',
          'Reg/Force3DoF':'True',
          'RGBD/NeighborLinkRefining':'True',
          'Grid/RangeMin':'0.2', # ignore laser scan points on the robot itself
          'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D)
    }


    remappings=[
          ('rgb/image', '/camera/color/image_raw'),
          ('depth/image', '/camera/aligned_depth_to_color/image_raw'),
          ('rgb/camera_info', '/camera/color/camera_info'),
          ('left/image_rect', '/camera/infra1/image_rect_raw'),
          ('right/image_rect', '/camera/infra2/image_rect_raw'),
          ('left/camera_info', '/camera/infra1/camera_info'),
          ('right/camera_info', '/camera/infra2/camera_info'),
          ('scan_cloud', '/ouster/points')
          ]
        
    return LaunchDescription([
        
        DeclareLaunchArgument(
            'qos', default_value='2',
            description='QoS used for input sensor topics'),
            
        DeclareLaunchArgument(
            'localization', default_value='false',
            description='Launch in localization mode.'),
            
        Node(
            package='imu_filter_madgwick',
            executable='imu_filter_madgwick_node',
            name='imu_filter',
            output='screen',
            parameters=[{
                'use_mag':False,
                'world_frame':'enu',
                'publish_tf':False,
                'fixed_frame':'os_imu'
            }],
            remappings=[
                ('imu/data_raw', 'ouster/imu'),
                ('imu/data', 'ouster/imu_filtered')
            ]),

        Node(
            condition=IfCondition(icp_odom),
            package='rtabmap_odom', executable='icp_odometry', output='screen',
            parameters=[{
              'frame_id':'base_link',
              'odom_frame_id':'odom',
              'wait_for_transform':0.2,
              'wait_imu_to_init':False,
              'expected_update_rate':15.0,
              'subscribe_scan_cloud':True, 
              'subscribe_scan':False,
              'subscribe_rgbd':False,
              'publish_tf':True,
              'scan_cloud_max_points':32000,
              'scan_range_max':20.0, 
              'scan_range_min':0.2,
              'deskewing':False,
              'use_sim_time':use_sim_time,
              'approx_sync':True,
              'queue_size':10
            }],
            remappings=remappings,
            arguments=[
              'Icp/PointToPlane', 'true',
              'Icp/Iterations', '10',
              'Icp/VoxelSize', '0.1',
              'Icp/Epsilon', '0.001',
              'Icp/PointToPlaneK', '20',
              'Icp/PointToPlaneRadius', '0',
              'Icp/MaxTranslation', '2',
              'Icp/MaxCorrespondenceDistance', '1',
              'Icp/PM', 'true',
              'Icp/PMOutlierRatio', '0.1',
              'Icp/CorrespondenceRatio', '0.01',
              'Icp/ReciprocalCorrespondences', 'false',
              'Odom/ScanKeyFrameThr', '0.8',
              'OdomF2M/ScanSubtractRadius', '0.05',
              'OdomF2M/ScanMaxSize', '10000',
              'OdomF2M/BundleAdjustment', 'false',
            ]),

        Node(
            condition=IfCondition(vis_odom),
            package='rtabmap_odom', executable='rgbd_odometry', output='screen',
            parameters=[{
                'frame_id':'base_link',
                'subscribe_depth': True,
                'subscribe_odom_info':True,
                'approx_sync':False}],
            remappings=remappings),
        
        # SLAM mode:
        Node(
            condition=UnlessCondition(localization),
            package='rtabmap_slam', executable='rtabmap', output='screen',
            parameters=[parameters],
            remappings=remappings,
            arguments=['-d',
                       'Rtabmap/DetectionRate','1.0',
                        'Kp/DetectorStrategy', '2',
                        'Vis/FeatureType','2',
                        'Kp/MaxFeatures','500',
                        'Kp/MaxDepth','4.0',
                        'Vis/MaxDepth','4.0',
                        'Kp/MinDepth','0.5',
                        'Vis/MinDepth','0.5',
                        'Vis/MinInliers','15',
                        'Reg/Strategy','1',
                        'Reg/Force3DoF','True',
                        'RGBD/NeighborLinkRefining','True',
                        'Grid/RangeMin','0.2', # ignore laser scan points on the robot itself
                        'Optimizer/GravitySigma','0']), # Disable, # This will delete the previous database (~/.ros/rtabmap.db)
            
        # Localization mode:
        Node(
            condition=IfCondition(localization),
            package='rtabmap_slam', executable='rtabmap', output='screen',
            parameters=[parameters,
              {'Mem/IncrementalMemory':'False',
               'Mem/InitWMWithAllNodes':'True'}],
            remappings=remappings),

        Node(
            package='rtabmap_viz', executable='rtabmap_viz', output='screen',
            parameters=[parameters],
            remappings=remappings)
    ])

Rtabmap Log:

[INFO] [1700552696.245563741] [rtabmap]: rtabmap(maps): map_filter_radius          = 0.000000
[INFO] [1700552696.245777878] [rtabmap]: rtabmap(maps): map_filter_angle           = 30.000000
[INFO] [1700552696.245796737] [rtabmap]: rtabmap(maps): map_cleanup                = true
[INFO] [1700552696.245810056] [rtabmap]: rtabmap(maps): map_always_update          = false
[INFO] [1700552696.245822661] [rtabmap]: rtabmap(maps): map_empty_ray_tracing      = true
[INFO] [1700552696.245834977] [rtabmap]: rtabmap(maps): cloud_output_voxelized     = true
[INFO] [1700552696.245847065] [rtabmap]: rtabmap(maps): cloud_subtract_filtering   = false
[INFO] [1700552696.245859271] [rtabmap]: rtabmap(maps): cloud_subtract_filtering_min_neighbors = 2
[INFO] [1700552696.245931577] [rtabmap]: rtabmap(maps): octomap_tree_depth         = 16
[INFO] [1700552696.273266132] [rtabmap]: rtabmap: frame_id      = base_link
[INFO] [1700552696.273295856] [rtabmap]: rtabmap: map_frame_id  = map
[INFO] [1700552696.273308479] [rtabmap]: rtabmap: log_to_rosout_level  = 4
[INFO] [1700552696.273319153] [rtabmap]: rtabmap: initial_pose  = 
[INFO] [1700552696.273329045] [rtabmap]: rtabmap: use_action_for_goal  = true
[INFO] [1700552696.273342196] [rtabmap]: rtabmap: tf_delay      = 0.050000
[INFO] [1700552696.273356913] [rtabmap]: rtabmap: tf_tolerance  = 0.100000
[INFO] [1700552696.273368042] [rtabmap]: rtabmap: odom_sensor_sync   = false
[INFO] [1700552696.273377558] [rtabmap]: rtabmap: gen_scan  = false
[INFO] [1700552696.273386721] [rtabmap]: rtabmap: gen_depth  = false
[INFO] [1700552696.273396483] [rtabmap]: rtabmap: scan_cloud_max_points = 0
[INFO] [1700552696.273406297] [rtabmap]: rtabmap: scan_cloud_is_2d = false
[INFO] [1700552696.315768947] [rtabmap]: Setting RTAB-Map parameter "Grid/RangeMin"="0.2"
[INFO] [1700552696.318516713] [rtabmap]: Setting RTAB-Map parameter "Kp/DetectorStrategy"="2"
[INFO] [1700552696.318861014] [rtabmap]: Setting RTAB-Map parameter "Kp/MaxDepth"="4.0"
[INFO] [1700552696.318996413] [rtabmap]: Setting RTAB-Map parameter "Kp/MinDepth"="0.5"
[INFO] [1700552696.322371550] [rtabmap]: Setting RTAB-Map parameter "Optimizer/GravitySigma"="0"
[INFO] [1700552696.325822290] [rtabmap]: Setting RTAB-Map parameter "RGBD/NeighborLinkRefining"="True"
[INFO] [1700552696.328363786] [rtabmap]: Setting RTAB-Map parameter "Reg/Force3DoF"="True"
[INFO] [1700552696.328526880] [rtabmap]: Setting RTAB-Map parameter "Reg/Strategy"="1"
[INFO] [1700552696.335974605] [rtabmap]: Setting RTAB-Map parameter "Vis/FeatureType"="2"
[INFO] [1700552696.336374452] [rtabmap]: Setting RTAB-Map parameter "Vis/MaxDepth"="4.0"
[INFO] [1700552696.336586998] [rtabmap]: Setting RTAB-Map parameter "Vis/MinDepth"="0.5"
[INFO] [1700552696.336683655] [rtabmap]: Setting RTAB-Map parameter "Vis/MinInliers"="15"
[INFO] [1700552696.337962729] [rtabmap]: Update RTAB-Map parameter "Grid/RangeMin"="0.2" from arguments
[INFO] [1700552696.337996901] [rtabmap]: Update RTAB-Map parameter "Kp/DetectorStrategy"="2" from arguments
[INFO] [1700552696.338010756] [rtabmap]: Update RTAB-Map parameter "Kp/MaxDepth"="4.0" from arguments
[INFO] [1700552696.338022065] [rtabmap]: Update RTAB-Map parameter "Kp/MaxFeatures"="500" from arguments
[INFO] [1700552696.338034916] [rtabmap]: Update RTAB-Map parameter "Kp/MinDepth"="0.5" from arguments
[INFO] [1700552696.338045646] [rtabmap]: Update RTAB-Map parameter "Optimizer/GravitySigma"="0" from arguments
[INFO] [1700552696.338056571] [rtabmap]: Update RTAB-Map parameter "RGBD/NeighborLinkRefining"="True" from arguments
[INFO] [1700552696.338068339] [rtabmap]: Update RTAB-Map parameter "Reg/Force3DoF"="True" from arguments
[INFO] [1700552696.338078870] [rtabmap]: Update RTAB-Map parameter "Reg/Strategy"="1" from arguments
[INFO] [1700552696.338089133] [rtabmap]: Update RTAB-Map parameter "Rtabmap/DetectionRate"="1.0" from arguments
[INFO] [1700552696.338099629] [rtabmap]: Update RTAB-Map parameter "Vis/FeatureType"="2" from arguments
[INFO] [1700552696.338110085] [rtabmap]: Update RTAB-Map parameter "Vis/MaxDepth"="4.0" from arguments
[INFO] [1700552696.338119946] [rtabmap]: Update RTAB-Map parameter "Vis/MinDepth"="0.5" from arguments
[INFO] [1700552696.338201596] [rtabmap]: Update RTAB-Map parameter "Vis/MinInliers"="15" from arguments
[WARN] [1700552696.338283944] [rtabmap]: Setting "Grid/Sensor" parameter to 0 (default 1) as "subscribe_scan" or "subscribe_scan_cloud" or "gen_scan" is true. The occupancy grid map will be constructed from laser scans. To get occupancy grid map from cloud projection, set "Grid/Sensor" to true. To suppress this warning, add <param name="Grid/Sensor" type="string" value="0"/>
[INFO] [1700552696.338315467] [rtabmap]: Setting "Grid/RangeMax" parameter to 0 (default 5.000000) as "subscribe_scan" or "subscribe_scan_cloud" or "gen_scan" is true and Grid/Sensor is 0.
[INFO] [1700552696.338333478] [rtabmap]: Setting "Icp/PointToPlaneRadius" parameter to 0 (default 0.000000) as "subscribe_scan_cloud" is true.
[WARN] [1700552696.338347429] [rtabmap]: Setting "RGBD/ProximityPathMaxNeighbors" parameter to 1 (default 0) as "subscribe_scan_cloud" is true and "Reg/Strategy" uses ICP. To disable, set "RGBD/ProximityPathMaxNeighbors" to 0. To suppress this warning, add <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="1"/>
[INFO] [1700552696.338565934] [rtabmap]: RTAB-Map detection rate = 1.000000 Hz
[INFO] [1700552696.338831080] [rtabmap]: rtabmap: Deleted database "/root/.ros/rtabmap.db" (--delete_db_on_start or -d are set).
[INFO] [1700552696.338873441] [rtabmap]: rtabmap: Using database from "/root/.ros/rtabmap.db" (0 MB).
[INFO] [1700552699.280183795] [rtabmap]: rtabmap: Database version = "0.21.1".
[INFO] [1700552699.280257417] [rtabmap]: rtabmap: SLAM mode (Mem/IncrementalMemory=true)
[INFO] [1700552699.305008531] [rtabmap]: Setup callbacks
[INFO] [1700552699.305191169] [rtabmap]: rtabmap: subscribe_depth = true
[INFO] [1700552699.305211576] [rtabmap]: rtabmap: subscribe_rgb = true
[INFO] [1700552699.305223125] [rtabmap]: rtabmap: subscribe_stereo = false
[INFO] [1700552699.305234229] [rtabmap]: rtabmap: subscribe_rgbd = false (rgbd_cameras=1)
[INFO] [1700552699.305246347] [rtabmap]: rtabmap: subscribe_odom_info = true
[INFO] [1700552699.305256493] [rtabmap]: rtabmap: subscribe_user_data = false
[INFO] [1700552699.305266540] [rtabmap]: rtabmap: subscribe_scan = false
[INFO] [1700552699.305276629] [rtabmap]: rtabmap: subscribe_scan_cloud = true
[INFO] [1700552699.305286790] [rtabmap]: rtabmap: subscribe_scan_descriptor = false
[INFO] [1700552699.305297303] [rtabmap]: rtabmap: queue_size      = 10
[INFO] [1700552699.305307958] [rtabmap]: rtabmap: qos_image       = 2
[INFO] [1700552699.305318391] [rtabmap]: rtabmap: qos_camera_info = 2
[INFO] [1700552699.305328644] [rtabmap]: rtabmap: qos_scan        = 2
[INFO] [1700552699.305338685] [rtabmap]: rtabmap: qos_odom        = 0
[INFO] [1700552699.305349331] [rtabmap]: rtabmap: qos_user_data   = 0
[INFO] [1700552699.305359722] [rtabmap]: rtabmap: approx_sync     = true
[INFO] [1700552699.305379741] [rtabmap]: Setup depth callback
[INFO] [1700552699.314794013] [rtabmap]: 
rtabmap subscribed to (approx sync):
   /odom \
   /camera/color/image_raw \
   /camera/aligned_depth_to_color/image_raw \
   /camera/color/camera_info \
   /ouster/points \
   /odom_info
[WARN] [1700552704.314934175] [rtabmap]: rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=10). 
rtabmap subscribed to (approx sync):
   /odom \
   /camera/color/image_raw \
   /camera/aligned_depth_to_color/image_raw \
   /camera/color/camera_info \
   /ouster/points \
   /odom_info

Rostopic hz of the subscribed topics:

ros2 topic hz odom_info
average rate: 27.218
	min: 0.031s max: 0.044s std dev: 0.00328s window: 29
average rate: 27.200
	min: 0.028s max: 0.044s std dev: 0.00362s window: 57

ros2 topic hz odom
average rate: 28.528
	min: 0.031s max: 0.038s std dev: 0.00179s window: 30
average rate: 28.443
	min: 0.031s max: 0.041s std dev: 0.00192s window: 59
average rate: 28.007
	min: 0.031s max: 0.066s std dev: 0.00388s window: 87

ros2 topic hz /ouster/points
average rate: 6.671
	min: 0.099s max: 0.501s std dev: 0.13277s window: 8
average rate: 8.181
	min: 0.099s max: 0.501s std dev: 0.09191s window: 18
average rate: 8.748
	min: 0.099s max: 0.501s std dev: 0.07446s window: 28

ros2 topic hz /camera/color/image_raw
average rate: 28.994
	min: 0.029s max: 0.066s std dev: 0.00604s window: 30
average rate: 29.483
	min: 0.017s max: 0.066s std dev: 0.00529s window: 60
average rate: 29.665
	min: 0.017s max: 0.066s std dev: 0.00433s window: 91

ros2 topic hz /camera/aligned_depth_to_color/image_raw 
average rate: 29.955
	min: 0.032s max: 0.035s std dev: 0.00062s window: 31
average rate: 29.975
	min: 0.032s max: 0.035s std dev: 0.00073s window: 62
average rate: 29.976
	min: 0.032s max: 0.035s std dev: 0.00071s window: 92
	
ros2 topic hz /camera/color/camera_info 
WARNING: topic [/camera/color/camera_info] does not appear to be published yet
average rate: 30.023
	min: 0.032s max: 0.035s std dev: 0.00057s window: 31
average rate: 29.999
	min: 0.027s max: 0.040s std dev: 0.00160s window: 61
average rate: 29.999
	min: 0.027s max: 0.040s std dev: 0.00137s window: 91

The rqt graph of my topics are as the following: The tf frames is as the following:

[cjsurjadi]

Here's a clearer view of the RQT graph:

[cjsurjadi]

Here's a short rosbag I collected:
rosbag

[matlabbe]

The ouster is not using host clock, but its own starting at 0. You will have to configure the ouster to use time from ethernet device (PTP sync).

Here the time of the ouster:

ros2 topic echo /ouster/points --no-arr
header:
  stamp:
    sec: 60
    nanosec: 857867360
  frame_id: os_lidar
height: 32
width: 1024
fields: '<sequence type: sensor_msgs/msg/PointField, length: 9>'
is_bigendian: false
point_step: 48
row_step: 49152
data: '<sequence type: uint8, length: 1572864>'
is_dense: true

Here the time of the images received at "same" time than the ouster msg above:

ros2 topic echo /camera/color/image_raw --no-arr
header:
  stamp:
    sec: 1700559427
    nanosec: 943689697
  frame_id: camera_color_optical_frame
height: 480
width: 640
encoding: rgb8
is_bigendian: 0
step: 1920
data: '<sequence type: uint8, length: 921600>'

They cannot be synchronized together for that issue.

On a side note, these seem to give better lidar odometry (PMOutlierRatio should not be as low as under 0.5, unless it is a very dynamic environment):

'Icp/PMOutlierRatio', '0.6',
'Icp/ReciprocalCorrespondences', 'true',
'OdomF2M/ScanSubtractRadius', '0.1',
'OdomF2M/ScanMaxSize', '20000',

[cjsurjadi]

Hi there,

Thanks for the help. I managed to solve the problem by launching the ouster driver node with the parameter `'timestamp_mode':'TIME_FROM_ROS_TIME'. This makes the Ouster lidar publish in ROS epoch time, and the messages can thus be synchronized. Rtabmap is working well now. However, I see that rtabmap becomes slower. From the logs, I can see the RTAB-Map process even going up to 1.90s, especially as the map turns larger. Is there any parameters that I can tune to improve this problem? Or can I set the loop closure detection to be slower (e.g. every 3 seconds) while the localization can stay faster?

[matlabbe]

Can you share a database to see all timing statistics?

[cjsurjadi]

Here's a database I recorded

[matlabbe]

Here some observations:

* I would remove Vis/MaxDepth and Kp/MaxDepth, they are limiting too much the features extracted to find visual loop closures:


* Here are the steps using the most computation time (with "total" including all those durations):



Which kind of computer is it running on?

If you don't care about the RGB/depth images, you can set Mem/BinDataKept to false to save on compression time.

Do you really need "RGBD/NeighborLinkRefining"? If you are using icp_odometry, that could be removed to save more time.

RGBD/ProximityMaxPaths could be set to 1 to reduce proximity detection time.

Do you need a 3D occupancy grid, or a projected 2D one could be fine? You would save a lot on grid creation with Grid/3D=false

You may increase Icp/VoxelSize to 0.15 or 0.20 m if you are on a constrained computer, this will save time everytime ICP is done.

cheers,
Mathieu