L515 + T265 RTABMAP stops updating topics after some seconds

[uhroosh]

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>

[matlabbe]

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.

[uhroosh]

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?

[uhroosh]

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

[uhroosh]

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!

[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.

[matlabbe]

Based on

uhroosh wrote

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?

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.