mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-09 03:07:45 +08:00
Merge branch 'ros2' of github.com:introlab/rtabmap_ros into rolling-devel
This commit is contained in:
matlabbe
@@ -3,7 +3,7 @@ project(rtabmap_examples)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
|
||||
install(DIRECTORY launch
|
||||
install(DIRECTORY launch config
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
|
||||
@@ -0,0 +1,72 @@
|
||||
# Requirements:
|
||||
# A OAK-D camera
|
||||
# Install depthai-ros package (https://github.com/luxonis/depthai-ros)
|
||||
# Example:
|
||||
# $ ros2 launch rtabmap_examples depthai.launch.py camera_model:=OAK-D
|
||||
|
||||
import os
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
from launch.actions import IncludeLaunchDescription
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
parameters=[{'frame_id':'oak-d-base-frame',
|
||||
'subscribe_rgbd':True,
|
||||
'subscribe_odom_info':True,
|
||||
'approx_sync':False,
|
||||
'wait_imu_to_init':True}]
|
||||
|
||||
remappings=[('imu', '/imu/data')]
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
# Launch camera driver
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([os.path.join(
|
||||
get_package_share_directory('depthai_examples'), 'launch'),
|
||||
'/stereo_inertial_node.launch.py']),
|
||||
launch_arguments={'depth_aligned': 'false',
|
||||
'enableRviz': 'false',
|
||||
'monoResolution': '400p'}.items(),
|
||||
),
|
||||
|
||||
# Sync right/depth/camera_info together
|
||||
Node(
|
||||
package='rtabmap_sync', executable='rgbd_sync', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=[('rgb/image', '/right/image_rect'),
|
||||
('rgb/camera_info', '/right/camera_info'),
|
||||
('depth/image', '/stereo/depth')]),
|
||||
|
||||
# Compute quaternion of the IMU
|
||||
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')]),
|
||||
|
||||
# Visual odometry
|
||||
Node(
|
||||
package='rtabmap_odom', executable='rgbd_odometry', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=remappings),
|
||||
|
||||
# VSLAM
|
||||
Node(
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=remappings,
|
||||
arguments=['-d']),
|
||||
|
||||
# Visualization
|
||||
Node(
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=remappings)
|
||||
])
|
||||
@@ -88,7 +88,7 @@ def generate_launch_description():
|
||||
# Image rectification and publishing synchronized camera_info
|
||||
Node(
|
||||
package='rtabmap_util', executable='yaml_to_camera_info.py', output='screen',
|
||||
parameters=[{'yaml_path': [FindPackageShare('rtabmap_examples'), '/launch/config/euroc_left.yaml']}],
|
||||
parameters=[{'yaml_path': [FindPackageShare('rtabmap_examples'), '/config/euroc_left.yaml']}],
|
||||
remappings=[
|
||||
('image', '/cam0/image_raw'),
|
||||
('camera_info', 'left/camera_info')],
|
||||
@@ -96,7 +96,7 @@ def generate_launch_description():
|
||||
|
||||
Node(
|
||||
package='rtabmap_util', executable='yaml_to_camera_info.py', output='screen',
|
||||
parameters=[{'yaml_path': [FindPackageShare('rtabmap_examples'), '/launch/config/euroc_right.yaml']}],
|
||||
parameters=[{'yaml_path': [FindPackageShare('rtabmap_examples'), '/config/euroc_right.yaml']}],
|
||||
remappings=[
|
||||
('image', '/cam1/image_raw'),
|
||||
('camera_info', 'right/camera_info')],
|
||||
|
||||
@@ -1,21 +1,20 @@
|
||||
# Requirements:
|
||||
# A Kinect for Azure
|
||||
# Install Azure_Kinect_ROS_Driver ros2 package (https://github.com/microsoft/Azure_Kinect_ROS_Driver/tree/humble)
|
||||
# To install Kinect SDK on Ubuntu 22.04, see https://github.com/microsoft/Azure-Kinect-Sensor-SDK/issues/1790#issuecomment-1531626651
|
||||
# udev rules: https://github.com/microsoft/Azure-Kinect-Sensor-SDK/blob/5f79890933e1c81e325633152b2f2799df825b8b/docs/usage.md#linux-device-setup
|
||||
# Install imu_filter_madgwick ros2 package
|
||||
# Example:
|
||||
# $ ros2 launch rtabmap_examples k4a.launch.py
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
def generate_launch_description():
|
||||
parameters=[{
|
||||
'frame_id':'camera_base',
|
||||
'subscribe_rgbd':True,
|
||||
'subscribe_odom_info':True,
|
||||
'qos':1}]
|
||||
'subscribe_odom_info':True}]
|
||||
|
||||
remappings=[
|
||||
('imu', '/imu/data'),
|
||||
@@ -32,11 +31,9 @@ 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,
|
||||
'qos':1,
|
||||
'queue_size':30,
|
||||
'keep_color':True,
|
||||
# Color image needs to be rectified,
|
||||
|
||||
@@ -12,8 +12,7 @@ def generate_launch_description():
|
||||
'frame_id':'camera_link',
|
||||
'subscribe_depth':True,
|
||||
'subscribe_odom_info':True,
|
||||
'approx_sync':True,
|
||||
'qos':1}]
|
||||
'approx_sync':True}]
|
||||
|
||||
remappings=[
|
||||
('rgb/image', '/kinect/rgb/image_raw'),
|
||||
|
||||
@@ -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),
|
||||
])
|
||||
|
||||
|
||||
@@ -0,0 +1,273 @@
|
||||
# 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, 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 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')
|
||||
|
||||
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
|
||||
|
||||
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'
|
||||
|
||||
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 = lidar_topic_deskewed
|
||||
else:
|
||||
viz_topic = 'odom_filtered_input_scan'
|
||||
|
||||
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,
|
||||
'slerp': deskewing_slerp}],
|
||||
remappings=[
|
||||
('input_cloud', lidar_topic)
|
||||
]),
|
||||
|
||||
# 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': (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')]),
|
||||
|
||||
# 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', lidar_topic_deskewed)]))
|
||||
|
||||
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(
|
||||
'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(
|
||||
'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),
|
||||
])
|
||||
|
||||
|
||||
@@ -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),
|
||||
])
|
||||
|
||||
|
||||
@@ -2,16 +2,16 @@
|
||||
# A realsense D400 series
|
||||
# Install realsense2 ros2 package (make sure you have this patch: https://github.com/IntelRealSense/realsense-ros/issues/2564#issuecomment-1336288238)
|
||||
# Example:
|
||||
# $ ros2 launch realsense2_camera rs_launch.py align_depth.enable:=true
|
||||
#
|
||||
# $ ros2 launch rtabmap_examples realsense_d400.launch.py
|
||||
# OR
|
||||
# $ ros2 launch rtabmap_launch rtabmap.launch.py frame_id:=camera_link args:="-d" rgb_topic:=/camera/color/image_raw depth_topic:=/camera/aligned_depth_to_color/image_raw camera_info_topic:=/camera/color/camera_info approx_sync:=false
|
||||
|
||||
import os
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
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 Node, SetParameter
|
||||
from launch.actions import IncludeLaunchDescription
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
|
||||
def generate_launch_description():
|
||||
parameters=[{
|
||||
@@ -27,7 +27,18 @@ def generate_launch_description():
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
# Nodes to launch
|
||||
# Make sure IR emitter is enabled
|
||||
SetParameter(name='depth_module.emitter_enabled', value=1),
|
||||
|
||||
# Launch camera driver
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([os.path.join(
|
||||
get_package_share_directory('realsense2_camera'), 'launch'),
|
||||
'/rs_launch.py']),
|
||||
launch_arguments={'align_depth.enable': 'true',
|
||||
'rgb_camera.profile': '640x360x30'}.items(),
|
||||
),
|
||||
|
||||
Node(
|
||||
package='rtabmap_odom', executable='rgbd_odometry', output='screen',
|
||||
parameters=parameters,
|
||||
|
||||
@@ -2,14 +2,17 @@
|
||||
# A realsense D435i
|
||||
# Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera)
|
||||
# Example:
|
||||
# $ ros2 launch realsense2_camera rs_launch.py enable_gyro:=true enable_accel:=true unite_imu_method:=1 enable_sync:=true
|
||||
#
|
||||
# $ ros2 launch rtabmap_examples realsense_d435i_color.launch.py
|
||||
|
||||
import os
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
|
||||
from launch_ros.actions import Node, SetParameter
|
||||
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
def generate_launch_description():
|
||||
parameters=[{
|
||||
@@ -23,11 +26,32 @@ def generate_launch_description():
|
||||
('imu', '/imu/data'),
|
||||
('rgb/image', '/camera/color/image_raw'),
|
||||
('rgb/camera_info', '/camera/color/camera_info'),
|
||||
('depth/image', '/camera/realigned_depth_to_color/image_raw')]
|
||||
('depth/image', '/camera/aligned_depth_to_color/image_raw')]
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
# Nodes to launch
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument(
|
||||
'unite_imu_method', default_value='2',
|
||||
description='0-None, 1-copy, 2-linear_interpolation. Use unite_imu_method:="1" if imu topics stop being published.'),
|
||||
|
||||
# Make sure IR emitter is enabled
|
||||
SetParameter(name='depth_module.emitter_enabled', value=1),
|
||||
|
||||
# Launch camera driver
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([os.path.join(
|
||||
get_package_share_directory('realsense2_camera'), 'launch'),
|
||||
'/rs_launch.py']),
|
||||
launch_arguments={'camera_namespace': '',
|
||||
'enable_gyro': 'true',
|
||||
'enable_accel': 'true',
|
||||
'unite_imu_method': LaunchConfiguration('unite_imu_method'),
|
||||
'align_depth.enable': 'true',
|
||||
'enable_sync': 'true',
|
||||
'rgb_camera.profile': '640x360x30'}.items(),
|
||||
),
|
||||
|
||||
Node(
|
||||
package='rtabmap_odom', executable='rgbd_odometry', output='screen',
|
||||
parameters=parameters,
|
||||
@@ -43,26 +67,7 @@ def generate_launch_description():
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=remappings),
|
||||
|
||||
# Because of this issue: https://github.com/IntelRealSense/realsense-ros/issues/2564
|
||||
# Generate point cloud from not aligned depth
|
||||
Node(
|
||||
package='rtabmap_util', executable='point_cloud_xyz', output='screen',
|
||||
parameters=[{'approx_sync':False}],
|
||||
remappings=[('depth/image', '/camera/depth/image_rect_raw'),
|
||||
('depth/camera_info', '/camera/depth/camera_info'),
|
||||
('cloud', '/camera/cloud_from_depth')]),
|
||||
|
||||
# Generate aligned depth to color camera from the point cloud above
|
||||
Node(
|
||||
package='rtabmap_util', executable='pointcloud_to_depthimage', output='screen',
|
||||
parameters=[{ 'decimation':2,
|
||||
'fixed_frame_id':'camera_link',
|
||||
'fill_holes_size':1}],
|
||||
remappings=[('camera_info', '/camera/color/camera_info'),
|
||||
('cloud', '/camera/cloud_from_depth'),
|
||||
('image_raw', '/camera/realigned_depth_to_color/image_raw')]),
|
||||
|
||||
|
||||
# Compute quaternion of the IMU
|
||||
Node(
|
||||
package='imu_filter_madgwick', executable='imu_filter_madgwick_node', output='screen',
|
||||
@@ -70,9 +75,4 @@ def generate_launch_description():
|
||||
'world_frame':'enu',
|
||||
'publish_tf':False}],
|
||||
remappings=[('imu/data_raw', '/camera/imu')]),
|
||||
|
||||
# The IMU frame is missing in TF tree, add it:
|
||||
Node(
|
||||
package='tf2_ros', executable='static_transform_publisher', output='screen',
|
||||
arguments=['0', '0', '0', '0', '0', '0', 'camera_gyro_optical_frame', 'camera_imu_optical_frame']),
|
||||
])
|
||||
|
||||
@@ -2,15 +2,18 @@
|
||||
# A realsense D435i
|
||||
# Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera)
|
||||
# Example:
|
||||
# $ ros2 launch realsense2_camera rs_launch.py enable_gyro:=true enable_accel:=true unite_imu_method:=1 enable_infra1:=true enable_infra2:=true enable_sync:=true
|
||||
# $ ros2 param set /camera/camera depth_module.emitter_enabled 0
|
||||
#
|
||||
# $ ros2 launch rtabmap_examples realsense_d435i_infra.launch.py
|
||||
|
||||
import os
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
|
||||
from launch_ros.actions import Node, SetParameter
|
||||
from launch.actions import IncludeLaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
def generate_launch_description():
|
||||
parameters=[{
|
||||
@@ -28,7 +31,28 @@ def generate_launch_description():
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
# Nodes to launch
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument(
|
||||
'unite_imu_method', default_value='2',
|
||||
description='0-None, 1-copy, 2-linear_interpolation. Use unite_imu_method:="1" if imu topics stop being published.'),
|
||||
|
||||
#Hack to disable IR emitter
|
||||
SetParameter(name='depth_module.emitter_enabled', value=0),
|
||||
|
||||
# Launch camera driver
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([os.path.join(
|
||||
get_package_share_directory('realsense2_camera'), 'launch'),
|
||||
'/rs_launch.py']),
|
||||
launch_arguments={'camera_namespace': '',
|
||||
'enable_gyro': 'true',
|
||||
'enable_accel': 'true',
|
||||
'unite_imu_method': LaunchConfiguration('unite_imu_method'),
|
||||
'enable_infra1': 'true',
|
||||
'enable_infra2': 'true',
|
||||
'enable_sync': 'true'}.items(),
|
||||
),
|
||||
|
||||
Node(
|
||||
package='rtabmap_odom', executable='rgbd_odometry', output='screen',
|
||||
parameters=parameters,
|
||||
@@ -52,9 +76,4 @@ def generate_launch_description():
|
||||
'world_frame':'enu',
|
||||
'publish_tf':False}],
|
||||
remappings=[('imu/data_raw', '/camera/imu')]),
|
||||
|
||||
# The IMU frame is missing in TF tree, add it:
|
||||
Node(
|
||||
package='tf2_ros', executable='static_transform_publisher', output='screen',
|
||||
arguments=['0', '0', '0', '0', '0', '0', 'camera_gyro_optical_frame', 'camera_imu_optical_frame']),
|
||||
])
|
||||
|
||||
@@ -2,15 +2,18 @@
|
||||
# A realsense D435i
|
||||
# Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera)
|
||||
# Example:
|
||||
# $ ros2 launch realsense2_camera rs_launch.py enable_gyro:=true enable_accel:=true unite_imu_method:=1 enable_infra1:=true enable_infra2:=true enable_sync:=true
|
||||
# $ ros2 param set /camera/camera depth_module.emitter_enabled 0
|
||||
#
|
||||
# $ ros2 launch rtabmap_examples realsense_d435i_stereo.launch.py
|
||||
|
||||
import os
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
|
||||
from launch_ros.actions import Node, SetParameter
|
||||
from launch.actions import IncludeLaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
def generate_launch_description():
|
||||
parameters=[{
|
||||
@@ -28,7 +31,28 @@ def generate_launch_description():
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
# Nodes to launch
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument(
|
||||
'unite_imu_method', default_value='2',
|
||||
description='0-None, 1-copy, 2-linear_interpolation. Use unite_imu_method:="1" if imu topics stop being published.'),
|
||||
|
||||
#Hack to disable IR emitter
|
||||
SetParameter(name='depth_module.emitter_enabled', value=0),
|
||||
|
||||
# Launch camera driver
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([os.path.join(
|
||||
get_package_share_directory('realsense2_camera'), 'launch'),
|
||||
'/rs_launch.py']),
|
||||
launch_arguments={'camera_namespace': '',
|
||||
'enable_gyro': 'true',
|
||||
'enable_accel': 'true',
|
||||
'unite_imu_method': LaunchConfiguration('unite_imu_method'),
|
||||
'enable_infra1': 'true',
|
||||
'enable_infra2': 'true',
|
||||
'enable_sync': 'true'}.items(),
|
||||
),
|
||||
|
||||
Node(
|
||||
package='rtabmap_odom', executable='stereo_odometry', output='screen',
|
||||
parameters=parameters,
|
||||
@@ -52,9 +76,4 @@ def generate_launch_description():
|
||||
'world_frame':'enu',
|
||||
'publish_tf':False}],
|
||||
remappings=[('imu/data_raw', '/camera/imu')]),
|
||||
|
||||
# The IMU frame is missing in TF tree, add it:
|
||||
Node(
|
||||
package='tf2_ros', executable='static_transform_publisher', output='screen',
|
||||
arguments=['0', '0', '0', '0', '0', '0', 'camera_gyro_optical_frame', 'camera_imu_optical_frame']),
|
||||
])
|
||||
|
||||
@@ -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(
|
||||
|
||||
@@ -29,7 +29,7 @@ from ament_index_python.packages import get_package_share_directory
|
||||
def generate_launch_description():
|
||||
|
||||
config_rviz = os.path.join(
|
||||
get_package_share_directory('rtabmap_examples'), 'launch', 'config', 'slam_D405x2_config.rviz')
|
||||
get_package_share_directory('rtabmap_examples'), 'config', 'slam_D405x2_config.rviz')
|
||||
|
||||
rviz_node = launch_ros.actions.Node(
|
||||
package='rviz2', executable='rviz2', output='screen',
|
||||
|
||||
@@ -31,7 +31,7 @@ from ament_index_python.packages import get_package_share_directory
|
||||
def generate_launch_description():
|
||||
|
||||
config_rviz = os.path.join(
|
||||
get_package_share_directory('rtabmap_examples'), 'launch', 'config', 'slam_D405x3_config.rviz')
|
||||
get_package_share_directory('rtabmap_examples'), 'config', 'slam_D405x3_config.rviz')
|
||||
|
||||
rviz_node = launch_ros.actions.Node(
|
||||
package='rviz2', executable='rviz2', output='screen',
|
||||
|
||||
@@ -1,126 +0,0 @@
|
||||
# Example:
|
||||
# $ ros2 launch velodyne_driver velodyne_driver_node-VLP16-launch.py
|
||||
# $ ros2 launch velodyne_pointcloud velodyne_transform_node-VLP16-launch.py
|
||||
#
|
||||
# SLAM:
|
||||
# $ ros2 launch rtabmap_examples vlp16.launch.py
|
||||
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
use_sim_time = LaunchConfiguration('use_sim_time')
|
||||
deskewing = LaunchConfiguration('deskewing')
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument(
|
||||
'use_sim_time', default_value='false',
|
||||
description='Use simulation (Gazebo) clock if true'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'deskewing', default_value='true',
|
||||
description='Enable lidar deskewing'),
|
||||
|
||||
# Nodes to launch
|
||||
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,
|
||||
'use_sim_time':use_sim_time,
|
||||
# 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_util', executable='point_cloud_assembler', output='screen',
|
||||
parameters=[{
|
||||
'max_clouds':10,
|
||||
'fixed_frame_id':'',
|
||||
'use_sim_time':use_sim_time,
|
||||
}],
|
||||
remappings=[
|
||||
('cloud', 'odom_filtered_input_scan')
|
||||
]),
|
||||
|
||||
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,
|
||||
'use_sim_time':use_sim_time,
|
||||
# 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', 'assembled_cloud')
|
||||
],
|
||||
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,
|
||||
'use_sim_time':use_sim_time,
|
||||
}],
|
||||
remappings=[
|
||||
('scan_cloud', 'odom_filtered_input_scan')
|
||||
]),
|
||||
])
|
||||
|
||||
|
||||
@@ -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),
|
||||
])
|
||||
@@ -0,0 +1,101 @@
|
||||
# Requirements:
|
||||
# A ZED camera
|
||||
# Install zed ros2 wrapper package (https://github.com/stereolabs/zed-ros2-wrapper)
|
||||
# Example:
|
||||
# $ ros2 launch rtabmap_examples zed.launch.py camera_model:=zed2i
|
||||
|
||||
import os
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription, LaunchContext
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch_ros.actions import Node
|
||||
from launch.actions import IncludeLaunchDescription, OpaqueFunction
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.conditions import UnlessCondition
|
||||
|
||||
import tempfile
|
||||
|
||||
parameters = []
|
||||
remappings = []
|
||||
|
||||
def launch_setup(context: LaunchContext, *args, **kwargs):
|
||||
|
||||
# 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'")
|
||||
|
||||
parameters=[{'frame_id':'zed_camera_link',
|
||||
'subscribe_rgbd':True,
|
||||
'approx_sync':False,
|
||||
'wait_imu_to_init':True}]
|
||||
|
||||
remappings=[('imu', '/zed/zed_node/imu/data')]
|
||||
|
||||
if LaunchConfiguration('use_zed_odometry').perform(context) in ["True", "true"]:
|
||||
remappings.append(('odom', '/zed/zed_node/odom'))
|
||||
else:
|
||||
parameters.append({'subscribe_odom_info': True})
|
||||
|
||||
return [
|
||||
# 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_map_tf': 'false'}.items(),
|
||||
),
|
||||
|
||||
# Sync rgb/depth/camera_info together
|
||||
Node(
|
||||
package='rtabmap_sync', executable='rgbd_sync', output='screen',
|
||||
parameters=parameters,
|
||||
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')]),
|
||||
|
||||
# Visual odometry
|
||||
Node(
|
||||
package='rtabmap_odom', executable='rgbd_odometry', output='screen',
|
||||
condition=UnlessCondition(LaunchConfiguration('use_zed_odometry')),
|
||||
parameters=parameters,
|
||||
remappings=remappings,),
|
||||
|
||||
# VSLAM
|
||||
Node(
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=remappings,
|
||||
arguments=['-d']),
|
||||
|
||||
# Visualization
|
||||
Node(
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=remappings)
|
||||
]
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument(
|
||||
'use_zed_odometry', default_value='false',
|
||||
description='Use zed\'s computed odometry instead of using rtabmap\'s odometry.'),
|
||||
|
||||
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']"),
|
||||
|
||||
OpaqueFunction(function=launch_setup)
|
||||
])
|
||||
@@ -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.5</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>
|
||||
|
||||
Reference in New Issue
Block a user