RTAB crashes after set_mode_localization service call

[hoffer]

I have been tweaking a setup with a ZEDx camera providing odometry for rtabmap_slam. I finally got to a point where I am staying localized while mapping and building a good 2d occupancy grid. My next step was to test localization on a previous mapping session. When I call the set_mode_localization service to do so, I receive the following error. I've seen another post that mentions this could be an issue with IMU data but not sure. I am using ZED's latest and greatest integrated IMU camera with the ROS2 wrapper.

[rtabmap-10] [INFO] [1723920457.139763492] [rtabmap.rtabmap]: rtabmap: Set localization mode
[rtabmap-10] [INFO] [1723920463.705418993] [rtabmap.rtabmap]: rtabmap: Localization mode enabled!
[rtabmap-10] [WARN] [1723920463.711877905] [rtabmap.rtabmap]: rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ ros2 topic 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"). Parameter "approx_sync" is false, which means that input topics should have all the exact timestamp for the callback to be called.
[rtabmap-10] rtabmap subscribed to (exact sync):
[rtabmap-10]    /zed_node/odom \
[rtabmap-10]    /rtabmap/rgbd_image \
[rtabmap-10]    /scan
[rtabmap-10] [INFO] [1723920463.714555693] [rtabmap.rtabmap]: Parameters event received!
[rtabmap-10] [INFO] [1723920463.714628816] [rtabmap.rtabmap]: Setting RTAB-Map parameter "Mem/IncrementalMemory"="false"
[rtabmap-10] [INFO] [1723920463.714645136] [rtabmap.rtabmap]: rtabmap: Updating parameters
[rtabmap-10] [INFO] [1723920463.714685746] [rtabmap.rtabmap]: RTAB-Map rate detection = 1.000000 Hz
[rtabmap-10] [INFO] [1723920464.409669094] [rtabmap.rtabmap]: rtabmap (3885): Rate=1.00s, Limit=0.000s, Conversion=0.0028s, RTAB-Map=0.3368s, Maps update=0.0377s pub=0.0007s (local map=258, WM=258)
[rtabmap-10] [FATAL] [1723920464.883376970] [rtabmap.rtabmap]: [FATAL] (2024-08-17 14:47:44.883) Link.cpp:142::setInfMatrix() Condition (uIsFinite(infMatrix.at<double>(5,5)) && infMatrix.at<double>(5,5)>0) not met! [Angular information yaw should not be null! Value=-82327.580866 (set to 1 if unknown or <=1/9999 to be ignored in some computations).]
[rtabmap-10] terminate called after throwing an instance of 'UException'
[rtabmap-10]   what():  [FATAL] (2024-08-17 14:47:44.883) Link.cpp:142::setInfMatrix() Condition (uIsFinite(infMatrix.at<double>(5,5)) && infMatrix.at<double>(5,5)>0) not met! [Angular information yaw should not be null! Value=-82327.580866 (set to 1 if unknown or <=1/9999 to be ignored in some computations).]
[ERROR] [rtabmap-10]: process has died [pid 153020, exit code -6, cmd '/opt/ros/humble/lib/rtabmap_slam/rtabmap --Grid/GroundIsObstacle false --RGBD/OptimizeMaxError 3 --Reg/Force3DoF true --ros-args --log-level rtabmap.rtabmap:=info --log-level rtabmap:=info --ros-args -r __node:=rtabmap -r __ns:=/rtabmap -p use_sim_time:=False --params-file /tmp/launch_params_xdwfhdae -r map:=map -r rgb/image:=/zed_node/rgb/image_rect_color -r depth/image:=/zed_node/depth/depth_registered -r rgb/camera_info:=/zed_node/rgb/camera_info -r rgbd_image:=rgbd_image -r left/image_rect:=/stereo_camera/left/image_rect_color -r right/image_rect:=/stereo_camera/right/image_rect -r left/camera_info:=/stereo_camera/left/camera_info -r right/camera_info:=/stereo_camera/right/camera_info -r scan:=/scan -r scan_cloud:=/scan_cloud -r user_data:=/user_data -r user_data_async:=/user_data_async -r gps/fix:=/gps/fix -r tag_detections:=/detections -r fiducial_transforms:=/fiducial_transforms -r odom:=/zed_node/odom -r imu:=/zed_node/imu/data -r goal_out:=/goal_pose'].

[matlabbe]

Hi,

I was not able to reproduce the issue with this example (with use_zed_odometry:=true) and toggling mapping/localization on-off. An odometry topic with invalid covariance may have been received. I verified and zed_wrapper should return odometry with fixed covariance "1e-10" like the following, so you may verify if you see the same thing:

ros2 topic echo /zed/zed_node/odom --field pose.covariance

[1.00000001e-10 0.00000000e+00 0.00000000e+00 0.00000000e+00
 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000001e-10
 0.00000000e+00 0.00000000e+00 0.00000000e+00 0.00000000e+00
 0.00000000e+00 0.00000000e+00 1.00000001e-10 0.00000000e+00
 0.00000000e+00 0.00000000e+00 0.00000000e+00 0.00000000e+00
 0.00000000e+00 1.00000001e-10 0.00000000e+00 0.00000000e+00
 0.00000000e+00 0.00000000e+00 0.00000000e+00 0.00000000e+00
 1.00000001e-10 0.00000000e+00 0.00000000e+00 0.00000000e+00
 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000001e-10]

Otherwise, if you can reproduce easily and the covariance looks okay, you may try to launch rtabmap node with "--udebug" argument to show more debug logs.

cheers,
Mathieu