L515 + T265 RTABMAP stops updating topics after some seconds
Locked
 
|
Hello,
I'm running ROS Noetic on a Raspberry Pi 4, mounted on a robot connected to a L515 and T265. I am running my roslaunch command in a separate terminal and then running a python script that subscribes to the topics published by RTABMAP for various processing. However, I noticed that after 30 or so RTABMAP updates (this can vary depending if the robot is moving around or not), RTABMAP stops updating/publishing to the T265 odometry topics and the L515 depth/color image topics. There is nothing printed in the terminal that indicates why this is happening, simply the update messages just stop occurring. Is there anything I can do to fix this problem? Here is my current launch file: <launch> <arg name="device_type_camera1" default="t265"/> <arg name="device_type_camera2" default="l515"/> <arg name="serial_no_camera1" default=""/> <arg name="serial_no_camera2" default=""/> <arg name="camera1" default="t265"/> <arg name="camera2" default="l515"/> <arg name="clip_distance" default="-2"/> <arg name="use_rviz" default="false"/> <arg name="use_rtabmapviz" default="false"/> <include file="$(find realsense2_camera)/launch/rs_d400_and_t265.launch"> <arg name="device_type_camera1" value="$(arg device_type_camera1)"/> <arg name="device_type_camera2" value="$(arg device_type_camera2)"/> <arg name="serial_no_camera1" value="$(arg serial_no_camera1)"/> <arg name="serial_no_camera2" value="$(arg serial_no_camera2)"/> <arg name="camera1" value="$(arg camera1)"/> <arg name="camera2" value="$(arg camera2)"/> <arg name="clip_distance" value="$(arg clip_distance)"/> </include> <include file="$(find rtabmap_ros)/launch/rtabmap.launch"> <arg name="rtabmap_args" value="--delete_db_on_start --Params:Grid/FromDepth=true --Params:Grid/RayTracing=true --Params:Grid/RangeMax=5.0" /> <arg name="depth_topic" value="/$(arg camera2)/aligned_depth_to_color/image_raw"/> <arg name="frame_id" value="$(arg camera2)_link"/> <arg name="visual_odometry" value="false"/> <arg name="odom_topic" value="/$(arg camera1)/odom/sample"/> <arg name="rgb_topic" value="/$(arg camera2)/color/image_raw"/> <arg name="camera_info_topic" value="/$(arg camera2)/color/camera_info"/> <arg name="queue_size" value="400"/> <arg name="rviz" value="$(arg use_rviz)"/> <arg name="rtabmapviz" value="$(arg use_rtabmapviz)"/> </include> </launch> |
Administrator
|
I remember having an issue at some point with T265 where the timestamps in the odometry topics would drift from the system time used by other cameras. At some point, rtabmap won't be able to synchronize the topics. You may open side-by-side terminals and rostopic echo the timestamps of odom topic from T265 and of one of the topic of the L515. Doublecheck that the stamps are not drifting apart. |
|
|
This post was updated on .
Hi,
Thanks for your response. I did the following commands in separate terminals: rostopic echo /t265/odom/sample/header/stamp rostopic echo /l515/color/image_raw/header/stamp I noticed that eventually the T265 stops publishing new timestamps entirely after a couple minutes or so (i.e. the terminal output just stops). I also noticed that when the T265 stops publishing the timestamps, RTABMAP instantly breaks, which makes sense. The T265 is connected to USB 3.1 and the L515 is connected to USB 2.1 (I cannot switch the L515 to USB 3.1 because the other port on my Raspberry Pi 4 is broken). Is there anything I can do to fix this problem? |
|
|
output.txt
I've attached some example output from one of my runs. I also occasionally see this error: 04/03 20:44:06,408 ERROR [546217746752] (uvc-streamer.cpp:106) uvc streamer watchdog triggered on endpoint: 131 |
|
|
In reply to this post by matlabbe
Hello,
I was wondering if there was a fix for this? If not, is there a way to work around this issue? I'm still not sure what I can do to solve my problem. Thanks in advance! |
|
|
In reply to this post by uhroosh
Also found this thread on github:
https://github.com/IntelRealSense/librealsense/issues/7620 At this point, I'm not sure if it's an issue with the T265 itself, Realsense ROS, or RTABMAP. |
|
Administrator
|
Based on That doesn't look a rtabmap issue. Without starting rtabmap, you could try just starting realsense L515 and T265, then do "rostopic hz ..." on all topics that rtabmap would subscribe to, then measure how much time until T265 is broken. If the T265 fails and intel doesn't support it anymore, you may have to change the camera. Note that depending on your task, it could be possible to use L515 alone with rtabmap. There are some RGB-Depth sync issues with L515 when using it for visual odometry, but by avoiding fast rotations it can be usable. |
|
|
Hi,
Thanks for your response. Yes, it seems to be an issue with the T265's interaction with ROS and not with RTABMAP itself. When I read from the T265 using pyrealsense2, there is no such problem, this only occurs when I have a ROS node that is publishing or subscribing to the /t265/odom/sample topic. |
|
Administrator
|
This post was updated on .
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)... |
|
Administrator
|
This post was updated on .
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. |
«
Return to Official RTAB-Map Forum
|
1 view|%1 views
| Free forum by Nabble | Edit this page |