feat(slam): add rtabmap_ros
This commit is contained in:
X-lanni
@@ -0,0 +1,253 @@
|
||||
# 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, 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.
|
||||
#
|
||||
# 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_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))
|
||||
|
||||
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'
|
||||
|
||||
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 or not deskewing:
|
||||
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': 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),
|
||||
'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,
|
||||
'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',
|
||||
'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))
|
||||
}
|
||||
|
||||
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:
|
||||
if rgbd_cameras == 1:
|
||||
remappings.append(('rgbd_image', LaunchConfiguration('rgbd_image_topic')))
|
||||
else:
|
||||
remappings.append(('rgbd_images', LaunchConfiguration('rgbd_images_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,
|
||||
'rgbd_cameras': rgbd_cameras}],
|
||||
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 and deskewing:
|
||||
# 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,
|
||||
'slerp': deskewing_slerp}],
|
||||
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(
|
||||
'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',
|
||||
description='Quality of Service: 0=system default, 1=reliable, 2=best effort.'),
|
||||
|
||||
OpaqueFunction(function=launch_setup),
|
||||
])
|
||||
|
||||
|
||||
Reference in New Issue
Block a user