/localization_pose has delayed messages but rtabmap continues to publish TF correctly

[abylikhsanov]

I have encountered something strange. The topic /localization_pose had either delayed messages or slightly out of the real position of the robot but rtabmap still managed to correctly publish map -> odom tf. I am attaching a rosbag where you can clearly see what I am talking about.

The launch file is:

<launch>
    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="screen">
        <remap from="rgb/image"       to="/camera/color/image_raw"/>
        <remap from="depth/image"     to="/camera/aligned_depth_to_color/image_raw"/>
        <remap from="rgb/camera_info" to="/camera/color/camera_info"/>
        <remap from="rgbd_image"      to="rgbd_image"/>
        <param name="approx_sync"       value="true"/>
    </node>

    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" >
        <param name="frame_id" type="string" value="base_footprint"/>

        <param name="subscribe_depth" type="bool" value="false"/>
        <param name="subscribe_rgbd"  type="bool" value="true"/>
        <param name="subscribe_stereo"  type="bool" value="false"/>
        <param name="subscribe_scan"  type="bool" value="true"/>

        <remap from="rgb/image"       to="/stereo/left/image_rect"/>
        <remap from="rgb/camera_info" to="/camera/infra1/camera_info"/>
        <remap from="depth/image"     to="/stereo/depth"/>
        <remap from="rgbd_image"      to="rgbd_image"/>
        <remap from="imu" to="/not_used" />
        <remap from="scan" to="/scan_front_filtered"/>

        <remap from="odom" to="/wheel_odometry/wheel_odom"/>
        <param name="approx_sync " value="true" />
        <param name="odom_tf_linear_variance" value="0.001" />
        <param name="odom_tf_angular_variance" value="0.001" />
        <param name="odom_frame_id" value="odom" />
        <param name="database_path" value="/home/maps/map.db" />
        <param name="map_path" value = "$(find genius_launch)/config/maps/map.pgm" />

        <param name="Mem/CovOffDiagIgnored" type="string" value="false" />
        <param name="RGBD/NeighborLinkRefining" type="string" value="false"/>
        <param name="RGBD/ProximityBySpace"     type="string" value="true"/>
        <param name="RGBD/AngularUpdate"        type="string" value="0.01"/>
        <param name="RGBD/LinearUpdate"         type="string" value="0.01"/>
        <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
        <param name="Reg/Strategy"       type="string" value="0"/>
        <param name="Reg/Force3DoF"      type="string" value="true"/>
        <param name="Icp/CorrespondenceRatio"  type="string" value="0.01"/>
        <param name="Grid/Sensor"            type="string" value="false"/>
        <param name="Grid/RayTracing" value="true"/>
        <param name="Mem/IncrementalMemory" type="string" value="false"/>
        <param name="RGBD/OptimizeMaxError"                type="double"      value="3"  />
        <param name="Optimizer/Strategy" value="1" />
        <param name="Mem/UseOdomGravity" value="false" />
        <param name="Rtabmap/PublishRAMUsage" value="true" />
        <param name="Kp/DetectorStrategy" value="6" />

        <param name="RGBD/StartAtOrigin" value="true" />
        <param name="RGBD/ProximityOdomGuess" value="false" />
        <param name="RGBD/ProximityPathFilteringRadius" value="2.0" />
        <param name="Kp/MaxFeatures" value="1500" />
        <param name="Rtabmap/DetectionRate" value="3" />
        <param name="Vis/MinInliers" value="30" />
        <param name="RGBD/MaxOdomCacheSize" value="10" />

        <param name="RGBD/ProximityMaxGraphDepth" value="150" />
        <param name="RGBD/ProximityAngle" value="45.0" />
        <param name="RGBD/ProximityPathMaxNeighbors" value="0" />
        <param name="SuperPoint/ModelPath" value="$(find genius_launch)/config/superpoint.pt" />

    </node>

</launch>

2022-08-04-11-56-54_0.zip

[matlabbe]

The localization_pose delay is large, so in RVIZ we we only the pose 2 seconds in the past:

rostopic delay /localization_pose
subscribed to [/localization_pose]
WARNING: may be using simulated time
average delay: 1.984
    min: 1.984s max: 1.984s std dev: 0.00000s window: 2
average delay: 2.083
    min: 1.984s max: 2.181s std dev: 0.09839s window: 3
average delay: 2.098
    min: 1.984s max: 2.197s std dev: 0.09230s window: 5

For some reason rtabmap node requires 2 seconds to process the input data. We would need the debug output log to debug more (add args="--udebug" in rtabmap's xml attributes) .