Robot with Intel RealSense D435i only (no odom) "no data for 5 seconds" error

[catmanjan]

[ WARN] [1651320614.752468194]: /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"). Parameter "approx_sync" is false, which means that input topics should have all the exact timestamp for the callback to be called.
/rtabmap/rtabmap subscribed to (exact sync):
   /rtabmap/odom \
   /rtabmap/rgbd_image

I am using only a D435i, here is hz output

                 topic                      rate   min_delta   max_delta   std_dev   window
===========================================================================================
/camera/aligned_depth_to_color/image_raw   18.05   0.0143      0.3117      0.04567   225   
/camera/color/image_raw                    28.75   0.003833    0.1071      0.01915   225   
/camera/color/camera_info                  28.97   7.081e-05   0.1117      0.02208   361   

And finally, my launch file.

<launch>

    <!--<node pkg="rosserial_python" type="serial_node.py" name="rosserial" output="screen" />-->

    <!-- taken from intel realsense doco -->
    <node pkg="imu_filter_madgwick" type="imu_filter_node" name="ImuFilter">
        <param name="use_mag" type="bool" value="false" />
        <param name="_publish_tf" type="bool" value="false" />
        <param name="_world_frame" type="string" value="enu" />
        <remap from="/imu/data_raw" to="/camera/imu"/>
        <remap from="/imu/data" to="/rtabmap/imu"/>
    </node>

    <!-- taken from intel realsense doco -->
    <include file="$(find realsense2_camera)/launch/rs_camera.launch">
        <arg name="align_depth" value="true"/>
        <arg name="enable_gyro" value="true"/>
        <arg name="enable_accel" value="true"/>
        <arg name="unite_imu_method" value="copy"/>
    </include>

  <group ns="rtabmap">
    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="screen">
      <remap from="rgb/image"        to="/camera/color/image_raw"/>
      <remap from="depth/image"      to="/camera/aligned_depth_to_color/image_raw"/>
      <remap from="rgb/camera_info"  to="/camera/color/camera_info"/>
      <remap from="rgbd_image"       to="rgbd_image"/> <!-- output -->

      <!-- Should be true for not synchronized camera topics 
           (e.g., false for kinectv2, zed, realsense, true for xtion, kinect360)-->
      <param name="approx_sync"       value="false"/> 
    </node>

    <!-- Odometry -->
    <node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
      <param name="subscribe_rgbd" type="bool"   value="true"/>
      <param name="frame_id"       type="string" value="base_link"/>
      <remap from="rgbd_image" to="rgbd_image"/>
    </node>

    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
          <param name="frame_id"        type="string" value="base_link"/>
          <param name="subscribe_depth" type="bool"   value="false"/>
          <param name="subscribe_rgbd"  type="bool"   value="true"/>

          <remap from="odom"       to="odom"/>
          <remap from="rgbd_image" to="rgbd_image"/>

          <param name="queue_size"  type="int"  value="10"/>
          <param name="approx_sync" type="bool" value="false"/>

          <!-- RTAB-Map's parameters -->
          <param name="RGBD/AngularUpdate"        type="string" value="0.01"/>
          <param name="RGBD/LinearUpdate"         type="string" value="0.01"/>
          <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
    </node>
  </group>

</launch>

Any obvious issues? This was mostly a copy paste job from intel and this page http://wiki.ros.org/rtabmap_ros/Tutorials/SetupOnYourRobot#Kinect

[MJA0111]

Hi, Please try changing frame_id from "base_link" to "camera_link". If it didn't workout, please provide your rqt_graph and frame tree (view_frames). also please check if /rtabmap/rgbd_image and /rtabmap/odom are publishing the messages with the right frequency.

kind regards, Javad

[Dave-van-der-Meer]

Hi,

I have a similar (if not the same) problem. I use ROS2 Foxy with a Realsense D455 camera.

I get the following output on the rtabmap terminal:

[rtabmapviz-3] [WARN] [1653031838.878952523] [rtabmapviz]: rtabmapviz: 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 "queue_size" parameter (current=10). Parameter "approx_sync" is false, which means that input topics should have all the exact timestamp for the callback to be called.
[rtabmapviz-3] rtabmapviz subscribed to (exact sync):
[rtabmapviz-3]    /odom \
[rtabmapviz-3]    /camera/color/image_raw \
[rtabmapviz-3]    /camera/depth/image_rect_raw \
[rtabmapviz-3]    /camera/color/camera_info

I have modified the ROS2 launchfile of the realsense_d400.launch.py to the following:

# Requirements:
#   A realsense D455
#   Install realsense2 ros2 package (refactor branch)
# Example:
#   $ ros2 launch realsense2_camera rs_launch.py align_depth:=true
#
#   $ ros2 launch rtabmap_ros realsense_d455.launch.py
#   OR
#   $ ros2 launch rtabmap_ros rtabmap.launch.py frame_id:=camera_link args:="-d" rgb_topic:=/camera/color/image_raw depth_topic:=/camera/aligned_depth_to_color/image_raw camera_info_topic:=/camera/color/camera_info approx_sync:=false

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

def generate_launch_description():
    parameters=[{
          'frame_id':'camera_link',
          'subscribe_depth':True,
          'approx_sync':False}]

    remappings=[
          ('rgb/image', '/camera/color/image_raw'),
          ('rgb/camera_info', '/camera/color/camera_info'),
          ('depth/image', '/camera/depth/image_rect_raw')]

    return LaunchDescription([

        # Nodes to launch
        Node(
            package='rtabmap_ros', executable='rgbd_odometry', output='screen',
            parameters=parameters,
            remappings=remappings),

        Node(
            package='rtabmap_ros', executable='rtabmap', output='screen',
            parameters=parameters,
            remappings=remappings,
            arguments=['-d']),

        Node(
            package='rtabmap_ros', executable='rtabmapviz', output='screen',
            parameters=parameters,
            remappings=remappings),
    ])

My rqt_graph looks as follows:

I notice that the /odom topic is supposed to be published by the node /rgbd_odometry. The topic is published, but no messages are sent.

I also noticed that the tf_tree does not link the /odom to the /camera_link. Not sure if this should be the case or not. There is a tf from /map -> /odom and from /camera_link -> /camera_depth_frame -> /camera_depth_optical_frame and from /camera_link -> /camera_color_frame -> /camera_color_optical_frame.

I did not modify the rtabmap.launch.py file.

I recon that there might be differences with ROS2, but I think that the underlying problem is the same.

[Dave-van-der-Meer]

I experimented a little and figured out that I forgot to enable the align_depth.enable property. Now, it works. Not sure if this could also be the issue of @catmanjan ? Can you show us the launch file you use to launch the cameras?

[Brac24]

I had the exact same issue as @Dave-van-der-Meer also using ROS 2 Foxy with a D455. My issue seems to be that the node topics are not getting remapped I believe. The rgbd_odometry node prints out that it is subscribed to rgb/camera_info which was remapped in the launch file and the launch file is identical to that of @Dave-van-der-Meer. Even when I kill the rtabmap process you can see that the rgbd_odometry node launched with a -r rgb/camera_info:=/camera/color/camera_info which I believe means that it's remapping that topic. Unless this is a print error and it is actually remapped. I've checked that /camera/color/camera_info is publishing.

I'd like to add my rqt graph which actually shows /camera/color/camera_info topic going to rgbd_odometry node

[Dave-van-der-Meer]

@Brac24 did you do colcon build after remapping the topics in the launch file? In ROS2, you need to do this each time you change a launch file unlike in ROS1 where launch files did not need to be rebuilt. I had this issue as well as I did not know I need to rebuild each time.

[Brac24]

I guess that's not really an option because rtabmap is only available as a prebuilt package for me. I am using a docker container provided by this https://github.com/dusty-nv/jetson-containers/blob/master/Dockerfile.ros.slam. So colcon build doesn't do anything. If I change the launch file the rqt_graph does change though which makes me think that it is getting remapped and it's just a printing error but I might be wrong. I might have to try installing rtabmap on this container in my own workspace or something. rtabmap_ros is also an old version of rtabmap so that might be the root issue.

[matlabbe]

that it is getting remapped and it's just a printing error but I might be wrong

This has been fixed a while ago, on my computer the topics shown in the log are the remapped ones.

[Brac24]

To be honest I can't remember what was done to fix this issue but it is working now. Thank you