feat(slam): add rtabmap_ros
This commit is contained in:
X-lanni
@@ -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)
|
||||
])
|
||||
@@ -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_examples euroc_datasets.launch.py gt:=true
|
||||
# $ cd V1_01_easy
|
||||
# $ ros2 bag play V1_01_easy.db3 --clock
|
||||
#
|
||||
# $ ros2 launch rtabmap_examples 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_odom', executable='stereo_odometry', output='screen',
|
||||
parameters=[parameters],
|
||||
remappings=remappings),
|
||||
|
||||
Node(
|
||||
condition=IfCondition(ground_truth),
|
||||
package='rtabmap_slam', 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_slam', executable='rtabmap', output='screen',
|
||||
parameters=[parameters],
|
||||
remappings=remappings,
|
||||
arguments=['-d']),
|
||||
|
||||
Node(
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
||||
parameters=[parameters],
|
||||
remappings=remappings),
|
||||
|
||||
# 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'), '/config/euroc_left.yaml']}],
|
||||
remappings=[
|
||||
('image', '/cam0/image_raw'),
|
||||
('camera_info', 'left/camera_info')],
|
||||
namespace='stereo_camera'),
|
||||
|
||||
Node(
|
||||
package='rtabmap_util', executable='yaml_to_camera_info.py', output='screen',
|
||||
parameters=[{'yaml_path': [FindPackageShare('rtabmap_examples'), '/config/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_util', 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,71 @@
|
||||
# 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_ros.actions import Node
|
||||
|
||||
def generate_launch_description():
|
||||
parameters=[{
|
||||
'frame_id':'camera_base',
|
||||
'subscribe_rgbd':True,
|
||||
'subscribe_odom_info':True}]
|
||||
|
||||
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_odom', executable='rgbd_odometry', output='screen',
|
||||
parameters=[{ 'frame_id':'camera_base',
|
||||
'approx_sync':True,
|
||||
'approx_sync_max_interval':0.01,
|
||||
'wait_imu_to_init':True,
|
||||
'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_slam', executable='rtabmap', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=remappings,
|
||||
arguments=['-d']),
|
||||
|
||||
# Visualization node
|
||||
Node(
|
||||
package='rtabmap_viz', executable='rtabmap_viz', 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,53 @@
|
||||
# Requirements:
|
||||
# A kinect for xbox 360
|
||||
# Install kinect_ros2 package (use this fork: https://github.com/matlabbe/kinect_ros2)
|
||||
|
||||
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':True}]
|
||||
|
||||
remappings=[
|
||||
('rgb/image', '/kinect/rgb/image_raw'),
|
||||
('rgb/camera_info', '/kinect/rgb/camera_info'),
|
||||
('depth/image', '/kinect/depth_registered/image_raw')]
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
# Nodes to launch
|
||||
Node(
|
||||
package='kinect_ros2', executable='kinect_ros2_node', output='screen',
|
||||
parameters=[{'depth_registration':True}],
|
||||
namespace="kinect"),
|
||||
|
||||
# Optical rotation
|
||||
Node(
|
||||
package='tf2_ros', executable='static_transform_publisher', output='screen',
|
||||
arguments=["0", "0", "0", "-1.57", "0", "-1.57", "camera_link", "kinect_rgb"]),
|
||||
|
||||
Node(
|
||||
package='rtabmap_odom', executable='rgbd_odometry', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=remappings,
|
||||
namespace="rtabmap"),
|
||||
|
||||
Node(
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=remappings,
|
||||
arguments=['-d'],
|
||||
namespace="rtabmap"),
|
||||
|
||||
Node(
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=remappings,
|
||||
namespace="rtabmap"),
|
||||
])
|
||||
@@ -0,0 +1,253 @@
|
||||
# Description:
|
||||
# In this example, we keep only minimal data to do LiDAR SLAM.
|
||||
#
|
||||
# Example:
|
||||
# Launch your lidar sensor:
|
||||
# $ ros2 launch velodyne_driver velodyne_driver_node-VLP16-launch.py
|
||||
# $ ros2 launch velodyne_pointcloud velodyne_transform_node-VLP16-launch.py
|
||||
#
|
||||
# If an IMU is used, make sure TF between lidar/base frame and imu is
|
||||
# already calibrated. In this example, we assume the imu topic has
|
||||
# already the orientation estimated, if not, you can use
|
||||
# imu_filter_madgwick_node (with use_mag:=false publish_tf:=false)
|
||||
# and set imu_topic to output topic of the filter.
|
||||
#
|
||||
# If a camera is used, make sure TF between lidar/base frame and camera is
|
||||
# already calibrated. To provide image data to this example, you should use
|
||||
# rtabmap_sync's rgbd_sync or stereo_sync node.
|
||||
#
|
||||
# Launch the example by adjusting the lidar topic and base frame:
|
||||
# $ ros2 launch rtabmap_examples lidar3d.launch.py lidar_topic:=/velodyne_points frame_id:=velodyne
|
||||
|
||||
from launch import LaunchDescription, LaunchContext
|
||||
from launch.actions import DeclareLaunchArgument, OpaqueFunction
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
def launch_setup(context: LaunchContext, *args, **kwargs):
|
||||
|
||||
frame_id = LaunchConfiguration('frame_id')
|
||||
|
||||
imu_topic = LaunchConfiguration('imu_topic')
|
||||
imu_used = imu_topic.perform(context) != ''
|
||||
|
||||
rgbd_image_topic = LaunchConfiguration('rgbd_image_topic')
|
||||
rgbd_images_topic = LaunchConfiguration('rgbd_images_topic')
|
||||
rgbd_image_used = rgbd_image_topic.perform(context) != '' or rgbd_images_topic.perform(context) != ''
|
||||
rgbd_cameras = 0 if rgbd_images_topic.perform(context) != '' else 1
|
||||
|
||||
voxel_size = LaunchConfiguration('voxel_size')
|
||||
voxel_size_value = float(voxel_size.perform(context))
|
||||
|
||||
use_sim_time = LaunchConfiguration('use_sim_time')
|
||||
|
||||
lidar_topic = LaunchConfiguration('lidar_topic')
|
||||
lidar_topic_value = lidar_topic.perform(context)
|
||||
lidar_topic_deskewed = lidar_topic_value + "/deskewed"
|
||||
|
||||
localization = LaunchConfiguration('localization').perform(context)
|
||||
localization = localization == 'true' or localization == 'True'
|
||||
|
||||
deskewing = LaunchConfiguration('deskewing').perform(context)
|
||||
deskewing = deskewing == 'true' or deskewing == 'True'
|
||||
|
||||
deskewing_slerp = LaunchConfiguration('deskewing_slerp').perform(context)
|
||||
deskewing_slerp = deskewing_slerp == 'true' or deskewing_slerp == 'True'
|
||||
|
||||
fixed_frame_from_imu = False
|
||||
fixed_frame_id = LaunchConfiguration('fixed_frame_id').perform(context)
|
||||
if not fixed_frame_id and imu_used:
|
||||
fixed_frame_from_imu = True
|
||||
fixed_frame_id = frame_id.perform(context) + "_stabilized"
|
||||
|
||||
if not fixed_frame_id or not deskewing:
|
||||
lidar_topic_deskewed = lidar_topic
|
||||
|
||||
# Rule of thumb:
|
||||
max_correspondence_distance = voxel_size_value * 10.0
|
||||
|
||||
shared_parameters = {
|
||||
'use_sim_time': use_sim_time,
|
||||
'frame_id': frame_id,
|
||||
'qos': LaunchConfiguration('qos'),
|
||||
'approx_sync': rgbd_image_used,
|
||||
'wait_for_transform': 0.2,
|
||||
# RTAB-Map's internal parameters are strings:
|
||||
'Icp/PointToPlane': 'true',
|
||||
'Icp/Iterations': '10',
|
||||
'Icp/VoxelSize': str(voxel_size_value),
|
||||
'Icp/Epsilon': '0.001',
|
||||
'Icp/PointToPlaneK': '20',
|
||||
'Icp/PointToPlaneRadius': '0',
|
||||
'Icp/MaxTranslation': '3',
|
||||
'Icp/MaxCorrespondenceDistance': str(max_correspondence_distance),
|
||||
'Icp/Strategy': '1',
|
||||
'Icp/OutlierRatio': '0.7',
|
||||
}
|
||||
|
||||
icp_odometry_parameters = {
|
||||
'expected_update_rate': LaunchConfiguration('expected_update_rate'),
|
||||
'deskewing': not fixed_frame_id and deskewing, # If fixed_frame_id is set, we do deskewing externally below
|
||||
'odom_frame_id': 'icp_odom',
|
||||
'guess_frame_id': fixed_frame_id,
|
||||
'deskewing_slerp': deskewing_slerp,
|
||||
# RTAB-Map's internal parameters are strings:
|
||||
'Odom/ScanKeyFrameThr': '0.4',
|
||||
'OdomF2M/ScanSubtractRadius': str(voxel_size_value),
|
||||
'OdomF2M/ScanMaxSize': '15000',
|
||||
'OdomF2M/BundleAdjustment': 'false',
|
||||
'Icp/CorrespondenceRatio': '0.01'
|
||||
}
|
||||
if imu_used:
|
||||
icp_odometry_parameters['wait_imu_to_init'] = True
|
||||
|
||||
rtabmap_parameters = {
|
||||
'subscribe_depth': False,
|
||||
'subscribe_rgb': False,
|
||||
'subscribe_odom_info': True,
|
||||
'subscribe_scan_cloud': True,
|
||||
'map_frame_id': 'new_map',
|
||||
'odom_sensor_sync': True, # This will adjust camera position based on difference between lidar and camera stamps.
|
||||
# RTAB-Map's internal parameters are strings:
|
||||
'RGBD/ProximityMaxGraphDepth': '0',
|
||||
'RGBD/ProximityPathMaxNeighbors': '1',
|
||||
'RGBD/AngularUpdate': '0.05',
|
||||
'RGBD/LinearUpdate': '0.05',
|
||||
'RGBD/CreateOccupancyGrid': 'false',
|
||||
'Mem/NotLinkedNodesKept': 'false',
|
||||
'Mem/STMSize': '30',
|
||||
'Reg/Strategy': '1',
|
||||
'Icp/CorrespondenceRatio': str(LaunchConfiguration('min_loop_closure_overlap').perform(context))
|
||||
}
|
||||
|
||||
arguments = []
|
||||
if localization:
|
||||
rtabmap_parameters['Mem/IncrementalMemory'] = 'False'
|
||||
rtabmap_parameters['Mem/InitWMWithAllNodes'] = 'True'
|
||||
else:
|
||||
arguments.append('-d') # This will delete the previous database (~/.ros/rtabmap.db)
|
||||
|
||||
remappings = [('odom', 'icp_odom')]
|
||||
if imu_used:
|
||||
remappings.append(('imu', LaunchConfiguration('imu_topic')))
|
||||
else:
|
||||
remappings.append(('imu', 'imu_not_used'))
|
||||
if rgbd_image_used:
|
||||
if rgbd_cameras == 1:
|
||||
remappings.append(('rgbd_image', LaunchConfiguration('rgbd_image_topic')))
|
||||
else:
|
||||
remappings.append(('rgbd_images', LaunchConfiguration('rgbd_images_topic')))
|
||||
|
||||
nodes = [
|
||||
Node(
|
||||
package='rtabmap_odom', executable='icp_odometry', output='screen',
|
||||
parameters=[shared_parameters, icp_odometry_parameters],
|
||||
remappings=remappings + [('scan_cloud', lidar_topic_deskewed)]),
|
||||
|
||||
Node(
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
parameters=[shared_parameters, rtabmap_parameters,
|
||||
{'subscribe_rgbd': rgbd_image_used,
|
||||
'rgbd_cameras': rgbd_cameras}],
|
||||
remappings=remappings + [('scan_cloud', lidar_topic_deskewed)],
|
||||
arguments=arguments),
|
||||
|
||||
Node(
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
||||
parameters=[shared_parameters, rtabmap_parameters],
|
||||
remappings=remappings + [('scan_cloud', 'odom_filtered_input_scan')])
|
||||
]
|
||||
|
||||
if fixed_frame_from_imu:
|
||||
# Create a stabilized base frame based on imu for lidar deskewing
|
||||
nodes.append(
|
||||
Node(
|
||||
package='rtabmap_util', executable='imu_to_tf', output='screen',
|
||||
parameters=[{
|
||||
'use_sim_time': use_sim_time,
|
||||
'fixed_frame_id': fixed_frame_id,
|
||||
'base_frame_id': frame_id,
|
||||
'wait_for_transform_duration': 0.001}],
|
||||
remappings=[('imu/data', imu_topic)]))
|
||||
|
||||
if fixed_frame_id and deskewing:
|
||||
# Lidar deskewing
|
||||
nodes.append(
|
||||
Node(
|
||||
package='rtabmap_util', executable='lidar_deskewing', output='screen',
|
||||
parameters=[{
|
||||
'use_sim_time': use_sim_time,
|
||||
'fixed_frame_id': fixed_frame_id,
|
||||
'wait_for_transform': 0.2,
|
||||
'slerp': deskewing_slerp}],
|
||||
remappings=[
|
||||
('input_cloud', lidar_topic)
|
||||
])
|
||||
)
|
||||
|
||||
return nodes
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument(
|
||||
'use_sim_time', default_value='false',
|
||||
description='Use simulated clock.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'deskewing', default_value='true',
|
||||
description='Enable lidar deskewing.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'frame_id', default_value='velodyne',
|
||||
description='Base frame of the robot.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'fixed_frame_id', default_value='',
|
||||
description='Fixed frame used for lidar deskewing. If not set, we will generate one from IMU.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'localization', default_value='false',
|
||||
description='Localization mode.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'lidar_topic', default_value='/velodyne_points',
|
||||
description='Name of the lidar PointCloud2 topic.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'imu_topic', default_value='',
|
||||
description='IMU topic (ignored if empty).'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'rgbd_image_topic', default_value='',
|
||||
description='RGBD image topic (ignored if empty). Would be the output of a rtabmap_sync\'s rgbd_sync, stereo_sync or rgb_sync node.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'rgbd_images_topic', default_value='',
|
||||
description='RGBD images topic (ignored if empty, override "rgbd_image_topic" if set). Would be the output of a rtabmap_sync\'s rgbdx_sync node.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'expected_update_rate', default_value='15.0',
|
||||
description='Expected lidar frame rate. Ideally, set it slightly higher than actual frame rate, like 15 Hz for 10 Hz lidar scans.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'voxel_size', default_value='0.1',
|
||||
description='Voxel size (m) of the downsampled lidar point cloud. For indoor, set it between 0.1 and 0.3. For outdoor, set it to 0.5 or over.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'min_loop_closure_overlap', default_value='0.2',
|
||||
description='Minimum scan overlap pourcentage to accept a loop closure.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'deskewing_slerp', default_value='true',
|
||||
description='Use fast slerp interpolation between first and last stamps of the scan for deskewing. It would less accruate than requesting TF for every points, but a lot faster. Enable this if the delay of the deskewed scan is significant larger than the original scan.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'qos', default_value='1',
|
||||
description='Quality of Service: 0=system default, 1=reliable, 2=best effort.'),
|
||||
|
||||
OpaqueFunction(function=launch_setup),
|
||||
])
|
||||
|
||||
|
||||
@@ -0,0 +1,273 @@
|
||||
# Description:
|
||||
# In this example, we will record ALL lidar scans. An IMU or low latency odometry is required for this example.
|
||||
#
|
||||
# Example:
|
||||
# Launch your lidar sensor:
|
||||
# $ ros2 launch velodyne_driver velodyne_driver_node-VLP16-launch.py
|
||||
# $ ros2 launch velodyne_pointcloud velodyne_transform_node-VLP16-launch.py
|
||||
#
|
||||
# Launch your IMU sensor, make sure TF between lidar/base frame and imu is already calibrated.
|
||||
# In this example, we assume the imu topic has
|
||||
# already the orientation estimated, if not, you can launch
|
||||
# imu_filter_madgwick_node (with use_mag:=false publish_tf:=false)
|
||||
# and set imu_topic to output topic of the filter.
|
||||
#
|
||||
# If a camera is used, make sure TF between lidar/base frame and camera is
|
||||
# already calibrated. To provide image data to this example, you should use
|
||||
# rtabmap_sync's rgbd_sync or stereo_sync node.
|
||||
#
|
||||
# Launch the example by adjusting the lidar topic, imu topic and base frame:
|
||||
# $ ros2 launch rtabmap_examples lidar3d.launch.py lidar_topic:=/velodyne_points imu_topic:=/imu/data frame_id:=velodyne
|
||||
|
||||
from launch import LaunchDescription, LaunchContext
|
||||
from launch.actions import DeclareLaunchArgument, OpaqueFunction
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
def launch_setup(context: LaunchContext, *args, **kwargs):
|
||||
|
||||
frame_id = LaunchConfiguration('frame_id')
|
||||
|
||||
external_odom_frame_id = LaunchConfiguration('external_odom_frame_id').perform(context)
|
||||
|
||||
fixed_frame_from_imu = False
|
||||
fixed_frame_id = LaunchConfiguration('fixed_frame_id').perform(context)
|
||||
if not fixed_frame_id:
|
||||
if external_odom_frame_id:
|
||||
fixed_frame_id = external_odom_frame_id
|
||||
else:
|
||||
fixed_frame_from_imu = True
|
||||
fixed_frame_id = frame_id.perform(context) + "_stabilized"
|
||||
|
||||
imu_topic = LaunchConfiguration('imu_topic')
|
||||
|
||||
rgbd_image_topic = LaunchConfiguration('rgbd_image_topic')
|
||||
rgbd_images_topic = LaunchConfiguration('rgbd_images_topic')
|
||||
rgbd_image_used = rgbd_image_topic.perform(context) != '' or rgbd_images_topic.perform(context) != ''
|
||||
rgbd_cameras = 0 if rgbd_images_topic.perform(context) != '' else 1
|
||||
|
||||
lidar_topic = LaunchConfiguration('lidar_topic')
|
||||
lidar_topic_value = lidar_topic.perform(context)
|
||||
lidar_topic_deskewed = lidar_topic_value + "/deskewed"
|
||||
|
||||
voxel_size = LaunchConfiguration('voxel_size')
|
||||
voxel_size_value = float(voxel_size.perform(context))
|
||||
|
||||
use_sim_time = LaunchConfiguration('use_sim_time')
|
||||
|
||||
localization = LaunchConfiguration('localization').perform(context)
|
||||
localization = localization == 'true' or localization == 'True'
|
||||
|
||||
deskewing_slerp = LaunchConfiguration('deskewing_slerp').perform(context)
|
||||
deskewing_slerp = deskewing_slerp == 'true' or deskewing_slerp == 'True'
|
||||
|
||||
# Rule of thumb:
|
||||
max_correspondence_distance = voxel_size_value * 10.0
|
||||
|
||||
shared_parameters = {
|
||||
'use_sim_time': use_sim_time,
|
||||
'frame_id': frame_id,
|
||||
'qos': LaunchConfiguration('qos'),
|
||||
'approx_sync': rgbd_image_used,
|
||||
'wait_for_transform': 0.2,
|
||||
# RTAB-Map's internal parameters are strings:
|
||||
'Icp/PointToPlane': 'true',
|
||||
'Icp/Iterations': '10',
|
||||
'Icp/VoxelSize': str(voxel_size_value),
|
||||
'Icp/Epsilon': '0.001',
|
||||
'Icp/PointToPlaneK': '20',
|
||||
'Icp/PointToPlaneRadius': '0',
|
||||
'Icp/MaxTranslation': '3',
|
||||
'Icp/MaxCorrespondenceDistance': str(max_correspondence_distance),
|
||||
'Icp/Strategy': '1',
|
||||
'Icp/OutlierRatio': '0.7',
|
||||
}
|
||||
|
||||
icp_odometry_parameters = {
|
||||
'expected_update_rate': LaunchConfiguration('expected_update_rate'),
|
||||
'wait_imu_to_init': True,
|
||||
'odom_frame_id': 'icp_odom',
|
||||
'guess_frame_id': fixed_frame_id,
|
||||
# RTAB-Map's internal parameters are strings:
|
||||
'Odom/ScanKeyFrameThr': '0.4',
|
||||
'OdomF2M/ScanSubtractRadius': str(voxel_size_value),
|
||||
'OdomF2M/ScanMaxSize': '15000',
|
||||
'OdomF2M/BundleAdjustment': 'false',
|
||||
'Icp/CorrespondenceRatio': '0.01'
|
||||
}
|
||||
|
||||
rtabmap_parameters = {
|
||||
'subscribe_depth': False,
|
||||
'subscribe_rgb': False,
|
||||
'subscribe_odom_info': not external_odom_frame_id,
|
||||
'subscribe_scan_cloud': True,
|
||||
'odom_frame_id': (external_odom_frame_id if external_odom_frame_id else ""),
|
||||
'odom_sensor_sync': True, # This will adjust camera position based on difference between lidar and camera stamps.
|
||||
# RTAB-Map's internal parameters are strings:
|
||||
'Rtabmap/DetectionRate': '0', # indirectly set to 1 Hz by the assembling time below (1s)
|
||||
'RGBD/ProximityMaxGraphDepth': '0',
|
||||
'RGBD/ProximityPathMaxNeighbors': '1',
|
||||
'RGBD/AngularUpdate': '0.05',
|
||||
'RGBD/LinearUpdate': '0.05',
|
||||
'RGBD/CreateOccupancyGrid': 'false',
|
||||
'Mem/NotLinkedNodesKept': 'false',
|
||||
'Mem/STMSize': '30',
|
||||
'Reg/Strategy': '1',
|
||||
'Icp/CorrespondenceRatio': str(LaunchConfiguration('min_loop_closure_overlap').perform(context))
|
||||
}
|
||||
|
||||
remappings = [('imu', imu_topic),
|
||||
('odom', 'icp_odom')]
|
||||
if rgbd_image_used:
|
||||
if rgbd_cameras == 1:
|
||||
remappings.append(('rgbd_image', LaunchConfiguration('rgbd_image_topic')))
|
||||
else:
|
||||
remappings.append(('rgbd_images', LaunchConfiguration('rgbd_images_topic')))
|
||||
|
||||
arguments = []
|
||||
if localization:
|
||||
rtabmap_parameters['Mem/IncrementalMemory'] = 'False'
|
||||
rtabmap_parameters['Mem/InitWMWithAllNodes'] = 'True'
|
||||
else:
|
||||
arguments.append('-d') # This will delete the previous database (~/.ros/rtabmap.db)
|
||||
|
||||
if external_odom_frame_id:
|
||||
viz_topic = lidar_topic_deskewed
|
||||
else:
|
||||
viz_topic = 'odom_filtered_input_scan'
|
||||
|
||||
nodes = [
|
||||
# Lidar deskewing
|
||||
Node(
|
||||
package='rtabmap_util', executable='lidar_deskewing', output='screen',
|
||||
parameters=[{
|
||||
'use_sim_time': use_sim_time,
|
||||
'fixed_frame_id': fixed_frame_id,
|
||||
'wait_for_transform': 0.2,
|
||||
'slerp': deskewing_slerp}],
|
||||
remappings=[
|
||||
('input_cloud', lidar_topic)
|
||||
]),
|
||||
|
||||
# Assemble deskewed scans based on icp odometry
|
||||
Node(
|
||||
package='rtabmap_util', executable='point_cloud_assembler', output='screen',
|
||||
parameters=[{
|
||||
'use_sim_time': use_sim_time,
|
||||
'assembling_time': LaunchConfiguration('assembling_time'),
|
||||
'fixed_frame_id': (external_odom_frame_id if external_odom_frame_id else "")}], # This will make the node subscribing to icp odometry topic "icp_odom"
|
||||
remappings=[('cloud', lidar_topic_deskewed),
|
||||
('odom', 'icp_odom')]),
|
||||
|
||||
# Update the map
|
||||
Node(
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
parameters=[shared_parameters, rtabmap_parameters,
|
||||
{'subscribe_rgbd': rgbd_image_used,
|
||||
'rgbd_cameras': rgbd_cameras,
|
||||
'topic_queue_size': 40,
|
||||
'sync_queue_size': 40,}],
|
||||
remappings=remappings + [('scan_cloud', 'assembled_cloud'), ('gps/fix', LaunchConfiguration('gps_topic'))],
|
||||
arguments=arguments),
|
||||
|
||||
# Just for visualization
|
||||
Node(
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
||||
parameters=[shared_parameters, rtabmap_parameters],
|
||||
remappings=remappings + [('scan_cloud', viz_topic)])
|
||||
]
|
||||
|
||||
if not external_odom_frame_id:
|
||||
# Lidar odometry
|
||||
nodes.append(
|
||||
Node(
|
||||
package='rtabmap_odom', executable='icp_odometry', output='screen',
|
||||
parameters=[shared_parameters, icp_odometry_parameters],
|
||||
remappings=remappings + [('scan_cloud', lidar_topic_deskewed)]))
|
||||
|
||||
if fixed_frame_from_imu:
|
||||
# Create a stabilized base frame based on imu for lidar deskewing
|
||||
nodes.append(
|
||||
Node(
|
||||
package='rtabmap_util', executable='imu_to_tf', output='screen',
|
||||
parameters=[{
|
||||
'use_sim_time': use_sim_time,
|
||||
'fixed_frame_id': fixed_frame_id,
|
||||
'base_frame_id': frame_id,
|
||||
'wait_for_transform_duration': 0.001}],
|
||||
remappings=[('imu/data', imu_topic)]))
|
||||
|
||||
return nodes
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument(
|
||||
'use_sim_time', default_value='false',
|
||||
description='Use simulated clock.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'frame_id', default_value='velodyne',
|
||||
description='Base frame of the robot.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'fixed_frame_id', default_value='',
|
||||
description='Fixed frame used for lidar deskewing. If not set, we will generate one from IMU or external_odom_frame_id if not null.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'external_odom_frame_id', default_value='',
|
||||
description='Provide external odometry with TF, disabling icp_odometry.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'localization', default_value='false',
|
||||
description='Localization mode.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'lidar_topic', default_value='/velodyne_points',
|
||||
description='Name of the lidar PointCloud2 topic.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'imu_topic', default_value='/imu/data',
|
||||
description='Name of an IMU topic.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'gps_topic', default_value='/gps/fix',
|
||||
description='Name of a GPS topic.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'rgbd_image_topic', default_value='',
|
||||
description='RGBD image topic (ignored if empty). Would be the output of a rtabmap_sync\'s rgbd_sync, stereo_sync or rgb_sync node.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'rgbd_images_topic', default_value='',
|
||||
description='RGBD images topic (ignored if empty, override "rgbd_image_topic" if set). Would be the output of a rtabmap_sync\'s rgbdx_sync node.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'voxel_size', default_value='0.1',
|
||||
description='Voxel size (m) of the downsampled lidar point cloud. For indoor, set it between 0.1 and 0.3. For outdoor, set it to 0.5 or over.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'min_loop_closure_overlap', default_value='0.2',
|
||||
description='Minimum scan overlap pourcentage to accept a loop closure.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'expected_update_rate', default_value='15.0',
|
||||
description='Expected lidar frame rate. Ideally, set it slightly higher than actual frame rate, like 15 Hz for 10 Hz lidar scans.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'assembling_time', default_value='1.0',
|
||||
description='How much time (sec) we assemble lidar scans before sending them to mapping node.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'deskewing_slerp', default_value='true',
|
||||
description='Use fast slerp interpolation between first and last stamps of the scan for deskewing. It would less accruate than requesting TF for every points, but a lot faster. Enable this if the delay of the deskewed scan is significant larger than the original scan.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'qos', default_value='1',
|
||||
description='Quality of Service: 0=system default, 1=reliable, 2=best effort.'),
|
||||
|
||||
OpaqueFunction(function=launch_setup),
|
||||
])
|
||||
|
||||
|
||||
@@ -0,0 +1,305 @@
|
||||
# Description:
|
||||
# In this example, we will record ALL lidar scans from 2 lidars. An IMU or low latency odometry is required for this example.
|
||||
#
|
||||
# Example:
|
||||
# Launch your lidar sensors
|
||||
# In this example, we assume the lidar topics have a frame_id linked to same parent (e.g., base_link) and
|
||||
# the extrinsics are known (URDF) and/or already calibrated.
|
||||
#
|
||||
# Launch your IMU sensor, make sure TF between lidar/base frame and imu is already calibrated.
|
||||
# In this example, we assume the imu topic has
|
||||
# already the orientation estimated, if not, you can launch
|
||||
# imu_filter_madgwick_node (with use_mag:=false publish_tf:=false)
|
||||
# and set imu_topic to output topic of the filter.
|
||||
#
|
||||
# If a camera is used, make sure TF between lidar/base frame and camera is
|
||||
# already calibrated. To provide image data to this example, you should use
|
||||
# rtabmap_sync's rgbd_sync or stereo_sync node.
|
||||
#
|
||||
# Launch the example by adjusting the lidar topics, imu topic and base frame:
|
||||
# $ ros2 launch rtabmap_examples lidar3d.launch.py lidar1_topic:=/lidar1/velodyne_points lidar2_topic:=/lidar1/velodyne_points imu_topic:=/imu/data frame_id:=base_link
|
||||
|
||||
from launch import LaunchDescription, LaunchContext
|
||||
from launch.actions import DeclareLaunchArgument, OpaqueFunction
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
def launch_setup(context: LaunchContext, *args, **kwargs):
|
||||
|
||||
frame_id = LaunchConfiguration('frame_id')
|
||||
|
||||
external_odom_frame_id = LaunchConfiguration('external_odom_frame_id').perform(context)
|
||||
|
||||
fixed_frame_from_imu = False
|
||||
fixed_frame_id = LaunchConfiguration('fixed_frame_id').perform(context)
|
||||
if not fixed_frame_id:
|
||||
if external_odom_frame_id:
|
||||
fixed_frame_id = external_odom_frame_id
|
||||
else:
|
||||
fixed_frame_from_imu = True
|
||||
fixed_frame_id = frame_id.perform(context) + "_stabilized"
|
||||
|
||||
imu_topic = LaunchConfiguration('imu_topic')
|
||||
|
||||
rgbd_image_topic = LaunchConfiguration('rgbd_image_topic')
|
||||
rgbd_images_topic = LaunchConfiguration('rgbd_images_topic')
|
||||
rgbd_image_used = rgbd_image_topic.perform(context) != '' or rgbd_images_topic.perform(context) != ''
|
||||
rgbd_cameras = 0 if rgbd_images_topic.perform(context) != '' else 1
|
||||
|
||||
lidar1_topic = LaunchConfiguration('lidar1_topic')
|
||||
lidar1_topic_value = lidar1_topic.perform(context)
|
||||
lidar1_topic_deskewed = lidar1_topic_value + "/deskewed"
|
||||
|
||||
lidar2_topic = LaunchConfiguration('lidar2_topic')
|
||||
lidar2_topic_value = lidar2_topic.perform(context)
|
||||
lidar2_topic_deskewed = lidar2_topic_value + "/deskewed"
|
||||
|
||||
voxel_size = LaunchConfiguration('voxel_size')
|
||||
voxel_size_value = float(voxel_size.perform(context))
|
||||
|
||||
use_sim_time = LaunchConfiguration('use_sim_time')
|
||||
|
||||
localization = LaunchConfiguration('localization').perform(context)
|
||||
localization = localization == 'true' or localization == 'True'
|
||||
|
||||
deskewing_slerp = LaunchConfiguration('deskewing_slerp').perform(context)
|
||||
deskewing_slerp = deskewing_slerp == 'true' or deskewing_slerp == 'True'
|
||||
|
||||
# Rule of thumb:
|
||||
max_correspondence_distance = voxel_size_value * 10.0
|
||||
|
||||
shared_parameters = {
|
||||
'use_sim_time': use_sim_time,
|
||||
'frame_id': frame_id,
|
||||
'qos': LaunchConfiguration('qos'),
|
||||
'approx_sync': rgbd_image_used,
|
||||
'wait_for_transform': 0.2,
|
||||
# RTAB-Map's internal parameters are strings:
|
||||
'Icp/PointToPlane': 'true',
|
||||
'Icp/Iterations': '10',
|
||||
'Icp/VoxelSize': str(voxel_size_value),
|
||||
'Icp/Epsilon': '0.001',
|
||||
'Icp/PointToPlaneK': '20',
|
||||
'Icp/PointToPlaneRadius': '0',
|
||||
'Icp/MaxTranslation': '3',
|
||||
'Icp/MaxCorrespondenceDistance': str(max_correspondence_distance),
|
||||
'Icp/Strategy': '1',
|
||||
'Icp/OutlierRatio': '0.7',
|
||||
}
|
||||
|
||||
icp_odometry_parameters = {
|
||||
'expected_update_rate': LaunchConfiguration('expected_update_rate'),
|
||||
'wait_imu_to_init': True,
|
||||
'odom_frame_id': 'icp_odom',
|
||||
'guess_frame_id': fixed_frame_id,
|
||||
# RTAB-Map's internal parameters are strings:
|
||||
'Odom/ScanKeyFrameThr': '0.4',
|
||||
'OdomF2M/ScanSubtractRadius': str(voxel_size_value),
|
||||
'OdomF2M/ScanMaxSize': '15000',
|
||||
'OdomF2M/BundleAdjustment': 'false',
|
||||
'Icp/CorrespondenceRatio': '0.01'
|
||||
}
|
||||
|
||||
rtabmap_parameters = {
|
||||
'subscribe_depth': False,
|
||||
'subscribe_rgb': False,
|
||||
'subscribe_odom_info': not external_odom_frame_id,
|
||||
'subscribe_scan_cloud': True,
|
||||
'odom_frame_id': (external_odom_frame_id if external_odom_frame_id else ""),
|
||||
'odom_sensor_sync': True, # This will adjust camera position based on difference between lidar and camera stamps.
|
||||
# RTAB-Map's internal parameters are strings:
|
||||
'Rtabmap/DetectionRate': '0', # indirectly set to 1 Hz by the assembling time below (1s)
|
||||
'RGBD/ProximityMaxGraphDepth': '0',
|
||||
'RGBD/ProximityPathMaxNeighbors': '1',
|
||||
'RGBD/AngularUpdate': '0.05',
|
||||
'RGBD/LinearUpdate': '0.05',
|
||||
'RGBD/CreateOccupancyGrid': 'false',
|
||||
'Mem/NotLinkedNodesKept': 'false',
|
||||
'Mem/STMSize': '30',
|
||||
'Reg/Strategy': '1',
|
||||
'Icp/CorrespondenceRatio': str(LaunchConfiguration('min_loop_closure_overlap').perform(context))
|
||||
}
|
||||
|
||||
remappings = [('imu', imu_topic),
|
||||
('odom', 'icp_odom')]
|
||||
if rgbd_image_used:
|
||||
if rgbd_cameras == 1:
|
||||
remappings.append(('rgbd_image', LaunchConfiguration('rgbd_image_topic')))
|
||||
else:
|
||||
remappings.append(('rgbd_images', LaunchConfiguration('rgbd_images_topic')))
|
||||
|
||||
arguments = []
|
||||
if localization:
|
||||
rtabmap_parameters['Mem/IncrementalMemory'] = 'False'
|
||||
rtabmap_parameters['Mem/InitWMWithAllNodes'] = 'True'
|
||||
else:
|
||||
arguments.append('-d') # This will delete the previous database (~/.ros/rtabmap.db)
|
||||
|
||||
if external_odom_frame_id:
|
||||
viz_topic = "combined_cloud"
|
||||
else:
|
||||
viz_topic = 'odom_filtered_input_scan'
|
||||
|
||||
nodes = [
|
||||
# Lidar1 deskewing
|
||||
Node(
|
||||
package='rtabmap_util', executable='lidar_deskewing', name="lidar1_deskewing", output='screen',
|
||||
parameters=[{
|
||||
'use_sim_time': use_sim_time,
|
||||
'fixed_frame_id': fixed_frame_id,
|
||||
'wait_for_transform': 0.2,
|
||||
'slerp': deskewing_slerp}],
|
||||
remappings=[
|
||||
('input_cloud', lidar1_topic)
|
||||
]),
|
||||
|
||||
# Lidar2 deskewing
|
||||
Node(
|
||||
package='rtabmap_util', executable='lidar_deskewing', name="lidar2_deskewing", output='screen',
|
||||
parameters=[{
|
||||
'use_sim_time': use_sim_time,
|
||||
'fixed_frame_id': fixed_frame_id,
|
||||
'wait_for_transform': 0.2,
|
||||
'slerp': deskewing_slerp}],
|
||||
remappings=[
|
||||
('input_cloud', lidar2_topic)
|
||||
]),
|
||||
|
||||
# Combine the two lidars in single point cloud
|
||||
Node(
|
||||
package='rtabmap_util', executable='point_cloud_aggregator', output='screen',
|
||||
parameters=[{
|
||||
'use_sim_time': use_sim_time,
|
||||
'approx_sync': True,
|
||||
'fixed_frame_id': fixed_frame_id,
|
||||
'count': 2}],
|
||||
remappings=[
|
||||
('cloud1', lidar1_topic_deskewed),
|
||||
('cloud2', lidar2_topic_deskewed)]),
|
||||
|
||||
# Assemble combined deskewed scans based on icp odometry
|
||||
Node(
|
||||
package='rtabmap_util', executable='point_cloud_assembler', output='screen',
|
||||
parameters=[{
|
||||
'use_sim_time': use_sim_time,
|
||||
'assembling_time': LaunchConfiguration('assembling_time'),
|
||||
'fixed_frame_id': (external_odom_frame_id if external_odom_frame_id else "")}], # This will make the node subscribing to icp odometry topic "icp_odom"
|
||||
remappings=[('cloud', "combined_cloud"),
|
||||
('odom', 'icp_odom')]),
|
||||
|
||||
# Update the map
|
||||
Node(
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
parameters=[shared_parameters, rtabmap_parameters,
|
||||
{'subscribe_rgbd': rgbd_image_used,
|
||||
'rgbd_cameras': rgbd_cameras,
|
||||
'topic_queue_size': 40,
|
||||
'sync_queue_size': 40,}],
|
||||
remappings=remappings + [('scan_cloud', 'assembled_cloud'), ('gps/fix', LaunchConfiguration('gps_topic'))],
|
||||
arguments=arguments),
|
||||
|
||||
# Just for visualization
|
||||
Node(
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
||||
parameters=[shared_parameters, rtabmap_parameters],
|
||||
remappings=remappings + [('scan_cloud', viz_topic)])
|
||||
]
|
||||
|
||||
if not external_odom_frame_id:
|
||||
# Lidar odometry
|
||||
nodes.append(
|
||||
Node(
|
||||
package='rtabmap_odom', executable='icp_odometry', output='screen',
|
||||
parameters=[shared_parameters, icp_odometry_parameters],
|
||||
remappings=remappings + [('scan_cloud', "combined_cloud")]))
|
||||
|
||||
if fixed_frame_from_imu:
|
||||
# Create a stabilized base frame based on imu for lidar deskewing
|
||||
nodes.append(
|
||||
Node(
|
||||
package='rtabmap_util', executable='imu_to_tf', output='screen',
|
||||
parameters=[{
|
||||
'use_sim_time': use_sim_time,
|
||||
'fixed_frame_id': fixed_frame_id,
|
||||
'base_frame_id': frame_id,
|
||||
'wait_for_transform_duration': 0.001}],
|
||||
remappings=[('imu/data', imu_topic)]))
|
||||
|
||||
return nodes
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument(
|
||||
'use_sim_time', default_value='false',
|
||||
description='Use simulated clock.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'frame_id', default_value='velodyne',
|
||||
description='Base frame of the robot.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'fixed_frame_id', default_value='',
|
||||
description='Fixed frame used for lidar deskewing. If not set, we will generate one from IMU or external_odom_frame_id if not null.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'external_odom_frame_id', default_value='',
|
||||
description='Provide external odometry with TF, disabling icp_odometry.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'localization', default_value='false',
|
||||
description='Localization mode.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'lidar1_topic', default_value='/lidar1/velodyne_points',
|
||||
description='Name of the lidar1\'s PointCloud2 topic.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'lidar2_topic', default_value='/lidar2/velodyne_points',
|
||||
description='Name of the lidar2\'s PointCloud2 topic.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'imu_topic', default_value='/imu/data',
|
||||
description='Name of an IMU topic.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'gps_topic', default_value='/gps/fix',
|
||||
description='Name of a GPS topic.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'rgbd_image_topic', default_value='',
|
||||
description='RGBD image topic (ignored if empty). Would be the output of a rtabmap_sync\'s rgbd_sync, stereo_sync or rgb_sync node.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'rgbd_images_topic', default_value='',
|
||||
description='RGBD images topic (ignored if empty, override "rgbd_image_topic" if set). Would be the output of a rtabmap_sync\'s rgbdx_sync node.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'voxel_size', default_value='0.1',
|
||||
description='Voxel size (m) of the downsampled lidar point cloud. For indoor, set it between 0.1 and 0.3. For outdoor, set it to 0.5 or over.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'min_loop_closure_overlap', default_value='0.2',
|
||||
description='Minimum scan overlap pourcentage to accept a loop closure.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'expected_update_rate', default_value='15.0',
|
||||
description='Expected lidar frame rate. Ideally, set it slightly higher than actual frame rate, like 15 Hz for 10 Hz lidar scans.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'assembling_time', default_value='1.0',
|
||||
description='How much time (sec) we assemble lidar scans before sending them to mapping node.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'deskewing_slerp', default_value='true',
|
||||
description='Use fast slerp interpolation between first and last stamps of the scan for deskewing. It would less accruate than requesting TF for every points, but a lot faster. Enable this if the delay of the deskewed scan is significant larger than the original scan.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'qos', default_value='1',
|
||||
description='Quality of Service: 0=system default, 1=reliable, 2=best effort.'),
|
||||
|
||||
OpaqueFunction(function=launch_setup),
|
||||
])
|
||||
|
||||
|
||||
@@ -0,0 +1,57 @@
|
||||
# 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 rtabmap_examples realsense_d400.launch.py
|
||||
|
||||
import os
|
||||
|
||||
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.launch_description_sources import PythonLaunchDescriptionSource
|
||||
|
||||
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([
|
||||
|
||||
# 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,
|
||||
remappings=remappings),
|
||||
|
||||
Node(
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=remappings,
|
||||
arguments=['-d']),
|
||||
|
||||
Node(
|
||||
package='rtabmap_viz', executable='rtabmap_viz', 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 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_ros.actions import Node, SetParameter
|
||||
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
|
||||
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/aligned_depth_to_color/image_raw')]
|
||||
|
||||
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),
|
||||
|
||||
# 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,
|
||||
remappings=remappings),
|
||||
|
||||
Node(
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=remappings,
|
||||
arguments=['-d']),
|
||||
|
||||
Node(
|
||||
package='rtabmap_viz', executable='rtabmap_viz', 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')]),
|
||||
])
|
||||
@@ -0,0 +1,79 @@
|
||||
# Requirements:
|
||||
# A realsense D435i
|
||||
# Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera)
|
||||
# Example:
|
||||
# $ 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_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=[{
|
||||
'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([
|
||||
|
||||
# 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,
|
||||
remappings=remappings),
|
||||
|
||||
Node(
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=remappings,
|
||||
arguments=['-d']),
|
||||
|
||||
Node(
|
||||
package='rtabmap_viz', executable='rtabmap_viz', 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')]),
|
||||
])
|
||||
@@ -0,0 +1,79 @@
|
||||
# Requirements:
|
||||
# A realsense D435i
|
||||
# Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera)
|
||||
# Example:
|
||||
# $ 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_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=[{
|
||||
'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([
|
||||
|
||||
# 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,
|
||||
remappings=remappings),
|
||||
|
||||
Node(
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=remappings,
|
||||
arguments=['-d']),
|
||||
|
||||
Node(
|
||||
package='rtabmap_viz', executable='rtabmap_viz', 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')]),
|
||||
])
|
||||
@@ -0,0 +1,98 @@
|
||||
|
||||
# Example to run rgbd datasets:
|
||||
#
|
||||
# [ROS1] Prepare ROS1 rosbag for conversion to ROS2
|
||||
# $ wget http://vision.in.tum.de/rgbd/dataset/freiburg3/rgbd_dataset_freiburg3_long_office_household.bag
|
||||
# $ rosbag decompress rgbd_dataset_freiburg3_long_office_household.bag
|
||||
# $ wget https://gist.githubusercontent.com/matlabbe/897b775c38836ed8069a1397485ab024/raw/45e6ac01541973a17505000fc8c7ca399ae275eb/tum_rename_world_kinect_frame.py
|
||||
# $ python3 tum_rename_world_kinect_frame.py rgbd_dataset_freiburg3_long_office_household.bag
|
||||
# $ wget https://raw.githubusercontent.com/srv/srv_tools/kinetic/bag_tools/scripts/change_frame_id.py
|
||||
#
|
||||
# Edit change_frame_id.py, remove/comment lines beginning with "PKG" and "import roslib", change line "Exception, e" to "Exception"
|
||||
# $ roscore
|
||||
# $ python3 change_frame_id.py -o rgbd_dataset_freiburg3_long_office_household_frameid_fixed.bag -i rgbd_dataset_freiburg3_long_office_household.bag -f openni_rgb_optical_frame -t /camera/rgb/image_color
|
||||
#
|
||||
# [ROS2]
|
||||
# $ sudo pip install rosbags # See https://docs.openvins.com/dev-ros1-to-ros2.html
|
||||
# $ rosbags-convert --src rgbd_dataset_freiburg3_long_office_household_frameid_fixed.bag --dst rgbd_dataset_freiburg3_long_office_household_frameid_fixed
|
||||
#
|
||||
# $ ros2 launch rtabmap_examples rgbdslam_datasets.launch.py
|
||||
# $ cd rgbd_dataset_freiburg3_long_office_household_frameid_fixed
|
||||
# $ ros2 bag play rgbd_dataset_freiburg3_long_office_household_frameid_fixed.db3 --clock
|
||||
#
|
||||
# To get RMSE after the run:
|
||||
# $ rtabmap-report ~/.ros/rtabmap.db
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.actions import SetParameter
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
odom_parameters=[{
|
||||
'frame_id':'kinect',
|
||||
# ground truth here is just used to align odometry with ground truth's first pose
|
||||
'ground_truth_frame_id':'world',
|
||||
'ground_truth_base_frame_id':'kinect_gt',
|
||||
'keep_color': True,
|
||||
'wait_for_transform': 0.5,
|
||||
# RTAB-Map's parameters should all be string type:
|
||||
'Odom/Strategy':'0',
|
||||
'Odom/ResetCountdown':'15',
|
||||
'Odom/GuessSmoothingDelay':'0',
|
||||
}]
|
||||
slam_parameters=[{
|
||||
'frame_id':'kinect',
|
||||
# Record ground truth to compute RMSE
|
||||
'ground_truth_frame_id':'world',
|
||||
'ground_truth_base_frame_id':'kinect_gt',
|
||||
'subscribe_rgb':False,
|
||||
'subscribe_depth':False,
|
||||
'subscribe_rgbd':True,
|
||||
'subscribe_odom_info':True,
|
||||
# RTAB-Map's parameters should all be string type:
|
||||
'Mem/UseOdomFeatures': 'true',
|
||||
'Rtabmap/StartNewMapOnLoopClosure':'true',
|
||||
'RGBD/CreateOccupancyGrid':'false',
|
||||
'Rtabmap/CreateIntermediateNodes':'true',
|
||||
'RGBD/LinearUpdate':'0',
|
||||
'RGBD/AngularUpdate':'0'}]
|
||||
|
||||
odom_remappings=[
|
||||
('rgb/image', '/camera/rgb/image_color'),
|
||||
('rgb/camera_info', '/camera/rgb/camera_info'),
|
||||
('depth/image', '/camera/depth/image')]
|
||||
|
||||
# We will use the output of odometry to avoid re-extracting
|
||||
# the same features on slam side.
|
||||
slam_remappings=[
|
||||
("rgbd_image", "odom_rgbd_image")]
|
||||
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
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_odom', executable='rgbd_odometry', output='screen',
|
||||
parameters=odom_parameters,
|
||||
remappings=odom_remappings),
|
||||
|
||||
Node(
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
parameters=slam_parameters,
|
||||
remappings=slam_remappings,
|
||||
arguments=['-d']),
|
||||
|
||||
Node(
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
||||
parameters=slam_parameters,
|
||||
remappings=slam_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,144 @@
|
||||
# Program dedicated to launch 2 realsense cameras. D405 were tested!
|
||||
# (Please note that this program can manage maximum 4 depth cameras)
|
||||
|
||||
# Program modified by: Adrian Ricardez (https://github.com/adricort)
|
||||
# Date: 07.07.2023
|
||||
# Deutsches Zentrum für Luft- und Raumfahrt
|
||||
|
||||
# Requirements:
|
||||
|
||||
# Be sure that you did the build on your rtabmap workspace with the -DRTABMAP_SYNC_MULTI_RGBD=ON parameter.
|
||||
|
||||
# Launching the 2 realsense cameras (change your serial numbers):
|
||||
# $ ros2 launch realsense2_camera rs_multi_camera_launch.py pointcloud.enable1:=true pointcloud.enable2:=true filters:=colorizer align_depth:=true serial_no1:=_128422271521 serial_no2:=_128422272518
|
||||
|
||||
# Running the static publishers depending on the position of your cameras:
|
||||
# $ ros2 run tf2_ros static_transform_publisher --x 0.039 --y 0 --z 0 --yaw 0 --pitch 0 --roll 1.5708 --frame-id base_link --child-frame-id camera1_link
|
||||
# $ ros2 run tf2_ros static_transform_publisher --x -0 --y 0 --z 0.02 --yaw 1.5708 --pitch -1.5708 --roll 0 --frame-id base_link --child-frame-id camera2_link
|
||||
|
||||
# $ ros2 launch rtabmap_examples rtabmap_D405x2.launch.py
|
||||
|
||||
# You should be able to visualize now, with the right rviz config, the camera's SLAM
|
||||
# Have fun!
|
||||
|
||||
import os
|
||||
import launch
|
||||
import launch_ros
|
||||
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'), 'config', 'slam_D405x2_config.rviz')
|
||||
|
||||
rviz_node = launch_ros.actions.Node(
|
||||
package='rviz2', executable='rviz2', output='screen',
|
||||
arguments=[["-d"], [config_rviz]]
|
||||
)
|
||||
|
||||
rgbd_sync1_node = launch_ros.actions.Node(
|
||||
package='rtabmap_sync', executable='rgbd_sync', name='rgbd_sync1', output="screen",
|
||||
parameters=[{
|
||||
"approx_sync": False
|
||||
}],
|
||||
remappings=[
|
||||
("rgb/image", '/camera1/color/image_rect_raw'),
|
||||
("depth/image", '/camera1/depth/image_rect_raw'),
|
||||
("rgb/camera_info", '/camera1/color/camera_info'),
|
||||
("rgbd_image", 'rgbd_image')],
|
||||
namespace='realsense_camera1'
|
||||
)
|
||||
rgbd_sync2_node = launch_ros.actions.Node(
|
||||
package='rtabmap_sync', executable='rgbd_sync', name='rgbd_sync2', output="screen",
|
||||
parameters=[{
|
||||
"approx_sync": False
|
||||
}],
|
||||
remappings=[
|
||||
("rgb/image", '/camera2/color/image_rect_raw'),
|
||||
("depth/image", '/camera2/depth/image_rect_raw'),
|
||||
("rgb/camera_info", '/camera2/color/camera_info'),
|
||||
("rgbd_image", 'rgbd_image')],
|
||||
namespace='realsense_camera2'
|
||||
)
|
||||
|
||||
# RGB-D odometry
|
||||
rgbd_odometry_node = launch_ros.actions.Node(
|
||||
package='rtabmap_odom', executable='rgbd_odometry', output="screen",
|
||||
parameters=[{
|
||||
"frame_id": 'base_link',
|
||||
"odom_frame_id": 'odom',
|
||||
"publish_tf": True,
|
||||
"approx_sync": True,
|
||||
"subscribe_rgbd": True,
|
||||
}],
|
||||
remappings=[
|
||||
("rgbd_image", '/realsense_camera1/rgbd_image'),
|
||||
("odom", 'odom')],
|
||||
arguments=["--delete_db_on_start", ''],
|
||||
prefix='',
|
||||
namespace='rtabmap'
|
||||
)
|
||||
|
||||
# SLAM
|
||||
slam_node = launch_ros.actions.Node(
|
||||
package='rtabmap_slam', executable='rtabmap', output="screen",
|
||||
parameters=[{
|
||||
"rgbd_cameras":2,
|
||||
"subscribe_depth": True,
|
||||
"subscribe_rgbd": True,
|
||||
"subscribe_rgb": True,
|
||||
"subscribe_odom_info": True,
|
||||
"frame_id": 'base_link',
|
||||
"map_frame_id": 'map',
|
||||
"publish_tf": True,
|
||||
"database_path": '~/.ros/rtabmap.db',
|
||||
"approx_sync": True,
|
||||
"Mem/IncrementalMemory": "true",
|
||||
"Mem/InitWMWithAllNodes": "true"
|
||||
}],
|
||||
remappings=[
|
||||
("rgbd_image0", '/realsense_camera1/rgbd_image'),
|
||||
("rgbd_image1", '/realsense_camera2/rgbd_image'),
|
||||
("odom", 'odom')],
|
||||
arguments=["--delete_db_on_start"],
|
||||
prefix='',
|
||||
namespace='rtabmap'
|
||||
)
|
||||
|
||||
voxelcloud1_node = launch_ros.actions.Node(
|
||||
package='rtabmap_util', executable='point_cloud_xyzrgb', name='point_cloud_xyzrgb1', output='screen',
|
||||
parameters=[{
|
||||
"approx_sync": True,
|
||||
}],
|
||||
remappings=[
|
||||
('rgb/image', '/camera1/color/image_rect_raw'),
|
||||
('depth/image', '/camera1/depth/image_rect_raw'),
|
||||
('rgb/camera_info', '/camera1/color/camera_info'),
|
||||
('rgbd_image', 'rgbd_image'),
|
||||
('cloud', 'voxel_cloud1')]
|
||||
)
|
||||
|
||||
voxelcloud2_node = launch_ros.actions.Node(
|
||||
package='rtabmap_util', executable='point_cloud_xyzrgb', name='point_cloud_xyzrgb2', output='screen',
|
||||
parameters=[{
|
||||
"approx_sync": True,
|
||||
}],
|
||||
remappings=[
|
||||
('rgb/image', '/camera2/color/image_rect_raw'),
|
||||
('depth/image', '/camera2/depth/image_rect_raw'),
|
||||
('rgb/camera_info', '/camera2/color/camera_info'),
|
||||
('rgbd_image', 'rgbd_image'),
|
||||
('cloud', 'voxel_cloud2')]
|
||||
)
|
||||
|
||||
return launch.LaunchDescription(
|
||||
[
|
||||
rviz_node,
|
||||
rgbd_sync1_node,
|
||||
rgbd_sync2_node,
|
||||
rgbd_odometry_node,
|
||||
slam_node,
|
||||
voxelcloud1_node,
|
||||
voxelcloud2_node
|
||||
]
|
||||
)
|
||||
@@ -0,0 +1,173 @@
|
||||
# Program dedicated to launch 3 realsense cameras. D405 were tested!
|
||||
# (Please note that this program can manage maximum 4 depth cameras)
|
||||
|
||||
# Program modified by: Adrian Ricardez (https://github.com/adricort)
|
||||
# Date: 07.07.2023
|
||||
# Deutsches Zentrum für Luft- und Raumfahrt
|
||||
|
||||
# Requirements:
|
||||
|
||||
# Be sure that you did the build on your rtabmap workspace with the -DRTABMAP_SYNC_MULTI_RGBD=ON parameter.
|
||||
|
||||
# Launching the 3 realsense cameras (change your serial numbers):
|
||||
# $ ros2 launch realsense2_camera rs_launch.py pointcloud.enable:=true serial_no:=_128422271521
|
||||
# $ ros2 launch realsense2_camera rs_multi_camera_launch.py pointcloud.enable1:=true pointcloud.enable2:=true serial_no1:=_128422272518 serial_no2:=_128422272647
|
||||
|
||||
# Running the static publishers depending on the position of your cameras:
|
||||
# $ ros2 run tf2_ros static_transform_publisher --x 0.039 --y 0 --z 0 --yaw 0 --pitch 0 --roll 0 --frame-id base_link --child-frame-id camera_link
|
||||
# $ ros2 run tf2_ros static_transform_publisher --x 0 --y 0 --z 0.02 --yaw 1.5708 --pitch -1.5708 --roll 0 --frame-id base_link --child-frame-id camera1_link
|
||||
# $ ros2 run tf2_ros static_transform_publisher --x 0 --y 0 --z -0.02 --yaw 1.5708 --pitch 1.5708 --roll 0 --frame-id base_link --child-frame-id camera2_link
|
||||
|
||||
# $ ros2 launch rtabmap_examples rtabmap_D405x3.launch.py
|
||||
|
||||
# You should be able to visualize now, with the right rviz config, the camera's SLAM
|
||||
# Have fun!
|
||||
|
||||
import os
|
||||
import launch
|
||||
import launch_ros
|
||||
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'), 'config', 'slam_D405x3_config.rviz')
|
||||
|
||||
rviz_node = launch_ros.actions.Node(
|
||||
package='rviz2', executable='rviz2', output='screen',
|
||||
arguments=[["-d"], [config_rviz]]
|
||||
)
|
||||
|
||||
rgbd_sync1_node = launch_ros.actions.Node(
|
||||
package='rtabmap_sync', executable='rgbd_sync', name='rgbd_sync1', output="screen",
|
||||
parameters=[{
|
||||
"approx_sync": False
|
||||
}],
|
||||
remappings=[
|
||||
("rgb/image", '/camera/color/image_rect_raw'),
|
||||
("depth/image", '/camera/depth/image_rect_raw'),
|
||||
("rgb/camera_info", '/camera/color/camera_info'),
|
||||
("rgbd_image", 'rgbd_image')],
|
||||
namespace='realsense_camera1'
|
||||
)
|
||||
rgbd_sync2_node = launch_ros.actions.Node(
|
||||
package='rtabmap_sync', executable='rgbd_sync', name='rgbd_sync2', output="screen",
|
||||
parameters=[{
|
||||
"approx_sync": False
|
||||
}],
|
||||
remappings=[
|
||||
("rgb/image", '/camera1/color/image_rect_raw'),
|
||||
("depth/image", '/camera1/depth/image_rect_raw'),
|
||||
("rgb/camera_info", '/camera1/color/camera_info'),
|
||||
("rgbd_image", 'rgbd_image')],
|
||||
namespace='realsense_camera2'
|
||||
)
|
||||
rgbd_sync3_node = launch_ros.actions.Node(
|
||||
package='rtabmap_sync', executable='rgbd_sync', name='rgbd_sync3', output="screen",
|
||||
parameters=[{
|
||||
"approx_sync": False
|
||||
}],
|
||||
remappings=[
|
||||
("rgb/image", '/camera2/color/image_rect_raw'),
|
||||
("depth/image", '/camera2/depth/image_rect_raw'),
|
||||
("rgb/camera_info", '/camera2/color/camera_info'),
|
||||
("rgbd_image", 'rgbd_image')],
|
||||
namespace='realsense_camera3'
|
||||
)
|
||||
|
||||
# RGB-D odometry
|
||||
rgbd_odometry_node = launch_ros.actions.Node(
|
||||
package='rtabmap_odom', executable='rgbd_odometry', output="screen",
|
||||
parameters=[{
|
||||
"frame_id": 'base_link',
|
||||
"odom_frame_id": 'odom',
|
||||
"publish_tf": True,
|
||||
"approx_sync": True,
|
||||
"subscribe_rgbd": True,
|
||||
}],
|
||||
remappings=[
|
||||
("rgbd_image", '/realsense_camera1/rgbd_image'),
|
||||
("odom", 'odom')],
|
||||
arguments=["--delete_db_on_start", ''],
|
||||
prefix='',
|
||||
namespace='rtabmap'
|
||||
)
|
||||
|
||||
# SLAM
|
||||
slam_node = launch_ros.actions.Node(
|
||||
package='rtabmap_slam', executable='rtabmap', output="screen",
|
||||
parameters=[{
|
||||
"rgbd_cameras":3,
|
||||
"subscribe_depth": True,
|
||||
"subscribe_rgbd": True,
|
||||
"subscribe_rgb": True,
|
||||
"subscribe_odom_info": True,
|
||||
"frame_id": 'base_link',
|
||||
"map_frame_id": 'map',
|
||||
"publish_tf": True,
|
||||
"database_path": '~/.ros/rtabmap.db',
|
||||
"approx_sync": True,
|
||||
"Mem/IncrementalMemory": "true",
|
||||
"Mem/InitWMWithAllNodes": "true"
|
||||
}],
|
||||
remappings=[
|
||||
("rgbd_image0", '/realsense_camera1/rgbd_image'),
|
||||
("rgbd_image1", '/realsense_camera2/rgbd_image'),
|
||||
("rgbd_image2", '/realsense_camera3/rgbd_image'),
|
||||
("odom", 'odom')],
|
||||
arguments=["--delete_db_on_start"],
|
||||
prefix='',
|
||||
namespace='rtabmap'
|
||||
)
|
||||
|
||||
voxelcloud1_node = launch_ros.actions.Node(
|
||||
package='rtabmap_util', executable='point_cloud_xyzrgb', name='point_cloud_xyzrgb1', output='screen',
|
||||
parameters=[{
|
||||
"approx_sync": True,
|
||||
}],
|
||||
remappings=[
|
||||
('rgb/image', '/camera/color/image_rect_raw'),
|
||||
('depth/image', '/camera/depth/image_rect_raw'),
|
||||
('rgb/camera_info', '/camera/color/camera_info'),
|
||||
('rgbd_image', 'rgbd_image'),
|
||||
('cloud', 'voxel_cloud1')]
|
||||
)
|
||||
|
||||
voxelcloud2_node = launch_ros.actions.Node(
|
||||
package='rtabmap_util', executable='point_cloud_xyzrgb', name='point_cloud_xyzrgb2', output='screen',
|
||||
parameters=[{
|
||||
"approx_sync": True,
|
||||
}],
|
||||
remappings=[
|
||||
('rgb/image', '/camera1/color/image_rect_raw'),
|
||||
('depth/image', '/camera1/depth/image_rect_raw'),
|
||||
('rgb/camera_info', '/camera1/color/camera_info'),
|
||||
('rgbd_image', 'rgbd_image'),
|
||||
('cloud', 'voxel_cloud2')]
|
||||
)
|
||||
voxelcloud3_node = launch_ros.actions.Node(
|
||||
package='rtabmap_util', executable='point_cloud_xyzrgb', name='point_cloud_xyzrgb3', output='screen',
|
||||
parameters=[{
|
||||
"approx_sync": True,
|
||||
}],
|
||||
remappings=[
|
||||
('rgb/image', '/camera2/color/image_rect_raw'),
|
||||
('depth/image', '/camera2/depth/image_rect_raw'),
|
||||
('rgb/camera_info', '/camera2/color/camera_info'),
|
||||
('rgbd_image', 'rgbd_image'),
|
||||
('cloud', 'voxel_cloud3')]
|
||||
)
|
||||
|
||||
return launch.LaunchDescription(
|
||||
[
|
||||
rviz_node,
|
||||
rgbd_sync1_node,
|
||||
rgbd_sync2_node,
|
||||
rgbd_sync3_node,
|
||||
rgbd_odometry_node,
|
||||
slam_node,
|
||||
voxelcloud1_node,
|
||||
voxelcloud2_node,
|
||||
voxelcloud3_node,
|
||||
]
|
||||
)
|
||||
@@ -0,0 +1,121 @@
|
||||
# Example using zed odometry for lidar deskewing:
|
||||
# $ ros2 launch rtabmap_examples vlp16_zed.launch.py camera_model:=zed2i
|
||||
#
|
||||
# To use only zed's imu for deskewing:
|
||||
# $ ros2 launch rtabmap_examples vlp16_zed.launch.py camera_model:=zed2i use_zed_odometry:=false
|
||||
#
|
||||
|
||||
|
||||
import os
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription, LaunchContext
|
||||
from launch.actions import DeclareLaunchArgument, OpaqueFunction
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
from launch.actions import IncludeLaunchDescription
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
|
||||
import tempfile
|
||||
|
||||
def launch_setup(context: LaunchContext, *args, **kwargs):
|
||||
|
||||
assemble = LaunchConfiguration('assemble').perform(context)
|
||||
assemble = assemble == 'true' or assemble == 'True'
|
||||
|
||||
lidar3d_launch_file = 'lidar3d.launch.py'
|
||||
if assemble:
|
||||
lidar3d_launch_file = 'lidar3d_assemble.launch.py'
|
||||
|
||||
use_zed_odometry = LaunchConfiguration('use_zed_odometry').perform(context)
|
||||
use_zed_odometry = use_zed_odometry == 'true' or use_zed_odometry == 'True'
|
||||
|
||||
fixed_frame_id = ''
|
||||
if use_zed_odometry:
|
||||
fixed_frame_id = 'odom'
|
||||
|
||||
# Hack to override grab_resolution parameter without changing any files
|
||||
with tempfile.NamedTemporaryFile(mode='w+t', delete=False) as zed_override_file:
|
||||
zed_override_file.write("---\n"+
|
||||
"/**:\n"+
|
||||
" ros__parameters:\n"+
|
||||
" general:\n"+
|
||||
" grab_resolution: 'VGA'")
|
||||
|
||||
return [
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([os.path.join(
|
||||
get_package_share_directory('velodyne_driver'), 'launch'),
|
||||
'/velodyne_driver_node-VLP16-launch.py']),
|
||||
),
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([os.path.join(
|
||||
get_package_share_directory('velodyne_pointcloud'), 'launch'),
|
||||
'/velodyne_transform_node-VLP16-launch.py']),
|
||||
),
|
||||
|
||||
# Launch camera driver
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([os.path.join(
|
||||
get_package_share_directory('zed_wrapper'), 'launch'),
|
||||
'/zed_camera.launch.py']),
|
||||
launch_arguments={'camera_model': LaunchConfiguration('camera_model'),
|
||||
'ros_params_override_path': zed_override_file.name,
|
||||
'publish_tf': LaunchConfiguration('use_zed_odometry'), # publish VIO frame
|
||||
'publish_map_tf': 'false'}.items(),
|
||||
),
|
||||
|
||||
# Static transform between zed and velodyne frame (zed will be our base frame because VIO is already linked to it)
|
||||
Node(package='tf2_ros', executable='static_transform_publisher', arguments=["0", "0", "-0.05", "0", "0", "0", "zed_camera_link", "velodyne"]),
|
||||
|
||||
# Sync rgb/depth/camera_info together
|
||||
Node(
|
||||
package='rtabmap_sync', executable='rgbd_sync', output='screen',
|
||||
parameters=[{'approx_sync': False}],
|
||||
remappings=[('rgb/image', '/zed/zed_node/rgb/image_rect_color'),
|
||||
('rgb/camera_info', '/zed/zed_node/rgb/camera_info'),
|
||||
('depth/image', '/zed/zed_node/depth/depth_registered')]),
|
||||
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([os.path.join(
|
||||
get_package_share_directory('rtabmap_examples'), 'launch'),
|
||||
'/', lidar3d_launch_file]),
|
||||
launch_arguments={'voxel_size': LaunchConfiguration('voxel_size'),
|
||||
'localization': LaunchConfiguration('localization'),
|
||||
'frame_id': 'zed_camera_link',
|
||||
'lidar_topic': 'velodyne_points',
|
||||
'imu_topic': '/zed/zed_node/imu/data',
|
||||
'rgbd_image_topic': 'rgbd_image',
|
||||
'fixed_frame_id': fixed_frame_id}.items()),
|
||||
]
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument(
|
||||
'camera_model', default_value='',
|
||||
description="[REQUIRED] The model of the camera. Using a wrong camera model can disable camera features. Valid choices are: ['zed', 'zedm', 'zed2', 'zed2i', 'zedx', 'zedxm', 'virtual']"),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'use_zed_odometry', default_value='true',
|
||||
description='Use ZED\'s odometry for deskewing.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'qos', default_value='1',
|
||||
description='Quality of Service: 0=system default, 1=reliable, 2=best effort'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'localization', default_value='false',
|
||||
description='Localization mode.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'voxel_size', default_value='0.1',
|
||||
description='Voxel size (m) of the downsampled lidar point cloud. For indoor, set it between 0.1 and 0.3. For outdoor, set it to 0.5 or over.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'assemble', default_value='false',
|
||||
description='Assemble ALL lidar scans.'),
|
||||
|
||||
OpaqueFunction(function=launch_setup),
|
||||
])
|
||||
@@ -0,0 +1,101 @@
|
||||
# Requirements:
|
||||
# A ZED camera
|
||||
# Install zed ros2 wrapper package (https://github.com/stereolabs/zed-ros2-wrapper)
|
||||
# Example:
|
||||
# $ ros2 launch rtabmap_examples zed.launch.py camera_model:=zed2i
|
||||
|
||||
import os
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription, LaunchContext
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch_ros.actions import Node
|
||||
from launch.actions import IncludeLaunchDescription, OpaqueFunction
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.conditions import UnlessCondition
|
||||
|
||||
import tempfile
|
||||
|
||||
parameters = []
|
||||
remappings = []
|
||||
|
||||
def launch_setup(context: LaunchContext, *args, **kwargs):
|
||||
|
||||
# Hack to override grab_resolution parameter without changing any files
|
||||
with tempfile.NamedTemporaryFile(mode='w+t', delete=False) as zed_override_file:
|
||||
zed_override_file.write("---\n"+
|
||||
"/**:\n"+
|
||||
" ros__parameters:\n"+
|
||||
" general:\n"+
|
||||
" grab_resolution: 'VGA'")
|
||||
|
||||
parameters=[{'frame_id':'zed_camera_link',
|
||||
'subscribe_rgbd':True,
|
||||
'approx_sync':False,
|
||||
'wait_imu_to_init':True}]
|
||||
|
||||
remappings=[('imu', '/zed/zed_node/imu/data')]
|
||||
|
||||
if LaunchConfiguration('use_zed_odometry').perform(context) in ["True", "true"]:
|
||||
remappings.append(('odom', '/zed/zed_node/odom'))
|
||||
else:
|
||||
parameters.append({'subscribe_odom_info': True})
|
||||
|
||||
return [
|
||||
# Launch camera driver
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([os.path.join(
|
||||
get_package_share_directory('zed_wrapper'), 'launch'),
|
||||
'/zed_camera.launch.py']),
|
||||
launch_arguments={'camera_model': LaunchConfiguration('camera_model'),
|
||||
'ros_params_override_path': zed_override_file.name,
|
||||
'publish_tf': LaunchConfiguration('use_zed_odometry'),
|
||||
'publish_map_tf': 'false'}.items(),
|
||||
),
|
||||
|
||||
# Sync rgb/depth/camera_info together
|
||||
Node(
|
||||
package='rtabmap_sync', executable='rgbd_sync', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=[('rgb/image', '/zed/zed_node/rgb/image_rect_color'),
|
||||
('rgb/camera_info', '/zed/zed_node/rgb/camera_info'),
|
||||
('depth/image', '/zed/zed_node/depth/depth_registered')]),
|
||||
|
||||
# Visual odometry
|
||||
Node(
|
||||
package='rtabmap_odom', executable='rgbd_odometry', output='screen',
|
||||
condition=UnlessCondition(LaunchConfiguration('use_zed_odometry')),
|
||||
parameters=parameters,
|
||||
remappings=remappings,),
|
||||
|
||||
# VSLAM
|
||||
Node(
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=remappings,
|
||||
arguments=['-d']),
|
||||
|
||||
# Visualization
|
||||
Node(
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=remappings)
|
||||
]
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument(
|
||||
'use_zed_odometry', default_value='false',
|
||||
description='Use zed\'s computed odometry instead of using rtabmap\'s odometry.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'camera_model', default_value='',
|
||||
description="[REQUIRED] The model of the camera. Using a wrong camera model can disable camera features. Valid choices are: ['zed', 'zedm', 'zed2', 'zed2i', 'zedx', 'zedxm', 'virtual']"),
|
||||
|
||||
OpaqueFunction(function=launch_setup)
|
||||
])
|
||||
Reference in New Issue
Block a user