From aa1e17b1c15645f96be8c33dfc320cd574873066 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 7 Jun 2025 13:31:50 -0700 Subject: [PATCH] Updated lidar3d examples with new option "rgbd_images_topic" to input multi-camera data. --- rtabmap_examples/launch/lidar3d.launch.py | 18 +++++++++++++++--- .../launch/lidar3d_assemble.launch.py | 14 ++++++++++++-- .../launch/lidar3d_assemble_x2.launch.py | 14 ++++++++++++-- 3 files changed, 39 insertions(+), 7 deletions(-) diff --git a/rtabmap_examples/launch/lidar3d.launch.py b/rtabmap_examples/launch/lidar3d.launch.py index 89fbe205..6ddee09a 100644 --- a/rtabmap_examples/launch/lidar3d.launch.py +++ b/rtabmap_examples/launch/lidar3d.launch.py @@ -32,7 +32,9 @@ def launch_setup(context: LaunchContext, *args, **kwargs): imu_used = imu_topic.perform(context) != '' rgbd_image_topic = LaunchConfiguration('rgbd_image_topic') - rgbd_image_used = rgbd_image_topic.perform(context) != '' + rgbd_images_topic = LaunchConfiguration('rgbd_images_topic') + rgbd_image_used = rgbd_image_topic.perform(context) != '' or rgbd_images_topic.perform(context) != '' + rgbd_cameras = 0 if rgbd_images_topic.perform(context) != '' else 1 voxel_size = LaunchConfiguration('voxel_size') voxel_size_value = float(voxel_size.perform(context)) @@ -105,6 +107,7 @@ def launch_setup(context: LaunchContext, *args, **kwargs): 'subscribe_odom_info': True, 'subscribe_scan_cloud': True, 'map_frame_id': 'new_map', + 'odom_sensor_sync': True, # This will adjust camera position based on difference between lidar and camera stamps. # RTAB-Map's internal parameters are strings: 'RGBD/ProximityMaxGraphDepth': '0', 'RGBD/ProximityPathMaxNeighbors': '1', @@ -130,7 +133,10 @@ def launch_setup(context: LaunchContext, *args, **kwargs): else: remappings.append(('imu', 'imu_not_used')) if rgbd_image_used: - remappings.append(('rgbd_image', LaunchConfiguration('rgbd_image_topic'))) + if rgbd_cameras == 1: + remappings.append(('rgbd_image', LaunchConfiguration('rgbd_image_topic'))) + else: + remappings.append(('rgbd_images', LaunchConfiguration('rgbd_images_topic'))) nodes = [ Node( @@ -140,7 +146,9 @@ def launch_setup(context: LaunchContext, *args, **kwargs): Node( package='rtabmap_slam', executable='rtabmap', output='screen', - parameters=[shared_parameters, rtabmap_parameters, {'subscribe_rgbd': rgbd_image_used}], + parameters=[shared_parameters, rtabmap_parameters, + {'subscribe_rgbd': rgbd_image_used, + 'rgbd_cameras': rgbd_cameras}], remappings=remappings + [('scan_cloud', lidar_topic_deskewed)], arguments=arguments), @@ -215,6 +223,10 @@ 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( + '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( '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.'), diff --git a/rtabmap_examples/launch/lidar3d_assemble.launch.py b/rtabmap_examples/launch/lidar3d_assemble.launch.py index 030a085c..9c217640 100644 --- a/rtabmap_examples/launch/lidar3d_assemble.launch.py +++ b/rtabmap_examples/launch/lidar3d_assemble.launch.py @@ -42,7 +42,9 @@ def launch_setup(context: LaunchContext, *args, **kwargs): imu_topic = LaunchConfiguration('imu_topic') rgbd_image_topic = LaunchConfiguration('rgbd_image_topic') - rgbd_image_used = rgbd_image_topic.perform(context) != '' + rgbd_images_topic = LaunchConfiguration('rgbd_images_topic') + rgbd_image_used = rgbd_image_topic.perform(context) != '' or rgbd_images_topic.perform(context) != '' + rgbd_cameras = 0 if rgbd_images_topic.perform(context) != '' else 1 lidar_topic = LaunchConfiguration('lidar_topic') lidar_topic_value = lidar_topic.perform(context) @@ -117,7 +119,10 @@ def launch_setup(context: LaunchContext, *args, **kwargs): remappings = [('imu', imu_topic), ('odom', 'icp_odom')] if rgbd_image_used: - remappings.append(('rgbd_image', LaunchConfiguration('rgbd_image_topic'))) + if rgbd_cameras == 1: + remappings.append(('rgbd_image', LaunchConfiguration('rgbd_image_topic'))) + else: + remappings.append(('rgbd_images', LaunchConfiguration('rgbd_images_topic'))) arguments = [] if localization: @@ -159,6 +164,7 @@ def launch_setup(context: LaunchContext, *args, **kwargs): package='rtabmap_slam', executable='rtabmap', output='screen', parameters=[shared_parameters, rtabmap_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'))], @@ -233,6 +239,10 @@ 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( + '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( '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.'), diff --git a/rtabmap_examples/launch/lidar3d_assemble_x2.launch.py b/rtabmap_examples/launch/lidar3d_assemble_x2.launch.py index 8aceaab0..23da663f 100644 --- a/rtabmap_examples/launch/lidar3d_assemble_x2.launch.py +++ b/rtabmap_examples/launch/lidar3d_assemble_x2.launch.py @@ -42,7 +42,9 @@ def launch_setup(context: LaunchContext, *args, **kwargs): imu_topic = LaunchConfiguration('imu_topic') rgbd_image_topic = LaunchConfiguration('rgbd_image_topic') - rgbd_image_used = rgbd_image_topic.perform(context) != '' + rgbd_images_topic = LaunchConfiguration('rgbd_images_topic') + rgbd_image_used = rgbd_image_topic.perform(context) != '' or rgbd_images_topic.perform(context) != '' + rgbd_cameras = 0 if rgbd_images_topic.perform(context) != '' else 1 lidar1_topic = LaunchConfiguration('lidar1_topic') lidar1_topic_value = lidar1_topic.perform(context) @@ -121,7 +123,10 @@ def launch_setup(context: LaunchContext, *args, **kwargs): remappings = [('imu', imu_topic), ('odom', 'icp_odom')] if rgbd_image_used: - remappings.append(('rgbd_image', LaunchConfiguration('rgbd_image_topic'))) + if rgbd_cameras == 1: + remappings.append(('rgbd_image', LaunchConfiguration('rgbd_image_topic'))) + else: + remappings.append(('rgbd_images', LaunchConfiguration('rgbd_images_topic'))) arguments = [] if localization: @@ -187,6 +192,7 @@ def launch_setup(context: LaunchContext, *args, **kwargs): package='rtabmap_slam', executable='rtabmap', output='screen', parameters=[shared_parameters, rtabmap_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'))], @@ -265,6 +271,10 @@ 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( + '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( '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.'),