i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-09-13 14:50:19 +08:00
Code Issues Packages Projects Releases Wiki Activity

ROS2 various QOL updates (preparing new binary release) (#1225)

Browse Source 
* Updating examples and README

* updated main readme

* odom: Added always_check_imu_tf parameter. sync: increased default sync_queue_size from 2 to 5 (to be more flexible to different hardware), added input and output diagnostics. rtabmap_viz: fixed node with same name redeclared warning.

* update readme

* Added husky demo

* converted demo_robot_mapping.launch to ros2

* Converted stereo_outdoor demo from ros1 to ros2

* Converted multi-session demo to ros2

* Converted demo_find_object launch to ros2

* renamed files

* uniformized default qos, increased sync_queue_size default to 10

* Added vlp16 + imu example

* Added husky 3D lidar demos

* moved demos in subdir

* Added vlp16+zed example

* rtabmap_viz: ignore odom update if tf not ready when not using topic (to avoid showing red screen). Added champ VSLAM demo.

* forwarded camera_model arg for zed examples, removed qos for turtlebot4 demo

* pointcloud_to_depthimage: added more error logs, removed output camera_info published twice and match ros1 namespaces

* Created two base general examples for 3D lidar usage, then create hardware specific examples on top of them.

* Added isaac sim demo

* Final update of all demos. Fixed MapCloud rviz plugin crashing when floor/ceiling filtering is used and resulting cloud is empty.

* Fixed 2d scan deskewing output frame, moved nav2 params under "params" folder, added number of topics processed/dropped for odometry, added icp_odometry option for turtlebot3 scan-only demo.

* rtabmap_demos: added README with examples

* Added TOC

* bump version to 0.21.9
This commit is contained in:
matlabbe
2024-11-30 17:28:16 -08:00
committed by GitHub
parent e9aa8ed082
commit 3f6adad463
116 changed files with 6102 additions and 575 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/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_d435i_color.launch.py
+8 -7
View File
@@ -10,8 +10,9 @@ from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
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
def generate_launch_description():
parameters=[{
@@ -29,6 +30,11 @@ def generate_launch_description():
return LaunchDescription([
# 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),
@@ -40,7 +46,7 @@ def generate_launch_description():
launch_arguments={'camera_namespace': '',
'enable_gyro': 'true',
'enable_accel': 'true',
'unite_imu_method': '2',
'unite_imu_method': LaunchConfiguration('unite_imu_method'),
'align_depth.enable': 'true',
'enable_sync': 'true',
'rgb_camera.profile': '640x360x30'}.items(),
@@ -69,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
+8 -6
View File
@@ -11,7 +11,9 @@ from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
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
def generate_launch_description():
parameters=[{
@@ -29,6 +31,11 @@ def generate_launch_description():
return LaunchDescription([
# 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),
@@ -40,7 +47,7 @@ def generate_launch_description():
launch_arguments={'camera_namespace': '',
'enable_gyro': 'true',
'enable_accel': 'true',
'unite_imu_method': '2',
'unite_imu_method': LaunchConfiguration('unite_imu_method'),
'enable_infra1': 'true',
'enable_infra2': 'true',
'enable_sync': 'true'}.items(),
@@ -69,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
+8 -6
View File
@@ -11,7 +11,9 @@ from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
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
def generate_launch_description():
parameters=[{
@@ -29,6 +31,11 @@ def generate_launch_description():
return LaunchDescription([
# 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),
@@ -40,7 +47,7 @@ def generate_launch_description():
launch_arguments={'camera_namespace': '',
'enable_gyro': 'true',
'enable_accel': 'true',
'unite_imu_method': '2',
'unite_imu_method': LaunchConfiguration('unite_imu_method'),
'enable_infra1': 'true',
'enable_infra2': 'true',
'enable_sync': 'true'}.items(),
@@ -69,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
-127
View File
@@ -1,127 +0,0 @@
# Example:
# $ ros2 launch rtabmap_examples vlp16.launch.py
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
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
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']),
),
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_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', 'odom_filtered_input_scan')
],
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
+5 -1
View File
@@ -55,7 +55,7 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
'publish_map_tf': 'false'}.items(),
),
# Sync right/depth/camera_info together
# Sync rgb/depth/camera_info together
Node(
package='rtabmap_sync', executable='rgbd_sync', output='screen',
parameters=parameters,
@@ -93,5 +93,9 @@ def generate_launch_description():
'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="matlabbe@gmail.com">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>