search

introlab / rtabmap_ros

RTAB-Map's ROS package.
http://wiki.ros.org/rtabmap_ros
BSD 3-Clause "New" or "Revised" License
958 stars 558 forks source link

Rtabmap does not receive data #616

Closed GiuliaB97 closed 3 years ago

GiuliaB97 commented 3 years ago

Behavior Launching a simulation which has a zed2 stereocamera I am able to publish both the odom topic (/gazebo/zed2/odom) and the rectified left and right image as well as left and right camera_info however i get the following error:

ruggio@Hologram:~$ roslaunch rtabmap_ros rtabmap.launch 
... logging to /home/ruggio/.ros/log/6669b6b8-e9fd-11eb-8619-087190d77481/roslaunch-Hologram-4073.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://Hologram:39941/

SUMMARY
========

PARAMETERS
 * /rosdistro: melodic
 * /rosversion: 1.14.11
 * /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: base_link
 * /rtabmap/rtabmap/gen_depth: False
 * /rtabmap/rtabmap/gen_depth_decimation: 1
 * /rtabmap/rtabmap/gen_depth_fill_holes_error: 0.1
 * /rtabmap/rtabmap/gen_depth_fill_holes_size: 0
 * /rtabmap/rtabmap/gen_depth_fill_iterations: 1
 * /rtabmap/rtabmap/gen_scan: False
 * /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_frame_id_init: 
 * /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: 100
 * /rtabmap/rtabmap/scan_cloud_max_points: 0
 * /rtabmap/rtabmap/subscribe_depth: False
 * /rtabmap/rtabmap/subscribe_rgb: False
 * /rtabmap/rtabmap/subscribe_rgbd: False
 * /rtabmap/rtabmap/subscribe_scan: False
 * /rtabmap/rtabmap/subscribe_scan_cloud: False
 * /rtabmap/rtabmap/subscribe_scan_descriptor: False
 * /rtabmap/rtabmap/subscribe_stereo: True
 * /rtabmap/rtabmap/subscribe_user_data: False
 * /rtabmap/rtabmap/wait_for_transform_duration: 0.2

NODES
  /rtabmap/
    rtabmap (rtabmap_ros/rtabmap)

ROS_MASTER_URI=http://localhost:11311

process[rtabmap/rtabmap-1]: started with pid [4089]
[ INFO] [1626856536.944783881]: Starting node...
[ INFO] [1626856536.969043590]: Initializing nodelet with 12 worker threads.
[ INFO] [1626856537.067242528]: /rtabmap/rtabmap(maps): map_filter_radius          = 0.000000
[ INFO] [1626856537.067281875]: /rtabmap/rtabmap(maps): map_filter_angle           = 30.000000
[ INFO] [1626856537.067299774]: /rtabmap/rtabmap(maps): map_cleanup                = true
[ INFO] [1626856537.067315873]: /rtabmap/rtabmap(maps): map_always_update          = false
[ INFO] [1626856537.067331392]: /rtabmap/rtabmap(maps): map_empty_ray_tracing      = true
[ INFO] [1626856537.067348361]: /rtabmap/rtabmap(maps): cloud_output_voxelized     = true
[ INFO] [1626856537.067364320]: /rtabmap/rtabmap(maps): cloud_subtract_filtering   = false
[ INFO] [1626856537.067380319]: /rtabmap/rtabmap(maps): cloud_subtract_filtering_min_neighbors = 2
[ INFO] [1626856537.067663321]: /rtabmap/rtabmap(maps): octomap_tree_depth         = 16
[ INFO] [1626856537.091156060]: rtabmap: frame_id      = base_link
[ INFO] [1626856537.091204327]: rtabmap: map_frame_id  = map
[ INFO] [1626856537.091233525]: rtabmap: use_action_for_goal  = false
[ INFO] [1626856537.091260853]: rtabmap: tf_delay      = 0.050000
[ INFO] [1626856537.091285991]: rtabmap: tf_tolerance  = 0.100000
[ INFO] [1626856537.091308250]: rtabmap: odom_sensor_sync   = false
[ INFO] [1626856537.091839246]: rtabmap: stereo_to_depth = false
[ INFO] [1626856537.091860404]: rtabmap: gen_scan  = false
[ INFO] [1626856537.091877593]: rtabmap: gen_depth  = false
[ INFO] [1626856537.276169457, 336.525000000]: Setting RTAB-Map parameter "Mem/IncrementalMemory"="true"
[ INFO] [1626856537.276573291, 336.525000000]: Setting RTAB-Map parameter "Mem/InitWMWithAllNodes"="false"
[ INFO] [1626856537.646616403, 336.751000000]: Update RTAB-Map parameter "Kp/MaxFeatures"="750" from database
[ INFO] [1626856537.646734066, 336.751000000]: Update RTAB-Map parameter "Kp/RoiRatios"="0.03 0.03 0.04 0.04" from database
[ INFO] [1626856537.646846508, 336.751000000]: Update RTAB-Map parameter "RGBD/OptimizeMaxError"="0" from database
[ INFO] [1626856537.647295600, 336.752000000]: RTAB-Map detection rate = 1.000000 Hz
[ INFO] [1626856537.647395253, 336.752000000]: rtabmap: Using database from "/home/ruggio/.ros/rtabmap.db" (0 MB).
[ INFO] [1626856537.692209523, 336.782000000]: /rtabmap/rtabmap: subscribe_depth = false
[ INFO] [1626856537.692297487, 336.783000000]: /rtabmap/rtabmap: subscribe_rgb = false
[ INFO] [1626856537.692346924, 336.783000000]: /rtabmap/rtabmap: subscribe_stereo = true
[ INFO] [1626856537.692404510, 336.783000000]: /rtabmap/rtabmap: subscribe_rgbd = false (rgbd_cameras=1)
[ INFO] [1626856537.692466646, 336.783000000]: /rtabmap/rtabmap: subscribe_odom_info = false
[ INFO] [1626856537.692526502, 336.783000000]: /rtabmap/rtabmap: subscribe_user_data = false
[ INFO] [1626856537.692587588, 336.783000000]: /rtabmap/rtabmap: subscribe_scan = false
[ INFO] [1626856537.692646755, 336.783000000]: /rtabmap/rtabmap: subscribe_scan_cloud = false
[ INFO] [1626856537.692711010, 336.783000000]: /rtabmap/rtabmap: subscribe_scan_descriptor = false
[ INFO] [1626856537.692767997, 336.783000000]: /rtabmap/rtabmap: queue_size    = 100
[ INFO] [1626856537.692823953, 336.783000000]: /rtabmap/rtabmap: approx_sync   = false
[ INFO] [1626856537.692963244, 336.783000000]: Setup stereo callback
[ INFO] [1626856537.717592559, 336.799000000]: 
/rtabmap/rtabmap subscribed to (exact sync):
   /gazebo/zed2/odom \
   /zed2/left/image_rect_color \
   /zed2/right/image_rect_color \
   /zed2/left/camera_info \
   /zed2/right/camera_info
[ INFO] [1626856537.857088442, 336.880000000]: rtabmap 0.20.13 started...
[ WARN] [1626856545.443727439, 341.799000000]: /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):
   /gazebo/zed2/odom \
   /zed2/left/image_rect_color \
   /zed2/right/image_rect_color \
   /zed2/left/camera_info \
   /zed2/right/camera_info
^C[rtabmap/rtabmap-1] killing on exit
rtabmap: Saving database/long-term memory... (located at /home/ruggio/.ros/rtabmap.db)
rtabmap: Saving database/long-term memory...done! (located at /home/ruggio/.ros/rtabmap.db, 0 MB)
shutting down processing monitor...
... shutting down processing monitor complete
done

I have also tried to check if there where connections issues by launching: rqt_graph Output of rqt_graph

and rostopic hzshows:

ruggio@Hologram:~$ rostopic hz /gazebo/zed2/odom /zed2/left/image_rect_color /zed2/right/image_rect_color /zed2/left/camera_info /zed2/right/camera_info
subscribed to [/gazebo/zed2/odom]
subscribed to [/zed2/left/image_rect_color]
subscribed to [/zed2/right/image_rect_color]
subscribed to [/zed2/left/camera_info]
subscribed to [/zed2/right/camera_info]
WARNING: may be using simulated time
           topic                rate   min_delta   max_delta   std_dev    window
================================================================================
/gazebo/zed2/odom              14.98   0.057       0.075       0.005692   10    
/zed2/left/image_rect_color    15.41   0.051       0.072       0.005467   10    
/zed2/right/image_rect_color   16.34   0.009       0.072       0.01757    11    
/zed2/left/camera_info         15.03   0.062       0.073       0.003166   11    
/zed2/right/camera_info        14.9    0.064       0.072       0.00247    11    

           topic                rate   min_delta   max_delta   std_dev    window
================================================================================
/gazebo/zed2/odom              14.94   0.055       0.078       0.006597   20    
/zed2/left/image_rect_color    15.16   0.051       0.074       0.005114   20    
/zed2/right/image_rect_color   15.64   0.009       0.072       0.01284    20    
/zed2/left/camera_info         14.99   0.06        0.073       0.003649   20    
/zed2/right/camera_info        14.96   0.06        0.072       0.0027     21    

           topic                rate   min_delta   max_delta   std_dev    window
================================================================================
/gazebo/zed2/odom              15.01   0.055       0.078       0.006073   29    
/zed2/left/image_rect_color    15.12   0.051       0.074       0.004289   29    
/zed2/right/image_rect_color   15.49   0.009       0.072       0.01075    30    
/zed2/left/camera_info         15.0    0.06        0.073       0.002965   30    
/zed2/right/camera_info        15.02   0.06        0.072       0.002513   30    

           topic                rate   min_delta   max_delta   std_dev    window
================================================================================
/gazebo/zed2/odom              14.97   0.055       0.079       0.006118   39    
/zed2/left/image_rect_color    15.1    0.051       0.074       0.004143   39    
/zed2/right/image_rect_color   15.33   0.009       0.073       0.009469   39    
/zed2/left/camera_info         15.02   0.06        0.073       0.002917   39    
/zed2/right/camera_info        14.99   0.06        0.072       0.002655   40    

           topic                rate   min_delta   max_delta   std_dev    window
================================================================================
/gazebo/zed2/odom              15.02   0.055       0.079       0.006191   49    
/zed2/left/image_rect_color    15.09   0.051       0.074       0.004066   49    
/zed2/right/image_rect_color   15.26   0.009       0.073       0.008555   49    
/zed2/left/camera_info         15.02   0.059       0.073       0.002987   49    
/zed2/right/camera_info        14.99   0.06        0.072       0.002612   50    

           topic                rate   min_delta   max_delta   std_dev    window
================================================================================
/gazebo/zed2/odom              14.99   0.055       0.079       0.006017   58    
/zed2/left/image_rect_color    15.06   0.051       0.074       0.003778   58    
/zed2/right/image_rect_color   15.22   0.009       0.073       0.007892   59    
/zed2/left/camera_info         15.0    0.059       0.073       0.002773   59    
/zed2/right/camera_info        14.99   0.06        0.072       0.002448   59    

           topic                rate   min_delta   max_delta   std_dev    window
================================================================================
/gazebo/zed2/odom              14.99   0.055       0.079       0.00562    68    
/zed2/left/image_rect_color    15.06   0.051       0.074       0.003545   68    
/zed2/right/image_rect_color   15.19   0.009       0.073       0.007344   69    
/zed2/left/camera_info         15.01   0.059       0.073       0.002596   69    
/zed2/right/camera_info        15.0    0.06        0.072       0.002395   69    

           topic                rate   min_delta   max_delta   std_dev    window
================================================================================
/gazebo/zed2/odom              15.0    0.055       0.079       0.005388   78    
/zed2/left/image_rect_color    15.04   0.051       0.074       0.00341    78    
/zed2/right/image_rect_color   15.18   0.009       0.073       0.006985   78    
/zed2/left/camera_info         15.0    0.059       0.073       0.002506   78    
/zed2/right/camera_info        15.01   0.059       0.073       0.002593   79    

^C           topic                rate   min_delta   max_delta   std_dev    window
================================================================================
/gazebo/zed2/odom              14.98   0.055       0.079       0.005367   80    
/zed2/left/image_rect_color    15.05   0.051       0.074       0.00335    80    
/zed2/right/image_rect_color   15.15   0.009       0.074       0.006929   81    
/zed2/left/camera_info         15.0    0.059       0.073       0.002486   81    
/zed2/right/camera_info        14.99   0.059       0.074       0.002708   82

however at a first glance everything should be working fine.

This is the launch file I'm running:

<!-- -->
<launch>
  <!-- Convenience launch file to launch odometry, rtabmap and rtabmapviz nodes at once -->

  <!-- For stereo:=false
        Your RGB-D sensor should be already started with "depth_registration:=true".
        Examples:
           $ roslaunch freenect_launch freenect.launch depth_registration:=true 
           $ roslaunch openni2_launch openni2.launch depth_registration:=true -->

  <!-- For stereo:=true
        Your camera should be calibrated and publishing rectified left and right 
        images + corresponding camera_info msgs. You can use stereo_image_proc for image rectification.
        Example:
           $ roslaunch rtabmap_ros bumblebee.launch -->

  <!-- Choose between depth and stereo, set both to false to do only scan -->      
  <arg name="stereo"                    default="true"/>
  <arg     if="$(arg stereo)" name="depth"  default="false"/>
  <arg unless="$(arg stereo)" name="depth"  default="true"/>
  <arg name="subscribe_rgb"  default="$(arg depth)"/>

  <!-- Choose visualization -->
  <arg name="rtabmapviz"              default="false" /> 
  <arg name="rviz"                    default="false" />

  <!-- Localization-only mode -->
  <arg name="localization"            default="false"/>

  <!-- sim time for convenience, if playing a rosbag -->
  <arg name="use_sim_time"            default="false"/>
  <param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>

  <!-- Corresponding config files -->
  <arg name="cfg"                     default="" /> <!-- To change RTAB-Map's parameters, set the path of config file (*.ini) generated by the standalone app -->
  <arg name="gui_cfg"                 default="~/.ros/rtabmap_gui.ini" />
  <arg name="rviz_cfg"                default="$(find rtabmap_ros)/launch/config/rgbd.rviz" />

  <arg name="frame_id"                default="base_link"/>     <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
  <arg name="odom_frame_id"           default=""/>                <!-- If set, TF is used to get odometry instead of the topic -->
  <arg name="odom_frame_id_init"      default=""/>                <!-- If set, TF map->odom is published even if no odometry topic has been received yet. The frame id should match the one in the topic. -->
  <arg name="map_frame_id"            default="map"/>
  <arg name="ground_truth_frame_id"   default=""/>     <!-- e.g., "world" -->
  <arg name="ground_truth_base_frame_id" default=""/>  <!-- e.g., "tracker", a fake frame matching the frame "frame_id" (but on different TF tree) -->
  <arg name="namespace"               default="rtabmap"/>
  <arg name="database_path"           default="~/.ros/rtabmap.db"/>
  <arg name="queue_size"              default="100"/>
  <arg name="wait_for_transform"      default="0.2"/>
  <arg name="args"                    default=""/>              <!-- delete_db_on_start, udebug -->
  <arg name="rtabmap_args"            default="$(arg args)"/>   <!-- deprecated, use "args" argument -->
  <arg name="gdb"                     default="false"/>         <!-- Launch nodes in gdb for debugging (apt install xterm gdb) -->
  <arg     if="$(arg gdb)" name="launch_prefix" default="xterm -e gdb -q -ex run --args"/>
  <arg unless="$(arg gdb)" name="launch_prefix" default=""/>
  <arg name="output"                  default="screen"/>        <!-- Control node output (screen or log) -->
  <arg name="publish_tf_map"          default="true"/>

  <!-- if timestamps of the input topics are synchronized using approximate or exact time policy-->
  <arg     if="$(arg stereo)" name="approx_sync"  default="false"/>
  <arg unless="$(arg stereo)" name="approx_sync"  default="$(arg depth)"/>         

  <!-- RGB-D related topics -->
  <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
  <arg name="depth_topic"             default="/camera/depth_registered/image_raw" />
  <arg name="camera_info_topic"       default="/camera/rgb/camera_info" />
  <arg name="depth_camera_info_topic" default="$(arg camera_info_topic)" />

  <!-- stereo related topics -->
  <arg name="stereo_namespace"        default="/zed2"/>
  <arg name="left_image_topic"        default="$(arg stereo_namespace)/left/image_rect_color" />
  <arg name="right_image_topic"       default="$(arg stereo_namespace)/right/image_rect_color" />      <!-- using grayscale image for efficiency -->
  <arg name="left_camera_info_topic"  default="$(arg stereo_namespace)/left/camera_info" />
  <arg name="right_camera_info_topic" default="$(arg stereo_namespace)/right/camera_info" />

  <!-- Already synchronized RGB-D related topic, with rtabmap_ros/rgbd_sync nodelet -->
  <arg name="rgbd_sync"               default="false"/>         <!-- pre-sync rgb_topic, depth_topic, camera_info_topic -->
  <arg name="approx_rgbd_sync"        default="true"/>          <!-- false=exact synchronization -->
  <arg name="subscribe_rgbd"          default="$(arg rgbd_sync)"/>
  <arg name="rgbd_topic"              default="rgbd_image" />
  <arg name="depth_scale"             default="1.0" />         <!-- Deprecated, use rgbd_depth_scale instead -->
  <arg name="rgbd_depth_scale"        default="$(arg depth_scale)" />
  <arg name="rgbd_decimation"         default="1" />

  <arg name="compressed"              default="false"/>         <!-- If you want to subscribe to compressed image topics -->
  <arg name="rgb_image_transport"     default="compressed"/>    <!-- Common types: compressed, theora (see "rosrun image_transport list_transports") -->
  <arg name="depth_image_transport"   default="compressedDepth"/>  <!-- Depth compatible types: compressedDepth (see "rosrun image_transport list_transports") -->

  <arg name="subscribe_scan"          default="false"/>
  <arg name="scan_topic"              default="/scan"/>
  <arg name="subscribe_scan_cloud"    default="false"/>
  <arg name="scan_cloud_topic"        default="/scan_cloud"/>
  <arg name="subscribe_scan_descriptor" default="false"/>
  <arg name="scan_descriptor_topic"   default="/scan_descriptor"/>
  <arg name="scan_cloud_max_points"   default="0"/>
  <arg name="scan_cloud_filtered"     default="false"/> <!-- use filtered cloud from icp_odometry for mapping -->
  <arg name="gen_scan"                default="false"/> <!-- only works with depth image and if not subscribing to scan topic-->

  <arg name="gen_depth"                  default="false" /> <!-- Generate depth image from scan_cloud -->
  <arg name="gen_depth_decimation"       default="1" />
  <arg name="gen_depth_fill_holes_size"  default="0" />
  <arg name="gen_depth_fill_iterations"  default="1" />
  <arg name="gen_depth_fill_holes_error" default="0.1" />

  <arg name="visual_odometry"          default="false"/>          <!-- Launch rtabmap visual odometry node -->
  <arg name="icp_odometry"             default="false"/>         <!-- Launch rtabmap icp odometry node -->
  <arg name="odom_topic"               default="/gazebo/zed2/odom"/>          <!-- Odometry topic name -->
  <arg name="vo_frame_id"              default="$(arg odom_topic)"/> <!-- Visual/Icp odometry frame ID for TF -->
  <arg name="publish_tf_odom"          default="true"/>
  <arg name="odom_tf_angular_variance" default="1"/>             <!-- If TF is used to get odometry, this is the default angular variance -->
  <arg name="odom_tf_linear_variance"  default="1"/>             <!-- If TF is used to get odometry, this is the default linear variance -->
  <arg name="odom_args"                default=""/>              <!-- More arguments for odometry (overwrite same parameters in rtabmap_args) -->
  <arg name="odom_sensor_sync"         default="false"/>
  <arg name="odom_guess_frame_id"        default=""/>
  <arg name="odom_guess_min_translation" default="0"/>
  <arg name="odom_guess_min_rotation"    default="0"/>
  <arg name="odom_max_rate"            default="0"/>
  <arg name="odom_expected_rate"       default="0"/>
  <arg name="imu_topic"                default="/imu/data"/>          <!-- only used with VIO approaches -->
  <arg name="wait_imu_to_init"         default="false"/>
  <arg name="use_odom_features"        default="false"/>

  <arg name="scan_cloud_assembling"              default="false"/>
  <arg name="scan_cloud_assembling_time"         default="1"/>
  <arg name="scan_cloud_assembling_fixed_frame"  default=""/>
  <arg name="scan_cloud_assembling_voxel_size"   default="0.05"/>
  <arg name="scan_cloud_assembling_noise_radius"   default="0.0"/>    <!-- 0=disabled -->
  <arg name="scan_cloud_assembling_noise_min_neighbors"   default="5"/>

  <arg name="subscribe_user_data"      default="false"/>             <!-- user data synchronized subscription -->
  <arg name="user_data_topic"          default="/user_data"/>
  <arg name="user_data_async_topic"    default="/user_data_async" /> <!-- user data async subscription (rate should be lower than map update rate) -->

  <arg name="gps_topic"                default="/gps/fix" />         <!-- gps async subscription -->

  <arg name="tag_topic"                default="/tag_detections" />  <!-- apriltags async subscription -->
  <arg name="tag_linear_variance"      default="0.0001" />
  <arg name="tag_angular_variance"     default="9999" />             <!-- >=9999 means ignore rotation in optimization, when rotation estimation of the tag is not reliable -->

  <!-- These arguments should not be modified directly, see referred topics without "_relay" suffix above -->
  <arg if="$(arg compressed)"     name="rgb_topic_relay"           default="$(arg rgb_topic)_relay"/>
  <arg unless="$(arg compressed)" name="rgb_topic_relay"           default="$(arg rgb_topic)"/>
  <arg if="$(arg compressed)"     name="depth_topic_relay"         default="$(arg depth_topic)_relay"/>
  <arg unless="$(arg compressed)" name="depth_topic_relay"         default="$(arg depth_topic)"/>
  <arg if="$(arg compressed)"     name="left_image_topic_relay"    default="$(arg left_image_topic)_relay"/>
  <arg unless="$(arg compressed)" name="left_image_topic_relay"    default="$(arg left_image_topic)"/>
  <arg if="$(arg compressed)"     name="right_image_topic_relay"   default="$(arg right_image_topic)_relay"/>
  <arg unless="$(arg compressed)" name="right_image_topic_relay"   default="$(arg right_image_topic)"/>
  <arg if="$(arg rgbd_sync)"      name="rgbd_topic_relay"          default="$(arg rgbd_topic)"/>
  <arg unless="$(arg rgbd_sync)"  name="rgbd_topic_relay"          default="$(arg rgbd_topic)_relay"/>

  <!-- Nodes -->
  <group ns="$(arg namespace)">

    <!-- relays -->
    <group if="$(arg depth)">
      <group unless="$(arg subscribe_rgbd)">
        <node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg rgb_topic) raw out:=$(arg rgb_topic_relay)" />
        <node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="$(arg depth_image_transport) in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" />
      </group>
      <group if="$(arg rgbd_sync)">
        <node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg rgb_topic) raw out:=$(arg rgb_topic_relay)" />
        <node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="$(arg depth_image_transport) in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" />
        <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="$(arg output)">
          <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
          <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
          <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
          <remap from="rgbd_image"      to="$(arg rgbd_topic_relay)"/>
          <param name="approx_sync"     type="bool"   value="$(arg approx_rgbd_sync)"/>
          <param name="queue_size"      type="int"    value="$(arg queue_size)"/>
          <param name="depth_scale"     type="double" value="$(arg rgbd_depth_scale)"/>
          <param name="decimation"     type="double" value="$(arg rgbd_decimation)"/>
        </node>
      </group>
    </group>
    <group if="$(arg stereo)">
      <group unless="$(arg subscribe_rgbd)">
        <node if="$(arg compressed)" name="republish_left"  type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg left_image_topic) raw out:=$(arg left_image_topic_relay)" />
        <node if="$(arg compressed)" name="republish_right" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg right_image_topic) raw out:=$(arg right_image_topic_relay)" />
      </group>
      <group if="$(arg rgbd_sync)">
        <node if="$(arg compressed)" name="republish_left"  type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg left_image_topic) raw out:=$(arg left_image_topic_relay)" />
        <node if="$(arg compressed)" name="republish_right" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg right_image_topic) raw out:=$(arg right_image_topic_relay)" />
        <node pkg="nodelet" type="nodelet" name="stereo_sync" args="standalone rtabmap_ros/stereo_sync" output="$(arg output)">
          <remap from="left/image_rect"   to="$(arg left_image_topic_relay)"/>
          <remap from="right/image_rect"  to="$(arg right_image_topic_relay)"/>
          <remap from="left/camera_info"  to="$(arg left_camera_info_topic)"/>
          <remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
          <remap from="rgbd_image"      to="$(arg rgbd_topic_relay)"/>
          <param name="approx_sync"     type="bool"   value="$(arg approx_rgbd_sync)"/>
          <param name="queue_size"      type="int"    value="$(arg queue_size)"/>
        </node>
      </group>
    </group>

    <group unless="$(arg rgbd_sync)">
       <group if="$(arg subscribe_rgbd)">
         <node name="republish_rgbd_image"  type="rgbd_relay" pkg="rtabmap_ros">
           <remap     if="$(arg compressed)" from="rgbd_image" to="$(arg rgbd_topic)/compressed"/>
           <remap     if="$(arg compressed)" from="$(arg rgbd_topic)/compressed_relay" to="$(arg rgbd_topic_relay)"/>
           <remap unless="$(arg compressed)" from="rgbd_image" to="$(arg rgbd_topic)"/>
           <param if="$(arg compressed)" name="uncompress" value="true"/>
         </node>
      </group>
    </group>

    <!-- Visual odometry -->
    <group unless="$(arg icp_odometry)">
      <group if="$(arg visual_odometry)">

        <!-- RGB-D Odometry -->
        <node unless="$(arg stereo)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
          <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
          <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
          <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
          <remap from="rgbd_image"      to="$(arg rgbd_topic_relay)"/>
          <remap from="odom"            to="$(arg odom_topic)"/>
          <remap from="imu"             to="$(arg imu_topic)"/>

          <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
          <param name="odom_frame_id"               type="string" value="$(arg vo_frame_id)"/>
          <param name="publish_tf"                  type="bool"   value="$(arg publish_tf_odom)"/>
          <param name="ground_truth_frame_id"       type="string" value="$(arg ground_truth_frame_id)"/>
          <param name="ground_truth_base_frame_id"  type="string" value="$(arg ground_truth_base_frame_id)"/>
          <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
          <param name="wait_imu_to_init"            type="bool"   value="$(arg wait_imu_to_init)"/>
          <param name="approx_sync"                 type="bool"   value="$(arg approx_sync)"/>
          <param name="config_path"                 type="string" value="$(arg cfg)"/>
          <param name="queue_size"                  type="int"    value="$(arg queue_size)"/>
          <param name="subscribe_rgbd"              type="bool"   value="$(arg subscribe_rgbd)"/>
          <param name="guess_frame_id"              type="string" value="$(arg odom_guess_frame_id)"/>
          <param name="guess_min_translation"       type="double" value="$(arg odom_guess_min_translation)"/>
          <param name="guess_min_rotation"          type="double" value="$(arg odom_guess_min_rotation)"/>
          <param name="expected_update_rate"        type="double" value="$(arg odom_expected_rate)"/>
          <param name="max_update_rate"             type="double" value="$(arg odom_max_rate)"/>
          <param name="keep_color"                  type="bool"   value="$(arg use_odom_features)"/>
        </node>

        <!-- Stereo Odometry -->
        <node if="$(arg stereo)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
          <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
          <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
          <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
          <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
          <remap from="rgbd_image"             to="$(arg rgbd_topic_relay)"/>
          <remap from="odom"                   to="$(arg odom_topic)"/>
          <remap from="imu"                    to="$(arg imu_topic)"/>

          <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
          <param name="odom_frame_id"               type="string" value="$(arg vo_frame_id)"/>
          <param name="publish_tf"                  type="bool"   value="$(arg publish_tf_odom)"/>
          <param name="ground_truth_frame_id"       type="string" value="$(arg ground_truth_frame_id)"/>
          <param name="ground_truth_base_frame_id"  type="string" value="$(arg ground_truth_base_frame_id)"/>
          <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
          <param name="wait_imu_to_init"            type="bool"   value="$(arg wait_imu_to_init)"/>
          <param name="approx_sync"                 type="bool"   value="$(arg approx_sync)"/>
          <param name="config_path"                 type="string" value="$(arg cfg)"/>
          <param name="queue_size"                  type="int"    value="$(arg queue_size)"/>
          <param name="subscribe_rgbd"              type="bool"   value="$(arg subscribe_rgbd)"/>
          <param name="guess_frame_id"              type="string" value="$(arg odom_guess_frame_id)"/>
          <param name="guess_min_translation"       type="double" value="$(arg odom_guess_min_translation)"/>
          <param name="guess_min_rotation"          type="double" value="$(arg odom_guess_min_rotation)"/>
          <param name="expected_update_rate"        type="double" value="$(arg odom_expected_rate)"/>
          <param name="max_update_rate"             type="double" value="$(arg odom_max_rate)"/>
          <param name="keep_color"                  type="bool"   value="$(arg use_odom_features)"/>
        </node>
      </group>
    </group>

    <!-- ICP Odometry -->
    <node if="$(arg icp_odometry)" pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
      <remap from="scan"                   to="$(arg scan_topic)"/>
      <remap from="scan_cloud"             to="$(arg scan_cloud_topic)"/>
      <remap from="odom"                   to="$(arg odom_topic)"/>
      <remap from="imu"                    to="$(arg imu_topic)"/>

      <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
      <param name="odom_frame_id"               type="string" value="$(arg vo_frame_id)"/>
      <param name="publish_tf"                  type="bool"   value="$(arg publish_tf_odom)"/>
      <param name="ground_truth_frame_id"       type="string" value="$(arg ground_truth_frame_id)"/>
      <param name="ground_truth_base_frame_id"  type="string" value="$(arg ground_truth_base_frame_id)"/>
      <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
      <param name="wait_imu_to_init"            type="bool"   value="$(arg wait_imu_to_init)"/>
      <param name="config_path"                 type="string" value="$(arg cfg)"/>
      <param name="queue_size"                  type="int"    value="$(arg queue_size)"/>
      <param name="guess_frame_id"              type="string" value="$(arg odom_guess_frame_id)"/>
      <param name="guess_min_translation"       type="double" value="$(arg odom_guess_min_translation)"/>
      <param name="guess_min_rotation"          type="double" value="$(arg odom_guess_min_rotation)"/>
      <param name="scan_cloud_max_points"       type="int"    value="$(arg scan_cloud_max_points)"/>
      <param name="expected_update_rate"        type="double" value="$(arg odom_expected_rate)"/>
      <param name="max_update_rate"             type="double" value="$(arg odom_max_rate)"/>
    </node>

    <node if="$(arg scan_cloud_assembling)" pkg="rtabmap_ros" type="point_cloud_assembler" name="point_cloud_assembler" output="screen">
      <remap     if="$(arg scan_cloud_filtered)" from="cloud" to="odom_filtered_input_scan"/>
      <remap unless="$(arg scan_cloud_filtered)" from="cloud" to="$(arg scan_cloud_topic)"/>

      <remap from="odom"            to="$(arg odom_topic)"/>
      <param name="assembling_time" type="double" value="$(arg scan_cloud_assembling_time)"/>
      <param name="fixed_frame_id"  type="string" value="$(arg scan_cloud_assembling_fixed_frame)"/>
      <param name="voxel_size"      type="double" value="$(arg scan_cloud_assembling_voxel_size)"/>
      <param name="noise_radius"        type="double" value="$(arg scan_cloud_assembling_noise_radius)"/>
      <param name="noise_min_neighbors" type="int"    value="$(arg scan_cloud_assembling_noise_min_neighbors)"/>
    </node>

    <!-- Visual SLAM (robot side) -->
    <!-- args: "delete_db_on_start" and "udebug" -->
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="$(arg output)" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
      <param     if="$(arg stereo)" name="subscribe_depth"  type="bool" value="false"/>
      <param unless="$(arg stereo)" name="subscribe_depth"  type="bool" value="$(arg depth)"/>
      <param name="subscribe_rgb"        type="bool"   value="$(arg subscribe_rgb)"/>
      <param name="subscribe_rgbd"       type="bool"   value="$(eval subscribe_rgbd or use_odom_features)"/>
      <param name="subscribe_stereo"     type="bool"   value="$(arg stereo)"/>
      <param name="subscribe_scan"       type="bool"   value="$(arg subscribe_scan)"/>
      <param name="subscribe_scan_cloud" type="bool"   value="$(arg subscribe_scan_cloud)"/>
      <param name="subscribe_scan_descriptor" type="bool" value="$(arg subscribe_scan_descriptor)"/>
      <param name="subscribe_user_data"  type="bool"   value="$(arg subscribe_user_data)"/>
      <param if="$(arg visual_odometry)" name="subscribe_odom_info" type="bool" value="true"/>
      <param if="$(arg icp_odometry)"    name="subscribe_odom_info" type="bool" value="true"/>
      <param name="frame_id"             type="string" value="$(arg frame_id)"/>
      <param name="map_frame_id"         type="string" value="$(arg map_frame_id)"/>
      <param name="odom_frame_id"        type="string" value="$(arg odom_frame_id)"/>
      <param name="odom_frame_id_init"   type="string" value="$(arg odom_frame_id_init)"/>
      <param name="publish_tf"           type="bool"   value="$(arg publish_tf_map)"/>
      <param name="gen_scan"             type="bool"   value="$(arg gen_scan)"/>
      <param name="ground_truth_frame_id"       type="string" value="$(arg ground_truth_frame_id)"/>
      <param name="ground_truth_base_frame_id"  type="string" value="$(arg ground_truth_base_frame_id)"/>
      <param name="odom_tf_angular_variance" type="double" value="$(arg odom_tf_angular_variance)"/>
      <param name="odom_tf_linear_variance"  type="double" value="$(arg odom_tf_linear_variance)"/>
      <param name="odom_sensor_sync"         type="bool"   value="$(arg odom_sensor_sync)"/>
      <param name="wait_for_transform_duration"  type="double"   value="$(arg wait_for_transform)"/>
      <param name="database_path"        type="string" value="$(arg database_path)"/>
      <param name="approx_sync"          type="bool"   value="$(eval approx_sync and not use_odom_features)"/>
      <param name="config_path"          type="string" value="$(arg cfg)"/>
      <param name="queue_size"           type="int" value="$(arg queue_size)"/>
      <param name="scan_cloud_max_points"     type="int" value="$(arg scan_cloud_max_points)"/>
      <param name="landmark_linear_variance"   type="double" value="$(arg tag_linear_variance)"/>
      <param name="landmark_angular_variance"  type="double" value="$(arg tag_angular_variance)"/> 
      <param name="gen_depth"                  type="bool"   value="$(arg gen_depth)" />
      <param name="gen_depth_decimation"       type="int"    value="$(arg gen_depth_decimation)" />
      <param name="gen_depth_fill_holes_size"  type="int"    value="$(arg gen_depth_fill_holes_size)" />
      <param name="gen_depth_fill_iterations"  type="int"    value="$(arg gen_depth_fill_iterations)" />
      <param name="gen_depth_fill_holes_error" type="double" value="$(arg gen_depth_fill_holes_error)" />

      <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
      <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>

      <remap     if="$(arg use_odom_features)" from="rgbd_image" to="odom_rgbd_image"/>
      <remap unless="$(arg use_odom_features)" from="rgbd_image" to="$(arg rgbd_topic_relay)"/>

      <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
      <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
      <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
      <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>

      <remap from="scan"                   to="$(arg scan_topic)"/>
      <remap if="$(eval scan_cloud_assembling)" from="scan_cloud" to="assembled_cloud"/>
      <remap if="$(eval scan_cloud_filtered and not scan_cloud_assembling)" from="scan_cloud" to="odom_filtered_input_scan"/>
      <remap if="$(eval not scan_cloud_filtered and not scan_cloud_assembling)" from="scan_cloud" to="$(arg scan_cloud_topic)"/>
      <remap from="scan_descriptor"        to="$(arg scan_descriptor_topic)"/>
      <remap from="user_data"              to="$(arg user_data_topic)"/>
      <remap from="user_data_async"        to="$(arg user_data_async_topic)"/>
      <remap from="gps/fix"                to="$(arg gps_topic)"/>
      <remap from="tag_detections"         to="$(arg tag_topic)"/>
      <remap from="odom"                   to="$(arg odom_topic)"/>
      <remap from="imu"                    to="$(arg imu_topic)"/>

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

    <!-- Visualisation RTAB-Map -->
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg gui_cfg)" output="$(arg output)" launch-prefix="$(arg launch_prefix)">
      <param     if="$(arg stereo)" name="subscribe_depth"  type="bool" value="false"/>
      <param unless="$(arg stereo)" name="subscribe_depth"  type="bool" value="$(arg depth)"/>
      <param name="subscribe_rgb"        type="bool"   value="$(arg subscribe_rgb)"/>
      <param name="subscribe_rgbd"       type="bool"   value="$(eval subscribe_rgbd or use_odom_features)"/>
      <param name="subscribe_stereo"     type="bool"   value="$(arg stereo)"/>
      <param unless="$(arg icp_odometry)" name="subscribe_scan"       type="bool"   value="$(arg subscribe_scan)"/>
      <param     if="$(arg icp_odometry)" name="subscribe_scan_cloud" type="bool"   value="$(eval subscribe_scan or subscribe_scan_cloud)"/>
      <param unless="$(arg icp_odometry)" name="subscribe_scan_cloud" type="bool"   value="$(arg subscribe_scan_cloud)"/>
      <param name="subscribe_scan_descriptor" type="bool" value="$(arg subscribe_scan_descriptor)"/>
      <param if="$(arg visual_odometry)" name="subscribe_odom_info" type="bool" value="true"/>
      <param if="$(arg icp_odometry)"    name="subscribe_odom_info" type="bool" value="true"/>
      <param name="frame_id"             type="string" value="$(arg frame_id)"/>
      <param name="odom_frame_id"        type="string" value="$(arg odom_frame_id)"/>
      <param name="wait_for_transform_duration" type="double"   value="$(arg wait_for_transform)"/>
      <param name="queue_size"           type="int"    value="$(arg queue_size)"/>
      <param name="approx_sync"          type="bool"   value="$(eval approx_sync and not use_odom_features)"/>

      <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
      <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>

      <remap     if="$(arg use_odom_features)" from="rgbd_image" to="odom_rgbd_image"/>
      <remap unless="$(arg use_odom_features)" from="rgbd_image" to="$(arg rgbd_topic_relay)"/>

      <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
      <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
      <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
      <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>

      <remap unless="$(arg icp_odometry)" from="scan" to="$(arg scan_topic)"/>
      <remap     if="$(arg icp_odometry)" from="scan_cloud" to="odom_filtered_input_scan"/>
      <remap unless="$(arg icp_odometry)" from="scan_cloud" to="$(arg scan_cloud_topic)"/>
      <remap from="scan_descriptor"        to="$(arg scan_descriptor_topic)"/>
      <remap from="odom"                   to="$(arg odom_topic)"/>
    </node>

  </group>

  <!-- Visualization RVIZ -->
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(arg rviz_cfg)"/>
  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb" output="$(arg output)">
    <remap if="$(arg stereo)" from="left/image"        to="$(arg left_image_topic_relay)"/>
    <remap if="$(arg stereo)" from="right/image"       to="$(arg right_image_topic_relay)"/>
    <remap if="$(arg stereo)" from="left/camera_info"  to="$(arg left_camera_info_topic)"/>
    <remap if="$(arg stereo)" from="right/camera_info" to="$(arg right_camera_info_topic)"/>
    <remap unless="$(arg subscribe_rgbd)" from="rgb/image"         to="$(arg rgb_topic_relay)"/>
    <remap unless="$(arg subscribe_rgbd)" from="depth/image"       to="$(arg depth_topic_relay)"/>
    <remap unless="$(arg subscribe_rgbd)" from="rgb/camera_info"   to="$(arg camera_info_topic)"/>
    <remap from="rgbd_image"        to="$(arg rgbd_topic_relay)"/>
    <remap from="cloud"             to="voxel_cloud" />

    <param name="decimation"  type="double" value="4"/>
    <param name="voxel_size"  type="double" value="0.0"/>
    <param name="approx_sync" type="bool"   value="$(arg approx_sync)"/>
  </node>

</launch>

My Environment

matlabbe commented 3 years ago

Is it fixed? seems like timestamps between the cameras were not exactly the same.

GiuliaB97 commented 3 years ago

Yes, After a while i notice that in the simulation i was using rtabmap were already installed and running (even the documentation said nothing about it). Therefore, i was basically trying to run it multiple times and this caused some problems in the system. By stopping to do so everything works fine, thus i closed the issue.

Thank you very much.