Updated lidar3d examples (and new 2x lidars example)

rtabmap_examples/launch/lidar3d.launch.py

@@ -8,7 +8,7 @@
 #
 #   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 
+#     already the orientation estimated, if 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.
 #
@@ -46,6 +46,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'
+  
   fixed_frame_from_imu = False
   fixed_frame_id =  LaunchConfiguration('fixed_frame_id').perform(context)
   if not fixed_frame_id and imu_used:
@@ -78,10 +81,11 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
   }
   
   icp_odometry_parameters = {
-    'expected_update_rate': 15.0,
+    'expected_update_rate': LaunchConfiguration('expected_update_rate'),
     '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,
+    'deskewing_slerp': deskewing_slerp,
     # RTAB-Map's internal parameters are strings:
     'Odom/ScanKeyFrameThr': '0.4',
     'OdomF2M/ScanSubtractRadius': str(voxel_size_value),
@@ -106,7 +110,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')
   }
   
   arguments = []
@@ -162,7 +166,8 @@ 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)
         ])
@@ -206,9 +211,21 @@ def generate_launch_description():
       '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(
+      '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(
       '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(
+      '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',

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
+    if external_odom_frame_id:
-    fixed_frame_id = frame_id.perform(context) + "_stabilized"
+      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,
+                   'topic_queue_size': 40,
-                   'sync_queue_size': 20,}],
+                   '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.'),

rtabmap_examples/launch/lidar3d_assemble_x2.launch.py

@@ -0,0 +1,291 @@
+# Description:
+#   In this example, we will record ALL lidar scans from 2 lidars. An IMU or low latency odometry is required for this example.
+# 
+# Example:
+#   Launch your lidar sensors
+#     In this example, we assume the lidar topics have a frame_id linked to same parent (e.g., base_link) and
+#     the extrinsics are known (URDF) and/or already calibrated.
+#   
+#   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, 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.
+#
+#   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 topics, imu topic and base frame:
+#   $ ros2 launch rtabmap_examples lidar3d.launch.py lidar1_topic:=/lidar1/velodyne_points lidar2_topic:=/lidar1/velodyne_points imu_topic:=/imu/data frame_id:=base_link
+
+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')
+  
+  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:
+    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')
+  
+  rgbd_image_topic = LaunchConfiguration('rgbd_image_topic')
+  rgbd_image_used =  rgbd_image_topic.perform(context) != ''
+  
+  lidar1_topic = LaunchConfiguration('lidar1_topic')
+  lidar1_topic_value = lidar1_topic.perform(context)
+  lidar1_topic_deskewed = lidar1_topic_value + "/deskewed"
+  
+  lidar2_topic = LaunchConfiguration('lidar2_topic')
+  lidar2_topic_value = lidar2_topic.perform(context)
+  lidar2_topic_deskewed = lidar2_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'
+  
+  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
+
+  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': LaunchConfiguration('expected_update_rate'),
+    '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': 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)
+    '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': LaunchConfiguration('min_loop_closure_overlap')
+  }
+  
+  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)
+  
+  if external_odom_frame_id:
+    viz_topic = "combined_cloud"
+  else:
+    viz_topic = 'odom_filtered_input_scan'
+  
+  nodes = [
+    # Lidar1 deskewing
+    Node(
+      package='rtabmap_util', executable='lidar_deskewing', name="lidar1_deskewing", output='screen',
+      parameters=[{
+        'use_sim_time': use_sim_time,
+        'fixed_frame_id': fixed_frame_id,
+        'wait_for_transform': 0.2,
+        'slerp': deskewing_slerp}],
+      remappings=[
+          ('input_cloud', lidar1_topic)
+      ]),
+
+    # Lidar2 deskewing
+    Node(
+      package='rtabmap_util', executable='lidar_deskewing', name="lidar2_deskewing", output='screen',
+      parameters=[{
+        'use_sim_time': use_sim_time,
+        'fixed_frame_id': fixed_frame_id,
+        'wait_for_transform': 0.2,
+        'slerp': deskewing_slerp}],
+      remappings=[
+          ('input_cloud', lidar2_topic)
+      ]),
+
+    # Combine the two lidars in single point cloud
+    Node(
+      package='rtabmap_util', executable='point_cloud_aggregator', output='screen',
+      parameters=[{
+        'use_sim_time': use_sim_time,
+        'approx_sync': True,
+        'fixed_frame_id': fixed_frame_id,
+        'count': 2}],
+      remappings=[
+        ('cloud1', lidar1_topic_deskewed),
+        ('cloud2', lidar2_topic_deskewed)]),
+    
+    # Assemble combined 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': (external_odom_frame_id if external_odom_frame_id else "")}], # This will make the node subscribing to icp odometry topic "icp_odom"
+      remappings=[('cloud', "combined_cloud"),
+                  ('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': 40,
+                   'sync_queue_size': 40,}],
+      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', 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', "combined_cloud")]))
+  
+  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 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',
+      description='Localization mode.'),
+
+    DeclareLaunchArgument(
+      'lidar1_topic', default_value='/lidar1/velodyne_points',
+      description='Name of the lidar1\'s PointCloud2 topic.'),
+    
+    DeclareLaunchArgument(
+      'lidar2_topic', default_value='/lidar2/velodyne_points',
+      description='Name of the lidar2\'s 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(
+      '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.'),
+
+    OpaqueFunction(function=launch_setup),
+  ])
+
+    

rtabmap_util/src/nodelets/point_cloud_aggregator.cpp

@@ -111,10 +111,10 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
 				get_name(),
 				approx?"approx":"exact",
 				approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
-				cloudSub_1_.getTopic().c_str(),
+				cloudSub_1_.getSubscriber()->get_topic_name(),
-				cloudSub_2_.getTopic().c_str(),
+				cloudSub_2_.getSubscriber()->get_topic_name(),
-				cloudSub_3_.getTopic().c_str(),
+				cloudSub_3_.getSubscriber()->get_topic_name(),
-				cloudSub_4_.getTopic().c_str());
+				cloudSub_4_.getSubscriber()->get_topic_name());
 	}
 	else if(count == 3)
 	{
@@ -135,9 +135,9 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
 				this->get_name(),
 				approx?"approx":"exact",
 				approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
-				cloudSub_1_.getTopic().c_str(),
+				cloudSub_1_.getSubscriber()->get_topic_name(),
-				cloudSub_2_.getTopic().c_str(),
+				cloudSub_2_.getSubscriber()->get_topic_name(),
-				cloudSub_3_.getTopic().c_str());
+				cloudSub_3_.getSubscriber()->get_topic_name());
 	}
 	else
 	{
@@ -157,8 +157,8 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
 				this->get_name(),
 				approx?"approx":"exact",
 				approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
-				cloudSub_1_.getTopic().c_str(),
+				cloudSub_1_.getSubscriber()->get_topic_name(),
-				cloudSub_2_.getTopic().c_str());
+				cloudSub_2_.getSubscriber()->get_topic_name());
 	}
 
 
@@ -175,10 +175,11 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
 						this->get_name(),
 						approx?"":"Parameter \"approx_sync\" is false, which means that input "
 							"topics should have all the exact timestamp for the callback to be called.",
-							subscribedTopicsMsg.c_str());
+						subscribedTopicsMsg.c_str());
 			}
 		}
 	});
+	RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg.c_str());
 }
 
 PointCloudAggregator::~PointCloudAggregator()

rtabmap_util/src/nodelets/point_cloud_assembler.cpp

@@ -154,9 +154,9 @@ PointCloudAssembler::PointCloudAssembler(const rclcpp::NodeOptions & options) :
 		exactInfoSync_->registerCallback(std::bind(&rtabmap_util::PointCloudAssembler::callbackCloudOdomInfo, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 		subscribedTopicsMsg_ = uFormat("\n%s subscribed to (exact sync):\n   %s,\n   %s",
 							get_name(),
-							syncCloudSub_.getTopic().c_str(),
+							syncCloudSub_.getSubscriber()->get_topic_name(),
-							syncOdomSub_.getTopic().c_str(),
+							syncOdomSub_.getSubscriber()->get_topic_name(),
-							syncOdomInfoSub_.getTopic().c_str());
+							syncOdomInfoSub_.getSubscriber()->get_topic_name());
 	}
 	else
 	{
@@ -166,8 +166,8 @@ PointCloudAssembler::PointCloudAssembler(const rclcpp::NodeOptions & options) :
 		exactSync_->registerCallback(std::bind(&rtabmap_util::PointCloudAssembler::callbackCloudOdom, this, std::placeholders::_1, std::placeholders::_2));
 		subscribedTopicsMsg_ = uFormat("\n%s subscribed to (exact sync):\n   %s,\n   %s",
 							get_name(),
-							syncCloudSub_.getTopic().c_str(),
+							syncCloudSub_.getSubscriber()->get_topic_name(),
-							syncOdomSub_.getTopic().c_str());
+							syncOdomSub_.getSubscriber()->get_topic_name());
 	}
 
 	warningThread_ = new std::thread([&](){