From a73a79253cd7708330ed6f6967d07a94c0ac254c Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Sun, 24 Nov 2024 22:08:01 -0800
Subject: [PATCH] Created two base general examples for 3D lidar usage, then
 create hardware specific examples on top of them.

---
 rtabmap_examples/launch/k4a.launch.py         |   1 -
 rtabmap_examples/launch/lidar3d.launch.py     | 220 +++++++++++++++++
 .../launch/lidar3d_assemble.launch.py         | 226 ++++++++++++++++++
 rtabmap_examples/launch/vlp16.launch.py       | 127 ----------
 rtabmap_examples/launch/vlp16_imu.launch.py   | 183 --------------
 .../launch/vlp16_imu_assemble.launch.py       | 201 ----------------
 rtabmap_examples/launch/vlp16_zed.launch.py   | 121 ++++++++++
 .../launch/vlp16_zed_vio_assemble.launch.py   | 185 --------------
 8 files changed, 567 insertions(+), 697 deletions(-)
 create mode 100644 rtabmap_examples/launch/lidar3d.launch.py
 create mode 100644 rtabmap_examples/launch/lidar3d_assemble.launch.py
 delete mode 100644 rtabmap_examples/launch/vlp16.launch.py
 delete mode 100644 rtabmap_examples/launch/vlp16_imu.launch.py
 delete mode 100644 rtabmap_examples/launch/vlp16_imu_assemble.launch.py
 create mode 100644 rtabmap_examples/launch/vlp16_zed.launch.py
 delete mode 100644 rtabmap_examples/launch/vlp16_zed_vio_assemble.launch.py

diff --git a/rtabmap_examples/launch/k4a.launch.py b/rtabmap_examples/launch/k4a.launch.py
index bf06124c2..d09693a60 100644
--- a/rtabmap_examples/launch/k4a.launch.py
+++ b/rtabmap_examples/launch/k4a.launch.py
@@ -31,7 +31,6 @@ def generate_launch_description():
         Node(
             package='rtabmap_odom', executable='rgbd_odometry', output='screen',
             parameters=[{ 'frame_id':'camera_base',
-                          'subscribe_odom_info':True,
                           'approx_sync':True,
                           'approx_sync_max_interval':0.01,
                           'wait_imu_to_init':True,
diff --git a/rtabmap_examples/launch/lidar3d.launch.py b/rtabmap_examples/launch/lidar3d.launch.py
new file mode 100644
index 000000000..daec929fd
--- /dev/null
+++ b/rtabmap_examples/launch/lidar3d.launch.py
@@ -0,0 +1,220 @@
+# Description:
+#   In this example, we keep only minimal data to do LiDAR SLAM.
+#
+# Example:
+#   Launch your lidar sensor:
+#   $ ros2 launch velodyne_driver velodyne_driver_node-VLP16-launch.py
+#   $ ros2 launch velodyne_pointcloud velodyne_transform_node-VLP16-launch.py
+#
+#   If an IMU is used, 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 use 
+#     imu_filter_madgwick_node (with use_mag:=false publish_tf:=false)
+#     and set imu_topic to output topic of the filter.
+#
+#   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.
+#
+#   Launch the example by adjusting the lidar topic and base frame:
+#   $ ros2 launch rtabmap_examples lidar3d.launch.py lidar_topic:=/velodyne_points frame_id:=velodyne
+
+from launch import LaunchDescription, LaunchContext
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+def launch_setup(context: LaunchContext, *args, **kwargs):
+  
+  frame_id = LaunchConfiguration('frame_id')
+  
+  imu_topic = LaunchConfiguration('imu_topic')
+  imu_used =  imu_topic.perform(context) != ''
+  
+  rgbd_image_topic = LaunchConfiguration('rgbd_image_topic')
+  rgbd_image_used =  rgbd_image_topic.perform(context) != ''
+  
+  voxel_size = LaunchConfiguration('voxel_size')
+  voxel_size_value = float(voxel_size.perform(context))
+  
+  use_sim_time = LaunchConfiguration('use_sim_time')
+  
+  lidar_topic = LaunchConfiguration('lidar_topic')
+  lidar_topic_value = lidar_topic.perform(context)
+  lidar_topic_deskewed = lidar_topic_value + "/deskewed"
+  
+  localization = LaunchConfiguration('localization').perform(context)
+  localization = localization == 'true' or localization == 'True'
+  
+  fixed_frame_from_imu = False
+  fixed_frame_id =  LaunchConfiguration('fixed_frame_id').perform(context)
+  if not fixed_frame_id and imu_used:
+    fixed_frame_from_imu = True
+    fixed_frame_id = frame_id.perform(context) + "_stabilized"
+  
+  if not fixed_frame_id:
+    lidar_topic_deskewed = lidar_topic
+  
+  # Rule of thumb:
+  max_correspondence_distance = voxel_size_value * 10.0
+
+  shared_parameters = {
+    'use_sim_time': use_sim_time,
+    'frame_id': frame_id,
+    'qos': LaunchConfiguration('qos'),
+    'approx_sync': rgbd_image_used,
+    'wait_for_transform': 0.2,
+    # RTAB-Map's internal parameters are strings:
+    'Icp/PointToPlane': 'true',
+    'Icp/Iterations': '10',
+    'Icp/VoxelSize': str(voxel_size_value),
+    'Icp/Epsilon': '0.001',
+    'Icp/PointToPlaneK': '20',
+    'Icp/PointToPlaneRadius': '0',
+    'Icp/MaxTranslation': '3',
+    'Icp/MaxCorrespondenceDistance': str(max_correspondence_distance),
+    'Icp/Strategy': '1',
+    'Icp/OutlierRatio': '0.7',
+  }
+  
+  icp_odometry_parameters = {
+    'expected_update_rate': 15.0,
+    'deskewing': not fixed_frame_id, # If fixed_frame_id is set, we do deskewing externally below
+    'odom_frame_id': 'icp_odom',
+    'guess_frame_id': fixed_frame_id,
+    # RTAB-Map's internal parameters are strings:
+    'Odom/ScanKeyFrameThr': '0.4',
+    'OdomF2M/ScanSubtractRadius': str(voxel_size_value),
+    'OdomF2M/ScanMaxSize': '15000',
+    'OdomF2M/BundleAdjustment': 'false',
+    'Icp/CorrespondenceRatio': '0.01'
+  }
+  if imu_used:
+    icp_odometry_parameters['wait_imu_to_init'] = True
+
+  rtabmap_parameters = {
+    'subscribe_depth': False,
+    'subscribe_rgb': False,
+    'subscribe_odom_info': True,
+    'subscribe_scan_cloud': True,
+    # RTAB-Map's internal parameters are strings:
+    'RGBD/ProximityMaxGraphDepth': '0',
+    'RGBD/ProximityPathMaxNeighbors': '1',
+    'RGBD/AngularUpdate': '0.05',
+    'RGBD/LinearUpdate': '0.05',
+    'RGBD/CreateOccupancyGrid': 'false',
+    'Mem/NotLinkedNodesKept': 'false',
+    'Mem/STMSize': '30',
+    'Reg/Strategy': '1',
+    'Icp/CorrespondenceRatio': '0.2'
+  }
+  
+  arguments = []
+  if localization:
+    rtabmap_parameters['Mem/IncrementalMemory'] = 'False'
+    rtabmap_parameters['Mem/InitWMWithAllNodes'] = 'True'
+  else:
+    arguments.append('-d') # This will delete the previous database (~/.ros/rtabmap.db)
+  
+  remappings = [('odom', 'icp_odom')]
+  if imu_used:
+    remappings.append(('imu', LaunchConfiguration('imu_topic')))
+  else:
+    remappings.append(('imu', 'imu_not_used'))
+  if rgbd_image_used:
+    remappings.append(('rgbd_image', LaunchConfiguration('rgbd_image_topic')))
+  
+  nodes = [
+    Node(
+      package='rtabmap_odom', executable='icp_odometry', output='screen',
+      parameters=[shared_parameters, icp_odometry_parameters],
+      remappings=remappings + [('scan_cloud', lidar_topic_deskewed)]),
+    
+    Node(
+      package='rtabmap_slam', executable='rtabmap', output='screen',
+      parameters=[shared_parameters, rtabmap_parameters, {'subscribe_rgbd': rgbd_image_used}],
+      remappings=remappings + [('scan_cloud', lidar_topic_deskewed)],
+      arguments=arguments), 
+  
+    Node(
+      package='rtabmap_viz', executable='rtabmap_viz', output='screen',
+      parameters=[shared_parameters, rtabmap_parameters],
+      remappings=remappings + [('scan_cloud', 'odom_filtered_input_scan')])
+  ]
+  
+  if fixed_frame_from_imu:
+    # Create a stabilized base frame based on imu for lidar deskewing
+    nodes.append(
+      Node(
+        package='rtabmap_util', executable='imu_to_tf', output='screen',
+        parameters=[{
+          'use_sim_time': use_sim_time,
+          'fixed_frame_id': fixed_frame_id,
+          'base_frame_id': frame_id,
+          'wait_for_transform_duration': 0.001}],
+        remappings=[('imu/data', imu_topic)]))
+
+  if fixed_frame_id:
+    # Lidar deskewing
+    nodes.append(
+      Node(
+        package='rtabmap_util', executable='lidar_deskewing', output='screen',
+        parameters=[{
+          'use_sim_time': use_sim_time,
+          'fixed_frame_id': fixed_frame_id,
+          'wait_for_transform': 0.2}],
+        remappings=[
+            ('input_cloud', lidar_topic)
+        ])
+    )
+      
+  return nodes
+  
+def generate_launch_description():
+  return LaunchDescription([
+
+    # Launch arguments
+    DeclareLaunchArgument(
+      'use_sim_time', default_value='false',
+      description='Use simulated clock.'),
+    
+    DeclareLaunchArgument(
+      'deskewing', default_value='true',
+      description='Enable lidar deskewing.'),
+    
+    DeclareLaunchArgument(
+      'frame_id', default_value='velodyne',
+      description='Base frame of the robot.'),
+    
+    DeclareLaunchArgument(
+      'fixed_frame_id', default_value='',
+      description='Fixed frame used for lidar deskewing. If not set, we will generate one from IMU.'),
+    
+    DeclareLaunchArgument(
+      'localization', default_value='false',
+      description='Localization mode.'),
+
+    DeclareLaunchArgument(
+      'lidar_topic', default_value='/velodyne_points',
+      description='Name of the lidar PointCloud2 topic.'),
+
+    DeclareLaunchArgument(
+      'imu_topic', default_value='',
+      description='IMU topic (ignored if empty).'),
+    
+    DeclareLaunchArgument(
+      'rgbd_image_topic', default_value='',
+      description='RGBD image topic (ignored if empty). Would be the output of a rtabmap_sync\'s rgbd_sync, stereo_sync or rgb_sync node.'),
+    
+    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(
+      'qos', default_value='1',
+      description='Quality of Service: 0=system default, 1=reliable, 2=best effort.'),
+
+    OpaqueFunction(function=launch_setup),
+  ])
+
+    
diff --git a/rtabmap_examples/launch/lidar3d_assemble.launch.py b/rtabmap_examples/launch/lidar3d_assemble.launch.py
new file mode 100644
index 000000000..118cbcfd6
--- /dev/null
+++ b/rtabmap_examples/launch/lidar3d_assemble.launch.py
@@ -0,0 +1,226 @@
+# Description:
+#   In this example, we will record ALL lidar scans. An IMU or low latency odometry is required for this example.
+# 
+# Example:
+#   Launch your lidar sensor:
+#   $ ros2 launch velodyne_driver velodyne_driver_node-VLP16-launch.py
+#   $ ros2 launch velodyne_pointcloud velodyne_transform_node-VLP16-launch.py
+#   
+#   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 
+#     imu_filter_madgwick_node (with use_mag:=false publish_tf:=false)
+#     and set imu_topic to output topic of the filter.
+#
+#   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.
+#
+#   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
+
+from launch import LaunchDescription, LaunchContext
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+def launch_setup(context: LaunchContext, *args, **kwargs):
+  
+  frame_id = LaunchConfiguration('frame_id')
+
+  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"
+  
+  imu_topic = LaunchConfiguration('imu_topic')
+  
+  rgbd_image_topic = LaunchConfiguration('rgbd_image_topic')
+  rgbd_image_used =  rgbd_image_topic.perform(context) != ''
+  
+  lidar_topic = LaunchConfiguration('lidar_topic')
+  lidar_topic_value = lidar_topic.perform(context)
+  lidar_topic_deskewed = lidar_topic_value + "/deskewed"
+  
+  voxel_size = LaunchConfiguration('voxel_size')
+  voxel_size_value = float(voxel_size.perform(context))
+  
+  use_sim_time = LaunchConfiguration('use_sim_time')
+  
+  localization = LaunchConfiguration('localization').perform(context)
+  localization = localization == 'true' or localization == 'True'
+  
+  # Rule of thumb:
+  max_correspondence_distance = voxel_size_value * 10.0
+
+  shared_parameters = {
+    'use_sim_time': use_sim_time,
+    'frame_id': frame_id,
+    'qos': LaunchConfiguration('qos'),
+    'approx_sync': rgbd_image_used,
+    'wait_for_transform': 0.2,
+    # RTAB-Map's internal parameters are strings:
+    'Icp/PointToPlane': 'true',
+    'Icp/Iterations': '10',
+    'Icp/VoxelSize': str(voxel_size_value),
+    'Icp/Epsilon': '0.001',
+    'Icp/PointToPlaneK': '20',
+    'Icp/PointToPlaneRadius': '0',
+    'Icp/MaxTranslation': '3',
+    'Icp/MaxCorrespondenceDistance': str(max_correspondence_distance),
+    'Icp/Strategy': '1',
+    'Icp/OutlierRatio': '0.7',
+  }
+
+  icp_odometry_parameters = {
+    'expected_update_rate': 15.0,
+    'wait_imu_to_init': True,
+    'odom_frame_id': 'icp_odom',
+    'guess_frame_id': fixed_frame_id,
+    # RTAB-Map's internal parameters are strings:
+    'Odom/ScanKeyFrameThr': '0.4',
+    'OdomF2M/ScanSubtractRadius': str(voxel_size_value),
+    'OdomF2M/ScanMaxSize': '15000',
+    'OdomF2M/BundleAdjustment': 'false',
+    'Icp/CorrespondenceRatio': '0.01'
+  }
+
+  rtabmap_parameters = {
+    'subscribe_depth': False,
+    'subscribe_rgb': False,
+    'subscribe_odom_info': True,
+    'subscribe_scan_cloud': True,
+    '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)
+    'RGBD/ProximityMaxGraphDepth': '0',
+    'RGBD/ProximityPathMaxNeighbors': '1',
+    'RGBD/AngularUpdate': '0.05',
+    'RGBD/LinearUpdate': '0.05',
+    'RGBD/CreateOccupancyGrid': 'false',
+    'Mem/NotLinkedNodesKept': 'false',
+    'Mem/STMSize': '30',
+    'Reg/Strategy': '1',
+    'Icp/CorrespondenceRatio': '0.2'
+  }
+  
+  remappings = [('imu', imu_topic),
+                ('odom', 'icp_odom')]
+  if rgbd_image_used:
+    remappings.append(('rgbd_image', LaunchConfiguration('rgbd_image_topic')))
+    
+  arguments = []
+  if localization:
+    rtabmap_parameters['Mem/IncrementalMemory'] = 'False'
+    rtabmap_parameters['Mem/InitWMWithAllNodes'] = 'True'
+  else:
+    arguments.append('-d') # This will delete the previous database (~/.ros/rtabmap.db)
+  
+  nodes = [
+    # Lidar deskewing
+    Node(
+      package='rtabmap_util', executable='lidar_deskewing', output='screen',
+      parameters=[{
+        'use_sim_time': use_sim_time,
+        'fixed_frame_id': fixed_frame_id,
+        'wait_for_transform': 0.2}],
+      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"
+      remappings=[('cloud', lidar_topic_deskewed),
+                  ('odom', 'icp_odom')]),
+    
+    # Update the map
+    Node(
+      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,}],
+      remappings=remappings + [('scan_cloud', 'assembled_cloud')],
+      arguments=arguments), 
+
+    # Just for visualization
+    Node(
+      package='rtabmap_viz', executable='rtabmap_viz', output='screen',
+      parameters=[shared_parameters, rtabmap_parameters],
+      remappings=remappings + [('scan_cloud', 'odom_filtered_input_scan')])
+  ]
+  
+  if fixed_frame_from_imu:
+    # Create a stabilized base frame based on imu for lidar deskewing
+    nodes.append(
+      Node(
+        package='rtabmap_util', executable='imu_to_tf', output='screen',
+        parameters=[{
+          'use_sim_time': use_sim_time,
+          'fixed_frame_id': fixed_frame_id,
+          'base_frame_id': frame_id,
+          'wait_for_transform_duration': 0.001}],
+        remappings=[('imu/data', imu_topic)]))
+  
+  return nodes
+  
+def generate_launch_description():
+  return LaunchDescription([
+
+    # Launch arguments
+    DeclareLaunchArgument(
+      'use_sim_time', default_value='false',
+      description='Use simulated clock.'),
+    
+    DeclareLaunchArgument(
+      'frame_id', default_value='velodyne',
+      description='Base frame of the robot.'),
+    
+    DeclareLaunchArgument(
+      'fixed_frame_id', default_value='',
+      description='Fixed frame used for lidar deskewing. If not set, we will generate one from IMU.'),
+    
+    DeclareLaunchArgument(
+      'localization', default_value='false',
+      description='Localization mode.'),
+
+    DeclareLaunchArgument(
+      'lidar_topic', default_value='/velodyne_points',
+      description='Name of the lidar PointCloud2 topic.'),
+
+    DeclareLaunchArgument(
+      'imu_topic', default_value='/imu/data',
+      description='Name of an IMU topic.'),
+    
+    DeclareLaunchArgument(
+      'rgbd_image_topic', default_value='',
+      description='RGBD image topic (ignored if empty). Would be the output of a rtabmap_sync\'s rgbd_sync, stereo_sync or rgb_sync node.'),
+    
+    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(
+      'assembling_time', default_value='1.0',
+      description='How much time (sec) we assemble lidar scans before sending them to mapping node.'),
+
+    DeclareLaunchArgument(
+      'qos', default_value='1',
+      description='Quality of Service: 0=system default, 1=reliable, 2=best effort.'),
+
+    OpaqueFunction(function=launch_setup),
+  ])
+
+    
diff --git a/rtabmap_examples/launch/vlp16.launch.py b/rtabmap_examples/launch/vlp16.launch.py
deleted file mode 100644
index a271241c5..000000000
--- a/rtabmap_examples/launch/vlp16.launch.py
+++ /dev/null
@@ -1,127 +0,0 @@
-# Example:
-#   $ ros2 launch rtabmap_examples vlp16.launch.py
-
-import os
-
-from ament_index_python.packages import get_package_share_directory
-
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
-from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
-from launch.actions import IncludeLaunchDescription
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-
-def generate_launch_description():
-
-    deskewing = LaunchConfiguration('deskewing')
-    qos = LaunchConfiguration('qos')
-    
-    return LaunchDescription([
-
-        # Launch arguments       
-        DeclareLaunchArgument(
-            'deskewing', default_value='true',
-            description='Enable lidar deskewing'),
-        
-        DeclareLaunchArgument(
-            'qos', default_value='1',
-            description='Quality of Service: 0=system default, 1=reliable, 2=best effort'),
-          
-        # Nodes to launch
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource([os.path.join(
-                get_package_share_directory('velodyne_driver'), 'launch'),
-                '/velodyne_driver_node-VLP16-launch.py']),
-        ),
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource([os.path.join(
-                get_package_share_directory('velodyne_pointcloud'), 'launch'),
-                '/velodyne_transform_node-VLP16-launch.py']),
-        ),
-
-        Node(
-            package='rtabmap_odom', executable='icp_odometry', output='screen',
-            parameters=[{
-              'frame_id':'velodyne',
-              'odom_frame_id':'odom',
-              'wait_for_transform':0.2,
-              'expected_update_rate':15.0,
-              'deskewing':deskewing,
-              'qos':qos,
-              # RTAB-Map's internal parameters are strings:
-              'Icp/PointToPlane': 'true',
-              'Icp/Iterations': '10',
-              'Icp/VoxelSize': '0.1',
-              'Icp/Epsilon': '0.001',
-              'Icp/PointToPlaneK': '20',
-              'Icp/PointToPlaneRadius': '0',
-              'Icp/MaxTranslation': '2',
-              'Icp/MaxCorrespondenceDistance': '1',
-              'Icp/Strategy': '1',
-              'Icp/OutlierRatio': '0.7',
-              'Icp/CorrespondenceRatio': '0.01',
-              'Odom/ScanKeyFrameThr': '0.4',
-              'OdomF2M/ScanSubtractRadius': '0.1',
-              'OdomF2M/ScanMaxSize': '15000',
-              'OdomF2M/BundleAdjustment': 'false'
-            }],
-            remappings=[
-              ('scan_cloud', '/velodyne_points')
-            ]),
-       
-        Node(
-            package='rtabmap_slam', executable='rtabmap', output='screen',
-            parameters=[{
-              'frame_id':'velodyne',
-              'subscribe_depth':False,
-              'subscribe_rgb':False,
-              'subscribe_scan_cloud':True,
-              'approx_sync':False,
-              'wait_for_transform':0.2,
-              'qos':qos,
-              # RTAB-Map's internal parameters are strings:
-              'RGBD/ProximityMaxGraphDepth': '0',
-              'RGBD/ProximityPathMaxNeighbors': '1',
-              'RGBD/AngularUpdate': '0.05',
-              'RGBD/LinearUpdate': '0.05',
-              'RGBD/CreateOccupancyGrid': 'false',
-              'Mem/NotLinkedNodesKept': 'false',
-              'Mem/STMSize': '30',
-              'Mem/LaserScanNormalK': '20',
-              'Reg/Strategy': '1',
-              'Icp/VoxelSize': '0.1',
-              'Icp/PointToPlaneK': '20',
-              'Icp/PointToPlaneRadius': '0',
-              'Icp/PointToPlane': 'true',
-              'Icp/Iterations': '10',
-              'Icp/Epsilon': '0.001',
-              'Icp/MaxTranslation': '3',
-              'Icp/MaxCorrespondenceDistance': '1',
-              'Icp/Strategy': '1',
-              'Icp/OutlierRatio': '0.7',
-              'Icp/CorrespondenceRatio': '0.2'
-            }],
-            remappings=[
-              ('scan_cloud', 'odom_filtered_input_scan')
-            ],
-            arguments=[
-              '-d' # This will delete the previous database (~/.ros/rtabmap.db)
-            ]), 
-     
-        Node(
-            package='rtabmap_viz', executable='rtabmap_viz', output='screen',
-            parameters=[{
-              'frame_id':'velodyne',
-              'odom_frame_id':'odom',
-              'subscribe_odom_info':True,
-              'subscribe_scan_cloud':True,
-              'approx_sync':False,
-              'qos':qos,
-            }],
-            remappings=[
-               ('scan_cloud', 'odom_filtered_input_scan')
-            ]),
-    ])
-    
-    
diff --git a/rtabmap_examples/launch/vlp16_imu.launch.py b/rtabmap_examples/launch/vlp16_imu.launch.py
deleted file mode 100644
index 82384e5bc..000000000
--- a/rtabmap_examples/launch/vlp16_imu.launch.py
+++ /dev/null
@@ -1,183 +0,0 @@
-# Example:
-#   $ ros2 launch rtabmap_examples vlp16_imu.launch.py
-
-import os
-
-from ament_index_python.packages import get_package_share_directory
-
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, OpaqueFunction
-from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
-from launch.actions import IncludeLaunchDescription
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-
-def launch_setup(context, *args, **kwargs):      
-    qos = LaunchConfiguration('qos')
-    scan_topic = LaunchConfiguration('scan_topic')
-    imu_topic = LaunchConfiguration('imu_topic')
-    imu_filtered_topic = ''.join([imu_topic.perform(context), "/filtered"])
-    frame_id = LaunchConfiguration('frame_id')
-    frame_id_stabilized = ''.join([frame_id.perform(context), "_stabilized"])
-    
-    return [
-        #################################
-        # Hardware specific nodes - BEGIN
-        #################################
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource([os.path.join(
-                get_package_share_directory('velodyne_driver'), 'launch'),
-                '/velodyne_driver_node-VLP16-launch.py']),
-        ),
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource([os.path.join(
-                get_package_share_directory('velodyne_pointcloud'), 'launch'),
-                '/velodyne_transform_node-VLP16-launch.py']),
-        ),
-        
-        # The IMU driver publishing /imu/data (adjust imu_topic argument if your imu publishes another topic name)
-        Node(package='imu_brick', executable='imu_brick_node'), # publishing /imu/data
-        
-        # Static transform between velodyne and imu frame
-        Node(package='tf2_ros', executable='static_transform_publisher', arguments=["0", "0", "0.05", "-1.57", "0", "0", "velodyne", "imu_link"]),
-        #################################
-        # Hardware specific nodes - END
-        #################################
-        
-        # Nodes below should work "as is" with different hardware
-        
-        # You can skip the imu filter node if your imu data has already the orientation computed
-        Node(
-            package='imu_filter_madgwick', executable='imu_filter_madgwick_node', output='screen',
-            parameters=[{
-              'use_mag':False,
-              'world_frame':'enu',
-              'publish_tf':False}],
-            remappings=[
-                ('imu/data_raw', imu_topic),
-                ('imu/data', imu_filtered_topic)
-            ]),
-
-        Node(
-            package='rtabmap_util', executable='imu_to_tf', output='screen',
-            parameters=[{
-              'fixed_frame_id':frame_id_stabilized,
-              'base_frame_id':frame_id,
-              'wait_for_transform_duration': 0.001}],
-            remappings=[
-                ('imu/data', imu_filtered_topic)
-            ]),
-
-        Node(
-            package='rtabmap_odom', executable='icp_odometry', output='screen',
-            parameters=[{
-              'frame_id':frame_id,
-              'odom_frame_id':'odom',
-              'guess_frame_id':frame_id_stabilized,
-              'wait_for_transform':0.2,
-              'expected_update_rate':15.0,
-              'deskewing':True, # Scans will be deskewed internally based on guess_frame_id (i.e., our stabilized imu frame) 
-              'wait_imu_to_init': True,
-              'qos':qos,
-              # RTAB-Map's internal parameters are strings:
-              'Icp/PointToPlane': 'true',
-              'Icp/Iterations': '10',
-              'Icp/VoxelSize': '0.1',
-              'Icp/Epsilon': '0.001',
-              'Icp/PointToPlaneK': '20',
-              'Icp/PointToPlaneRadius': '0',
-              'Icp/MaxTranslation': '2',
-              'Icp/MaxCorrespondenceDistance': '1',
-              'Icp/Strategy': '1',
-              'Icp/OutlierRatio': '0.7',
-              'Icp/CorrespondenceRatio': '0.01',
-              'Odom/ScanKeyFrameThr': '0.4',
-              'OdomF2M/ScanSubtractRadius': '0.1',
-              'OdomF2M/ScanMaxSize': '15000',
-              'OdomF2M/BundleAdjustment': 'false'
-            }],
-            remappings=[
-              ('scan_cloud', scan_topic),
-              ('imu', imu_filtered_topic)
-            ]),
-        
-        Node(
-            package='rtabmap_slam', executable='rtabmap', output='screen',
-            parameters=[{
-              'frame_id':frame_id,
-              'subscribe_depth':False,
-              'subscribe_rgb':False,
-              'subscribe_scan_cloud':True,
-              'approx_sync':False,
-              'wait_for_transform':0.2,
-              'qos':qos,
-              # RTAB-Map's internal parameters are strings:
-              'RGBD/ProximityMaxGraphDepth': '0',
-              'RGBD/ProximityPathMaxNeighbors': '1',
-              'RGBD/AngularUpdate': '0.05',
-              'RGBD/LinearUpdate': '0.05',
-              'RGBD/CreateOccupancyGrid': 'false',
-              'Mem/NotLinkedNodesKept': 'false',
-              'Mem/STMSize': '30',
-              'Mem/LaserScanNormalK': '20',
-              'Reg/Strategy': '1',
-              'Icp/VoxelSize': '0.1',
-              'Icp/PointToPlaneK': '20',
-              'Icp/PointToPlaneRadius': '0',
-              'Icp/PointToPlane': 'true',
-              'Icp/Iterations': '10',
-              'Icp/Epsilon': '0.001',
-              'Icp/MaxTranslation': '3',
-              'Icp/MaxCorrespondenceDistance': '1',
-              'Icp/Strategy': '1',
-              'Icp/OutlierRatio': '0.7',
-              'Icp/CorrespondenceRatio': '0.2'
-            }],
-            remappings=[
-              ('scan_cloud', 'odom_filtered_input_scan'), # Subscribe to deskewed scans from icp_odometry
-              ('imu', imu_filtered_topic)
-            ],
-            arguments=[
-              '-d' # This will delete the previous database (~/.ros/rtabmap.db)
-            ]), 
-      
-        Node(
-            package='rtabmap_viz', executable='rtabmap_viz', output='screen',
-            parameters=[{
-              'frame_id':frame_id,
-              'odom_frame_id':'odom',
-              'subscribe_odom_info':True,
-              'subscribe_scan_cloud':True,
-              'approx_sync':False,
-              'qos':qos,
-            }],
-            remappings=[
-                ('scan_cloud', 'odom_filtered_input_scan')
-            ]),
-      ]
-
-def generate_launch_description():
-    
-    return LaunchDescription([
-
-        # Launch arguments              
-        DeclareLaunchArgument(
-            'qos', default_value='1',
-            description='Quality of Service: 0=system default, 1=reliable, 2=best effort'),
-        
-        DeclareLaunchArgument(
-            'frame_id', default_value='velodyne',
-            description='Base frame of the robot'),
-        
-        DeclareLaunchArgument(
-            'imu_topic', default_value='/imu/data',
-            description='Imu topic name'),
-        
-        DeclareLaunchArgument(
-            'scan_topic', default_value='/velodyne_points',
-            description='Scan point cloud topic name'),
-        
-        OpaqueFunction(function=launch_setup)
-    ])
-    
-    
diff --git a/rtabmap_examples/launch/vlp16_imu_assemble.launch.py b/rtabmap_examples/launch/vlp16_imu_assemble.launch.py
deleted file mode 100644
index a5f8988d8..000000000
--- a/rtabmap_examples/launch/vlp16_imu_assemble.launch.py
+++ /dev/null
@@ -1,201 +0,0 @@
-# Example:
-#   $ ros2 launch rtabmap_examples vlp16_imu_assemble.launch.py
-
-import os
-
-from ament_index_python.packages import get_package_share_directory
-
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, OpaqueFunction
-from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
-from launch.actions import IncludeLaunchDescription
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-
-def launch_setup(context, *args, **kwargs):      
-    
-    qos = LaunchConfiguration('qos')
-    scan_topic = LaunchConfiguration('scan_topic')
-    deskewed_scan_topic = ''.join([scan_topic.perform(context), "/deskewed"])
-    imu_topic = LaunchConfiguration('imu_topic')
-    imu_filtered_topic = ''.join([imu_topic.perform(context), "/filtered"])
-    frame_id = LaunchConfiguration('frame_id')
-    frame_id_stabilized = ''.join([frame_id.perform(context), "_stabilized"])
-    
-    return [
-        #################################
-        # Hardware specific nodes - BEGIN
-        #################################
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource([os.path.join(
-                get_package_share_directory('velodyne_driver'), 'launch'),
-                '/velodyne_driver_node-VLP16-launch.py']),
-        ),
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource([os.path.join(
-                get_package_share_directory('velodyne_pointcloud'), 'launch'),
-                '/velodyne_transform_node-VLP16-launch.py']),
-        ),
-        
-        # The IMU driver publishing /imu/data (adjust imu_topic argument if your imu publishes another topic name)
-        Node(package='imu_brick', executable='imu_brick_node'),
-        
-        # Static transform between velodyne and imu frame
-        Node(package='tf2_ros', executable='static_transform_publisher', arguments=["0", "0", "0.05", "-1.57", "0", "0", "velodyne", "imu_link"]),
-        #################################
-        # Hardware specific nodes - END
-        #################################
-        
-        # Nodes below should work "as is" with different hardware
-        
-        # You can skip the imu filter node if your imu data has already the orientation computed
-        Node(
-            package='imu_filter_madgwick', executable='imu_filter_madgwick_node', output='screen',
-            parameters=[{
-              'use_mag':False,
-              'world_frame':'enu',
-              'publish_tf':False}],
-            remappings=[
-                ('imu/data_raw', imu_topic),
-                ('imu/data', imu_filtered_topic)
-            ]),
-
-        Node(
-            package='rtabmap_util', executable='imu_to_tf', output='screen',
-            parameters=[{
-              'fixed_frame_id':frame_id_stabilized,
-              'base_frame_id':frame_id,
-              'wait_for_transform_duration': 0.001}],
-            remappings=[
-                ('imu/data', imu_filtered_topic)
-            ]),
-        
-        # External Deskewing
-        Node(
-            package='rtabmap_util', executable='lidar_deskewing', output='screen',
-            parameters=[{
-              'fixed_frame_id':frame_id_stabilized}],
-            remappings=[
-                ('input_cloud', scan_topic)
-            ]),
-
-        Node(
-            package='rtabmap_odom', executable='icp_odometry', output='screen',
-            parameters=[{
-              'frame_id':frame_id,
-              'odom_frame_id':'odom',
-              'guess_frame_id':frame_id_stabilized,
-              'wait_for_transform':0.2,
-              'expected_update_rate':15.0,
-              'wait_imu_to_init': True,
-              'qos':qos,
-              # RTAB-Map's internal parameters are strings:
-              'Icp/PointToPlane': 'true',
-              'Icp/Iterations': '10',
-              'Icp/VoxelSize': '0.1',
-              'Icp/Epsilon': '0.001',
-              'Icp/PointToPlaneK': '20',
-              'Icp/PointToPlaneRadius': '0',
-              'Icp/MaxTranslation': '2',
-              'Icp/MaxCorrespondenceDistance': '1',
-              'Icp/Strategy': '1',
-              'Icp/OutlierRatio': '0.7',
-              'Icp/CorrespondenceRatio': '0.01',
-              'Odom/ScanKeyFrameThr': '0.4',
-              'OdomF2M/ScanSubtractRadius': '0.1',
-              'OdomF2M/ScanMaxSize': '15000',
-              'OdomF2M/BundleAdjustment': 'false'
-            }],
-            remappings=[
-              ('scan_cloud', deskewed_scan_topic), # Subscribing to deskewed scan topic
-              ('imu', imu_filtered_topic)
-            ]),
-        
-        #Assemble deskewed scans based on icp odometry
-        Node(
-            package='rtabmap_util', executable='point_cloud_assembler', output='screen',
-            parameters=[{'assembling_time': 1.0, 
-                         'fixed_frame_id': ""}],
-            remappings=[('cloud', deskewed_scan_topic)]),
-        
-        Node(
-            package='rtabmap_slam', executable='rtabmap', output='screen',
-            parameters=[{
-              'frame_id':frame_id,
-              'subscribe_depth':False,
-              'subscribe_rgb':False,
-              'subscribe_scan_cloud':True,
-              'approx_sync':False,
-              'wait_for_transform':0.2,
-              'qos':qos,
-              # RTAB-Map's internal parameters are strings:
-              'Rtabmap/DetectionRate': '0', # Rate fixed by assembling time above (1 Hz)
-              'RGBD/ProximityMaxGraphDepth': '0',
-              'RGBD/ProximityPathMaxNeighbors': '1',
-              'RGBD/AngularUpdate': '0.05',
-              'RGBD/LinearUpdate': '0.05',
-              'RGBD/CreateOccupancyGrid': 'false',
-              'Mem/NotLinkedNodesKept': 'false',
-              'Mem/STMSize': '30',
-              'Mem/LaserScanNormalK': '20',
-              'Reg/Strategy': '1',
-              'Icp/VoxelSize': '0.1',
-              'Icp/PointToPlaneK': '20',
-              'Icp/PointToPlaneRadius': '0',
-              'Icp/PointToPlane': 'true',
-              'Icp/Iterations': '10',
-              'Icp/Epsilon': '0.001',
-              'Icp/MaxTranslation': '3',
-              'Icp/MaxCorrespondenceDistance': '1',
-              'Icp/Strategy': '1',
-              'Icp/OutlierRatio': '0.7',
-              'Icp/CorrespondenceRatio': '0.2'
-            }],
-            remappings=[
-              ('scan_cloud', 'assembled_cloud'), # We subscribe to assembled scans
-              ('imu', imu_filtered_topic)
-            ],
-            arguments=[
-              '-d' # This will delete the previous database (~/.ros/rtabmap.db)
-            ]), 
-      
-        Node(
-            package='rtabmap_viz', executable='rtabmap_viz', output='screen',
-            parameters=[{
-              'frame_id':frame_id,
-              'odom_frame_id':'odom',
-              'subscribe_odom_info':True,
-              'subscribe_scan_cloud':True,
-              'approx_sync':False,
-              'qos':qos,
-            }],
-            remappings=[
-                ('scan_cloud', deskewed_scan_topic)
-            ]),
-      ]
-
-def generate_launch_description():
-    
-    return LaunchDescription([
-
-        # Launch arguments       
-        DeclareLaunchArgument(
-            'qos', default_value='1',
-            description='Quality of Service: 0=system default, 1=reliable, 2=best effort'),
-        
-        DeclareLaunchArgument(
-            'frame_id', default_value='velodyne',
-            description='Base frame of the robot'),
-        
-        DeclareLaunchArgument(
-            'imu_topic', default_value='/imu/data',
-            description='Imu topic name'),
-        
-        DeclareLaunchArgument(
-            'scan_topic', default_value='/velodyne_points',
-            description='Scan point cloud topic name'),
-        
-        OpaqueFunction(function=launch_setup)
-    ])
-    
-    
diff --git a/rtabmap_examples/launch/vlp16_zed.launch.py b/rtabmap_examples/launch/vlp16_zed.launch.py
new file mode 100644
index 000000000..67267e1ff
--- /dev/null
+++ b/rtabmap_examples/launch/vlp16_zed.launch.py
@@ -0,0 +1,121 @@
+# Example using zed odometry for lidar deskewing:
+#   $ ros2 launch rtabmap_examples vlp16_zed.launch.py camera_model:=zed2i
+#
+# To use only zed's imu for deskewing:
+#   $ ros2 launch rtabmap_examples vlp16_zed.launch.py camera_model:=zed2i use_zed_odometry:=false
+#
+
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription, LaunchContext
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+import tempfile
+
+def launch_setup(context: LaunchContext, *args, **kwargs):
+  
+  assemble = LaunchConfiguration('assemble').perform(context)
+  assemble = assemble == 'true' or assemble == 'True'
+  
+  lidar3d_launch_file = 'lidar3d.launch.py'
+  if assemble:
+    lidar3d_launch_file = 'lidar3d_assemble.launch.py'
+    
+  use_zed_odometry = LaunchConfiguration('use_zed_odometry').perform(context)
+  use_zed_odometry = use_zed_odometry == 'true' or use_zed_odometry == 'True'
+  
+  fixed_frame_id = ''
+  if use_zed_odometry:
+    fixed_frame_id = 'odom'
+  
+  # Hack to override grab_resolution parameter without changing any files
+  with tempfile.NamedTemporaryFile(mode='w+t', delete=False) as zed_override_file:
+      zed_override_file.write("---\n"+
+                "/**:\n"+
+                "    ros__parameters:\n"+
+                "        general:\n"+
+                "            grab_resolution: 'VGA'")
+
+  return [
+    IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([os.path.join(
+            get_package_share_directory('velodyne_driver'), 'launch'),
+            '/velodyne_driver_node-VLP16-launch.py']),
+    ),
+    IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([os.path.join(
+            get_package_share_directory('velodyne_pointcloud'), 'launch'),
+            '/velodyne_transform_node-VLP16-launch.py']),
+    ),
+    
+    # Launch camera driver
+    IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([os.path.join(
+            get_package_share_directory('zed_wrapper'), 'launch'),
+            '/zed_camera.launch.py']),
+            launch_arguments={'camera_model': LaunchConfiguration('camera_model'),
+                                'ros_params_override_path': zed_override_file.name,
+                                'publish_tf': LaunchConfiguration('use_zed_odometry'), # publish VIO frame
+                                'publish_map_tf': 'false'}.items(),
+    ),
+    
+    # Static transform between zed and velodyne frame (zed will be our base frame because VIO is already linked to it)
+    Node(package='tf2_ros', executable='static_transform_publisher', arguments=["0", "0", "-0.05", "0", "0", "0", "zed_camera_link", "velodyne"]),
+    
+    # Sync rgb/depth/camera_info together
+    Node(
+        package='rtabmap_sync', executable='rgbd_sync', output='screen',
+        parameters=[{'approx_sync': False}],
+        remappings=[('rgb/image', '/zed/zed_node/rgb/image_rect_color'),
+                    ('rgb/camera_info', '/zed/zed_node/rgb/camera_info'),
+                    ('depth/image', '/zed/zed_node/depth/depth_registered')]),
+
+    IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([os.path.join(
+            get_package_share_directory('rtabmap_examples'), 'launch'),
+            '/', lidar3d_launch_file]),
+        launch_arguments={'voxel_size': LaunchConfiguration('voxel_size'),
+                          'localization': LaunchConfiguration('localization'),
+                          'frame_id': 'zed_camera_link',
+                          'lidar_topic': 'velodyne_points',
+                          'imu_topic': '/zed/zed_node/imu/data',
+                          'rgbd_image_topic': 'rgbd_image',
+                          'fixed_frame_id': fixed_frame_id}.items()),
+  ]
+  
+def generate_launch_description():
+  return LaunchDescription([
+    # Launch arguments         
+    DeclareLaunchArgument(
+      'camera_model', default_value='',
+      description="[REQUIRED] The model of the camera. Using a wrong camera model can disable camera features. Valid choices are: ['zed', 'zedm', 'zed2', 'zed2i', 'zedx', 'zedxm', 'virtual']"),
+
+    DeclareLaunchArgument(
+      'use_zed_odometry', default_value='true',
+      description='Use ZED\'s odometry for deskewing.'),
+    
+    DeclareLaunchArgument(
+      'qos', default_value='1',
+      description='Quality of Service: 0=system default, 1=reliable, 2=best effort'),
+
+    DeclareLaunchArgument(
+      'localization', default_value='false',
+      description='Localization mode.'),
+
+    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(
+      'assemble', default_value='false',
+      description='Assemble ALL lidar scans.'),
+
+    OpaqueFunction(function=launch_setup),
+  ])
\ No newline at end of file
diff --git a/rtabmap_examples/launch/vlp16_zed_vio_assemble.launch.py b/rtabmap_examples/launch/vlp16_zed_vio_assemble.launch.py
deleted file mode 100644
index ea77bf84b..000000000
--- a/rtabmap_examples/launch/vlp16_zed_vio_assemble.launch.py
+++ /dev/null
@@ -1,185 +0,0 @@
-# Example:
-#   $ ros2 launch rtabmap_examples vlp16_zed_vio_assemble.launch.py
-
-import os
-
-from ament_index_python.packages import get_package_share_directory
-
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, OpaqueFunction
-from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
-from launch.actions import IncludeLaunchDescription
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-
-import tempfile
-
-def generate_launch_description():
-    
-    qos = LaunchConfiguration('qos')
-    
-    # Hack to override grab_resolution parameter without changing any files
-    with tempfile.NamedTemporaryFile(mode='w+t', delete=False) as zed_override_file:
-        zed_override_file.write("---\n"+
-                  "/**:\n"+
-                  "    ros__parameters:\n"+
-                  "        general:\n"+
-                  "            grab_resolution: 'VGA'")
-        
-    shared_icp_parameters ={
-        'Icp/PointToPlane': 'true',
-        'Icp/Iterations': '10',
-        'Icp/VoxelSize': '0.1',
-        'Icp/Epsilon': '0.001',
-        'Icp/PointToPlaneK': '20',
-        'Icp/PointToPlaneRadius': '0',
-        'Icp/MaxTranslation': '3',
-        'Icp/MaxCorrespondenceDistance': '1',
-        'Icp/Strategy': '1',
-        'Icp/OutlierRatio': '0.7',
-    }
-    
-    return LaunchDescription([
-        
-        # Launch arguments       
-        DeclareLaunchArgument(
-            'qos', default_value='1',
-            description='Quality of Service: 0=system default, 1=reliable, 2=best effort'),
-        
-        DeclareLaunchArgument(
-            'camera_model', default_value='',
-            description="[REQUIRED] The model of the camera. Using a wrong camera model can disable camera features. Valid choices are: ['zed', 'zedm', 'zed2', 'zed2i', 'zedx', 'zedxm', 'virtual']"),
-        
-        #################################
-        # Hardware specific nodes - BEGIN
-        #################################
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource([os.path.join(
-                get_package_share_directory('velodyne_driver'), 'launch'),
-                '/velodyne_driver_node-VLP16-launch.py']),
-        ),
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource([os.path.join(
-                get_package_share_directory('velodyne_pointcloud'), 'launch'),
-                '/velodyne_transform_node-VLP16-launch.py']),
-        ),
-        
-        # Launch camera driver
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource([os.path.join(
-                get_package_share_directory('zed_wrapper'), 'launch'),
-                '/zed_camera.launch.py']),
-                launch_arguments={'camera_model': LaunchConfiguration('camera_model'),
-                                    'ros_params_override_path': zed_override_file.name,
-                                    'publish_tf': 'true', # publish VIO frame
-                                    'publish_map_tf': 'false'}.items(),
-        ),
-        
-        # Static transform between zed and velodyne frame (zed will be our base frame because VIO is already linked to it)
-        Node(package='tf2_ros', executable='static_transform_publisher', arguments=["0", "0", "-0.05", "0", "0", "0", "zed_camera_link", "velodyne"]),
-        #################################
-        # Hardware specific nodes - END
-        #################################
-        
-        # Nodes below should work "as is" with different hardware
-        
-        # Sync rgb/depth/camera_info together
-        Node(
-            package='rtabmap_sync', executable='rgbd_sync', output='screen',
-            parameters=[{'approx_sync': False}],
-            remappings=[('rgb/image', '/zed/zed_node/rgb/image_rect_color'),
-                        ('rgb/camera_info', '/zed/zed_node/rgb/camera_info'),
-                        ('depth/image', '/zed/zed_node/depth/depth_registered')]),
-        
-        # External Deskewing using VIO
-        Node(
-            package='rtabmap_util', executable='lidar_deskewing', output='screen',
-            parameters=[{
-              'fixed_frame_id':'odom',
-              'wait_for_transform':0.1}],
-            remappings=[
-                ('input_cloud', 'velodyne_points')
-            ]),
-
-        Node(
-            package='rtabmap_odom', executable='icp_odometry', output='screen',
-            parameters=[{
-              'frame_id':'zed_camera_link',
-              'odom_frame_id':'icp_odom',
-              'guess_frame_id':'odom',
-              'wait_for_transform':0.2,
-              'expected_update_rate':15.0,
-              'wait_imu_to_init': True,
-              'qos':qos,
-              # RTAB-Map's internal parameters are strings:
-              'Odom/ScanKeyFrameThr': '0.4',
-              'OdomF2M/ScanSubtractRadius': '0.1',
-              'OdomF2M/ScanMaxSize': '15000',
-              'OdomF2M/BundleAdjustment': 'false',
-              'Icp/CorrespondenceRatio': '0.01'
-            }, shared_icp_parameters],
-            remappings=[
-              ('scan_cloud', 'velodyne_points/deskewed'), # Subscribing to deskewed scan topic
-              ('imu', '/zed/zed_node/imu/data'),
-              ('odom', 'icp_odom')
-            ]),
-        
-        #Assemble deskewed scans based on icp odometry
-        Node(
-            package='rtabmap_util', executable='point_cloud_assembler', output='screen',
-            parameters=[{'assembling_time': 1.0, 
-                         'fixed_frame_id': ""}],
-            remappings=[('cloud', 'velodyne_points/deskewed'),
-                        ('odom', 'icp_odom')]),
-        
-        Node(
-            package='rtabmap_slam', executable='rtabmap', output='screen',
-            parameters=[{
-              'frame_id':'zed_camera_link',
-              'subscribe_rgbd': True,
-              'subscribe_depth':False,
-              'subscribe_rgb':False,
-              'subscribe_scan_cloud':True,
-              'approx_sync':True,
-              'wait_for_transform':0.2,
-              'qos':qos,
-              'topic_queue_size': 30,
-              'sync_queue_size': 20,
-              # RTAB-Map's internal parameters are strings:
-              'Rtabmap/DetectionRate': '0', # Rate fixed by assembling time above (1 Hz)
-              'RGBD/ProximityMaxGraphDepth': '0',
-              'RGBD/ProximityPathMaxNeighbors': '1',
-              'RGBD/AngularUpdate': '0.05',
-              'RGBD/LinearUpdate': '0.05',
-              'RGBD/CreateOccupancyGrid': 'false',
-              'Mem/NotLinkedNodesKept': 'false',
-              'Mem/STMSize': '30',
-              'Mem/LaserScanNormalK': '20',
-              'Reg/Strategy': '1',
-              'Icp/CorrespondenceRatio': '0.2'
-            }, shared_icp_parameters],
-            remappings=[
-              ('scan_cloud', 'assembled_cloud'), # We subscribe to assembled scans
-              ('imu', '/zed/zed_node/imu/data'),
-              ('odom', '/icp_odom')
-            ],
-            arguments=[
-              '-d' # This will delete the previous database (~/.ros/rtabmap.db)
-            ]), 
-      
-        Node(
-            package='rtabmap_viz', executable='rtabmap_viz', output='screen',
-            parameters=[{
-              'frame_id':'zed_camera_link',
-              'odom_frame_id':'icp_odom',
-              'subscribe_odom_info':True,
-              'subscribe_scan_cloud':True,
-              'approx_sync':False,
-              'qos':qos,
-            }],
-            remappings=[
-                ('scan_cloud', 'velodyne_points/deskewed'),
-            ]),
-      ]
-    
-    )
\ No newline at end of file