Updated lidar3d examples (and new 2x lidars example)

rtabmap_examples/launch/lidar3d_assemble.launch.py

@@ -8,7 +8,7 @@
 #   
 #   Launch your IMU sensor, make sure TF between lidar/base frame and imu is already calibrated.
 #     In this example, we assume the imu topic has 
-#     already the orientation estimated, it not, you can launch 
+#     already the orientation estimated, if not, you can launch 
 #     imu_filter_madgwick_node (with use_mag:=false publish_tf:=false)
 #     and set imu_topic to output topic of the filter.
 #
@@ -28,11 +28,16 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
   
   frame_id = LaunchConfiguration('frame_id')
 
+  external_odom_frame_id =  LaunchConfiguration('external_odom_frame_id').perform(context)
+
   fixed_frame_from_imu = False
   fixed_frame_id =  LaunchConfiguration('fixed_frame_id').perform(context)
   if not fixed_frame_id:
-    fixed_frame_from_imu = True
-    fixed_frame_id = frame_id.perform(context) + "_stabilized"
+    if external_odom_frame_id:
+      fixed_frame_id = external_odom_frame_id
+    else:
+      fixed_frame_from_imu = True
+      fixed_frame_id = frame_id.perform(context) + "_stabilized"
   
   imu_topic = LaunchConfiguration('imu_topic')
   
@@ -51,6 +56,9 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
   localization = LaunchConfiguration('localization').perform(context)
   localization = localization == 'true' or localization == 'True'
   
+  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
 
@@ -74,7 +82,7 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
   }
 
   icp_odometry_parameters = {
-    'expected_update_rate': 15.0,
+    'expected_update_rate': LaunchConfiguration('expected_update_rate'),
     'wait_imu_to_init': True,
     'odom_frame_id': 'icp_odom',
     'guess_frame_id': fixed_frame_id,
@@ -89,8 +97,9 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
   rtabmap_parameters = {
     'subscribe_depth': False,
     'subscribe_rgb': False,
-    'subscribe_odom_info': True,
+    'subscribe_odom_info': not external_odom_frame_id,
     'subscribe_scan_cloud': True,
+    'odom_frame_id': (external_odom_frame_id if external_odom_frame_id else ""),
     'odom_sensor_sync': True, # This will adjust camera position based on difference between lidar and camera stamps.
     # RTAB-Map's internal parameters are strings:
     'Rtabmap/DetectionRate': '0', # indirectly set to 1 Hz by the assembling time below (1s)
@@ -102,7 +111,7 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
     'Mem/NotLinkedNodesKept': 'false',
     'Mem/STMSize': '30',
     'Reg/Strategy': '1',
-    'Icp/CorrespondenceRatio': '0.2'
+    'Icp/CorrespondenceRatio': LaunchConfiguration('min_loop_closure_overlap')
   }
   
   remappings = [('imu', imu_topic),
@@ -116,6 +125,11 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
     rtabmap_parameters['Mem/InitWMWithAllNodes'] = 'True'
   else:
     arguments.append('-d') # This will delete the previous database (~/.ros/rtabmap.db)
+    
+  if external_odom_frame_id:
+    viz_topic = lidar_topic_deskewed
+  else:
+    viz_topic = 'odom_filtered_input_scan'
   
   nodes = [
     # Lidar deskewing
@@ -124,24 +138,19 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
       parameters=[{
         'use_sim_time': use_sim_time,
         'fixed_frame_id': fixed_frame_id,
-        'wait_for_transform': 0.2}],
+        'wait_for_transform': 0.2,
+        'slerp': deskewing_slerp}],
       remappings=[
           ('input_cloud', lidar_topic)
       ]),
     
-    # Lidar odometry
-    Node(
-      package='rtabmap_odom', executable='icp_odometry', output='screen',
-      parameters=[shared_parameters, icp_odometry_parameters],
-      remappings=remappings + [('scan_cloud', lidar_topic_deskewed)]),
-    
     # Assemble deskewed scans based on icp odometry
     Node(
       package='rtabmap_util', executable='point_cloud_assembler', output='screen',
       parameters=[{
         'use_sim_time': use_sim_time,
         'assembling_time': LaunchConfiguration('assembling_time'), 
-        'fixed_frame_id': ""}], # This will make the node subscribing to icp odometry topic "odom"
+        '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')]),
     
@@ -150,8 +159,8 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
       package='rtabmap_slam', executable='rtabmap', output='screen',
       parameters=[shared_parameters, rtabmap_parameters,
                   {'subscribe_rgbd': rgbd_image_used,
-                   'topic_queue_size': 30,
-                   'sync_queue_size': 20,}],
+                   'topic_queue_size': 40,
+                   'sync_queue_size': 40,}],
       remappings=remappings + [('scan_cloud', 'assembled_cloud')],
       arguments=arguments), 
 
@@ -159,9 +168,17 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
     Node(
       package='rtabmap_viz', executable='rtabmap_viz', output='screen',
       parameters=[shared_parameters, rtabmap_parameters],
-      remappings=remappings + [('scan_cloud', 'odom_filtered_input_scan')])
+      remappings=remappings + [('scan_cloud', viz_topic)])
   ]
   
+  if not external_odom_frame_id:
+    # Lidar odometry
+    nodes.append(
+      Node(
+        package='rtabmap_odom', executable='icp_odometry', output='screen',
+        parameters=[shared_parameters, icp_odometry_parameters],
+        remappings=remappings + [('scan_cloud', lidar_topic_deskewed)]))
+  
   if fixed_frame_from_imu:
     # Create a stabilized base frame based on imu for lidar deskewing
     nodes.append(
@@ -190,7 +207,11 @@ def generate_launch_description():
     
     DeclareLaunchArgument(
       'fixed_frame_id', default_value='',
-      description='Fixed frame used for lidar deskewing. If not set, we will generate one from IMU.'),
+      description='Fixed frame used for lidar deskewing. If not set, we will generate one from IMU or external_odom_frame_id if not null.'),
+    
+    DeclareLaunchArgument(
+      'external_odom_frame_id', default_value='',
+      description='Provide external odometry with TF, disabling icp_odometry.'),
     
     DeclareLaunchArgument(
       'localization', default_value='false',
@@ -212,10 +233,22 @@ def generate_launch_description():
       '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(
+      'min_loop_closure_overlap', default_value='0.2',
+      description='Minimum scan overlap pourcentage to accept a loop closure.'),
+    
+    DeclareLaunchArgument(
+      'expected_update_rate', default_value='15.0',
+      description='Expected lidar frame rate. Ideally, set it slightly higher than actual frame rate, like 15 Hz for 10 Hz lidar scans.'),
+    
     DeclareLaunchArgument(
       'assembling_time', default_value='1.0',
       description='How much time (sec) we assemble lidar scans before sending them to mapping node.'),
 
+    DeclareLaunchArgument(
+      '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(
       'qos', default_value='1',
       description='Quality of Service: 0=system default, 1=reliable, 2=best effort.'),