trouble with rgbd_odometry and oakd

[an99990]

I am having issues running the rgbd_node. I checked and the topic are publishing.

rosrun rtabmap_ros rgbd_odometry rgb/image:=/stereo_inertial_publisher/color/image depth/image:=/stereo_inertial_publisher/stereo/depth rgb/camera_info:=/stereo_inertial_publisher/color/camera_info _frame_id:=oak_rgb_camera_optical_frame

 WARN] [1666448201.933058777]: /rgbd_odometry: 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. 
/rgbd_odometry subscribed to (approx sync):
   /stereo_inertial_publisher/color/image \
   /stereo_inertial_publisher/stereo/depth \
   /stereo_inertial_publisher/color/camera_info
[ WARN] [1666448206.933572900]: /rgbd_odometry: 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. 
/rgbd_odometry subscribed to (approx sync):
   /stereo_inertial_publisher/color/image \
   /stereo_inertial_publisher/stereo/depth \
   /stereo_inertial_publisher/color/camera_info
[ WARN] [1666448211.934165769]: /rgbd_odometry: 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. 
/rgbd_odometry subscribed to (approx sync):
   /stereo_inertial_publisher/color/image \
   /stereo_inertial_publisher/stereo/depth \
   /stereo_inertial_publisher/color/camera_info

[matlabbe]

Are the 10.4, 10.58 and 10.38 framerates? Can you share the rosbag? (or 10 seconds of it)

rgbd_odometry is using message_filters to synchronize the topics. If they are all published around 10 Hz, there must be a problem with the stamp in the headers.

[an99990]

https://drive.google.com/file/d/1THoemMb6hC6I2x1fRW-k-d9E9oIbVocq/view?usp=sharing

thank you

[matlabbe]

The framerate is not right on this bag:

rosbag info 2022-10-23-14-25-14.bag 
path:        2022-10-23-14-25-14.bag
version:     2.0
duration:    10.4s
start:       Oct 23 2022 11:25:14.47 (1666549514.47)
end:         Oct 23 2022 11:25:24.88 (1666549524.88)
size:        32.6 MB
messages:    126
compression: none [18/18 chunks]
types:       sensor_msgs/CameraInfo [c9a58c1b0b154e0e6da7578cb991d214]
             sensor_msgs/Image      [060021388200f6f0f447d0fcd9c64743]
topics:      /stereo_inertial_publisher/stereo/depth        14 msgs    : sensor_msgs/Image     
             stereo_inertial_publisher/color/camera_info   109 msgs    : sensor_msgs/CameraInfo
             stereo_inertial_publisher/color/image           3 msgs    : sensor_msgs/Image

We should expect the same number of topics for each of them.

[an99990]

The thing is that i want to fuse rtabmap visual odometry with the sensors, and feed it back to the odom topic of rtabmap. Im using robt_localization. Are you saying that i could fuse the sensors and feed it to odom topic, set rgbd_sync to true in rtabmap and i could achieve what im looking for ? How do i know that rtabmap is using the visual odometry AND the sensors odmetry ? thank you

[matlabbe]

You cannot use rtabmap.launch if you want to use robot_localization in the middle of visual odometry and rtabmap node. You will have to set publish_tf to false for rgbd_odometry node, feed its output odom topic to robot_localization, then connect robot_localization's odometry_filtered to rtabmap node. There is an old example here (I would now prefer using rgbd_sync though instead of feeding individual image topics to both rgbd_odometry and rtabmap nodes): https://github.com/introlab/rtabmap_ros/blob/6e34cde8e6fafe1f90b35420d112ec8ad52ac5e6/launch/tests/sensor_fusion.launch

[an99990]

hi thank you for pointing me this launch file. I have modified it according to my needs. I d like to know if it is possible to use the rgbd_sync node in that lunahc file, since my topics are coming at different rates here is the launch file.

<launch>  

  <!-- This launch assumes that you have already 
       started you preferred RGB-D sensor and your IMU.
       TF between frame_id and the sensors should already be set too. -->

  <arg name="frame_id"                default="oak_rgb_camera_optical_frame" />
  <arg name="rgb_topic"               default="/stereo_inertial_publisher/color/image" />
  <arg name="depth_topic"             default="/stereo_inertial_publisher/stereo/depth" />
  <arg name="camera_info_topic"       default="/stereo_inertial_publisher/color/camera_info" />
  <arg name="imu_topic"               default="/stereo_inertial_publisher/imu" />
  <arg name="imu_ignore_acc"          default="true" />
  <arg name="rgbd_sync"               default="true" />
  <arg name="imu_remove_gravitational_acceleration" default="true" />

  <!-- Localization-only mode -->
  <arg name="localization"      default="false"/>
  <arg     if="$(arg localization)" name="rtabmap_args"  default=""/>
  <arg unless="$(arg localization)" name="rtabmap_args"  default="--delete_db_on_start"/>  

  <group ns="rtabmap">
    <!-- Visual Odometry -->
    <node pkg="rtabmap_ros" type="rgbd_odometry" name="visual_odometry" output="screen" args="$(arg rtabmap_args)">
      <remap from="rgb/image"       to="$(arg rgb_topic)"/>
      <remap from="depth/image"     to="$(arg depth_topic)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
      <remap from="odom"            to="/vo"/>

      <param name="frame_id"               type="string" value="$(arg frame_id)"/>
      <param name="publish_tf"             type="bool"   value="false"/>
      <param name="publish_null_when_lost" type="bool"   value="true"/>
      <param name="guess_from_tf"          type="bool"   value="true"/>

      <param name="Odom/FillInfoData"      type="string" value="true"/>
      <param name="Odom/ResetCountdown"    type="string" value="1"/>
      <param name="Vis/FeatureType"        type="string" value="6"/> 
      <param name="OdomF2M/MaxSize"        type="string" value="1000"/>
    </node>

    <!-- SLAM -->
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
      <param name="frame_id"        type="string" value="$(arg frame_id)"/>

      <remap from="rgb/image"       to="$(arg rgb_topic)"/>
      <remap from="depth/image"     to="$(arg depth_topic)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
      <remap from="odom"            to="/odometry/filtered"/>

      <param name="Kp/DetectorStrategy"    type="string" value="6"/> <!-- use same features as odom -->

      <!-- localization mode -->
      <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/> 
      <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
      <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>

    </node>
  </group>

  <!-- Odometry fusion (EKF), refer to demo launch file in robot_localization for more info -->
  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true" output="screen">

      <param name="frequency" value="50"/>
      <param name="sensor_timeout" value="0.1"/>
      <param name="two_d_mode" value="false"/>

      <param name="odom_frame" value="odom"/>
      <param name="base_link_frame" value="$(arg frame_id)"/>
      <param name="world_frame" value="odom"/>

      <param name="transform_time_offset" value="0.0"/>

      <param name="odom0" value="/vo"/>
      <param name="imu0" value="$(arg imu_topic)"/> 
      <param name="robot_fuse" value="/odometry/filtered"/> 

      <!-- The order of the values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. -->
      <rosparam param="odom0_config">[true, true, true,
                                      false, false, false,
                                      true, true, true,
                                      false, false, false,
                                      false, false, false]</rosparam>

      <rosparam     if="$(arg imu_ignore_acc)" param="imu0_config">[
                                     false, false, false,
                                     true,  true,  true,
                                     false, false, false,
                                     true,  true,  true,
                                     false,  false,  false] </rosparam>
      <rosparam unless="$(arg imu_ignore_acc)" param="imu0_config">[
                                     false, false, false,
                                     true,  true,  true,
                                     false, false, false,
                                     true,  true,  true,
                                     true,  true,  true] </rosparam>  

      <param name="odom0_differential" value="false"/>
      <param name="imu0_differential" value="false"/>
      <param name="robot_fuse_differential" value="false"/>

      <param name="odom0_relative" value="true"/>
      <param name="imu0_relative" value="true"/>
      <param name="robot_fuse_relative" value="true"/>

      <param name="imu0_remove_gravitational_acceleration" value="$(arg imu_remove_gravitational_acceleration)"/>

      <param name="print_diagnostics" value="true"/>

      <!-- ======== ADVANCED PARAMETERS ======== -->
      <param name="odom0_queue_size" value="5"/>
      <param name="imu0_queue_size" value="50"/> 

      <!-- The values are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz,
           vroll, vpitch, vyaw, ax, ay, az. -->
      <rosparam param="process_noise_covariance">[0.005, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0.005, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0.006, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0.003, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0.003, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0.006, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0.0025, 0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0.0025, 0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0.004, 0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0.001, 0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.001, 0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.002, 0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.001, 0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.001, 0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.0015]</rosparam>

      <!-- The values are ordered as x, y,
           z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. -->
           <rosparam param="initial_estimate_covariance">[1e-9, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    1e-9, 0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    1e-9, 0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    1e-9, 0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    1e-9, 0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-9,  0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-9,  0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1e-9,  0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1e-9, 0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1e-9, 0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]</rosparam>

    </node>
</launch>

[matlabbe]

You can use rgbd_sync node, then set subscribe_rgbd to both rgbd_odometry and rtabmap nodes, then remap rgbd_image topic correctly.