We received odometry message, but we cannot get the corresponding TF world->camera_depth_optical_frame at data stamp 154.517000s error

[aasishkc4]

I am frequently getting th tf error which generates a wrong cloud in the reconstructed cloud.

[WARN] [1713155128.638353640] [rtabmap]: We received odometry message, but we cannot get the corresponding TF world->camera_depth_optical_frame at data stamp 154.517000s (odom msg stamp is 154.521000s). Make sure TF of odometry is also published to get more accurate pose estimation. This warning is only printed once.
[rtabmap-1] [INFO] [1713155129.087795657] [rtabmap]: rtabmap (1): Rate=0.20s, Limit=0.000s, Conversion=0.0071s, RTAB-Map=0.4313s, Maps update=0.0081s pub=0.0028s (local map=1, WM=1)
[rtabmap-1] [INFO] [1713155129.509579676] [rtabmap]: rtabmap (2): Rate=0.20s, Limit=0.000s, Conversion=0.0031s, RTAB-Map=0.4032s, Maps update=0.0079s pub=0.0037s (local map=2, WM=2)
[rtabmap-1] [INFO] [1713155131.551631242] [rtabmap]: rtabmap (3): Rate=0.20s, Limit=0.000s, Conversion=0.0035s, RTAB-Map=0.4233s, Maps update=0.1936s pub=0.0001s (local map=2, WM=2)
[rtabmap-1] [INFO] [1713155133.589584425] [rtabmap]: rtabmap (4): Rate=0.20s, Limit=0.000s, Conversion=0.0061s, RTAB-Map=0.4093s, Maps update=0.1911s pub=0.0001s (local map=2, WM=2)

Even though after the warning things are getting fine my the reconstructed cloud still has the wrong cloud.

This is my launch file with parameters

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
from launch.actions import TimerAction

def generate_launch_description():

    use_sim_time = LaunchConfiguration('use_sim_time')
    qos = LaunchConfiguration('qos')
    localization = LaunchConfiguration('localization')

    parameters={
          'frame_id':'camera_depth_optical_frame',
        #   'odom_frame_id': 'base_link',
        #   'visual_odometry': False,
          'map_frame_id':'map',
          'approx_sync': True,
          'approx_sync_max_interval': '0.01',
          'subscribe_scan_cloud': True,
          'Grid/Sensor': '2',
          'wait_imu_to_init': True,
          'use_sim_time':use_sim_time,
          'subscribe_depth':True,
          'use_action_for_goal':True,
          'map_always_update': True,
          'cloud_output_voxelized': True,
          'tf_delay': 0.1,
          'qos_image':qos,
        #   'qos_imu':qos,
        #   'qos_scan':qos,
        #   'qos_odom':qos,
          'queue_size': 30,
          # 'RGBD/LinearUpdate' : '0',
          'Reg/Force3DoF':'true',
          'Optimizer/GravitySigma':'0', # Disable imu constraints (we are already in 2D)
          'Rtabmap/DetectionRate': '5.0'
    }

    remappings=[
          ('rgb/image', '/rgb_camera/image_raw'),
          ('rgb/camera_info', '/rgb_camera/camera_info'),
          ('depth/image', '/filtered_depth_image')]

    return LaunchDescription([

        # Launch arguments
        DeclareLaunchArgument(
            'use_sim_time', default_value='true',
            description='Use simulation (Gazebo) clock if true'),

        DeclareLaunchArgument(
            'qos', default_value='2',
            description='QoS used for input sensor topics'),

        DeclareLaunchArgument(
            'localization', default_value='false',
            description='Launch in localization mode.'),



        # # SLAM mode:
        Node(
            condition=UnlessCondition(localization),
            package='rtabmap_slam', executable='rtabmap', output='screen',
            parameters=[parameters],
            remappings=remappings,
            arguments=['-d']), # 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),
        
  
    ])

[matlabbe]

Can you share the database?

Also, as computation time for RTAB-Map=0.4313s, your detection rate is too fast (5 hz), I'll suggest to keep it to default 1 hz.

[aasishkc4]

I am unable to upload the db file. Basically the above setup I am testing in the gazebo classic simulation and the point cloud published by the camera and lidar I am using to generate the map. Before that I am filtering the point cloud to remove the robot urdf from the cloud. Then the filtered cloud I am passing for the 3d reconstruction. And for the odom I have used the base_link, to which a plugin is attached which publishes the odom

<gazebo>
   <plugin name="libgazebo_ros_p3d" filename="libgazebo_ros_p3d.so">
     <ros>
       <remapping>odom:=odom</remapping>
     </ros>
       <frame_name>world</frame_name>
       <body_name>base_link</body_name>
       <update_rate>30</update_rate>
       <gaussian_noise>0.01</gaussian_noise>
   </plugin>
  </gazebo>

And the robot has the head which can roll and pitch to which the lidar and camera is attached. So, i provided the frame_id as "camera_depth_optical_frame".

[matlabbe]

Can you show the tf tree? (ros2 run tf2_tools view_frames.pyl

Is it look like world->odom->base_link->head_link->camera_link->camera_depth_optical_frame? In which frames libgazebo_ros_p3d is actually publishing odom topic and odom TF?