ICP Odometry stops output randomly

[masskro0]

Setup: ROS Humble Ubuntu 22.04

I noticed that the ICP Odometry randomly (which occur already after ~15sec) stops producing valid output. Non-valid output means that the default empty odometry message is being published. The node doesn't crash though, nor are any errors produced. The ratio suddenly jumps to 0 and stays there:

[icp_odometry-10] [DEBUG] [1716569858.071465548] [FrequencyStatus_debug_logger]: TICK 1027
[icp_odometry-10] [DEBUG] [1716569858.071532392] [rcl]: Subscription taking message
[icp_odometry-10] [DEBUG] [1716569858.071547423] [rcl]: Subscription take succeeded: true
[icp_odometry-10] [INFO] [1716569858.085004606] [icp_odometry]: Odom: ratio=0.669399, std dev=0.008015m|0.002535rad, update time=0.012426s
[icp_odometry-10] [DEBUG] [1716569858.085039052] [FrequencyStatus_debug_logger]: TICK 1028
[icp_odometry-10] [DEBUG] [1716569858.114277398] [rcl]: Subscription taking message
[icp_odometry-10] [DEBUG] [1716569858.114322505] [rcl]: Subscription take succeeded: true
[icp_odometry-10] [DEBUG] [1716569858.129458872] [rcl]: Subscription taking message
[icp_odometry-10] [DEBUG] [1716569858.129490823] [rcl]: Subscription take succeeded: true
[icp_odometry-10] [INFO] [1716569858.134768506] [icp_odometry]: Odom: ratio=0.000000, std dev=0.000000m|0.000000rad, update time=0.019377s
[icp_odometry-10] [DEBUG] [1716569858.134858230] [rcl]: Calling timer
[icp_odometry-10] [DEBUG] [1716569858.134909047] [rcl]: Calling timer
[icp_odometry-10] [DEBUG] [1716569858.134921288] [rcl]: Subscription taking message
[icp_odometry-10] [DEBUG] [1716569858.134936891] [rcl]: Subscription take succeeded: true
[icp_odometry-10] [INFO] [1716569858.136176607] [icp_odometry]: Odom: ratio=0.000000, std dev=0.000000m|0.000000rad, update time=0.000221s

After that snap, the node complains that the laser scan topic was not received, even though my scanner runs in the background (other nodes are using that laser scan as well and they don't complain).

My config file is minimal:

/icp_odometry:
  ros__parameters:
    frame_id: "base_link"
    odom_topic: "/rtabmap/icp_odom"
    publish_tf: false
    scan_topic: "/sick_tim_left/scan"

That's wierd because the odometry works perfectly for a short time and then suddenly stops. No errors, no unusual movement of my robot, and the sensors are running fine. The same happens with the visual odometry, but I leave that for another issue.

[matlabbe]

Can you share a small rosbag of the scan topic /sick_tim_left/scan till icp_odometry fails? Is the robot navigating in large empty space (out of range of the lidar)?

[masskro0]

rosbag2_2024_05_28-18_34_14.zip I attached the rosbag. Yes, there are spaces which are out of range. But if that's the problem, then the node should stop immediately after launching, as there are out of range spaces as well.

In case it's relevant, my ros-humble-rtabmap version is 0.21.4-1jammy.20240517.170731 and for ros-humble-rtabmap-odom it's 0.21.4-2jammy.20240522.172901.

[matlabbe]

I tried your bag:

ros2 bag play rosbag2_2024_05_28-18_34_14/rosbag2_2024_05_28-18_34_14_0.db3
ros2 run rtabmap_odom icp_odometry --ros-args -r scan:=/sick_tim_left/scan -p frame_id:=sick_tim_left

I don't see odometry getting lost in this one:

One thing I discovered this weekend is that on some machines icp can be super slow. Here the typical performance you should get:

[1717388334.118019026] Odom: ratio=0.680203, std dev=0.008103m|0.002562rad, update time=0.005686s
[1717388334.123897000] Odom: ratio=0.680203, std dev=0.008103m|0.002562rad, update time=0.005381s
[1717388334.185096811] Odom: ratio=0.679901, std dev=0.007922m|0.002505rad, update time=0.005960s
[1717388334.191052915] Odom: ratio=0.679901, std dev=0.007922m|0.002505rad, update time=0.005136s
[1717388334.251740149] Odom: ratio=0.658537, std dev=0.008134m|0.002572rad, update time=0.006070s
[1717388334.258144095] Odom: ratio=0.658537, std dev=0.008134m|0.002572rad, update time=0.005889s
[1717388334.318447767] Odom: ratio=0.681013, std dev=0.007983m|0.002525rad, update time=0.005901s
[1717388334.323697519] Odom: ratio=0.681013, std dev=0.007983m|0.002525rad, update time=0.004749s
[1717388334.385156413] Odom: ratio=0.680798, std dev=0.007840m|0.002479rad, update time=0.005346s

If you have some updates with processing time way higher than other updates, you may try setting export OMP_WAIT_POLICY=passive before launching icp_odometry.

[masskro0]

I think it's indeed a hardware performance issue. I couldn't reproduce the issue after setting that flag.

Not related to this issue, but I mentioned that my visual odometry also stops (setting itself to maximum covariance and some random value for yaw velocity). I got that fixed by following this guide for increasing speed/reducing accuracy.

To summarize, it's likely that both odometry nodes stopped due to performance issues. Thank you for your help!