rtabmap_demos tests and docs (#1462)

* rtabmap_demos tests and docs

* added bag testing

* added rtabmap_examples launch tests

* updating demo bag download paths

* added netherdrone demo

* Fixed rgb-only callback with lidar rejected.  Updated lidar params

* Added back OrbitOriented rviz view to ros2, with optional octomap wll clipping

* fixing ci

* lidar demo added intermediate_nodes option

* added netherdrone as demo test

* running rtabmap_demos tests on ci

* added rtabmap_launch tests, fixed ground_truth_base_frame_id usage

* ficing rolling

* updating demo test harnest

* densify golden trajectories to avoid tf missing

* fixing tf steps

* updated min icp ratio for netherdrone demo

* fixing image_transport arg->params

* export pose opt=0

* lets process all frames

* updated netherdrone golden

* fixing publishers queue size just for tests

* added playdback demo doc

* Adding more logs to debug ci

* Fixing QOS for CI to reliable, added find-object demo test

* name threads

* fixing camera info expected transient on lyrical/rolling. Fixing find_object not appearing idle

* 30 Hz polling backward comp

* fixing test tf sim lock

* fixing lyrical qos bag parsing

* faster replay

* lockstep

* fixing clock deadlock

* Added test on shutdown

* updated netherdrone golden poses

* Extended stereo outdoor test

* updated shutdown test

* updated test

* multi-thread flaky test

* adding backtrace when test fails

* increased closure slack for netherdrone

* g2o gauss newton on stereo

* adjusted maximum optimizer iterations

* updated default iterations

* Added netherdrone in list of demos

rtabmap_examples/launch/lidar3d_assemble.launch.py

@@ -14,13 +14,20 @@
 #
 #   If a camera is used, make sure TF between lidar/base frame and camera is
 #     already calibrated. To provide image data to this example, you should use
-#     rtabmap_sync's rgbd_sync or stereo_sync node.
+#     rtabmap_sync's rgbd_sync or stereo_sync node. For a camera without depth, use
+#     rgb_sync and set gen_depth:=true to make its depth from the lidar.
 #
 #   Launch the example by adjusting the lidar topic, imu topic and base frame:
 #   $ ros2 launch rtabmap_examples lidar3d.launch.py lidar_topic:=/velodyne_points imu_topic:=/imu/data frame_id:=velodyne
+#
+#   For a complete example with a recorded bag (Ouster on a rotating mast, IMU and
+#   a camera without depth), see rtabmap_demos' netherdrone_lidar3d_demo.launch.py.
+
+import os
 
 from launch import LaunchDescription, LaunchContext
 from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.conditions import IfCondition
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
 
@@ -52,6 +59,8 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
   
   voxel_size = LaunchConfiguration('voxel_size')
   voxel_size_value = float(voxel_size.perform(context))
+
+  lidar_range_min = float(LaunchConfiguration('lidar_range_min').perform(context))
   
   use_sim_time = LaunchConfiguration('use_sim_time')
   
@@ -61,8 +70,12 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
   deskewing_slerp = LaunchConfiguration('deskewing_slerp').perform(context)
   deskewing_slerp = deskewing_slerp == 'true' or deskewing_slerp == 'True'
   
-  # Rule of thumb:
-  max_correspondence_distance = voxel_size_value * 10.0
+  max_correspondence_distance = LaunchConfiguration('max_correspondence_distance').perform(context)
+  if max_correspondence_distance:
+    max_correspondence_distance = float(max_correspondence_distance)
+  else:
+    # Rule of thumb:
+    max_correspondence_distance = voxel_size_value * 10.0
 
   shared_parameters = {
     'use_sim_time': use_sim_time,
@@ -80,7 +93,7 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
     'Icp/MaxTranslation': '3',
     'Icp/MaxCorrespondenceDistance': str(max_correspondence_distance),
     'Icp/Strategy': '1',
-    'Icp/OutlierRatio': '0.7',
+    'Icp/OutlierRatio': LaunchConfiguration('icp_outlier_ratio').perform(context),
   }
 
   icp_odometry_parameters = {
@@ -88,10 +101,11 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
     'wait_imu_to_init': True,
     'odom_frame_id': 'icp_odom',
     'guess_frame_id': fixed_frame_id,
+    'scan_range_min': lidar_range_min,
     # RTAB-Map's internal parameters are strings:
-    'Odom/ScanKeyFrameThr': '0.4',
+    'Odom/ScanKeyFrameThr': LaunchConfiguration('odom_key_frame_threshold').perform(context),
     'OdomF2M/ScanSubtractRadius': str(voxel_size_value),
-    'OdomF2M/ScanMaxSize': '15000',
+    'OdomF2M/ScanMaxSize': LaunchConfiguration('odom_local_map_size').perform(context),
     'OdomF2M/BundleAdjustment': 'false',
     'Icp/CorrespondenceRatio': '0.01'
   }
@@ -107,6 +121,7 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
     'Rtabmap/DetectionRate': '0', # indirectly set to 1 Hz by the assembling time below (1s)
     'RGBD/ProximityMaxGraphDepth': '0',
     'RGBD/ProximityPathMaxNeighbors': '1',
+    'RGBD/ProximityAngle': '0', # assuming 360 lidar
     'RGBD/AngularUpdate': '0.05',
     'RGBD/LinearUpdate': '0.05',
     'RGBD/CreateOccupancyGrid': 'false',
@@ -115,6 +130,21 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
     'Reg/Strategy': '1',
     'Icp/CorrespondenceRatio': str(LaunchConfiguration('min_loop_closure_overlap').perform(context))
   }
+
+  # Only for the camera, if any
+  camera_parameters = {
+    'gen_depth': LaunchConfiguration('gen_depth').perform(context).lower() == 'true',
+    'gen_depth_decimation': int(LaunchConfiguration('gen_depth_decimation').perform(context)),
+    'gen_depth_fill_holes_size': int(LaunchConfiguration('gen_depth_fill_holes_size').perform(context)),
+    'Rtabmap/ImagesAlreadyRectified': str(LaunchConfiguration('rectify_images').perform(context).lower() != 'true').lower(),
+  }
+  if camera_parameters['gen_depth']:
+    # The depth made from the lidar is 32 bits float: save it in 16 bits (mm), the
+    # format compressed depth images use.
+    camera_parameters['Mem/SaveDepth16Format'] = 'true'
+
+  database_parameters = {
+  }
   
   remappings = [('imu', imu_topic),
                 ('odom', 'icp_odom')]
@@ -124,6 +154,11 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
     else:
       remappings.append(('rgbd_images', LaunchConfiguration('rgbd_images_topic')))
     
+  intermediate_nodes = LaunchConfiguration('intermediate_nodes').perform(context).lower() == 'true'
+  if intermediate_nodes and not external_odom_frame_id:
+    # Every odometry pose between the nodes is saved as a node without data.
+    rtabmap_parameters['Rtabmap/CreateIntermediateNodes'] = 'true'
+
   arguments = []
   if localization:
     rtabmap_parameters['Mem/IncrementalMemory'] = 'False'
@@ -144,7 +179,8 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
         'use_sim_time': use_sim_time,
         'fixed_frame_id': fixed_frame_id,
         'wait_for_transform': 0.2,
-        'slerp': deskewing_slerp}],
+        'slerp': deskewing_slerp,
+        'qos': LaunchConfiguration('qos')}],
       remappings=[
           ('input_cloud', lidar_topic)
       ]),
@@ -155,6 +191,10 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
       parameters=[{
         'use_sim_time': use_sim_time,
         'assembling_time': LaunchConfiguration('assembling_time'), 
+        'range_min': lidar_range_min,
+        'voxel_size': float(LaunchConfiguration('assembler_voxel_size').perform(context)),
+        'qos': LaunchConfiguration('qos'),
+        'qos_odom': LaunchConfiguration('qos'),
         'fixed_frame_id': (external_odom_frame_id if external_odom_frame_id else "")}], # This will make the node subscribing to icp odometry topic "icp_odom"
       remappings=[('cloud', lidar_topic_deskewed),
                   ('odom', 'icp_odom')]),
@@ -162,20 +202,23 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
     # Update the map
     Node(
       package='rtabmap_slam', executable='rtabmap', output='screen',
-      parameters=[shared_parameters, rtabmap_parameters,
+      parameters=[shared_parameters, rtabmap_parameters, database_parameters, camera_parameters,
                   {'subscribe_rgbd': rgbd_image_used,
                    'rgbd_cameras': rgbd_cameras,
                    'topic_queue_size': 40,
                    'sync_queue_size': 40,}],
-      remappings=remappings + [('scan_cloud', 'assembled_cloud'), ('gps/fix', LaunchConfiguration('gps_topic'))],
+      remappings=remappings + [('scan_cloud', 'assembled_cloud'), ('gps/fix', LaunchConfiguration('gps_topic')),
+                               ('inter_odom', 'icp_odom')],
       arguments=arguments), 
 
     # Just for visualization
     Node(
+      condition=IfCondition(LaunchConfiguration('rtabmap_viz')),
       package='rtabmap_viz', executable='rtabmap_viz', output='screen',
       parameters=[shared_parameters, rtabmap_parameters,
                   {'odometry_node_name': "icp_odometry"}],
-      remappings=remappings + [('scan_cloud', viz_topic)])
+      remappings=remappings + [('scan_cloud', viz_topic)],
+      arguments=['-d', LaunchConfiguration('rtabmap_viz_cfg')])
   ]
   
   if not external_odom_frame_id:
@@ -195,7 +238,8 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
           'use_sim_time': use_sim_time,
           'fixed_frame_id': fixed_frame_id,
           'base_frame_id': frame_id,
-          'wait_for_transform_duration': 0.001}],
+          'wait_for_transform_duration': 0.001,
+          'qos': LaunchConfiguration('qos')}],
         remappings=[('imu/data', imu_topic)]))
   
   return nodes
@@ -244,10 +288,50 @@ def generate_launch_description():
       'rgbd_images_topic', default_value='',
       description='RGBD images topic (ignored if empty, override "rgbd_image_topic" if set). Would be the output of a rtabmap_sync\'s rgbdx_sync node.'),
     
+    DeclareLaunchArgument(
+      'gen_depth', default_value='false',
+      description='With a camera without depth (rgbd_image_topic from rgb_sync), make its depth image by projecting the assembled lidar cloud into it, so that its visual features get 3D positions.'),
+
+    DeclareLaunchArgument(
+      'gen_depth_decimation', default_value='4',
+      description='Resolution divider of the depth made by gen_depth; it must divide the image size. Lidar points are sparse in a full resolution image anyway.'),
+
+    DeclareLaunchArgument(
+      'gen_depth_fill_holes_size', default_value='2',
+      description='Fill holes up to this many pixels (after decimation) in the depth made by gen_depth, between lidar points; 0 disables.'),
+
+    DeclareLaunchArgument(
+      'rectify_images', default_value='false',
+      description='The camera images are not rectified: rectify them with their camera_info before use.'),
+
     DeclareLaunchArgument(
       'voxel_size', default_value='0.1',
       description='Voxel size (m) of the downsampled lidar point cloud. For indoor, set it between 0.1 and 0.3. For outdoor, set it to 0.5 or over.'),
     
+    DeclareLaunchArgument(
+      'max_correspondence_distance', default_value='',
+      description='Maximum distance (m) between ICP correspondences. Empty: 10 times voxel_size.'),
+
+    DeclareLaunchArgument(
+      'icp_outlier_ratio', default_value='0.7',
+      description='Icp/OutlierRatio: expected ratio of outliers between scans.'),
+
+    DeclareLaunchArgument(
+      'odom_key_frame_threshold', default_value='0.4',
+      description='Odom/ScanKeyFrameThr: a new scan is added to the odometry local map when its overlap with it is below this ratio.'),
+
+    DeclareLaunchArgument(
+      'odom_local_map_size', default_value='15000',
+      description='OdomF2M/ScanMaxSize: maximum number of points of the odometry local map.'),
+
+    DeclareLaunchArgument(
+      'assembler_voxel_size', default_value='0.0',
+      description='Voxel size (m) of the clouds assembled for the map; 0 keeps every point.'),
+
+    DeclareLaunchArgument(
+      'lidar_range_min', default_value='0.0',
+      description='Lidar points closer than this (m) are ignored, by odometry and in the map; 0 keeps them all. Set it to remove hits on the robot itself.'),
+
     DeclareLaunchArgument(
       'min_loop_closure_overlap', default_value='0.2',
       description='Minimum scan overlap pourcentage to accept a loop closure.'),
@@ -264,10 +348,25 @@ def generate_launch_description():
       'deskewing_slerp', default_value='true',
       description='Use fast slerp interpolation between first and last stamps of the scan for deskewing. It would less accruate than requesting TF for every points, but a lot faster. Enable this if the delay of the deskewed scan is significant larger than the original scan.'),
 
+    DeclareLaunchArgument(
+      'intermediate_nodes', default_value='false',
+      description='Also save every odometry pose between the map\'s nodes in the database, as nodes without data (Rtabmap/CreateIntermediateNodes). Only with icp_odometry (external_odom_frame_id empty).'),
+
     DeclareLaunchArgument(
       'qos', default_value='1',
       description='Quality of Service: 0=system default, 1=reliable, 2=best effort.'),
 
+    DeclareLaunchArgument(
+      'rtabmap_viz', default_value='true',
+      description='Launch RTAB-Map UI.'),
+
+    DeclareLaunchArgument(
+      'rtabmap_viz_cfg', default_value='~/.ros/rtabmapGUI.ini',
+      description='Configuration file of rtabmap_viz, where it also saves its settings.'),
+
+
+
+
     OpaqueFunction(function=launch_setup),
   ])