mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-12 04:29:49 +08:00
matlabbe
82f0754bf7
* rtabmap_demos tests and docs * added bag testing * added rtabmap_examples launch tests * updating demo bag download paths * added netherdrone demo * Fixed rgb-only callback with lidar rejected. Updated lidar params * Added back OrbitOriented rviz view to ros2, with optional octomap wll clipping * fixing ci * lidar demo added intermediate_nodes option * added netherdrone as demo test * running rtabmap_demos tests on ci * added rtabmap_launch tests, fixed ground_truth_base_frame_id usage * ficing rolling * updating demo test harnest * densify golden trajectories to avoid tf missing * fixing tf steps * updated min icp ratio for netherdrone demo * fixing image_transport arg->params * export pose opt=0 * lets process all frames * updated netherdrone golden * fixing publishers queue size just for tests * added playdback demo doc * Adding more logs to debug ci * Fixing QOS for CI to reliable, added find-object demo test * name threads * fixing camera info expected transient on lyrical/rolling. Fixing find_object not appearing idle * 30 Hz polling backward comp * fixing test tf sim lock * fixing lyrical qos bag parsing * faster replay * lockstep * fixing clock deadlock * Added test on shutdown * updated netherdrone golden poses * Extended stereo outdoor test * updated shutdown test * updated test * multi-thread flaky test * adding backtrace when test fails * increased closure slack for netherdrone * g2o gauss newton on stereo * adjusted maximum optimizer iterations * updated default iterations * Added netherdrone in list of demos
548 lines
44 KiB
Python
548 lines
44 KiB
Python
#
|
|
# To avoid log buffering:
|
|
# "stdbuf -o L ros2 launch rtabmap_launch rtabmap.launch.py ..."
|
|
#
|
|
|
|
import os
|
|
|
|
from launch import LaunchDescription, Substitution, LaunchContext
|
|
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable, LogInfo, OpaqueFunction
|
|
from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir, PythonExpression
|
|
from launch.conditions import IfCondition, UnlessCondition
|
|
from launch_ros.actions import Node
|
|
from launch_ros.actions import SetParameter
|
|
from typing import Text
|
|
from ament_index_python.packages import get_package_share_directory
|
|
|
|
#Based on https://answers.ros.org/question/363763/ros2-how-best-to-conditionally-include-a-prefix-in-a-launchpy-file/
|
|
class ConditionalText(Substitution):
|
|
def __init__(self, text_if, text_else, condition):
|
|
self.text_if = text_if
|
|
self.text_else = text_else
|
|
self.condition = condition
|
|
|
|
def perform(self, context: 'LaunchContext') -> Text:
|
|
if self.condition == True or self.condition == 'true' or self.condition == 'True':
|
|
return self.text_if
|
|
else:
|
|
return self.text_else
|
|
|
|
class ConditionalBool(Substitution):
|
|
def __init__(self, text_if, text_else, condition):
|
|
self.text_if = text_if
|
|
self.text_else = text_else
|
|
self.condition = condition
|
|
|
|
def perform(self, context: 'LaunchContext') -> bool:
|
|
if self.condition:
|
|
return self.text_if
|
|
else:
|
|
return self.text_else
|
|
|
|
def launch_setup(context, *args, **kwargs):
|
|
|
|
rtabmap_viz_odometry_node_name = "rgbd_odometry"
|
|
use_icp_odometry = LaunchConfiguration('icp_odometry').perform(context)
|
|
use_icp_odometry = use_icp_odometry == 'true' or use_icp_odometry == 'True'
|
|
use_stereo_odometry = LaunchConfiguration('stereo').perform(context)
|
|
use_stereo_odometry = use_stereo_odometry == 'true' or use_stereo_odometry == 'True'
|
|
if use_icp_odometry:
|
|
rtabmap_viz_odometry_node_name = "icp_odometry"
|
|
elif use_stereo_odometry:
|
|
rtabmap_viz_odometry_node_name = "stereo_odometry"
|
|
|
|
return [
|
|
DeclareLaunchArgument('depth', default_value=ConditionalText('false', 'true', IfCondition(PythonExpression(["'", LaunchConfiguration('stereo'), "' == 'true'"]))._predicate_func(context)), description=''),
|
|
DeclareLaunchArgument('subscribe_rgb', default_value=LaunchConfiguration('depth'), description=''),
|
|
DeclareLaunchArgument('args', default_value=LaunchConfiguration('rtabmap_args'), description='Can be used to pass RTAB-Map\'s parameters or other flags like --udebug and --delete_db_on_start/-d'),
|
|
DeclareLaunchArgument('sync_queue_size', default_value=LaunchConfiguration('queue_size'), description='Queue size of topic synchronizers.'),
|
|
DeclareLaunchArgument('qos_image', default_value=LaunchConfiguration('qos'), description='Specific QoS used for image input data: 0=system default, 1=Reliable, 2=Best Effort.'),
|
|
DeclareLaunchArgument('qos_camera_info', default_value=LaunchConfiguration('qos'), description='Specific QoS used for camera info input data: 0=system default, 1=Reliable, 2=Best Effort.'),
|
|
DeclareLaunchArgument('qos_scan', default_value=LaunchConfiguration('qos'), description='Specific QoS used for scan input data: 0=system default, 1=Reliable, 2=Best Effort.'),
|
|
DeclareLaunchArgument('qos_odom', default_value=LaunchConfiguration('qos'), description='Specific QoS used for odometry input data: 0=system default, 1=Reliable, 2=Best Effort.'),
|
|
DeclareLaunchArgument('qos_user_data', default_value=LaunchConfiguration('qos'), description='Specific QoS used for user input data: 0=system default, 1=Reliable, 2=Best Effort.'),
|
|
DeclareLaunchArgument('qos_imu', default_value=LaunchConfiguration('qos'), description='Specific QoS used for imu input data: 0=system default, 1=Reliable, 2=Best Effort.'),
|
|
DeclareLaunchArgument('qos_gps', default_value=LaunchConfiguration('qos'), description='Specific QoS used for gps input data: 0=system default, 1=Reliable, 2=Best Effort.'),
|
|
DeclareLaunchArgument('qos_env_sensor', default_value=LaunchConfiguration('qos'), description='Specific QoS used for env sensor input data: 0=system default, 1=Reliable, 2=Best Effort.'),
|
|
|
|
DeclareLaunchArgument('odom_log_level', default_value=LaunchConfiguration('log_level'), description='Specific ROS logger level for odometry node.'),
|
|
|
|
#These arguments should not be modified directly, see referred topics without "_relay" suffix above
|
|
DeclareLaunchArgument('rgb_topic_relay', default_value=ConditionalText(''.join([LaunchConfiguration('rgb_topic').perform(context), "_relay"]), ''.join(LaunchConfiguration('rgb_topic').perform(context)), LaunchConfiguration('compressed').perform(context)), description='Should not be modified manually!'),
|
|
DeclareLaunchArgument('depth_topic_relay', default_value=ConditionalText(''.join([LaunchConfiguration('depth_topic').perform(context), "_relay"]), ''.join(LaunchConfiguration('depth_topic').perform(context)), LaunchConfiguration('compressed').perform(context)), description='Should not be modified manually!'),
|
|
DeclareLaunchArgument('left_image_topic_relay', default_value=ConditionalText(''.join([LaunchConfiguration('left_image_topic').perform(context), "_relay"]), ''.join(LaunchConfiguration('left_image_topic').perform(context)), LaunchConfiguration('compressed').perform(context)), description='Should not be modified manually!'),
|
|
DeclareLaunchArgument('right_image_topic_relay', default_value=ConditionalText(''.join([LaunchConfiguration('right_image_topic').perform(context), "_relay"]), ''.join(LaunchConfiguration('right_image_topic').perform(context)), LaunchConfiguration('compressed').perform(context)), description='Should not be modified manually!'),
|
|
DeclareLaunchArgument('rgbd_topic_relay', default_value=ConditionalText(''.join(LaunchConfiguration('rgbd_topic').perform(context)), ''.join([LaunchConfiguration('rgbd_topic').perform(context), "_relay"]), LaunchConfiguration('rgbd_sync').perform(context)), description='Should not be modified manually!'),
|
|
|
|
SetParameter(name='use_sim_time', value=LaunchConfiguration('use_sim_time')),
|
|
# 'use_sim_time' will be set on all nodes following the line above
|
|
|
|
# Relays RGB-Depth
|
|
Node(
|
|
package='image_transport', executable='republish', name='republish_rgb',
|
|
condition=IfCondition(PythonExpression(["'", LaunchConfiguration('stereo'), "' != 'true' and ('", LaunchConfiguration('subscribe_rgbd'), "' != 'true' or '", LaunchConfiguration('rgbd_sync'),"'=='true') and '", LaunchConfiguration('compressed'), "' == 'true'"])),
|
|
remappings=[
|
|
(['in/', LaunchConfiguration('rgb_image_transport')], [LaunchConfiguration('rgb_topic'), '/', LaunchConfiguration('rgb_image_transport')]),
|
|
('out', LaunchConfiguration('rgb_topic_relay'))],
|
|
# The transports: as arguments up to humble, as parameters from jazzy.
|
|
arguments=[LaunchConfiguration('rgb_image_transport'), 'raw'],
|
|
parameters=[{'in_transport': LaunchConfiguration('rgb_image_transport'), 'out_transport': 'raw'}],
|
|
namespace=LaunchConfiguration('namespace')),
|
|
Node(
|
|
package='image_transport', executable='republish', name='republish_depth',
|
|
condition=IfCondition(PythonExpression(["'", LaunchConfiguration('stereo'), "' != 'true' and ('", LaunchConfiguration('subscribe_rgbd'), "' != 'true' or '", LaunchConfiguration('rgbd_sync'),"'=='true') and '", LaunchConfiguration('compressed'), "' == 'true'"])),
|
|
remappings=[
|
|
(['in/', LaunchConfiguration('depth_image_transport')], [LaunchConfiguration('depth_topic'), '/', LaunchConfiguration('depth_image_transport')]),
|
|
('out', LaunchConfiguration('depth_topic_relay'))],
|
|
# The transports: as arguments up to humble, as parameters from jazzy.
|
|
arguments=[LaunchConfiguration('depth_image_transport'), 'raw'],
|
|
parameters=[{'in_transport': LaunchConfiguration('depth_image_transport'), 'out_transport': 'raw'}],
|
|
namespace=LaunchConfiguration('namespace')),
|
|
Node(
|
|
package='rtabmap_sync', executable='rgbd_sync', name="rgbd_sync", output="screen",
|
|
emulate_tty=True,
|
|
condition=IfCondition(PythonExpression(["'", LaunchConfiguration('stereo'), "' != 'true' and '", LaunchConfiguration('rgbd_sync'), "' == 'true'"])),
|
|
parameters=[{
|
|
"approx_sync": LaunchConfiguration('approx_rgbd_sync'),
|
|
"approx_sync_max_interval": LaunchConfiguration('approx_sync_max_interval'),
|
|
"topic_queue_size": LaunchConfiguration('topic_queue_size'),
|
|
"sync_queue_size": LaunchConfiguration('sync_queue_size'),
|
|
"qos": LaunchConfiguration('qos_image'),
|
|
"qos_camera_info": LaunchConfiguration('qos_camera_info'),
|
|
"depth_scale": LaunchConfiguration('depth_scale')}],
|
|
remappings=[
|
|
("rgb/image", LaunchConfiguration('rgb_topic_relay')),
|
|
("depth/image", LaunchConfiguration('depth_topic_relay')),
|
|
("rgb/camera_info", LaunchConfiguration('camera_info_topic')),
|
|
("rgbd_image", LaunchConfiguration('rgbd_topic_relay'))],
|
|
namespace=LaunchConfiguration('namespace')),
|
|
|
|
# Relays Stereo
|
|
Node(
|
|
package='image_transport', executable='republish', name='republish_left',
|
|
condition=IfCondition(PythonExpression(["'", LaunchConfiguration('stereo'), "' == 'true' and ('", LaunchConfiguration('subscribe_rgbd'), "' != 'true' or '", LaunchConfiguration('rgbd_sync'),"'=='true') and '", LaunchConfiguration('compressed'), "' == 'true'"])),
|
|
remappings=[
|
|
(['in/', LaunchConfiguration('rgb_image_transport')], [LaunchConfiguration('left_image_topic'), '/', LaunchConfiguration('rgb_image_transport')]),
|
|
('out', LaunchConfiguration('left_image_topic_relay'))],
|
|
# The transports: as arguments up to humble, as parameters from jazzy.
|
|
arguments=[LaunchConfiguration('rgb_image_transport'), 'raw'],
|
|
parameters=[{'in_transport': LaunchConfiguration('rgb_image_transport'), 'out_transport': 'raw'}],
|
|
namespace=LaunchConfiguration('namespace')),
|
|
Node(
|
|
package='image_transport', executable='republish', name='republish_right',
|
|
condition=IfCondition(PythonExpression(["'", LaunchConfiguration('stereo'), "' == 'true' and ('", LaunchConfiguration('subscribe_rgbd'), "' != 'true' or '", LaunchConfiguration('rgbd_sync'),"'=='true') and '", LaunchConfiguration('compressed'), "' == 'true'"])),
|
|
remappings=[
|
|
(['in/', LaunchConfiguration('rgb_image_transport')], [LaunchConfiguration('right_image_topic'), '/', LaunchConfiguration('rgb_image_transport')]),
|
|
('out', LaunchConfiguration('right_image_topic_relay'))],
|
|
# The transports: as arguments up to humble, as parameters from jazzy.
|
|
arguments=[LaunchConfiguration('rgb_image_transport'), 'raw'],
|
|
parameters=[{'in_transport': LaunchConfiguration('rgb_image_transport'), 'out_transport': 'raw'}],
|
|
namespace=LaunchConfiguration('namespace')),
|
|
Node(
|
|
package='rtabmap_sync', executable='stereo_sync', name="stereo_sync", output="screen",
|
|
emulate_tty=True,
|
|
condition=IfCondition(PythonExpression(["'", LaunchConfiguration('stereo'), "' == 'true' and '", LaunchConfiguration('rgbd_sync'), "' == 'true'"])),
|
|
parameters=[{
|
|
"approx_sync": LaunchConfiguration('approx_rgbd_sync'),
|
|
"approx_sync_max_interval": LaunchConfiguration('approx_sync_max_interval'),
|
|
"topic_queue_size": LaunchConfiguration('topic_queue_size'),
|
|
"sync_queue_size": LaunchConfiguration('sync_queue_size'),
|
|
"qos": LaunchConfiguration('qos_image'),
|
|
"qos_camera_info": LaunchConfiguration('qos_camera_info')}],
|
|
remappings=[
|
|
("left/image_rect", LaunchConfiguration('left_image_topic_relay')),
|
|
("right/image_rect", LaunchConfiguration('right_image_topic_relay')),
|
|
("left/camera_info", LaunchConfiguration('left_camera_info_topic')),
|
|
("right/camera_info", LaunchConfiguration('right_camera_info_topic')),
|
|
("rgbd_image", LaunchConfiguration('rgbd_topic_relay'))],
|
|
namespace=LaunchConfiguration('namespace')),
|
|
|
|
# Relay rgbd_image
|
|
Node(
|
|
package='rtabmap_util', executable='rgbd_relay', name="rgbd_relay", output="screen",
|
|
emulate_tty=True,
|
|
condition=IfCondition(PythonExpression(["'", LaunchConfiguration('rgbd_sync'), "' != 'true' and '", LaunchConfiguration('subscribe_rgbd'), "' == 'true' and '", LaunchConfiguration('compressed'), "' != 'true'"])),
|
|
parameters=[{
|
|
"qos": LaunchConfiguration('qos_image')}],
|
|
remappings=[
|
|
("rgbd_image", LaunchConfiguration('rgbd_topic')),
|
|
("rgbd_image_relay", LaunchConfiguration('rgbd_topic_relay'))],
|
|
namespace=LaunchConfiguration('namespace')),
|
|
Node(
|
|
package='rtabmap_util', executable='rgbd_relay', name="rgbd_relay_uncompress", output="screen",
|
|
emulate_tty=True,
|
|
condition=IfCondition(PythonExpression(["'", LaunchConfiguration('rgbd_sync'), "' != 'true' and '", LaunchConfiguration('subscribe_rgbd'), "' == 'true' and '", LaunchConfiguration('compressed'), "' == 'true'"])),
|
|
parameters=[{
|
|
"uncompress": True,
|
|
"qos": LaunchConfiguration('qos_image')}],
|
|
remappings=[
|
|
("rgbd_image", [LaunchConfiguration('rgbd_topic'), "/compressed"]),
|
|
("rgbd_image_relay", LaunchConfiguration('rgbd_topic_relay'))],
|
|
namespace=LaunchConfiguration('namespace')),
|
|
|
|
# RGB-D odometry
|
|
Node(
|
|
package='rtabmap_odom', executable='rgbd_odometry', name="rgbd_odometry", output="screen",
|
|
emulate_tty=True,
|
|
condition=IfCondition(PythonExpression(["'", LaunchConfiguration('icp_odometry'), "' != 'true' and '", LaunchConfiguration('visual_odometry'), "' == 'true' and '", LaunchConfiguration('stereo'), "' != 'true'"])),
|
|
parameters=[{
|
|
"frame_id": LaunchConfiguration('frame_id'),
|
|
"odom_frame_id": LaunchConfiguration('vo_frame_id'),
|
|
"publish_tf": LaunchConfiguration('publish_tf_odom'),
|
|
"ground_truth_frame_id": LaunchConfiguration('ground_truth_frame_id').perform(context),
|
|
"ground_truth_base_frame_id": LaunchConfiguration('ground_truth_base_frame_id').perform(context),
|
|
"wait_for_transform": LaunchConfiguration('wait_for_transform'),
|
|
"wait_imu_to_init": LaunchConfiguration('wait_imu_to_init'),
|
|
"always_check_imu_tf": LaunchConfiguration('always_check_imu_tf'),
|
|
"approx_sync": LaunchConfiguration('approx_sync'),
|
|
"approx_sync_max_interval": LaunchConfiguration('approx_sync_max_interval'),
|
|
"config_path": LaunchConfiguration('cfg').perform(context),
|
|
"topic_queue_size": LaunchConfiguration('topic_queue_size'),
|
|
"sync_queue_size": LaunchConfiguration('sync_queue_size'),
|
|
"qos": LaunchConfiguration('qos_image'),
|
|
"qos_camera_info": LaunchConfiguration('qos_camera_info'),
|
|
"qos_imu": LaunchConfiguration('qos_imu'),
|
|
"subscribe_rgbd": LaunchConfiguration('subscribe_rgbd'),
|
|
"guess_frame_id": LaunchConfiguration('odom_guess_frame_id').perform(context),
|
|
"guess_min_translation": LaunchConfiguration('odom_guess_min_translation'),
|
|
"guess_min_rotation": LaunchConfiguration('odom_guess_min_rotation'),
|
|
"always_process_most_recent_frame": LaunchConfiguration('odom_always_process_most_recent_frame')}],
|
|
remappings=[
|
|
("rgb/image", LaunchConfiguration('rgb_topic_relay')),
|
|
("depth/image", LaunchConfiguration('depth_topic_relay')),
|
|
("rgb/camera_info", LaunchConfiguration('camera_info_topic')),
|
|
("rgbd_image", LaunchConfiguration('rgbd_topic_relay')),
|
|
("odom", LaunchConfiguration('odom_topic')),
|
|
("imu", LaunchConfiguration('imu_topic'))],
|
|
arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args"), "--ros-args", "--log-level", [LaunchConfiguration('namespace'), '.rgbd_odometry:=', LaunchConfiguration('odom_log_level')], "--log-level", ['rgbd_odometry:=', LaunchConfiguration('odom_log_level')]],
|
|
prefix=LaunchConfiguration('launch_prefix'),
|
|
namespace=LaunchConfiguration('namespace')),
|
|
|
|
# Stereo odometry
|
|
Node(
|
|
package='rtabmap_odom', executable='stereo_odometry', name="stereo_odometry", output="screen",
|
|
emulate_tty=True,
|
|
condition=IfCondition(PythonExpression(["'", LaunchConfiguration('icp_odometry'), "' != 'true' and '", LaunchConfiguration('visual_odometry'), "' == 'true' and '", LaunchConfiguration('stereo'), "' == 'true'"])),
|
|
parameters=[{
|
|
"frame_id": LaunchConfiguration('frame_id'),
|
|
"odom_frame_id": LaunchConfiguration('vo_frame_id'),
|
|
"publish_tf": LaunchConfiguration('publish_tf_odom'),
|
|
"ground_truth_frame_id": LaunchConfiguration('ground_truth_frame_id').perform(context),
|
|
"ground_truth_base_frame_id": LaunchConfiguration('ground_truth_base_frame_id').perform(context),
|
|
"wait_for_transform": LaunchConfiguration('wait_for_transform'),
|
|
"wait_imu_to_init": LaunchConfiguration('wait_imu_to_init'),
|
|
"always_check_imu_tf": LaunchConfiguration('always_check_imu_tf'),
|
|
"approx_sync": LaunchConfiguration('approx_sync'),
|
|
"approx_sync_max_interval": LaunchConfiguration('approx_sync_max_interval'),
|
|
"config_path": LaunchConfiguration('cfg').perform(context),
|
|
"topic_queue_size": LaunchConfiguration('topic_queue_size'),
|
|
"sync_queue_size": LaunchConfiguration('sync_queue_size'),
|
|
"qos": LaunchConfiguration('qos_image'),
|
|
"qos_camera_info": LaunchConfiguration('qos_camera_info'),
|
|
"qos_imu": LaunchConfiguration('qos_imu'),
|
|
"subscribe_rgbd": LaunchConfiguration('subscribe_rgbd'),
|
|
"guess_frame_id": LaunchConfiguration('odom_guess_frame_id').perform(context),
|
|
"guess_min_translation": LaunchConfiguration('odom_guess_min_translation'),
|
|
"guess_min_rotation": LaunchConfiguration('odom_guess_min_rotation'),
|
|
"always_process_most_recent_frame": LaunchConfiguration('odom_always_process_most_recent_frame')}],
|
|
remappings=[
|
|
("left/image_rect", LaunchConfiguration('left_image_topic_relay')),
|
|
("right/image_rect", LaunchConfiguration('right_image_topic_relay')),
|
|
("left/camera_info", LaunchConfiguration('left_camera_info_topic')),
|
|
("right/camera_info", LaunchConfiguration('right_camera_info_topic')),
|
|
("rgbd_image", LaunchConfiguration('rgbd_topic_relay')),
|
|
("odom", LaunchConfiguration('odom_topic')),
|
|
("imu", LaunchConfiguration('imu_topic'))],
|
|
arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args"), "--ros-args", "--log-level", [LaunchConfiguration('namespace'), '.stereo_odometry:=', LaunchConfiguration('odom_log_level')], "--log-level", ['stereo_odometry:=', LaunchConfiguration('odom_log_level')]],
|
|
prefix=LaunchConfiguration('launch_prefix'),
|
|
namespace=LaunchConfiguration('namespace')),
|
|
|
|
# ICP odometry
|
|
Node(
|
|
package='rtabmap_odom', executable='icp_odometry', name="icp_odometry", output="screen",
|
|
emulate_tty=True,
|
|
condition=IfCondition(LaunchConfiguration('icp_odometry')),
|
|
parameters=[{
|
|
"frame_id": LaunchConfiguration('frame_id'),
|
|
"odom_frame_id": LaunchConfiguration('vo_frame_id'),
|
|
"publish_tf": LaunchConfiguration('publish_tf_odom'),
|
|
"ground_truth_frame_id": LaunchConfiguration('ground_truth_frame_id').perform(context),
|
|
"ground_truth_base_frame_id": LaunchConfiguration('ground_truth_base_frame_id').perform(context),
|
|
"wait_for_transform": LaunchConfiguration('wait_for_transform'),
|
|
"wait_imu_to_init": LaunchConfiguration('wait_imu_to_init'),
|
|
"always_check_imu_tf": LaunchConfiguration('always_check_imu_tf'),
|
|
"approx_sync": LaunchConfiguration('approx_sync'),
|
|
"config_path": LaunchConfiguration('cfg').perform(context),
|
|
"topic_queue_size": LaunchConfiguration('topic_queue_size'),
|
|
"sync_queue_size": LaunchConfiguration('sync_queue_size'),
|
|
"qos": LaunchConfiguration('qos_image'),
|
|
"qos_imu": LaunchConfiguration('qos_imu'),
|
|
"guess_frame_id": LaunchConfiguration('odom_guess_frame_id').perform(context),
|
|
"guess_min_translation": LaunchConfiguration('odom_guess_min_translation'),
|
|
"guess_min_rotation": LaunchConfiguration('odom_guess_min_rotation'),
|
|
"always_process_most_recent_frame": LaunchConfiguration('odom_always_process_most_recent_frame')}],
|
|
remappings=[
|
|
("scan", LaunchConfiguration('scan_topic')),
|
|
("scan_cloud", LaunchConfiguration('scan_cloud_topic')),
|
|
("odom", LaunchConfiguration('odom_topic')),
|
|
("imu", LaunchConfiguration('imu_topic'))],
|
|
arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args"), "--ros-args", "--log-level", [LaunchConfiguration('namespace'), '.icp_odometry:=', LaunchConfiguration('odom_log_level')], "--log-level", ['icp_odometry:=', LaunchConfiguration('odom_log_level')]],
|
|
prefix=LaunchConfiguration('launch_prefix'),
|
|
namespace=LaunchConfiguration('namespace')),
|
|
|
|
Node(
|
|
package='rtabmap_slam', executable='rtabmap', name="rtabmap", output="screen",
|
|
emulate_tty=True,
|
|
parameters=[{
|
|
"subscribe_depth": LaunchConfiguration('depth'),
|
|
"subscribe_rgbd": LaunchConfiguration('subscribe_rgbd'),
|
|
"subscribe_rgb": LaunchConfiguration('subscribe_rgb'),
|
|
"subscribe_stereo": LaunchConfiguration('stereo'),
|
|
"subscribe_scan": LaunchConfiguration('subscribe_scan'),
|
|
"subscribe_scan_cloud": LaunchConfiguration('subscribe_scan_cloud'),
|
|
"subscribe_user_data": LaunchConfiguration('subscribe_user_data'),
|
|
"subscribe_odom_info": ConditionalBool(True, False, IfCondition(PythonExpression(["'", LaunchConfiguration('icp_odometry'), "' == 'true' or '", LaunchConfiguration('visual_odometry'), "' == 'true'"]))._predicate_func(context)).perform(context),
|
|
"frame_id": LaunchConfiguration('frame_id'),
|
|
"map_frame_id": LaunchConfiguration('map_frame_id'),
|
|
"odom_frame_id": LaunchConfiguration('odom_frame_id').perform(context),
|
|
"publish_tf": LaunchConfiguration('publish_tf_map'),
|
|
"initial_pose": LaunchConfiguration('initial_pose'),
|
|
"use_action_for_goal": LaunchConfiguration('use_action_for_goal'),
|
|
"ground_truth_frame_id": LaunchConfiguration('ground_truth_frame_id').perform(context),
|
|
"ground_truth_base_frame_id": LaunchConfiguration('ground_truth_base_frame_id').perform(context),
|
|
"odom_tf_angular_variance": LaunchConfiguration('odom_tf_angular_variance'),
|
|
"odom_tf_linear_variance": LaunchConfiguration('odom_tf_linear_variance'),
|
|
"odom_sensor_sync": LaunchConfiguration('odom_sensor_sync'),
|
|
"wait_for_transform": LaunchConfiguration('wait_for_transform'),
|
|
"database_path": LaunchConfiguration('database_path'),
|
|
"approx_sync": LaunchConfiguration('approx_sync'),
|
|
"config_path": LaunchConfiguration('cfg').perform(context),
|
|
"topic_queue_size": LaunchConfiguration('topic_queue_size'),
|
|
"sync_queue_size": LaunchConfiguration('sync_queue_size'),
|
|
"qos_image": LaunchConfiguration('qos_image'),
|
|
"qos_scan": LaunchConfiguration('qos_scan'),
|
|
"qos_odom": LaunchConfiguration('qos_odom'),
|
|
"qos_camera_info": LaunchConfiguration('qos_camera_info'),
|
|
"qos_imu": LaunchConfiguration('qos_imu'),
|
|
"qos_gps": LaunchConfiguration('qos_gps'),
|
|
"qos_env_sensor": LaunchConfiguration('qos_env_sensor'),
|
|
"qos_user_data": LaunchConfiguration('qos_user_data'),
|
|
"scan_normal_k": LaunchConfiguration('scan_normal_k'),
|
|
"landmark_linear_variance": LaunchConfiguration('tag_linear_variance'),
|
|
"landmark_angular_variance": LaunchConfiguration('tag_angular_variance'),
|
|
"Mem/IncrementalMemory": ConditionalText("true", "false", IfCondition(PythonExpression(["'", LaunchConfiguration('localization'), "' != 'true'"]))._predicate_func(context)).perform(context),
|
|
"Mem/InitWMWithAllNodes": ConditionalText("true", "false", IfCondition(PythonExpression(["'", LaunchConfiguration('localization'), "' == 'true'"]))._predicate_func(context)).perform(context)
|
|
}],
|
|
remappings=[
|
|
("map", LaunchConfiguration('map_topic')),
|
|
("rgb/image", LaunchConfiguration('rgb_topic_relay')),
|
|
("depth/image", LaunchConfiguration('depth_topic_relay')),
|
|
("rgb/camera_info", LaunchConfiguration('camera_info_topic')),
|
|
("rgbd_image", LaunchConfiguration('rgbd_topic_relay')),
|
|
("left/image_rect", LaunchConfiguration('left_image_topic_relay')),
|
|
("right/image_rect", LaunchConfiguration('right_image_topic_relay')),
|
|
("left/camera_info", LaunchConfiguration('left_camera_info_topic')),
|
|
("right/camera_info", LaunchConfiguration('right_camera_info_topic')),
|
|
("scan", LaunchConfiguration('scan_topic')),
|
|
("scan_cloud", LaunchConfiguration('scan_cloud_topic')),
|
|
("user_data", LaunchConfiguration('user_data_topic')),
|
|
("user_data_async", LaunchConfiguration('user_data_async_topic')),
|
|
("gps/fix", LaunchConfiguration('gps_topic')),
|
|
("tag_detections", LaunchConfiguration('tag_topic')),
|
|
("fiducial_transforms", LaunchConfiguration('fiducial_topic')),
|
|
("env_sensor", LaunchConfiguration('env_sensor_topic')),
|
|
("odom", LaunchConfiguration('odom_topic')),
|
|
("imu", LaunchConfiguration('imu_topic')),
|
|
("goal_out", LaunchConfiguration('output_goal_topic'))],
|
|
arguments=[LaunchConfiguration("args"), "--ros-args", "--log-level", [LaunchConfiguration('namespace'), '.rtabmap:=', LaunchConfiguration('log_level')], "--log-level", ['rtabmap:=', LaunchConfiguration('log_level')]],
|
|
prefix=LaunchConfiguration('launch_prefix'),
|
|
namespace=LaunchConfiguration('namespace')),
|
|
|
|
Node(
|
|
package='rtabmap_viz', executable='rtabmap_viz', name="rtabmap_viz", output='screen',
|
|
emulate_tty=True,
|
|
parameters=[{
|
|
"subscribe_depth": LaunchConfiguration('depth'),
|
|
"subscribe_rgbd": LaunchConfiguration('subscribe_rgbd'),
|
|
"subscribe_rgb": LaunchConfiguration('subscribe_rgb'),
|
|
"subscribe_stereo": LaunchConfiguration('stereo'),
|
|
"subscribe_scan": LaunchConfiguration('subscribe_scan'),
|
|
"subscribe_scan_cloud": LaunchConfiguration('subscribe_scan_cloud'),
|
|
"subscribe_user_data": LaunchConfiguration('subscribe_user_data'),
|
|
"subscribe_odom_info": ConditionalBool(True, False, IfCondition(PythonExpression(["'", LaunchConfiguration('icp_odometry'), "' == 'true' or '", LaunchConfiguration('visual_odometry'), "' == 'true'"]))._predicate_func(context)).perform(context),
|
|
"frame_id": LaunchConfiguration('frame_id'),
|
|
"odom_frame_id": LaunchConfiguration('odom_frame_id').perform(context),
|
|
"wait_for_transform": LaunchConfiguration('wait_for_transform'),
|
|
"approx_sync": LaunchConfiguration('approx_sync'),
|
|
"topic_queue_size": LaunchConfiguration('topic_queue_size'),
|
|
"sync_queue_size": LaunchConfiguration('sync_queue_size'),
|
|
"qos_image": LaunchConfiguration('qos_image'),
|
|
"qos_scan": LaunchConfiguration('qos_scan'),
|
|
"qos_odom": LaunchConfiguration('qos_odom'),
|
|
"qos_camera_info": LaunchConfiguration('qos_camera_info'),
|
|
"qos_user_data": LaunchConfiguration('qos_user_data'),
|
|
"odometry_node_name": rtabmap_viz_odometry_node_name
|
|
}],
|
|
remappings=[
|
|
("rgb/image", LaunchConfiguration('rgb_topic_relay')),
|
|
("depth/image", LaunchConfiguration('depth_topic_relay')),
|
|
("rgb/camera_info", LaunchConfiguration('camera_info_topic')),
|
|
("rgbd_image", LaunchConfiguration('rgbd_topic_relay')),
|
|
("left/image_rect", LaunchConfiguration('left_image_topic_relay')),
|
|
("right/image_rect", LaunchConfiguration('right_image_topic_relay')),
|
|
("left/camera_info", LaunchConfiguration('left_camera_info_topic')),
|
|
("right/camera_info", LaunchConfiguration('right_camera_info_topic')),
|
|
("scan", LaunchConfiguration('scan_topic')),
|
|
("scan_cloud", LaunchConfiguration('scan_cloud_topic')),
|
|
("odom", LaunchConfiguration('odom_topic'))],
|
|
condition=IfCondition(LaunchConfiguration("rtabmap_viz")),
|
|
arguments=[LaunchConfiguration("gui_cfg"), "--ros-args", "--log-level", [LaunchConfiguration('namespace'), '.rtabmap_viz:=', LaunchConfiguration('log_level')], "--log-level", ['rtabmap_viz:=', LaunchConfiguration('log_level')]],
|
|
prefix=LaunchConfiguration('launch_prefix'),
|
|
namespace=LaunchConfiguration('namespace')),
|
|
Node(
|
|
package='rviz2', executable='rviz2', name="rviz2", output='screen',
|
|
condition=IfCondition(LaunchConfiguration("rviz")),
|
|
arguments=[["-d"], [LaunchConfiguration("rviz_cfg")]]),
|
|
Node(
|
|
package='rtabmap_util', executable='point_cloud_xyzrgb', name="point_cloud_xyzrgb", output='screen',
|
|
emulate_tty=True,
|
|
condition=IfCondition(LaunchConfiguration("rviz")),
|
|
parameters=[{
|
|
"decimation": 4,
|
|
"voxel_size": 0.0,
|
|
"approx_sync": LaunchConfiguration('approx_sync'),
|
|
"approx_sync_max_interval": LaunchConfiguration('approx_sync_max_interval'),
|
|
"qos": LaunchConfiguration('qos_image'),
|
|
"qos_camera_info": LaunchConfiguration('qos_camera_info')
|
|
}],
|
|
remappings=[
|
|
('left/image', LaunchConfiguration('left_image_topic_relay')),
|
|
('right/image', LaunchConfiguration('right_image_topic_relay')),
|
|
('left/camera_info', LaunchConfiguration('left_camera_info_topic')),
|
|
('right/camera_info', LaunchConfiguration('right_camera_info_topic')),
|
|
('rgb/image', LaunchConfiguration('rgb_topic_relay')),
|
|
('depth/image', LaunchConfiguration('depth_topic_relay')),
|
|
('rgb/camera_info', LaunchConfiguration('camera_info_topic')),
|
|
('rgbd_image', LaunchConfiguration('rgbd_topic_relay')),
|
|
('cloud', 'voxel_cloud')]),
|
|
]
|
|
|
|
def generate_launch_description():
|
|
|
|
config_rviz = os.path.join(
|
|
get_package_share_directory('rtabmap_launch'), 'launch', 'config', 'rgbd.rviz'
|
|
)
|
|
|
|
return LaunchDescription([
|
|
|
|
# Arguments
|
|
DeclareLaunchArgument('stereo', default_value='false', description='Use stereo input instead of RGB-D.'),
|
|
|
|
DeclareLaunchArgument('localization', default_value='false', description='Launch in localization mode.'),
|
|
DeclareLaunchArgument('rtabmap_viz', default_value='true', description='Launch RTAB-Map UI (optional).'),
|
|
DeclareLaunchArgument('rviz', default_value='false', description='Launch RVIZ (optional).'),
|
|
|
|
DeclareLaunchArgument('use_sim_time', default_value='false', description='Use simulation (Gazebo) clock if true'),
|
|
|
|
DeclareLaunchArgument('log_level', default_value='info', description="ROS logging level (debug, info, warn, error). For RTAB-Map\'s logger level, use \"args\" argument."),
|
|
|
|
# Config files
|
|
DeclareLaunchArgument('cfg', default_value='', description='To change RTAB-Map\'s parameters, set the path of config file (*.ini) generated by the standalone app.'),
|
|
DeclareLaunchArgument('gui_cfg', default_value='~/.ros/rtabmap_gui.ini', description='Configuration path of rtabmap_viz.'),
|
|
DeclareLaunchArgument('rviz_cfg', default_value=config_rviz, description='Configuration path of rviz2.'),
|
|
|
|
DeclareLaunchArgument('frame_id', default_value='base_link', description='Fixed frame id of the robot (base frame), you may set "base_link" or "base_footprint" if they are published. For camera-only config, this could be "camera_link".'),
|
|
DeclareLaunchArgument('odom_frame_id', default_value='', description='If set, TF is used to get odometry instead of the topic.'),
|
|
DeclareLaunchArgument('map_frame_id', default_value='map', description='Output map frame id (TF).'),
|
|
DeclareLaunchArgument('map_topic', default_value='map', description='Map topic name.'),
|
|
DeclareLaunchArgument('publish_tf_map', default_value='true', description='Publish TF between map and odomerty.'),
|
|
DeclareLaunchArgument('namespace', default_value='rtabmap', description=''),
|
|
DeclareLaunchArgument('database_path', default_value='~/.ros/rtabmap.db', description='Where is the map saved/loaded.'),
|
|
DeclareLaunchArgument('topic_queue_size', default_value='10', description='Queue size of individual topic subscribers.'),
|
|
DeclareLaunchArgument('queue_size', default_value='10', description='Backward compatibility, use "sync_queue_size" instead.'),
|
|
DeclareLaunchArgument('qos', default_value='0', description='General QoS used for sensor input data: 0=system default, 1=Reliable, 2=Best Effort.'),
|
|
DeclareLaunchArgument('wait_for_transform', default_value='0.2', description=''),
|
|
DeclareLaunchArgument('rtabmap_args', default_value='', description='Backward compatibility, use "args" instead.'),
|
|
DeclareLaunchArgument('launch_prefix', default_value='', description='For debugging purpose, it fills prefix tag of the nodes, e.g., "xterm -e gdb -ex run --args"'),
|
|
DeclareLaunchArgument('output', default_value='screen', description='Control node output (screen or log).'),
|
|
DeclareLaunchArgument('initial_pose', default_value='', description='Set an initial pose (only in localization mode). Format: "x y z roll pitch yaw" or "x y z qx qy qz qw". Default: see "RGBD/StartAtOrigin" doc'),
|
|
|
|
DeclareLaunchArgument('output_goal_topic', default_value='/goal_pose', description='Output goal topic (can be connected to nav2).'),
|
|
DeclareLaunchArgument('use_action_for_goal', default_value='false', description='Connect to nav2\'s navigate_to_pose action server instead of publishing the output goal topic.'),
|
|
|
|
DeclareLaunchArgument('ground_truth_frame_id', default_value='', description='e.g., "world"'),
|
|
DeclareLaunchArgument('ground_truth_base_frame_id', default_value=[LaunchConfiguration('frame_id'), '_gt'], description='e.g., "tracker", a fake frame matching the frame "frame_id" (but on different TF tree). Empty: the ground truth is not used.'),
|
|
|
|
DeclareLaunchArgument('approx_sync', default_value='false', description='If timestamps of the input topics should be synchronized using approximate or exact time policy.'),
|
|
DeclareLaunchArgument('approx_sync_max_interval', default_value='0.0', description='(sec) 0 means infinite interval duration (used with approx_sync=true)'),
|
|
|
|
# RGB-D related topics
|
|
DeclareLaunchArgument('rgb_topic', default_value='/camera/rgb/image_rect_color', description=''),
|
|
DeclareLaunchArgument('depth_topic', default_value='/camera/depth_registered/image_raw', description=''),
|
|
DeclareLaunchArgument('camera_info_topic', default_value='/camera/rgb/camera_info', description=''),
|
|
|
|
# Stereo related topics
|
|
DeclareLaunchArgument('stereo_namespace', default_value='/stereo_camera', description=''),
|
|
DeclareLaunchArgument('left_image_topic', default_value=[LaunchConfiguration('stereo_namespace'), '/left/image_rect_color'], description=''),
|
|
DeclareLaunchArgument('right_image_topic', default_value=[LaunchConfiguration('stereo_namespace'), '/right/image_rect'], description='Use grayscale image for efficiency'),
|
|
DeclareLaunchArgument('left_camera_info_topic', default_value=[LaunchConfiguration('stereo_namespace'), '/left/camera_info'], description=''),
|
|
DeclareLaunchArgument('right_camera_info_topic', default_value=[LaunchConfiguration('stereo_namespace'), '/right/camera_info'], description=''),
|
|
|
|
# Use Pre-sync RGBDImage format
|
|
DeclareLaunchArgument('rgbd_sync', default_value='false', description='Pre-sync rgb_topic, depth_topic, camera_info_topic.'),
|
|
DeclareLaunchArgument('approx_rgbd_sync', default_value='true', description='false=exact synchronization.'),
|
|
DeclareLaunchArgument('subscribe_rgbd', default_value=LaunchConfiguration('rgbd_sync'), description='Already synchronized RGB-D related topic, e.g., with rtabmap_sync/rgbd_sync nodelet.'),
|
|
DeclareLaunchArgument('rgbd_topic', default_value='rgbd_image', description=''),
|
|
DeclareLaunchArgument('depth_scale', default_value='1.0', description=''),
|
|
|
|
# Image topic compression
|
|
DeclareLaunchArgument('compressed', default_value='false', description='If you want to subscribe to compressed image topics'),
|
|
DeclareLaunchArgument('rgb_image_transport', default_value='compressed', description='Common types: compressed, theora (see "rosrun image_transport list_transports")'),
|
|
DeclareLaunchArgument('depth_image_transport', default_value='compressedDepth', description='Depth compatible types: compressedDepth (see "rosrun image_transport list_transports")'),
|
|
|
|
# LiDAR
|
|
DeclareLaunchArgument('subscribe_scan', default_value='false', description=''),
|
|
DeclareLaunchArgument('scan_topic', default_value='/scan', description=''),
|
|
DeclareLaunchArgument('subscribe_scan_cloud', default_value='false', description=''),
|
|
DeclareLaunchArgument('scan_cloud_topic', default_value='/scan_cloud', description=''),
|
|
DeclareLaunchArgument('scan_normal_k', default_value='0', description=''),
|
|
|
|
# Odometry
|
|
DeclareLaunchArgument('visual_odometry', default_value='true', description='Launch rtabmap visual odometry node.'),
|
|
DeclareLaunchArgument('icp_odometry', default_value='false', description='Launch rtabmap icp odometry node.'),
|
|
DeclareLaunchArgument('odom_topic', default_value='odom', description='Odometry topic name.'),
|
|
DeclareLaunchArgument('vo_frame_id', default_value=LaunchConfiguration('odom_topic'), description='Visual/Icp odometry frame ID for TF.'),
|
|
DeclareLaunchArgument('publish_tf_odom', default_value='true', description=''),
|
|
DeclareLaunchArgument('odom_tf_angular_variance', default_value='0.01', description='If TF is used to get odometry, this is the default angular variance'),
|
|
DeclareLaunchArgument('odom_tf_linear_variance', default_value='0.001', description='If TF is used to get odometry, this is the default linear variance'),
|
|
DeclareLaunchArgument('odom_args', default_value='', description='More arguments for odometry (overwrite same parameters in rtabmap_args).'),
|
|
DeclareLaunchArgument('odom_sensor_sync', default_value='true', description='Correct each sensor\'s position for the motion between its stamp and the odometry\'s, using TF.'),
|
|
DeclareLaunchArgument('odom_guess_frame_id', default_value='', description=''),
|
|
DeclareLaunchArgument('odom_guess_min_translation', default_value='0.0', description=''),
|
|
DeclareLaunchArgument('odom_guess_min_rotation', default_value='0.0', description=''),
|
|
DeclareLaunchArgument('odom_always_process_most_recent_frame', default_value='true', description='Odometry: always process latest frame to reduce delay, skipping frames in case odometry is slower than camera frame rate. In case you want to make sure to process all frames (e.g., from a rosbag/dataset) and you don\'t care about delay, set this to false.'),
|
|
|
|
# imu
|
|
DeclareLaunchArgument('imu_topic', default_value='/imu/data', description='Used with VIO approaches and for SLAM graph optimization (gravity constraints).'),
|
|
DeclareLaunchArgument('wait_imu_to_init', default_value='false', description=''),
|
|
DeclareLaunchArgument('always_check_imu_tf', default_value='true', description='The odometry node will always check if TF between IMU frame and base frame has changed. If false, it is checked till a valid transform is initialized.'),
|
|
|
|
# User Data
|
|
DeclareLaunchArgument('subscribe_user_data', default_value='false', description='User data synchronized subscription.'),
|
|
DeclareLaunchArgument('user_data_topic', default_value='/user_data', description=''),
|
|
DeclareLaunchArgument('user_data_async_topic', default_value='/user_data_async', description='User data async subscription (rate should be lower than map update rate).'),
|
|
|
|
#GPS
|
|
DeclareLaunchArgument('gps_topic', default_value='/gps/fix', description='GPS async subscription. This is used for SLAM graph optimization and loop closure candidates selection.'),
|
|
|
|
# Tag/Landmark
|
|
DeclareLaunchArgument('tag_topic', default_value='/detections', description='AprilTag topic async subscription. This is used for SLAM graph optimization and loop closure detection. Landmark poses are also published accordingly to current optimized map. Required: Remove optional frame name parameters from apriltag\'s cfg file so that TF frame can be deducted from topic\'s family and id.'),
|
|
DeclareLaunchArgument('tag_linear_variance', default_value='0.0001', description=''),
|
|
DeclareLaunchArgument('tag_angular_variance', default_value='9999.0', description='>=9999 means rotation is ignored in optimization, when rotation estimation of the tag is not reliable or not computed.'),
|
|
DeclareLaunchArgument('fiducial_topic', default_value='/fiducial_transforms', description='aruco_detect async subscription, use tag_linear_variance and tag_angular_variance to set covariance.'),
|
|
|
|
DeclareLaunchArgument('env_sensor_topic', default_value='/env_sensor', description='A rtabmap_msgs/EnvSensor topic.'),
|
|
OpaqueFunction(function=launch_setup)
|
|
])
|
|
|