Posted by
matlabbe on
URL: http://official-rtab-map-forum.206.s1.nabble.com/L515-T265-RTABMAP-stops-updating-topics-after-some-seconds-tp10937p11052.html
So I tried on a RPI4 running Raspbian GNU/Linux 12 (bookworm) with docker.
Note that the only way I could get L515 working is by downscaling the color resolution using directly rs_camera.launch instead of rs_d400_and_t265.launch:
docker run -it --rm \
--network host \
-v /dev:/dev \
--privileged \
introlab3it/rtabmap_ros:noetic-latest \
roslaunch realsense2_camera rs_t265.launch camera:=t265
docker run -it --rm \
--network host \
-v /dev:/dev --privileged \
introlab3it/rtabmap_ros:noetic-latest \
roslaunch realsense2_camera rs_camera.launch \
align_depth:=true \
color_fps:=15 \
color_width:=640 \
color_height:=360 \
device_type:=l515 \
tf_prefix:=l515 \
camera:=l515
docker run -it --rm \
--network host \
introlab3it/rtabmap_ros:noetic-latest \
rosrun tf static_transform_publisher 0 0 0 0 0 0 t265_link l515_link 100
However, this setup works only till T265 stops publishing for unknown reason that you described. We can see it just by launching the cameras as above and do a "rostopic hz" on all topics till T265 fails:
docker run -it --rm \
--network host \
introlab3it/rtabmap_ros:noetic-latest \
rostopic hz /t265/odom/sample /l515/aligned_depth_to_color/image_raw /l515/color/image_raw /l515/color/camera_info
topic rate min_delta max_delta std_dev window
=========================================================================================
/t265/odom/sample 199.6 2.503e-05 0.1076 0.01281 39187
/l515/aligned_depth_to_color/image_raw 14.44 0.01261 0.2972 0.01467 39187
/l515/color/image_raw 14.98 0.002516 0.1649 0.01314 2833
/l515/color/camera_info 14.99 5.794e-05 0.1762 0.01303 2833
topic rate min_delta max_delta std_dev window
=========================================================================================
/t265/odom/sample 199.6 2.503e-05 0.1076 0.01281 39333
/l515/aligned_depth_to_color/image_raw 14.44 0.01261 0.2972 0.01463 39333
/l515/color/image_raw 14.98 0.002516 0.1649 0.01311 2849
/l515/color/camera_info 14.99 5.794e-05 0.1762 0.013 2849
topic rate min_delta max_delta std_dev window
=========================================================================================
/l515/aligned_depth_to_color/image_raw 14.44 0.01261 0.2972 0.01463 2861
/l515/color/image_raw 15.0 0.002516 0.1649 0.01333 2861
/l515/color/camera_info 15.01 5.794e-05 0.1762 0.0132 2973
Nevertheless, rtabmap would normally be used like this if T265 doesn't fail:
docker run -it --rm \
-e ROS_HOME=/tmp/.ros \
--network host \
-v ~/.ros:/tmp/.ros \
introlab3it/rtabmap_ros:noetic-latest \
roslaunch rtabmap_launch rtabmap.launch \
args:="-d --Mem/UseOdomGravity true --Optimizer/GravitySigma 0.3" \
odom_topic:=/t265/odom/sample \
frame_id:=t265_link \
rgbd_sync:=true \
depth_topic:=/l515/aligned_depth_to_color/image_raw \
rgb_topic:=/l515/color/image_raw \
camera_info_topic:=/l515/color/camera_info \
approx_rgbd_sync:=false \
visual_odometry:=false \
queue_size:=30 \
rtabmap_viz:=false
[ INFO] [1743969415.420977938]: rtabmap (44): Rate=1.00s, Limit=0.000s, Conversion=0.0027s, RTAB-Map=0.1664s, Maps update=0.0002s pub=0.0001s (local map=1, WM=1)
[ INFO] [1743969416.531241919]: rtabmap (45): Rate=1.00s, Limit=0.000s, Conversion=0.0028s, RTAB-Map=0.1885s, Maps update=0.0002s pub=0.0001s (local map=1, WM=1)
[ INFO] [1743969419.932134244]: rtabmap (46): Rate=1.00s, Limit=0.000s, Conversion=0.0028s, RTAB-Map=0.2069s, Maps update=0.0004s pub=0.0002s (local map=1, WM=1)
[ INFO] [1743969421.008611783]: rtabmap (47): Rate=1.00s, Limit=0.000s, Conversion=0.0112s, RTAB-Map=0.2212s, Maps update=0.0002s pub=0.0001s (local map=1, WM=1)
[ INFO] [1743969422.158917302]: rtabmap (48): Rate=1.00s, Limit=0.000s, Conversion=0.0043s, RTAB-Map=0.1500s, Maps update=0.0002s pub=0.0001s (local map=1, WM=1)
[ INFO] [1743969426.485254394]: rtabmap (49): Rate=1.00s, Limit=0.000s, Conversion=0.0027s, RTAB-Map=0.1599s, Maps update=0.0003s pub=0.0001s (local map=1, WM=1)
[ WARN] [1743969431.712111663]: /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 "sync_queue_size" and/or "topic_queue_size" parameters (current=30 and 1 respectively).
/rtabmap/rtabmap subscribed to (approx sync):
/t265/odom/sample \
/rtabmap/rgbd_image
From these results, I can confirm that we cannot use T265 + L515 setup reliably on RPI4.
EDIT: I also tried the standalone version (without ROS) and we still have the same issue that at some point the poses from T265 are not received anymore:
export XAUTH=/tmp/.docker.xauth
touch $XAUTH
xauth nlist $DISPLAY | sed -e 's/^..../ffff/' | xauth -f $XAUTH nmerge -
docker run -it --rm \
-v /dev:/dev \
--privileged \
--env="DISPLAY=$DISPLAY" \
--env="QT_X11_NO_MITSHM=1" \
--volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \
--env="XAUTHORITY=$XAUTH" \
--volume="$XAUTH:$XAUTH" \
-v ~/Documents/RTAB-Map:/root/Documents/RTAB-Map \
-e OMP_WAIT_POLICY=passive \
introlab3it/rtabmap_ros:noetic-latest rtabmap
[ WARN] (2025-04-06 20:13:36.475) CameraRealSense2.cpp:252::getPoseAndIMU() Could not find poses to interpolate at image time 1743970416.312478 after waiting 35 ms (last is 1743970416.277909)...
[ WARN] (2025-04-06 20:13:36.513) CameraRealSense2.cpp:330::getPoseAndIMU() Could not find acc data to interpolate at image time 1743970416.312478 after waiting 35 ms (last is 1743970416.265224)...
[ WARN] (2025-04-06 20:13:36.624) CameraRealSense2.cpp:252::getPoseAndIMU() Could not find poses to interpolate at image time 1743970416.445794 after waiting 35 ms (last is 1743970416.277909)...
[ WARN] (2025-04-06 20:13:36.665) CameraRealSense2.cpp:330::getPoseAndIMU() Could not find acc data to interpolate at image time 1743970416.445794 after waiting 35 ms (last is 1743970416.265224)...
[ WARN] (2025-04-06 20:13:36.826) CameraRealSense2.cpp:252::getPoseAndIMU() Could not find poses to interpolate at image time 1743970416.646033 after waiting 35 ms (last is 1743970416.277909)...
[ WARN] (2025-04-06 20:13:36.866) CameraRealSense2.cpp:330::getPoseAndIMU() Could not find acc data to interpolate at image time 1743970416.646033 after waiting 35 ms (last is 1743970416.265224)...
[ WARN] (2025-04-06 20:13:37.009) CameraRealSense2.cpp:252::getPoseAndIMU() Could not find poses to interpolate at image time 1743970416.846282 after waiting 35 ms (last is 1743970416.277909)...