# # 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'))], arguments=[LaunchConfiguration('rgb_image_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'))], arguments=[LaunchConfiguration('depth_image_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'))], arguments=[LaunchConfiguration('rgb_image_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'))], arguments=[LaunchConfiguration('rgb_image_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='', description='e.g., "tracker", a fake frame matching the frame "frame_id" (but on different TF tree)'), 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='false', description=''), 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) ])