How to use rtabmap_ros/data_throttle for Realsense camera?

[khlau]

I am using D415 on raspi 3 B and I trying to run rtabmap on my PC.  The tutorial in http://wiki.ros.org/rtabmap_ros/Tutorials/RemoteMapping and the last .launch file in http://wiki.ros.org/rtabmap_ros/Tutorials/SetupOnYourRobot are both not work.

From rqt_graph, I can see that there is no connection between _in and _out ( e.g color/image_raw and data_throttled_image_relay)
I think I have to implement something but I dont know what are they. Can someone advise me the solution?

This is the .launch file I used on robot<launch>

<include file="$(find realsense2_camera)/launch/rs_camera.launch">
     <arg name="align_depth" value="True" />
  </include>

  <arg name="rate"  default="5"/>
  <arg name="decimation"  default="1"/> <!-- Reduce the image size, e.g., 2 means "width/2 x height/2". -->
  <arg name="approx_sync" default="true" />

  <!-- Use same nodelet used by realsense2 -->
  <group ns="camera">
    <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle realsense2_camera_manager" output="screen">

      <param name="rate" type="double" value="$(arg rate)"/>
      <param name="decimation" type="int" value="$(arg decimation)"/>
      <param name="approx_sync" type="bool" value="$(arg approx_sync)"/>

      <remap from="rgb/image_in"       to="color/image_raw"/>
      <remap from="depth/image_in"     to="aligned_depth_to_color/image_raw"/>
      <remap from="rgb/camera_info_in" to="color/camera_info"/>

      <remap from="rgb/image_out"       to="/data_throttled_image_relay"/>
      <remap from="depth/image_out"     to="/depth_registered/image_raw_relay"/>
      <remap from="rgb/camera_info_out" to="/data_throttled_camera_info"/>

    </node>
  </group>
</launch>

[matlabbe]

Can you show rqt_graph?

Does nodelet manager "realsense2_camera_manager" exist?

You can try setting a standalone nodelet instead:

<node pkg="nodelet" type="nodelet" name="data_throttle" args="standalone rtabmap_ros/data_throttle " output="screen">

Can you show:

$ rosnode info data_throttle

$ rostopic hz /camera/color/image_raw
$ rostopic hz /camera/aligned_depth_to_color/image_raw
$ rostopic hz /camera/color/camera_info

$ rostopic hz /data_throttled_image_relay
$ rostopic hz /depth_registered/image_raw_relay
$ rostopic hz /data_throttled_camera_info

cheers,
Mathieu

[khlau]

using

 <node pkg="nodelet" type="nodelet" name="data_throttle" args="standalone rtabmap_ros/data_throttle " output="screen">

cause either the raspi reboot or hang ( rcu_preemp deteced stalls on CPU/Tasks)

so I am going to use mine .launch config. but using /camera/depth/image_rect_raw instead of  /camera/aligned_depth_to_color/image_raw since there is messages from /depth/image_rect_raw but not from aligned_depth_to_color/image_raw
I run this on rasp pi
$ roslaunch my_realsense2_camera_throttle.launch rate:=5  

I run this(from http://wiki.ros.org/rtabmap_ros/Tutorials/RemoteMapping) on my pc
$ roslaunch rtabmap_ros rtabmap.launch \
rgb_topic:=/camera/data_throttled_image \
depth_topic:=/camera/data_throttled_image_depth \
camera_info_topic:=/camera/data_throttled_camera_info \
compressed:=true \
rtabmap_args:="--delete_db_on_start" \
rviz:=true rtabmapviz:=false



and result this rqt_graph which is hard to see the overlap part but very clean to see there is  no connection between camear/color and camera/data_throttled


$ rosnode info data_throttle
--------------------------------------------------------------------------------
Node [/data_throttle]
Publications: None

Subscriptions: None

Services: None

cannot contact [/data_throttle]: unknown node


$ rostopic hz /camera/color/image_raw
subscribed to [/camera/color/image_raw]
average rate: 2.811
        min: 0.356s max: 0.356s std dev: 0.00000s window: 2
average rate: 2.640
        min: 0.356s max: 0.393s std dev: 0.01502s window: 5
average rate: 2.530
        min: 0.356s max: 0.432s std dev: 0.02640s window: 7
average rate: 2.527
        min: 0.356s max: 0.432s std dev: 0.02343s window: 9
average rate: 2.447
        min: 0.356s max: 0.453s std dev: 0.02969s window: 12
average rate: 2.384
        min: 0.356s max: 0.539s std dev: 0.04592s window: 13
average rate: 2.358
        min: 0.356s max: 0.539s std dev: 0.04670s window: 16

$ rostopic hz /camera/aligned_depth_to_color/image_raw
subscribed to [/camera/aligned_depth_to_color/image_raw]
no new messages
no new messages
no new messages
no new messages
no new messages

$ rostopic hz /camera/depth/image_rect_raw
subscribed to [/camera/depth/image_rect_raw]
no new messages
no new messages
no new messages
no new messages
no new messages
average rate: 0.405
        min: 2.468s max: 2.468s std dev: 0.00000s window: 2
no new messages
no new messages




$ rostopic hz /camera/color/camera_info
subscribed to [/camera/color/camera_info]
average rate: 29.768
        min: 0.025s max: 0.039s std dev: 0.00335s window: 24
average rate: 29.772
        min: 0.003s max: 0.176s std dev: 0.02086s window: 58
average rate: 29.777
        min: 0.003s max: 0.176s std dev: 0.01748s window: 88
average rate: 29.744
        min: 0.000s max: 0.182s std dev: 0.02134s window: 118
average rate: 29.771
        min: 0.000s max: 0.182s std dev: 0.01925s window: 147


$ rostopic hz /data_throttled_image_relay
subscribed to [/data_throttled_image_relay]
no new messages
no new messages
no new messages
no new messages

$ rostopic hz /depth_registered/image_raw_relay
subscribed to [/depth_registered/image_raw_relay]
no new messages
no new messages


$ rostopic hz /data_throttled_camera_info
subscribed to [/data_throttled_camera_info]
no new messages
no new messages
no new messages

My Question:
1. I have the word 'relay','republish', 'data_throttled', 'compress' and 'image_transport' in my mind after google a lot but I don't understand how should they work together in this case? Can you briefly explain here?

2. Anyone know how "args="load rtabmap_ros/data_throttle camera_nodelet_manager"" in this .launch( from http://wiki.ros.org/rtabmap_ros/Tutorials/SetupOnYourRobot )???

<!-- Throttling messages -->
  <group ns="camera">
    <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen">
      <param name="rate" type="double" value="5.0"/>

      <remap from="rgb/image_in"       to="rgb/image_rect_color"/>
      <remap from="depth/image_in"     to="depth_registered/image_raw"/>
      <remap from="rgb/camera_info_in" to="rgb/camera_info"/>

      <remap from="rgb/image_out"       to="data_throttled_image"/>
      <remap from="depth/image_out"     to="data_throttled_image_depth"/>
      <remap from="rgb/camera_info_out" to="data_throttled_camera_info"/>
    </node>
  </group>

I tried to find camera_nodelet_manager but I can't.

thx for your time!!!!

[matlabbe]

Hi,

For your first question:
'relay','republish' and 'image_transport': see http://wiki.ros.org/topic_tools/relay for general topics or http://wiki.ros.org/image_transport for images. When using rtabmap.launch, because multiple nodes are subscribing to same image topics, we create a relay to subscribe those topics once on the network, but republish to multiples nodes locally on the remote computer. For example, without relays, if two nodes on remote computer are connected to same image topic coming from the robot, the images will be published 2 times over the network.

'data_throttled': throttling means that we reduce frame rate of a topic. Example, republishing 30 Hz images at 5 Hz. It can be used to decrease network bandwidth usage.

'compressed', 'compressedDepth' and 'image_transport': when subscribing to a node publishing images, we can choose to subscribe to raw or compressed version. When publishing over the network, the remote nodes may want to subscribe to compressed topics to save bandwidth.

For your second question:
As stated in the tutorial, "camera_nodelet_manager" is created by openni camera launch. In your case, you should use the nodelet manager created by realsense camera launch. I have access today to a D435 camera so I could test your setup. Here is the modified my_realsense2_camera_throttle.launch:

<launch>

<include file="$(find realsense2_camera)/launch/rs_camera.launch">
     <arg name="align_depth" value="True" />
  </include>

  <arg name="rate"  default="5"/>
  <arg name="decimation"  default="1"/> <!-- Reduce the image size, e.g., 2 means "width/2 x height/2". -->
  <arg name="approx_sync" default="false" />

  <!-- Use same nodelet used by realsense2 -->
  <group ns="camera">
    <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle realsense2_camera_manager" output="screen">

      <param name="rate" type="double" value="$(arg rate)"/>
      <param name="decimation" type="int" value="$(arg decimation)"/>
      <param name="approx_sync" type="bool" value="$(arg approx_sync)"/>

      <remap from="rgb/image_in"       to="color/image_raw"/>
      <remap from="depth/image_in"     to="aligned_depth_to_color/image_raw"/>
      <remap from="rgb/camera_info_in" to="color/camera_info"/>

      <remap from="rgb/image_out"       to="data_throttled_image"/>
      <remap from="depth/image_out"     to="data_throttled_image_depth"/>
      <remap from="rgb/camera_info_out" to="data_throttled_camera_info"/>

    </node>
  </group>
</launch>

Usage:

$ roslaunch my_realsense2_camera_throttle.launch rate:=5 

$ roslaunch rtabmap_ros rtabmap.launch \
   rgb_topic:=/camera/data_throttled_image\
   depth_topic:=/camera/data_throttled_image_depth\
   camera_info_topic:=/camera/data_throttled_camera_info\
   compressed:=true\
   approx_sync:=false\
   rtabmap_args:="--delete_db_on_start"\
   rviz:=true \
   rtabmapviz:=false

I noticed some problems on my computer with the realsense driver. It does't always initialize correctly (WARNING [140535635199744] (backend-v4l2.cpp:1013) Frames didn't arrived within 5 seconds) and I need to disconnect/connect the USB of the camera. Make sure images are streamed from the camera with "rostopic hz":

$ rostopic hz /camera/color/camera_info /camera/color/image_raw /camera/aligned_depth_to_color/image_raw
subscribed to [/camera/color/camera_info]
subscribed to [/camera/color/image_raw]
subscribed to [/camera/aligned_depth_to_color/image_raw]
                 topic                      rate   min_delta   max_delta    std_dev    window
=============================================================================================
/camera/color/camera_info                  29.77   0.03334     0.03387     0.0001095   30    
/camera/color/image_raw                    29.78   0.0333      0.03387     0.0001147   30    
/camera/aligned_depth_to_color/image_raw   30.02   0.02771     0.04182     0.002807    30

cheers,
Mathieu

[khlau]

First at all, Mathieu, thx for your effort.

I hv tried your .launch and still not work in my setting. I am curious why I don't see any topic for compressed data in the graph? How can I make sure my setup support compressed data, e.g DS415 ??




Mathieu, how can you get rate 30?

$ rostopic hz /camera/color/camera_info /camera/color/image_raw /camera/aligned_depth_to_color/image_raw
                 topic                  rate    min_delta   max_delta   std_dev   window
=============================================================================================
/camera/color/camera_info                  27.45     9.06e-06    0.4837      0.03231   3912  
/camera/color/image_raw                    4.78      0.1612      0.5561      0.02674   3912  
/camera/aligned_depth_to_color/image_raw   0.02533   32.09       46.86       7.387     680   \

Actually, I always this warning on PC but I think they will disappear after the rqt_graph looks correct

[ WARN] [1542682836.496048833]: /rtabmap/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. Parameter "approx_sync" is false, which means that input topics should have all the exact timestamp for the callback to be called.
/rtabmap/rgbd_odometry subscribed to (exact sync):
   /camera/data_throttled_image_relay,
   /camera/data_throttled_image_depth_relay,
   /camera/data_throttled_camera_info
[ WARN] [1542682837.316805918]: /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,
   /camera/data_throttled_image_relay,
   /camera/data_throttled_image_depth_relay,
   /camera/data_throttled_camera_info,
   /rtabmap/odom_info

This warning from rasp pi 3:
[ INFO] [1542680511.283879222]: Start updating dynamic parameters...
 20/11 10:21:51,400 WARNING [1995505824] (types.cpp:57) start_streaming(...) failed. UVC device is already streaming!
 20/11 10:21:51,402 WARNING [1995505824] (types.cpp:57) start_streaming(...) failed. UVC device is already streaming!
 20/11 10:21:51,403 WARNING [1995505824] (types.cpp:57) start_streaming(...) failed. UVC device is already streaming!
 20/11 10:21:51,403 WARNING [1995505824] (types.cpp:57) start_streaming(...) failed. UVC device is already streaming!
[ INFO] [1542680511.477625240]: Done updating dynamic parameters...

[matlabbe]

I tested on my PC, which would explain tbe 30 hz. Make sure you did "rostopic hz" on RPI3 to see if it can actually stream at high frame rate.

Try launching everything en your PC to compare. It may be a hardware limitation, like RPI3 is just not enough powerful. By default, the realsense requires an USB3, it could have some issues with the USB2 of the RPI3.

EDIT: how to use realsense on rpi3b: https://github.com/IntelRealSense/librealsense/blob/master/doc/RaspberryPi3.md

Cheers,
Mathieu