Re: L515 + T265 RTABMAP stops updating topics after some seconds

Posted by matlabbe on
URL: http://official-rtab-map-forum.206.s1.nabble.com/L515-T265-RTABMAP-stops-updating-topics-after-some-seconds-tp10937p11053.html

I tried on a laptop with Ubuntu 20.04 using same docker images as above. The T265 is not failing, but now it is L515 data that get unsynchronized over time. This can be seen by using rostopic delay on T265 and L515 topics:

rostopic delay /t265/odom/sample/header/stamp
average delay: 0.027
	min: 0.005s max: 0.073s std dev: 0.01334s window: 44240
average delay: 0.027
	min: 0.005s max: 0.073s std dev: 0.01334s window: 44446

rostopic delay /l515/color/image_raw

...
average delay: 1.884
	min: 1.777s max: 1.988s std dev: 0.05840s window: 750
average delay: 1.887
	min: 1.777s max: 1.991s std dev: 0.05961s window: 765
average delay: 1.889
	min: 1.777s max: 1.995s std dev: 0.06083s window: 780
average delay: 1.891
	min: 1.777s max: 2.002s std dev: 0.06202s window: 795
average delay: 1.893
	min: 1.777s max: 2.005s std dev: 0.06302s window: 810
average delay: 1.895
	min: 1.777s max: 2.008s std dev: 0.06422s window: 825
average delay: 1.897
	min: 1.777s max: 2.011s std dev: 0.06541s window: 840
average delay: 1.899
	min: 1.777s max: 2.015s std dev: 0.06660s window: 855


...

The max is increasing over time for T515, meaning its clock is drifting against the system clock. When we launch L515 launch file, we can see at the beginning:

[ WARN] [1743971497.465971437]: frame's time domain is HARDWARE_CLOCK. Timestamps may reset periodically.

confirming we are not using system clock. As suggested in this reply: https://github.com/IntelRealSense/librealsense/issues/10427#issuecomment-1105946367 we should enable global_time_enabled before launching l515 launch file:

rosparam set /l515/rgb_camera/global_time_enabled true
rosparam set /l515/l500_depth_sensor/global_time_enabled true
rosparam set /l515/motion_module/global_time_enabled true

... launch L515...

This confirms that T265+L515 can work on a desktop/laptop computer (with Ubuntu 20.04 host). Here the full sequence using docker and ros (with UI):

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 introlab3it/rtabmap_ros:noetic-latest bash -c   "\
   rosparam set /l515/rgb_camera/global_time_enabled true && \
   rosparam set /l515/l500_depth_sensor/global_time_enabled true && \
   rosparam set /l515/motion_module/global_time_enabled true"

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

# adjust TF between T265 and L515 frames based on your setup
docker run -it --rm    \
   --network host     \
   introlab3it/rtabmap_ros:noetic-latest     \
   rosrun tf static_transform_publisher 0 0 0.06 0 0 0 t265_link l515_link 100

# running rtabmap with UI
export XAUTH=/tmp/.docker.xauth
touch $XAUTH
xauth nlist $DISPLAY | sed -e 's/^..../ffff/' | xauth -f $XAUTH nmerge -

docker run -it --rm  \
   --privileged  \
   --env="DISPLAY=$DISPLAY" \
   --env="QT_X11_NO_MITSHM=1" \
   --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \
   --env="XAUTHORITY=$XAUTH" \
   --volume="$XAUTH:$XAUTH" \
   -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:=true

EDIT: It seems from ubuntu 22.04 host, it won't detect L515.