Raytracing using stereo camera for better SLAM and obstacle avoidance.

[stevemartinov]

Hi Mathieu,

I just finished reading your paper:
https://introlab.3it.usherbrooke.ca/mediawiki-introlab/images/7/7a/Labbe18JFR_preprint.pdf

And I will be honest, you gave me an interest in getting my hands dirty in computer vision!

When I was reading your paper, you mentioned that by enabling raytracing, we can improve obstacle avoidance. You have recommended placing the camera close to the ground so the ground can be visible.

I did not understand what raytracing means and thought that you meant by using LiDAR. After reading your paper, I also read about ray tracing which turns out to be a technique in computer vision!

So my questions are:
1. Will raytracing help to detect featureless or dynamic objects if I use a stereo camera?
2. If so, what is the best setting for that in RTABmap? I found that there is a param:  Grid/RayTracing=true, is that enabling 2d ray tracing? How about 3D ray tracing?

I think you know my typical environment and you have seen that I place my camera 3-4 cm above the ground.

BTW, I will reread the paper as I do not fully understand the all parameters but I just wanted to be clear with raytracing technique in RTABmap.

[matlabbe]

Hi,

I'm glad that you find the paper interesting, there were a lot of stuff discussed in it. For your questions:

1. Ray tracing is not used to detect obstacles, but to detect empty space between the camera and the obstacle. If the camera doesn't see the ground and ray tracing is off, you will only see obstacles in the map, no empty space. As ray tracing detect empty space, it can be used to clear dynamic obstacles from the map. For example, there is someone on front of the camera a 1 meter, and obstacle is added 1 meter in front of the robot. If the person move away, without ray tracing the obstacle is still there even if the camera can see the wall 2 meters away in same direction where the person was. With ray tracing, the "obstacle" not there anymore can be cleared.

2. When Grid/RayTracing=true, ray tracing is done in 2d if Grid/3D=false and in 3d if Grid/3D=true. For 3d ray tracing, rtabmap should be built with OctoMap dependency to work. I don't really recommend 3D ray tracing if you have a robot moving only in 2D (very expensive in computation time as shown in Table 10 of the paper), for which is better to use Grid/3D=false with Grid/RayTracing=true.

Note that if your stereo camera cannot see the ground anyway, because it is textureless, no need to put it as close to ground.

Just for some more info, 2D ray tracing in rtabmap is simply tracing a line in a 2d grid (like drawing pixels in paint). 3D ray tracing is done by OctoMap library, they use an OcTree to subdivide the space (similar to a 3D grid) for efficient ray tracing in 3D (similar to when you shoot with a gun in a video game, ray tracing  is done to detect obstacles hit by the bullet).

cheers,
Mathieu

[stevemartinov]

Hi,

Thanks. I place the camera close to the ground purely from the design obstructions.

So for the dynamic obstacle detection, do I need to use rtabmap_ros/obstacles_detection node at all? Or is it better to simply use move_base costmap2D?

Also, which camera is better for such environments? Pure stereo camera with wide FoV (global shutters) or depth IR cameras like ZED and Intel D435? I cannot clearly say which one should I use.

Kind regards,
Steve

[johanehr]

Hi, Mathieu @matlabbe!

RTAB-MAP sure seems to be a great SLAM solution - so far it has been much easier to work with than LeGO-LOAM or ORB-SLAM2, but maybe I'm also getting better at ROS.

A friend and I are working on our Master's thesis together, with the goal of conducting autonomous exploration with a quadcopter. We are using an Intel RealSense D435i RGB-D camera, set to a low resolution of 424 x 240 and a stable 50 fps (when using a fixed exposure time), as well as the integrated IMU. We are launching the whole setup in a modified version of the process described here: https://github.com/intel-ros/realsense/wiki/SLAM-with-D435i , including optimizations from: http://wiki.ros.org/rtabmap_ros/Tutorials/Advanced%20Parameter%20Tuning . We have some problems that may be related to Grid/RayTracing=true...

1. When running RTAB-MAP, we are able to get a decent OctoMap in Rviz (/rtabmap/octomap_full and /rtabmap/octomap_binary) for "occupied voxels", but we can't see anything at all when selecting the choice of seeing only "free voxels". We tried enabling Grid/RayTracing, but this did not seem to help... We have also had instances where the OctoMap does not seem to overlap with the point clouds (as if the loop closures did not get applied to the OctoMap).

2. We occasionally (when re-playing a ROS bag) get problems with the whole map warping (say, a corridor suddenly heading straight down) or "teleporting" to a new location, where a "new" copy of the area is created. Each "area" seems to be accurate enough (for example, the curve in the corridor in the image is not an artifact, and we do perform loop closures quite consistently normally). We're guessing that this is caused by an issue with the odometry, but are at a loss for how to fix it... Note that this is when just walking around with the sensor, before we introduce any vibrations from drone flight. See the image below for an example:


3. We are currently evaluating whether to offload the RTAB-MAP calculations to a companion computer (initially our Intel i7-6600U laptops, but we have a spare i7-3770K desktop that could be brought in) or whether our onboard Aaeon Up Board (Intel Cherry Trail, 4GB RAM) is powerful enough. We seem to be at risk of maxing out the USB2.0 bus, which our USB WiFi dongle is connected to. Maybe getting a USB 3.0 hub and WiFi dongle would be recommended... Any anecdotal guidance would be appreciated!

We'd be very thankful for some guidance!

[matlabbe]

Hi,

1) Which rtabmap version are you using? The free voxels does nothing here too with the ColorOccupancyGrid plugin. They should be in the topic though. The empty space can be also shown with /rtabmap/octomap_empty_space topic for convenience. But yes, Grid/RayTracing should be true to get empty cells. The Octomap published by rtabmap node should overlap the point cloud and be corrected on loop closure. Just tested on Demo Robot Mapping example by changing in the launch the parameter Grid/FromDepth to true. Here the octomap without and with loop closures:


2) Can you share the database with this issue? Maybe there is a wrong loop closure.

3) If you do odometry onboard, you don't need to send at 50 Hz the data out of the drone. RTAB-Map by default updates only at 1 Hz. Throttling the images that should be sent over wifi. There is an example here.

cheers,
Mathieu

[karleno]

Hi, thank you a lot for all the help and the great piece of software that is RTAB-MAP. I am the one working with "johanehr" on the autonomous drone exploration project, and am currently struggling a bit with remote mapping. Our current goal is off-loading the RTAB-MAP calculations to a laptop, and running all camera nodes on an on-board computer (Aaeon Up Board as previously mentioned). The camera we have is an Intel Realsense D435i camera, and I'm trying to follow the remote mapping tutorial found on this link: Remote Mapping. On the Up board, I run the following launch files: 1) "opensource_tracking_SUPERFAST_NO_RTABMAP.launch"

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43

<launch> <arg name="rate" default="5"/> <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="compressed_rate" type="double" value="$(arg rate)"/> <param name="approx_sync" type="bool" value="$(arg approx_sync)"/> <!-- <remap from="color/image_raw" to="rgb/image_rect_color"/> <remap from="depth/image_rect_raw" to="depth_registered/image_raw"/> <remap from="color/camera_info" to="rgb/camera_info"/> --> <remap from="rgb/image_in" to="color/image_raw"/> <remap from="depth/image_in" to="depth/image_rect_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> <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="screen"> <remap from="rgb/image" to="color/image_raw"/> <remap from="depth/image" to="depth/image_rect_raw"/> <remap from="rgb/camera_info" to="color/camera_info"/> <remap from="rgbd_image" to="rgbd_image"/> <param name="approx_sync" value="$(arg approx_sync)"/> </node> </group> </launch>

, and 2) "remotemappingtut.launch":

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43

<launch> <arg name="rate" default="5"/> <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="compressed_rate" type="double" value="$(arg rate)"/> <param name="approx_sync" type="bool" value="$(arg approx_sync)"/> <!-- <remap from="color/image_raw" to="rgb/image_rect_color"/> <remap from="depth/image_rect_raw" to="depth_registered/image_raw"/> <remap from="color/camera_info" to="rgb/camera_info"/> --> <remap from="rgb/image_in" to="color/image_raw"/> <remap from="depth/image_in" to="depth/image_rect_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> <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="screen"> <remap from="rgb/image" to="color/image_raw"/> <remap from="depth/image" to="depth/image_rect_raw"/> <remap from="rgb/camera_info" to="color/camera_info"/> <remap from="rgbd_image" to="rgbd_image"/> <param name="approx_sync" value="$(arg approx_sync)"/> </node> </group> </launch>

. Now the first question is regarding the rgdb_sync nodelet, I don't quite understand whether stating that "approx_sync = false" is enough to make the D435i topics synced or if I need to run the nodelet as well. Also, I get different rqt_graph:s depending on whether I run the rgbd_sync nodelet as a "standalone" or if i "load" it together with the realsense2_camera_manager, as seen on line 32 in "remotemappingtut.launch". First, the graph when running nodelet as standalone: and secondly by loading it, together with realsense2_camera_manager: . The two options yield different connections to the rgbd_sync nodelet, and I wonder which way is the correct one, if any of these? The next question is regarding having odometry on the robot and throttling what's sent out of the robot. I've successfully throttled message with the help from this post: How to use rtabmap_ros/data_throttle for Realsense camera? . The launch file that I run off-board looks like this ("opensource_tracking_only_rtabmap_throttled.launch"):

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30

<launch> <arg name="offline" default="false"/> <include file="$(find rtabmap_ros)/launch/rtabmap.launch"> <arg name="compressed" value="true"/> <arg name="args" value="--delete_db_on_start"/> <arg name="rgb_topic" value="/camera/data_throttled_image"/> <arg name="depth_topic" value="/camera/data_throttled_image_depth"/> <arg name="camera_info_topic" value="/camera/data_throttled_camera_info"/> <arg name="rtabmapviz" value="false"/> <arg name="rviz" value="true"/> <arg name="odom_topic" value="/odometry/filtered"/> <arg name="visual_odometry" value="false"/> <!-- <arg name="approx_sync" value="false"/> <arg name="rgbd_sync" value="true"/> <arg name="approx_rgbd_sync" value="false"/> --> <!-- <remap from="/camera/aligned_depth_to_color/image_raw/compressedDepth" to="/camera/data_throttled_image/compressedDepth" /> <remap from="/camera/color/image_raw/compressed" to="/camera/data_throttled_image/compressed" /> --> </include> <!-- <param name="/ukf_se/odom0_config" value="{true,true,true,}"/> --> </launch>

In this file, odom_topic is set to /odometry/filtered and visual_odometry is false. What I try to achieve is to perform odometry on-board and throttling the messages that are sent over WiFi. However, eventhough when I run the off-board launch file with approx_sync:=false, the resulting output is as follows:

[ WARN] [1553848298.491931635]: /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 "queue_size" parameter (current=10).
/rtabmap/rtabmap subscribed to (approx sync):
   /odometry/filtered,
   /camera/data_throttled_image_relay,
   /camera/data_throttled_image_depth_relay,
   /camera/data_throttled_camera_info

It should be added that the output after launching the on-board files confirm that exact sync is to be used (output of remotemappingtut.launch is

/camera/rgbd_sync subscribed to (exact sync):
   /camera/color/image_raw,
   /camera/depth/image_rect_raw,
   /camera/color/camera_info

) . Running rostopic hz for the relayed topics gives:

topic                      rate   min_delta   max_delta   std_dev   window
===========================================================================================
/camera/data_throttled_image_relay         4.074   0.1446      0.3659      0.05938   25    
/camera/data_throttled_image_depth_relay   4.069   0.1454      0.3932      0.06352   25    
/camera/data_throttled_camera_info         4.16    0.1434      0.3662      0.06035   25

Which suggests that the topics aren't synced. I'm not sure whether this poses a problem, will it matter if the sync is approximate? Any help regarding these matters is appreciated and again, thanks for the great piece of software as well as active support here on the forums. Cheers!

[matlabbe]

Hi,

Based on the remote tutorial, you don't need data_throttle nodelet, use only rgbd_sync like this:

<launch>
  <arg name="rate"  default="5"/>
  <arg name="approx_sync" default="false" />

  <group ns="camera">
    <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/rgbd_sync camera_nodelet_manager" output="screen">
      <param name="compressed_rate"  type="double" value="$(arg rate)"/>
      <param name="approx_sync"      type="bool"   value="$(arg approx_sync)"/>

      <remap from="rgb/image"       to="color/image_raw"/>
      <remap from="depth/image"     to="depth/image_rect_raw"/>
      <remap from="rgb/camera_info" to="color/camera_info"/>

      <remap from="rgbd_image"      to="rgbd_image"/> <!-- output -->
    </node>
  </group>      
</launch>

You can create a standalone nodelet or use it inside the nodelet manager of the camera (recommended to avoid serialization of image topics between camera driver and rgbd_sync). rqt_graph won't look the same if you start it in its own nodelet manager or in another one, but both approaches should work. "approx_sync" should be false here with realsense cameras, images are already synchronized (same stamps).

For the remote launch file, subscribe to rgbd_image, not the image topics:

<launch>
    <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
        <arg name="compressed" value="true"/>
        <arg name="args" value="--delete_db_on_start"/>
        <arg name="subscribe_rgbd" value="true"/>
        <arg name="rgbd_topic" value="/camera/rgbd_image"/>
        <arg name="rtabmapviz" value="false"/>
        <arg name="rviz" value="true"/>

        <arg name="odom_topic" value="/odometry/filtered"/>
        <arg name="visual_odometry" value="false"/>
    </include>
</launch>

EDIT: You would need the latest rtabmap version to make this example works (rtabmap_ros/rgbd_relay is required).

cheers,
Mathieu

[karleno]

Thank you for the answer and help. I have som follow-up questions though:

I have two rtabmap packages at the moment, "rtabmap" and "rtabmap_ros". Does both of them have to be updated? Does it matter if rtabmap is only on version 0.17.6? Because when I run the launch files that you sent in your last reply, with rtabmap_ros version 0.19.1, I get the error:

unused args [subscribe_depth] for include of [.../catkin_ws/src/rtabmap_ros/launch/rtabmap.launch]
The traceback for the exception was written to the log file

In an attempt to set subscribe_depth to false, I've instead tried setting "stereo" to "false" in the remote launch file. This results in the following error output:

[rtabmap/rtabmap-2] process has died [pid 30286, exit code 127, cmd /home/karl/catkin_ws/devel/lib/rtabmap_ros/rtabmap --delete_db_on_start rgb/image:=/camera/rgb/image_rect_color_relay depth/image:=/camera/depth_registered/image_raw_relay rgb/camera_info:=/camera/rgb/camera_info rgbd_image:=/camera/rgbd_image_relay left/image_rect:=/stereo_camera/left/image_rect_color_relay right/image_rect:=/stereo_camera/right/image_rect_relay left/camera_info:=/stereo_camera/left/camera_info right/camera_info:=/stereo_camera/right/camera_info scan:=/scan scan_cloud:=/scan_cloud user_data:=/user_data user_data_async:=/user_data_async gps/fix:=/gps/fix tag_detections:=/tag_detections odom:=/odometry/filtered __name:=rtabmap __log:=/home/karl/.ros/log/9dbabd16-56e7-11e9-830a-40a5ef07349b/rtabmap-rtabmap-2.log].
log file: /home/karl/.ros/log/9dbabd16-56e7-11e9-830a-40a5ef07349b/rtabmap-rtabmap-2*.log
[ERROR] [1554390525.253360074]: Failed to load nodelet [/points_xyzrgb] of type [rtabmap_ros/point_cloud_xyzrgb] even after refreshing the cache: Failed to load library /home/karl/catkin_ws/devel/lib//librtabmap_plugins.so. Make sure that you are calling the PLUGINLIB_EXPORT_CLASS macro in the library code, and that names are consistent between this macro and your XML. Error string: Could not load library (Poco exception = librealsense2.so.2: cannot open shared object file: No such file or directory)
[ERROR] [1554390525.253425511]: The error before refreshing the cache was: Failed to load library /home/karl/catkin_ws/devel/lib//librtabmap_plugins.so. Make sure that you are calling the PLUGINLIB_EXPORT_CLASS macro in the library code, and that names are consistent between this macro and your XML. Error string: Could not load library (Poco exception = librealsense2.so.2: cannot open shared object file: No such file or directory)
[ERROR] [1554390525.471739106]: Failed to load nodelet [/rtabmap/republish_rgbd_image] of type [rtabmap_ros/rgbd_relay] even after refreshing the cache: Failed to load library /home/karl/catkin_ws/devel/lib//librtabmap_plugins.so. Make sure that you are calling the PLUGINLIB_EXPORT_CLASS macro in the library code, and that names are consistent between this macro and your XML. Error string: Could not load library (Poco exception = librealsense2.so.2: cannot open shared object file: No such file or directory)
[ERROR] [1554390525.471796469]: The error before refreshing the cache was: Failed to load library /home/karl/catkin_ws/devel/lib//librtabmap_plugins.so. Make sure that you are calling the PLUGINLIB_EXPORT_CLASS macro in the library code, and that names are consistent between this macro and your XML. Error string: Could not load library (Poco exception = librealsense2.so.2: cannot open shared object file: No such file or directory)
libGL error: failed to create drawable
[points_xyzrgb-4] process has died [pid 30288, exit code 255, cmd /opt/ros/kinetic/lib/nodelet/nodelet standalone rtabmap_ros/point_cloud_xyzrgb left/image:=/stereo_camera/left/image_rect_color_relay right/image:=/stereo_camera/right/image_rect_relay left/camera_info:=/stereo_camera/left/camera_info right/camera_info:=/stereo_camera/right/camera_info rgbd_image:=/camera/rgbd_image_relay cloud:=voxel_cloud __name:=points_xyzrgb __log:=/home/karl/.ros/log/9dbabd16-56e7-11e9-830a-40a5ef07349b/points_xyzrgb-4.log].
log file: /home/karl/.ros/log/9dbabd16-56e7-11e9-830a-40a5ef07349b/points_xyzrgb-4*.log
[ERROR] [1554390526.211364045]: PluginlibFactory: The plugin for class 'rtabmap_ros/MapCloud' failed to load.  Error: Failed to load library /home/karl/catkin_ws/devel/lib//librtabmap_plugins.so. Make sure that you are calling the PLUGINLIB_EXPORT_CLASS macro in the library code, and that names are consistent between this macro and your XML. Error string: Could not load library (Poco exception = librealsense2.so.2: cannot open shared object file: No such file or directory)
[ERROR] [1554390526.217853372]: PluginlibFactory: The plugin for class 'rtabmap_ros/Info' failed to load.  Error: Failed to load library /home/karl/catkin_ws/devel/lib//librtabmap_plugins.so. Make sure that you are calling the PLUGINLIB_EXPORT_CLASS macro in the library code, and that names are consistent between this macro and your XML. Error string: Could not load library (Poco exception = librealsense2.so.2: cannot open shared object file: No such file or directory)

Also, in the remote launch file you sent, on line 7, is it a typo that "rgb_topic" is set, should it be "rgbd_topic"? Considering that the correspoding value is "/camera/rgbd_image"...

On-board launch file (now with nodelet embedded in main launch):

<launch>
  <arg name="rate"  default="5"/>
  <arg name="approx_sync" default="false" />

  <include file="$(find realsense2_camera)/launch/rs_camera.launch">
      <arg name="align_depth" value="True"/>
      <arg name="linear_accel_cov" value="1.0"/>
      <arg name="unite_imu_method" value="linear_interpolation"/>
    	<arg name="color_width"         value="424"/>
    	<arg name="color_height"        value="240"/>
    	<arg name="infra_width"        value="424"/>
    	<arg name="infra_height"       value="240"/>
    	<arg name="depth_width"         value="424"/>
    	<arg name="depth_height"        value="240"/>
      <arg name="fisheye_width"       value="424"/>
   	  <arg name="fisheye_height"      value="240"/>

  	  <arg name="fisheye_fps"  value="60"/>
      <arg name="depth_fps"    value="60"/>
      <arg name="infra_fps"    value="60"/>
      <arg name="color_fps"    value="60"/>

  </include>

  <group ns="camera">
    <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/rgbd_sync camera_nodelet_manager" output="screen">
      <param name="compressed_rate"  type="double" value="$(arg rate)"/>
      <param name="approx_sync"      type="bool"   value="$(arg approx_sync)"/>

      <remap from="rgb/image"       to="color/image_raw"/>
      <remap from="depth/image"     to="depth/image_rect_raw"/>
      <remap from="rgb/camera_info" to="color/camera_info"/>

      <remap from="rgbd_image"      to="rgbd_image"/> <!-- output -->
    </node>
  </group>



  <node pkg="imu_filter_madgwick" type="imu_filter_node" name="ImuFilter">
      <param name="_use_ma" 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"/>
  </node>

    <!-- check type of params (might not actually be string but double...)-->
    <!--
    <node name="$(anon dynparam)" pkg="dynamic_reconfigure" type="dynparam"   args="set_from_parameters /camera/RGB_Camera">
      <param name="Enable_Auto_Exposure"    type="bool" value="false" />
      <param name="Exposure"                type="int"  value="30" />
      <param name="Auto_Exposure_Priority"  type="bool" value="true"/>
      <param name="Frames_Queue_Size"       type="int"  value="50" />
    </node>
-->

    <include file="$(find robot_localization)/launch/ukf_template.launch"/>
    <param name="/ukf_se/frequency" value="300"/>
    <param name="/ukf_se/base_link_frame" value="camera_link"/>
    <param name="/ukf_se/odom0" value="rtabmap/odom"/>
    <rosparam param="/ukf_se/odom0_config">[true,true,true,
                                            true,true,true,
                                            true,true,true,
                                            true,true,true,
                                            true,true,true]
    </rosparam>
    <param name="/ukf_se/odom0_relative" value="true"/>
    <param name="/ukf_se/odom0_pose_rejection_threshold" value="10000000"/>
    <param name="/ukf_se/odom0_twist_rejection_threshold" value="10000000"/>

    <param name="/ukf_se/imu0" value="/imu/data"/>
    <rosparam param="/ukf_se/imu0_config">[false, false, false,
                                           true,  true,  true,
                                           true,  true,  true,
                                           true,  true,  true,
                                           true,  true,  true]
    </rosparam>
    <param name="/ukf_se/imu0_differential" value="true"/>
    <param name="/ukf_se/imu0_relative" value="false"/>
    <param name="/ukf_se/use_control" value="false"/>
    <!-- <param name="/ukf_se/odom0_config" value="{true,true,true,}"/> -->


</launch>

And the remote launch file:

<launch>
    <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
        <arg name="compressed" value="true"/>
        <arg name="args" value="--delete_db_on_start"/>
      <!-- <arg name="subscribe_depth" value="false"/> -->

       <arg name="stereo" value="true"/>

        <arg name="subscribe_rgbd" value="true"/>
        <arg name="rgbd_topic" value="/camera/rgbd_image"/> <!-- rgb changed to rgbd -->
        <arg name="rtabmapviz" value="false"/>
        <arg name="rviz" value="true"/>

        <arg name="odom_topic" value="/odometry/filtered"/>
        <arg name="visual_odometry" value="false"/>
    </include>
</launch>

Thank you for any help and guidance!

[matlabbe]

Hi,

Indeed, subscribe_depth doesn't exist. I removed it from the previous post. Setting "stereo" or not doesn't affect the launch, as subscribe_rgbd has priority.

The error is:

Poco exception = librealsense2.so.2: cannot open shared object file: No such file or directory

For some reasons the linker is not able to find librealsense2.so.2, where is it installed? You may add its path to LD_LIBRARY_PATH.

Note that I don't recommend to mix different rtabmap and rtabmap_ros versions. To use latest from source, uninstall the rtabmap and rtabmap_ros binaries, then follow step 2 and 3 of this section.

For subscribe_rgb, I don't find it in the prrevious post, but it should be a typo, it should be subscribe_rgbd.

cheers,
Mathieu

[karleno]

Okay, once again thank you a lot for the assistance. I managed to fix the librealsense2.so.2 problem by exporting its path with LD_LIBRARY_PATH as you said. Regarding the typo I mentioned earlier, what I meant was in the topics you declared in the remote launch file. You wrote:

        <arg name="rgb_topic" value="/camera/rgbd_image"/>

and my question was whether it should be rgb_topic that is set, or if it really should be rgbd_topic, since the value that is related to it is /camera/rgbd_image. I just found it a bit confusing...

Now, when I run this launch file on board the robot:

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85

<launch> <include file="$(find realsense2_camera)/launch/rs_camera.launch"> <arg name="align_depth" value="True"/> <arg name="linear_accel_cov" value="1.0"/> <arg name="unite_imu_method" value="linear_interpolation"/> <arg name="color_width" value="424"/> <arg name="color_height" value="240"/> <arg name="infra_width" value="424"/> <arg name="infra_height" value="240"/> <arg name="depth_width" value="424"/> <arg name="depth_height" value="240"/> <arg name="fisheye_width" value="424"/> <arg name="fisheye_height" value="240"/> <arg name="fisheye_fps" value="30"/> <arg name="depth_fps" value="30"/> <arg name="infra_fps" value="30"/> <arg name="color_fps" value="30"/> </include> <arg name="rate" default="5"/> <arg name="approx_sync" default="false" /> <group ns="camera"> <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/rgbd_sync camera_nodelet_manager" output="screen"> <param name="compressed_rate" type="double" value="$(arg rate)"/> <param name="approx_sync" type="bool" value="$(arg approx_sync)"/> <remap from="rgb/image" to="color/image_raw"/> <remap from="depth/image" to="depth/image_rect_raw"/> <remap from="rgb/camera_info" to="color/camera_info"/> <remap from="rgbd_image" to="rgbd_image"/> <!-- output --> </node> </group> <node pkg="imu_filter_madgwick" type="imu_filter_node" name="ImuFilter"> <param name="_use_ma" 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"/> </node> <!-- <node name="$(anon dynparam)" pkg="dynamic_reconfigure" type="dynparam" args="set_from_parameters /camera/RGB_Camera"> <param name="Enable_Auto_Exposure" type="bool" value="false" /> <param name="Exposure" type="int" value="30" /> <param name="Auto_Exposure_Priority" type="bool" value="true"/> <param name="Frames_Queue_Size" type="int" value="50" /> </node> --> <include file="$(find robot_localization)/launch/ukf_template.launch"/> <param name="/ukf_se/frequency" value="300"/> <param name="/ukf_se/base_link_frame" value="camera_link"/> <param name="/ukf_se/odom0" value="rtabmap/odom"/> <rosparam param="/ukf_se/odom0_config">[true,true,true, true,true,true, true,true,true, true,true,true, true,true,true] </rosparam> <param name="/ukf_se/odom0_relative" value="true"/> <param name="/ukf_se/odom0_pose_rejection_threshold" value="10000000"/> <param name="/ukf_se/odom0_twist_rejection_threshold" value="10000000"/> <param name="/ukf_se/imu0" value="/imu/data"/> <rosparam param="/ukf_se/imu0_config">[false, false, false, true, true, true, true, true, true, true, true, true, true, true, true] </rosparam> <param name="/ukf_se/imu0_differential" value="true"/> <param name="/ukf_se/imu0_relative" value="false"/> <param name="/ukf_se/use_control" value="false"/> <!-- <param name="/ukf_se/odom0_config" value="{true,true,true,}"/> --> </launch>

and this launch file on the remote computer:

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20

<launch> <include file="$(find rtabmap_ros)/launch/rtabmap.launch"> <arg name="compressed" value="true"/> <arg name="args" value="--delete_db_on_start"/> <!-- <arg name="stereo" value="false"/> --> <arg name="subscribe_rgbd" value="true"/> <arg name="rgbd_topic" value="/camera/rgbd_image"/> <!-- rgb changed to rgbd --> <arg name="rtabmapviz" value="false"/> <arg name="rviz" value="true"/> <arg name="odom_topic" value="/odometry/filtered"/> <arg name="visual_odometry" value="false"/> <arg name="queue_size" value="1000"/> <arg name="approx_sync" value="false"/> <!-- test --> </include> </launch>

I get the following output from the remote launch file:

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91

started roslaunch server http://10.90.141.197:42241/ SUMMARY ======== PARAMETERS * /points_xyzrgb/approx_sync: False * /points_xyzrgb/decimation: 4.0 * /points_xyzrgb/voxel_size: 0.0 * /rosdistro: kinetic * /rosversion: 1.12.14 * /rtabmap/republish_rgbd_image/uncompress: True * /rtabmap/rtabmap/Mem/IncrementalMemory: true * /rtabmap/rtabmap/Mem/InitWMWithAllNodes: false * /rtabmap/rtabmap/approx_sync: False * /rtabmap/rtabmap/config_path: * /rtabmap/rtabmap/database_path: ~/.ros/rtabmap.db * /rtabmap/rtabmap/frame_id: camera_link * /rtabmap/rtabmap/ground_truth_base_frame_id: * /rtabmap/rtabmap/ground_truth_frame_id: * /rtabmap/rtabmap/landmark_angular_variance: 9999.0 * /rtabmap/rtabmap/landmark_linear_variance: 0.0001 * /rtabmap/rtabmap/map_frame_id: map * /rtabmap/rtabmap/odom_frame_id: * /rtabmap/rtabmap/odom_sensor_sync: False * /rtabmap/rtabmap/odom_tf_angular_variance: 1.0 * /rtabmap/rtabmap/odom_tf_linear_variance: 1.0 * /rtabmap/rtabmap/publish_tf: True * /rtabmap/rtabmap/queue_size: 1000 * /rtabmap/rtabmap/scan_normal_k: 0 * /rtabmap/rtabmap/subscribe_depth: True * /rtabmap/rtabmap/subscribe_rgbd: True * /rtabmap/rtabmap/subscribe_scan: False * /rtabmap/rtabmap/subscribe_scan_cloud: False * /rtabmap/rtabmap/subscribe_stereo: False * /rtabmap/rtabmap/subscribe_user_data: False * /rtabmap/rtabmap/wait_for_transform_duration: 0.2 NODES /rtabmap/ republish_rgbd_image (rtabmap_ros/rgbd_relay) rtabmap (rtabmap_ros/rtabmap) / points_xyzrgb (nodelet/nodelet) rviz (rviz/rviz) ROS_MASTER_URI=http://aaeon-up.local:11311/ process[rtabmap/republish_rgbd_image-1]: started with pid [10279] process[rtabmap/rtabmap-2]: started with pid [10280] process[rviz-3]: started with pid [10281] process[points_xyzrgb-4]: started with pid [10282] type is rtabmap_ros/point_cloud_xyzrgb [ INFO] [1554993246.936455236]: Starting node... [ INFO] [1554993247.064551895]: Initializing nodelet with 4 worker threads. [ INFO] [1554993247.773640784]: /rtabmap/rtabmap(maps): map_filter_radius = 0.000000 [ INFO] [1554993247.773706052]: /rtabmap/rtabmap(maps): map_filter_angle = 30.000000 [ INFO] [1554993247.773735824]: /rtabmap/rtabmap(maps): map_cleanup = true [ INFO] [1554993247.773759218]: /rtabmap/rtabmap(maps): map_always_update = false [ INFO] [1554993247.773780474]: /rtabmap/rtabmap(maps): map_empty_ray_tracing = true [ INFO] [1554993247.773798463]: /rtabmap/rtabmap(maps): cloud_output_voxelized = true [ INFO] [1554993247.773816131]: /rtabmap/rtabmap(maps): cloud_subtract_filtering = false [ INFO] [1554993247.773836110]: /rtabmap/rtabmap(maps): cloud_subtract_filtering_min_neighbors = 2 [ INFO] [1554993247.792051139]: /rtabmap/rtabmap(maps): octomap_tree_depth = 16 [ INFO] [1554993248.064722506]: Approximate time sync = false [ INFO] [1554993248.936549633]: rtabmap: frame_id = camera_link [ INFO] [1554993248.936606636]: rtabmap: map_frame_id = map [ INFO] [1554993248.936649969]: rtabmap: use_action_for_goal = false [ INFO] [1554993248.936691488]: rtabmap: tf_delay = 0.050000 [ INFO] [1554993248.936717004]: rtabmap: tf_tolerance = 0.100000 [ INFO] [1554993248.936740996]: rtabmap: odom_sensor_sync = false [ INFO] [1554993256.334602678]: Setting RTAB-Map parameter "Mem/IncrementalMemory"="true" [ INFO] [1554993256.344516490]: Setting RTAB-Map parameter "Mem/InitWMWithAllNodes"="false" [ INFO] [1554993276.796578777]: RTAB-Map detection rate = 1.000000 Hz [ INFO] [1554993276.797298050]: rtabmap: Deleted database "/home/karl/.ros/rtabmap.db" (--delete_db_on_start or -d are set). [ INFO] [1554993276.797913144]: rtabmap: Using database from "/home/karl/.ros/rtabmap.db" (0 MB). [ INFO] [1554993276.964858652]: rtabmap: Database version = "0.19.1". [ WARN] [1554993277.577076717]: rtabmap: Parameters subscribe_depth and subscribe_rgbd cannot be true at the same time. Parameter subscribe_depth is set to false. [ INFO] [1554993277.685600572]: /rtabmap/rtabmap: queue_size = 1000 [ INFO] [1554993277.685699014]: /rtabmap/rtabmap: rgbd_cameras = 1 [ INFO] [1554993277.685778739]: /rtabmap/rtabmap: approx_sync = false [ INFO] [1554993277.685927546]: Setup rgbd callback [ INFO] [1554993277.845020333]: /rtabmap/rtabmap subscribed to (exact sync): /odometry/filtered, /camera/rgbd_image_relay [ INFO] [1554993278.056668174]: rtabmap 0.19.1 started... [ WARN] [1554993282.845268805]: /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): /odometry/filtered, /camera/rgbd_image_relay

And when I run "rostopic hz /odometry/filtered /camera/rgbd_image_relay", there are no messages sent which would explain the output from the remote launch file, pasted above. In addition, the resulting rqt_graph looks like this:



Now I'm quite new at ROS and I can't quite get this remote mapping to work properly with the rgbd_sync nodelet. Do you have any idea how to fix the problem, how to publish /odometry/filtered and /camera/rgbd_image_relay correctly? I suspect the problem lies in the usage of the rgbd_sync nodelet but I can't get it to function at the moment...

Thank you in advance,

Karleno

[stevemartinov]

Can you actually try to resolve your issue first before asking the next day after?

[matlabbe]

rgb_topic is a typo, I changed to rgbd_topic.

Can you verify that you can receive /camera/rgbd_image on remote computer?

$ rostopic hz /camera/rgbd_image

If not verify onboard. Then if again not, there is a problem with rgbd_sync onboard.