mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-04 16:57:46 +08:00
ported rtabmap_ros_pkg_split to ros2
This commit is contained in:
matlabbe
@@ -0,0 +1,146 @@
|
||||
|
||||
# Example to run euroc datasets:
|
||||
# $ sudo pip install rosbags # See https://docs.openvins.com/dev-ros1-to-ros2.html
|
||||
# $ rosbags-convert V1_01_easy.bag
|
||||
# $ rosbags-convert MH_01_easy.bag
|
||||
#
|
||||
# $ ros2 launch rtabmap_ros euroc_datasets.launch.py gt:=true
|
||||
# $ cd V1_01_easy
|
||||
# $ ros2 bag play V1_01_easy.db3 --clock
|
||||
#
|
||||
# $ ros2 launch rtabmap_ros euroc_datasets.launch.py gt:=false
|
||||
# $ cd MH_01_easy
|
||||
# $ ros2 bag play MH_01_easy.db3 --clock
|
||||
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import GroupAction
|
||||
from launch.actions import IncludeLaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.actions import SetEnvironmentVariable
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.conditions import IfCondition, UnlessCondition
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.actions import PushRosNamespace
|
||||
from launch_ros.actions import SetParameter
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
ground_truth = LaunchConfiguration('gt')
|
||||
|
||||
parameters={
|
||||
'frame_id':'base_link',
|
||||
'subscribe_stereo':True,
|
||||
'subscribe_odom_info':True,
|
||||
'wait_imu_to_init':True,
|
||||
'approx_sync':False,
|
||||
# RTAB-Map's parameters should all be string type:
|
||||
'RGBD/CreateOccupancyGrid':'false',
|
||||
'Rtabmap/CreateIntermediateNodes':'true',
|
||||
'RGBD/LinearUpdate':'0',
|
||||
'RGBD/AngularUpdate':'0'}
|
||||
|
||||
remappings=[
|
||||
('left/image_rect', '/stereo_camera/left/image_rect'),
|
||||
('left/camera_info', '/stereo_camera/left/camera_info'),
|
||||
('right/image_rect', '/stereo_camera/right/image_rect'),
|
||||
('right/camera_info', '/stereo_camera/right/camera_info'),
|
||||
('imu', '/imu/data')]
|
||||
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'gt', default_value='false',
|
||||
description='If the VH rosbag sequence is used, you can enable ground truth.'),
|
||||
|
||||
SetParameter(name='use_sim_time', value=True),
|
||||
# 'use_sim_time' will be set on all nodes following the line above
|
||||
|
||||
# Nodes to launch
|
||||
Node(
|
||||
package='rtabmap_ros', executable='stereo_odometry', output='screen',
|
||||
parameters=[parameters],
|
||||
remappings=remappings),
|
||||
|
||||
Node(
|
||||
condition=IfCondition(ground_truth),
|
||||
package='rtabmap_ros', executable='rtabmap', output='screen',
|
||||
parameters=[parameters,
|
||||
{ 'ground_truth_frame_id':'world',
|
||||
'ground_truth_base_frame_id':'base_link_gt'}],
|
||||
remappings=remappings,
|
||||
arguments=['-d']),
|
||||
Node(
|
||||
condition=UnlessCondition(ground_truth),
|
||||
package='rtabmap_ros', executable='rtabmap', output='screen',
|
||||
parameters=[parameters],
|
||||
remappings=remappings,
|
||||
arguments=['-d']),
|
||||
|
||||
Node(
|
||||
package='rtabmap_ros', executable='rtabmapviz', output='screen',
|
||||
parameters=[parameters],
|
||||
remappings=remappings),
|
||||
|
||||
# Image rectification and publishing synchronized camera_info
|
||||
Node(
|
||||
package='rtabmap_ros', executable='yaml_to_camera_info.py', output='screen',
|
||||
parameters=[{'yaml_path': [FindPackageShare('rtabmap_ros'), '/launch/calibration/euroc_left.yaml']}],
|
||||
remappings=[
|
||||
('image', '/cam0/image_raw'),
|
||||
('camera_info', 'left/camera_info')],
|
||||
namespace='stereo_camera'),
|
||||
|
||||
Node(
|
||||
package='rtabmap_ros', executable='yaml_to_camera_info.py', output='screen',
|
||||
parameters=[{'yaml_path': [FindPackageShare('rtabmap_ros'), '/launch/calibration/euroc_right.yaml']}],
|
||||
remappings=[
|
||||
('image', '/cam1/image_raw'),
|
||||
('camera_info', 'right/camera_info')],
|
||||
namespace='stereo_camera'),
|
||||
|
||||
Node(
|
||||
package='image_proc', executable='image_proc', output='screen',
|
||||
remappings=[
|
||||
('image_raw', '/cam0/image_raw'),
|
||||
('image', '/cam0/image_raw')],
|
||||
namespace='stereo_camera/left'),
|
||||
Node(
|
||||
package='image_proc', executable='image_proc', output='screen',
|
||||
remappings=[
|
||||
('image_raw', '/cam1/image_raw'),
|
||||
('image', '/cam1/image_raw')],
|
||||
namespace='stereo_camera/right'),
|
||||
|
||||
Node(
|
||||
package='imu_complementary_filter', executable='complementary_filter_node', output='screen',
|
||||
parameters=[{'use_mag': False, 'world_frame':'enu', 'publish_tf':False}],
|
||||
remappings=[('imu/data_raw', '/imu0')]),
|
||||
|
||||
# create a fake tf tree
|
||||
Node(
|
||||
package='tf2_ros', executable='static_transform_publisher', output='screen',
|
||||
arguments=['0', '0', '0', '3.1415926', '-1.570796', '0', 'base_link', 'imu4']),
|
||||
Node(
|
||||
package='tf2_ros', executable='static_transform_publisher', output='screen',
|
||||
arguments=['-0.021640', '-0.064677', '0.009811', '1.555925', '0.025777', '0.003757', 'imu4', 'cam0']),
|
||||
Node(
|
||||
package='tf2_ros', executable='static_transform_publisher', output='screen',
|
||||
arguments=['-0.019844', '0.045369', '0.007862', '1.558237', '0.025393', '0.017907', 'imu4', 'cam1']),
|
||||
Node(
|
||||
package='tf2_ros', executable='static_transform_publisher', output='screen',
|
||||
arguments=['0.12395', '-0.02781', '-0.06901', '0', '0', '0', 'vicon/firefly_sbx/firefly_sbx', 'base_link_gt']),
|
||||
Node(
|
||||
package='tf2_ros', executable='static_transform_publisher', output='screen',
|
||||
arguments=['0', '0', '0', '0', '0', '0', 'world', 'map']),
|
||||
|
||||
Node(
|
||||
package='rtabmap_ros', executable='transform_to_tf.py', output='screen',
|
||||
parameters=[{'frame_id': 'world', 'child_frame_id': 'vicon/firefly_sbx/firefly_sbx'}],
|
||||
remappings=[('transform', '/vicon/firefly_sbx/firefly_sbx')]),
|
||||
])
|
||||
|
||||
|
||||
@@ -0,0 +1,74 @@
|
||||
# Requirements:
|
||||
# A Kinect for Azure
|
||||
# Install Azure_Kinect_ROS_Driver ros2 package (https://github.com/microsoft/Azure_Kinect_ROS_Driver/tree/humble)
|
||||
# Install imu_filter_madgwick ros2 package
|
||||
# Example:
|
||||
# $ ros2 launch rtabmap_ros 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}]
|
||||
|
||||
remappings=[
|
||||
('imu', '/imu/data'),
|
||||
('rgb/image', '/rgb/image_raw'),
|
||||
('rgb/camera_info', '/rgb/camera_info'),
|
||||
('depth/image', '/depth_to_rgb/image_raw'),
|
||||
('rgbd_image', 'odom_rgbd_image')]
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
# Nodes to launch
|
||||
|
||||
# Visual odometry node
|
||||
Node(
|
||||
package='rtabmap_ros', 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,
|
||||
# this will tell vo to rectify them for convenience:
|
||||
'Rtabmap/ImagesAlreadyRectified':'False'}],
|
||||
remappings=remappings),
|
||||
|
||||
# SLAM node
|
||||
Node(
|
||||
package='rtabmap_ros', executable='rtabmap', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=remappings,
|
||||
arguments=['-d']),
|
||||
|
||||
# Visualization node
|
||||
Node(
|
||||
package='rtabmap_ros', executable='rtabmapviz', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=remappings),
|
||||
|
||||
# Kinect for azure
|
||||
Node(
|
||||
package='azure_kinect_ros_driver', executable='node', output='screen',
|
||||
parameters=[{'color_enabled': True,
|
||||
'fps':15,
|
||||
'depth_mode':'WFOV_2X2BINNED'}]),
|
||||
|
||||
# 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')]),
|
||||
])
|
||||
@@ -0,0 +1,46 @@
|
||||
# Requirements:
|
||||
# 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_ros realsense_d400.launch.py
|
||||
# OR
|
||||
# $ ros2 launch rtabmap_ros 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
|
||||
|
||||
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_link',
|
||||
'subscribe_depth':True,
|
||||
'subscribe_odom_info':True,
|
||||
'approx_sync':False}]
|
||||
|
||||
remappings=[
|
||||
('rgb/image', '/camera/color/image_raw'),
|
||||
('rgb/camera_info', '/camera/color/camera_info'),
|
||||
('depth/image', '/camera/aligned_depth_to_color/image_raw')]
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
# Nodes to launch
|
||||
Node(
|
||||
package='rtabmap_ros', executable='rgbd_odometry', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=remappings),
|
||||
|
||||
Node(
|
||||
package='rtabmap_ros', executable='rtabmap', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=remappings,
|
||||
arguments=['-d']),
|
||||
|
||||
Node(
|
||||
package='rtabmap_ros', executable='rtabmapviz', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=remappings),
|
||||
])
|
||||
@@ -0,0 +1,78 @@
|
||||
# Requirements:
|
||||
# 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_ros realsense_d435i_color.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_link',
|
||||
'subscribe_depth':True,
|
||||
'subscribe_odom_info':True,
|
||||
'approx_sync':False,
|
||||
'wait_imu_to_init':True}]
|
||||
|
||||
remappings=[
|
||||
('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')]
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
# Nodes to launch
|
||||
Node(
|
||||
package='rtabmap_ros', executable='rgbd_odometry', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=remappings),
|
||||
|
||||
Node(
|
||||
package='rtabmap_ros', executable='rtabmap', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=remappings,
|
||||
arguments=['-d']),
|
||||
|
||||
Node(
|
||||
package='rtabmap_ros', executable='rtabmapviz', 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_ros', 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_ros', 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',
|
||||
parameters=[{'use_mag': False,
|
||||
'world_frame':'enu',
|
||||
'publish_tf':False}],
|
||||
remappings=[('imu/data_raw', '/camera/imu')]),
|
||||
|
||||
# The IMU frame is mising 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']),
|
||||
])
|
||||
@@ -0,0 +1,60 @@
|
||||
# Requirements:
|
||||
# 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_ros realsense_d435i_infra.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_link',
|
||||
'subscribe_depth':True,
|
||||
'subscribe_odom_info':True,
|
||||
'approx_sync':False,
|
||||
'wait_imu_to_init':True}]
|
||||
|
||||
remappings=[
|
||||
('imu', '/imu/data'),
|
||||
('rgb/image', '/camera/infra1/image_rect_raw'),
|
||||
('rgb/camera_info', '/camera/infra1/camera_info'),
|
||||
('depth/image', '/camera/depth/image_rect_raw')]
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
# Nodes to launch
|
||||
Node(
|
||||
package='rtabmap_ros', executable='rgbd_odometry', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=remappings),
|
||||
|
||||
Node(
|
||||
package='rtabmap_ros', executable='rtabmap', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=remappings,
|
||||
arguments=['-d']),
|
||||
|
||||
Node(
|
||||
package='rtabmap_ros', executable='rtabmapviz', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=remappings),
|
||||
|
||||
# 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', '/camera/imu')]),
|
||||
|
||||
# The IMU frame is mising 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']),
|
||||
])
|
||||
@@ -0,0 +1,60 @@
|
||||
# Requirements:
|
||||
# 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_ros realsense_d435i_stereo.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_link',
|
||||
'subscribe_stereo':True,
|
||||
'subscribe_odom_info':True,
|
||||
'wait_imu_to_init':True}]
|
||||
|
||||
remappings=[
|
||||
('imu', '/imu/data'),
|
||||
('left/image_rect', '/camera/infra1/image_rect_raw'),
|
||||
('left/camera_info', '/camera/infra1/camera_info'),
|
||||
('right/image_rect', '/camera/infra2/image_rect_raw'),
|
||||
('right/camera_info', '/camera/infra2/camera_info')]
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
# Nodes to launch
|
||||
Node(
|
||||
package='rtabmap_ros', executable='stereo_odometry', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=remappings),
|
||||
|
||||
Node(
|
||||
package='rtabmap_ros', executable='rtabmap', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=remappings,
|
||||
arguments=['-d']),
|
||||
|
||||
Node(
|
||||
package='rtabmap_ros', executable='rtabmapviz', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=remappings),
|
||||
|
||||
# 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', '/camera/imu')]),
|
||||
|
||||
# The IMU frame is mising 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']),
|
||||
])
|
||||
@@ -0,0 +1,73 @@
|
||||
|
||||
# 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://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
|
||||
|
||||
# $ ros2 launch rtabmap_ros 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
|
||||
|
||||
|
||||
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=[{
|
||||
'frame_id':'kinect',
|
||||
'subscribe_depth':True,
|
||||
'subscribe_odom_info':True,
|
||||
# RTAB-Map's parameters should all be string type:
|
||||
'Odom/Strategy':'0',
|
||||
'Odom/ResetCountdown':'15',
|
||||
'Odom/GuessSmoothingDelay':'0',
|
||||
'Rtabmap/StartNewMapOnLoopClosure':'true',
|
||||
'RGBD/CreateOccupancyGrid':'false',
|
||||
'Rtabmap/CreateIntermediateNodes':'true',
|
||||
'RGBD/LinearUpdate':'0',
|
||||
'RGBD/AngularUpdate':'0'}]
|
||||
|
||||
remappings=[
|
||||
('rgb/image', '/camera/rgb/image_color'),
|
||||
('rgb/camera_info', '/camera/rgb/camera_info'),
|
||||
('depth/image', '/camera/depth/image')]
|
||||
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
SetParameter(name='use_sim_time', value=True),
|
||||
# 'use_sim_time' will be set on all nodes following the line above
|
||||
|
||||
# Nodes to launch
|
||||
Node(
|
||||
package='rtabmap_ros', executable='rgbd_odometry', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=remappings),
|
||||
|
||||
Node(
|
||||
package='rtabmap_ros', executable='rtabmap', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=remappings,
|
||||
arguments=['-d']),
|
||||
|
||||
Node(
|
||||
package='rtabmap_ros', executable='rtabmapviz', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=remappings),
|
||||
|
||||
# /tf topic is missing in the converted ROS2 bag, create a fake tf
|
||||
Node(
|
||||
package='tf2_ros', executable='static_transform_publisher', output='screen',
|
||||
arguments=['0.0', '0.0', '0.0', '-1.57079632679', '0.0', '-1.57079632679', 'kinect', 'openni_rgb_optical_frame']),
|
||||
])
|
||||
@@ -0,0 +1,126 @@
|
||||
# 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_ros 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_ros', 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,
|
||||
}],
|
||||
remappings=[
|
||||
('scan_cloud', '/velodyne_points')
|
||||
],
|
||||
arguments=[
|
||||
'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.6',
|
||||
'OdomF2M/ScanSubtractRadius', '0.1',
|
||||
'OdomF2M/ScanMaxSize', '15000',
|
||||
'OdomF2M/BundleAdjustment', 'false',
|
||||
]),
|
||||
|
||||
Node(
|
||||
package='rtabmap_ros', 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_ros', 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,
|
||||
}],
|
||||
remappings=[
|
||||
('scan_cloud', 'assembled_cloud')
|
||||
],
|
||||
arguments=[
|
||||
'-d', # This will delete the previous database (~/.ros/rtabmap.db)
|
||||
'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',
|
||||
]),
|
||||
|
||||
Node(
|
||||
package='rtabmap_ros', executable='rtabmapviz', 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')
|
||||
]),
|
||||
])
|
||||
|
||||
|
||||
Reference in New Issue
Block a user