i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-07 10:17:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

merged ros2->jazzy-devel

Browse Source
This commit is contained in:
matlabbe
2025-06-08 16:04:29 -07:00
parent 4a1c257133 945101697a
commit 6ebb8e3184
55 changed files with 1043 additions and 221 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_examples/launch/lidar3d.launch.py
+43 -10
View File
@@ -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.
#
@@ -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))
@@ -46,13 +48,19 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
localization = LaunchConfiguration('localization').perform(context)
localization = localization == 'true' or localization == 'True'
deskewing = LaunchConfiguration('deskewing').perform(context)
deskewing = deskewing == 'true' or deskewing == '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:
fixed_frame_from_imu = True
fixed_frame_id = frame_id.perform(context) + "_stabilized"
if not fixed_frame_id:
if not fixed_frame_id or not deskewing:
lidar_topic_deskewed = lidar_topic
# Rule of thumb:
@@ -78,10 +86,11 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
}
icp_odometry_parameters = {
'expected_update_rate': 15.0,
'deskewing': not fixed_frame_id, # If fixed_frame_id is set, we do deskewing externally below
'expected_update_rate': LaunchConfiguration('expected_update_rate'),
'deskewing': not fixed_frame_id and deskewing, # 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),
@@ -97,6 +106,8 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
'subscribe_rgb': False,
'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',
@@ -106,7 +117,7 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
'Mem/NotLinkedNodesKept': 'false',
'Mem/STMSize': '30',
'Reg/Strategy': '1',
'Icp/CorrespondenceRatio': '0.2'
'Icp/CorrespondenceRatio': str(LaunchConfiguration('min_loop_closure_overlap').perform(context))
}
arguments = []
@@ -122,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(
@@ -132,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),
@@ -154,7 +170,7 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
'wait_for_transform_duration': 0.001}],
remappings=[('imu/data', imu_topic)]))
if fixed_frame_id:
if fixed_frame_id and deskewing:
# Lidar deskewing
nodes.append(
Node(
@@ -162,7 +178,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 +223,25 @@ 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.'),
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
+68 -21
View File
@@ -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,16 +28,23 @@ 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')
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)
@@ -51,6 +58,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 +84,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 +99,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,13 +113,16 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
'Mem/NotLinkedNodesKept': 'false',
'Mem/STMSize': '30',
'Reg/Strategy': '1',
'Icp/CorrespondenceRatio': '0.2'
'Icp/CorrespondenceRatio': str(LaunchConfiguration('min_loop_closure_overlap').perform(context))
}
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:
@@ -116,6 +130,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 +143,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,18 +164,27 @@ 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,}],
remappings=remappings + [('scan_cloud', 'assembled_cloud')],
'rgbd_cameras': rgbd_cameras,
'topic_queue_size': 40,
'sync_queue_size': 40,}],
remappings=remappings + [('scan_cloud', 'assembled_cloud'), ('gps/fix', LaunchConfiguration('gps_topic'))],
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')])
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 +213,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',
@@ -204,18 +231,38 @@ def generate_launch_description():
'imu_topic', default_value='/imu/data',
description='Name of an IMU topic.'),
DeclareLaunchArgument(
'gps_topic', default_value='/gps/fix',
description='Name of a GPS 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(
'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.'),
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
+305
View File
@@ -0,0 +1,305 @@
# 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_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)
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': str(LaunchConfiguration('min_loop_closure_overlap').perform(context))
}
remappings = [('imu', imu_topic),
('odom', 'icp_odom')]
if rgbd_image_used:
if rgbd_cameras == 1:
remappings.append(('rgbd_image', LaunchConfiguration('rgbd_image_topic')))
else:
remappings.append(('rgbd_images', LaunchConfiguration('rgbd_images_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,
'rgbd_cameras': rgbd_cameras,
'topic_queue_size': 40,
'sync_queue_size': 40,}],
remappings=remappings + [('scan_cloud', 'assembled_cloud'), ('gps/fix', LaunchConfiguration('gps_topic'))],
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(
'gps_topic', default_value='/gps/fix',
description='Name of a GPS 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(
'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.'),
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_examples/launch/rgbdslam_datasets.launch.py
+42 -17
View File
@@ -1,47 +1,72 @@
# Example to run rgbd datasets:
#
# [ROS1] Prepare ROS1 rosbag for conversion to ROS2
# $ wget http://vision.in.tum.de/rgbd/dataset/freiburg3/rgbd_dataset_freiburg3_long_office_household.bag
# $ rosbag decompress rgbd_dataset_freiburg3_long_office_household.bag
# $ wget https://gist.githubusercontent.com/matlabbe/897b775c38836ed8069a1397485ab024/raw/45e6ac01541973a17505000fc8c7ca399ae275eb/tum_rename_world_kinect_frame.py
# $ python3 tum_rename_world_kinect_frame.py rgbd_dataset_freiburg3_long_office_household.bag
# $ wget https://raw.githubusercontent.com/srv/srv_tools/kinetic/bag_tools/scripts/change_frame_id.py
#
# Edit change_frame_id.py, remove/comment lines beginning with "PKG" and "import roslib", change line "Exception, e" to "Exception"
# $ roscore
# $ python3 change_frame_id.py -o rgbd_dataset_freiburg3_long_office_household_frameid_fixed.bag -i rgbd_dataset_freiburg3_long_office_household.bag -f openni_rgb_optical_frame -t /camera/rgb/image_color
#
# [ROS2]
# $ sudo pip install rosbags # See https://docs.openvins.com/dev-ros1-to-ros2.html
# $ rosbags-convert rgbd_dataset_freiburg3_long_office_household_frameid_fixed.bag
# $ rosbags-convert --src rgbd_dataset_freiburg3_long_office_household_frameid_fixed.bag --dst rgbd_dataset_freiburg3_long_office_household_frameid_fixed
#
# $ ros2 launch rtabmap_examples rgbdslam_datasets.launch.py
# $ cd rgbd_dataset_freiburg3_long_office_household_frameid_fixed
# $ ros2 bag play rgbd_dataset_freiburg3_long_office_household_frameid_fixed.db3 --clock
#
# To get RMSE after the run:
# $ rtabmap-report ~/.ros/rtabmap.db
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.actions import SetParameter
def generate_launch_description():
parameters=[{
odom_parameters=[{
'frame_id':'kinect',
# ground truth here is just used to align odometry with ground truth's first pose
'ground_truth_frame_id':'world',
'ground_truth_base_frame_id':'kinect_gt',
'keep_color': True,
'wait_for_transform': 0.5,
# RTAB-Map's parameters should all be string type:
'Odom/Strategy':'0',
'Odom/ResetCountdown':'15',
'Odom/GuessSmoothingDelay':'0',
}]
slam_parameters=[{
'frame_id':'kinect',
'subscribe_depth':True,
# Record ground truth to compute RMSE
'ground_truth_frame_id':'world',
'ground_truth_base_frame_id':'kinect_gt',
'subscribe_rgb':False,
'subscribe_depth':False,
'subscribe_rgbd':True,
'subscribe_odom_info':True,
# RTAB-Map's parameters should all be string type:
'Odom/Strategy':'0',
'Odom/ResetCountdown':'15',
'Odom/GuessSmoothingDelay':'0',
'Mem/UseOdomFeatures': 'true',
'Rtabmap/StartNewMapOnLoopClosure':'true',
'RGBD/CreateOccupancyGrid':'false',
'Rtabmap/CreateIntermediateNodes':'true',
'RGBD/LinearUpdate':'0',
'RGBD/AngularUpdate':'0'}]
remappings=[
odom_remappings=[
('rgb/image', '/camera/rgb/image_color'),
('rgb/camera_info', '/camera/rgb/camera_info'),
('depth/image', '/camera/depth/image')]
# We will use the output of odometry to avoid re-extracting
# the same features on slam side.
slam_remappings=[
("rgbd_image", "odom_rgbd_image")]
return LaunchDescription([
@@ -52,19 +77,19 @@ def generate_launch_description():
# Nodes to launch
Node(
package='rtabmap_odom', executable='rgbd_odometry', output='screen',
parameters=parameters,
remappings=remappings),
parameters=odom_parameters,
remappings=odom_remappings),
Node(
package='rtabmap_slam', executable='rtabmap', output='screen',
parameters=parameters,
remappings=remappings,
parameters=slam_parameters,
remappings=slam_remappings,
arguments=['-d']),
Node(
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
parameters=parameters,
remappings=remappings),
parameters=slam_parameters,
remappings=slam_remappings),
# /tf topic is missing in the converted ROS2 bag, create a fake tf
Node(
rtabmap_examples/package.xml
+1 -1
View File
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>rtabmap_examples</name>
<version>0.21.10</version>
<version>0.22.0</version>
<description>RTAB-Map's example launch files.</description>
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>