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

Merge branch 'ros2' of github.com:introlab/rtabmap_ros into jazzy-devel

Browse Source
This commit is contained in:
matlabbe
2024-11-30 20:01:08 -08:00
parent 616ad5c028 723ac7cdba
commit e91c3bade1
134 changed files with 8008 additions and 1719 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_examples/CMakeLists.txt
+1 -1
View File
@@ -3,7 +3,7 @@ project(rtabmap_examples)
find_package(ament_cmake REQUIRED)
install(DIRECTORY launch
install(DIRECTORY launch config
DESTINATION share/${PROJECT_NAME}
)
rtabmap_examples/launch/config/euroc_left.yaml → rtabmap_examples/config/euroc_left.yaml
View File
rtabmap_examples/launch/config/euroc_right.yaml → rtabmap_examples/config/euroc_right.yaml
View File
rtabmap_examples/launch/config/slam_D405x2_config.rviz → rtabmap_examples/config/slam_D405x2_config.rviz
View File
rtabmap_examples/launch/config/slam_D405x3_config.rviz → rtabmap_examples/config/slam_D405x3_config.rviz
View File
rtabmap_examples/launch/depthai.launch.py
+72
View File
@@ -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)
])
rtabmap_examples/launch/euroc_datasets.launch.py
+2 -2
View File
@@ -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')],
rtabmap_examples/launch/k4a.launch.py
+3 -6
View File
@@ -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,
rtabmap_examples/launch/kinect_xbox_360.launch.py
+1 -2
View File
@@ -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'),
rtabmap_examples/launch/lidar3d.launch.py
+220
View File
@@ -0,0 +1,220 @@
# 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, it 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_image_used = rgbd_image_topic.perform(context) != ''
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'
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:
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': 15.0,
'deskewing': not fixed_frame_id, # If fixed_frame_id is set, we do deskewing externally below
'odom_frame_id': 'icp_odom',
'guess_frame_id': fixed_frame_id,
# 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,
# 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': '0.2'
}
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:
remappings.append(('rgbd_image', LaunchConfiguration('rgbd_image_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}],
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:
# 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}],
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(
'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(
'qos', default_value='1',
description='Quality of Service: 0=system default, 1=reliable, 2=best effort.'),
OpaqueFunction(function=launch_setup),
])
rtabmap_examples/launch/lidar3d_assemble.launch.py
+226
View File
@@ -0,0 +1,226 @@
# 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, it 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')
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"
imu_topic = LaunchConfiguration('imu_topic')
rgbd_image_topic = LaunchConfiguration('rgbd_image_topic')
rgbd_image_used = rgbd_image_topic.perform(context) != ''
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'
# 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': 15.0,
'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': True,
'subscribe_scan_cloud': True,
'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': '0.2'
}
remappings = [('imu', imu_topic),
('odom', 'icp_odom')]
if rgbd_image_used:
remappings.append(('rgbd_image', LaunchConfiguration('rgbd_image_topic')))
arguments = []
if localization:
rtabmap_parameters['Mem/IncrementalMemory'] = 'False'
rtabmap_parameters['Mem/InitWMWithAllNodes'] = 'True'
else:
arguments.append('-d') # This will delete the previous database (~/.ros/rtabmap.db)
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}],
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"
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,
'topic_queue_size': 30,
'sync_queue_size': 20,}],
remappings=remappings + [('scan_cloud', 'assembled_cloud')],
arguments=arguments),
# Just for visualization
Node(
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
parameters=[shared_parameters, rtabmap_parameters],
remappings=remappings + [('scan_cloud', '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)]))
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.'),
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(
'rgbd_image_topic', default_value='',
description='RGBD image topic (ignored if empty). Would be the output of a rtabmap_sync\'s rgbd_sync, stereo_sync or rgb_sync node.'),
DeclareLaunchArgument(
'voxel_size', default_value='0.1',
description='Voxel size (m) of the downsampled lidar point cloud. For indoor, set it between 0.1 and 0.3. For outdoor, set it to 0.5 or over.'),
DeclareLaunchArgument(
'assembling_time', default_value='1.0',
description='How much time (sec) we assemble lidar scans before sending them to mapping node.'),
DeclareLaunchArgument(
'qos', default_value='1',
description='Quality of Service: 0=system default, 1=reliable, 2=best effort.'),
OpaqueFunction(function=launch_setup),
])
rtabmap_examples/launch/realsense_d400.launch.py
+19 -8
View File
@@ -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,
rtabmap_examples/launch/realsense_d435i_color.launch.py
+31 -31
View File
@@ -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']),
])
rtabmap_examples/launch/realsense_d435i_infra.launch.py
+30 -11
View File
@@ -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']),
])
rtabmap_examples/launch/realsense_d435i_stereo.launch.py
+30 -11
View File
@@ -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']),
])
rtabmap_examples/launch/rtabmap_D405x2.launch.py
+1 -1
View File
@@ -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',
rtabmap_examples/launch/rtabmap_D405x3.launch.py
+1 -1
View File
@@ -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',
rtabmap_examples/launch/vlp16.launch.py
-126
View File
@@ -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')
]),
])
rtabmap_examples/launch/vlp16_zed.launch.py
+121
View File
@@ -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),
])
rtabmap_examples/launch/zed.launch.py
+101
View File
@@ -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)
])
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.5</version>
<version>0.21.9</version>
<description>RTAB-Map's example launch files.</description>
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>