Exemplo n.º 1
0
def generate_launch_description():
    composable_nodes = [
        ComposableNode(
            package='image_proc',
            plugin='image_proc::DebayerNode',
            name='debayer_node',
        ),
        ComposableNode(
            package='image_proc',
            plugin='image_proc::RectifyNode',
            name='rectify_mono_node',
            # Remap subscribers and publishers
            remappings=[('image', 'image_mono'),
                        ('camera_info', 'camera_info'),
                        ('image_rect', 'image_rect')],
        ),
        ComposableNode(
            package='image_proc',
            plugin='image_proc::RectifyNode',
            name='rectify_color_node',
            # Remap subscribers and publishers
            remappings=[('image', 'image_color'),
                        ('image_rect', 'image_rect_color')],
        )
    ]

    arg_container = DeclareLaunchArgument(
        name='container',
        default_value='',
        description=(
            'Name of an existing node container to load launched nodes into. '
            'If unset, a new container will be created.'))

    # If an existing container is not provided, start a container and load nodes into it
    image_processing_container = ComposableNodeContainer(
        condition=LaunchConfigurationEquals('container', ''),
        name='image_proc_container',
        namespace='',
        package='rclcpp_components',
        executable='component_container',
        composable_node_descriptions=composable_nodes,
        output='screen')

    # If an existing container name is provided, load composable nodes into it
    # This will block until a container with the provided name is available and nodes are loaded
    load_composable_nodes = LoadComposableNodes(
        condition=LaunchConfigurationNotEquals('container', ''),
        composable_node_descriptions=composable_nodes,
        target_container=LaunchConfiguration('container'),
    )

    return LaunchDescription([
        arg_container,
        image_processing_container,
        load_composable_nodes,
    ])
def generate_launch_description():
    def add_launch_arg(name: str, default_value=None):
        return DeclareLaunchArgument(name, default_value=default_value)

    nodes = [
        ComposableNode(
            package="ground_segmentation",
            plugin="ground_segmentation::ScanGroundFilterComponent",
            name="scan_ground_filter",
            remappings=[
                ("input", LaunchConfiguration("input/pointcloud")),
                ("output", LaunchConfiguration("output/pointcloud")),
            ],
            parameters=[{
                "global_slope_max_angle_deg": 10.0,
                "local_slope_max_angle_deg": 30.0,
                "split_points_distance_tolerance": 0.2,
                "split_height_distance": 0.2,
            }],
        ),
    ]

    loader = LoadComposableNodes(
        condition=LaunchConfigurationNotEquals("container", ""),
        composable_node_descriptions=nodes,
        target_container=LaunchConfiguration("container"),
    )

    container = ComposableNodeContainer(
        name="scan_ground_filter_container",
        namespace="",
        package="rclcpp_components",
        executable="component_container",
        composable_node_descriptions=nodes,
        output="screen",
        condition=LaunchConfigurationEquals("container", ""),
    )

    return launch.LaunchDescription([
        add_launch_arg("container", ""),
        add_launch_arg("input/pointcloud", "pointcloud"),
        add_launch_arg("output/pointcloud", "no_ground/pointcloud"),
        container,
        loader,
    ])
Exemplo n.º 3
0
def _load_composable_node(*,
                          package,
                          plugin,
                          name,
                          namespace='',
                          parameters=None,
                          remappings=None,
                          target_container=f'/{TEST_CONTAINER_NAME}'):
    return LoadComposableNodes(target_container=target_container,
                               composable_node_descriptions=[
                                   ComposableNode(
                                       package=package,
                                       plugin=plugin,
                                       name=name,
                                       namespace=namespace,
                                       parameters=parameters,
                                       remappings=remappings,
                                   )
                               ])
Exemplo n.º 4
0
def generate_launch_description():
    # Get the launch directory
    bringup_dir = get_package_share_directory('nav2_bringup')

    namespace = LaunchConfiguration('namespace')
    use_sim_time = LaunchConfiguration('use_sim_time')
    autostart = LaunchConfiguration('autostart')
    params_file = LaunchConfiguration('params_file')
    use_composition = LaunchConfiguration('use_composition')
    container_name = LaunchConfiguration('container_name')

    lifecycle_nodes = [
        'controller_server', 'smoother_server', 'planner_server',
        'behavior_server', 'bt_navigator', 'waypoint_follower'
    ]

    # Map fully qualified names to relative ones so the node's namespace can be prepended.
    # In case of the transforms (tf), currently, there doesn't seem to be a better alternative
    # https://github.com/ros/geometry2/issues/32
    # https://github.com/ros/robot_state_publisher/pull/30
    # TODO(orduno) Substitute with `PushNodeRemapping`
    #              https://github.com/ros2/launch_ros/issues/56
    remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')]

    # Create our own temporary YAML files that include substitutions
    param_substitutions = {
        'use_sim_time': use_sim_time,
        'autostart': autostart
    }

    configured_params = RewrittenYaml(source_file=params_file,
                                      root_key=namespace,
                                      param_rewrites=param_substitutions,
                                      convert_types=True)

    stdout_linebuf_envvar = SetEnvironmentVariable(
        'RCUTILS_LOGGING_BUFFERED_STREAM', '1')

    declare_namespace_cmd = DeclareLaunchArgument(
        'namespace', default_value='', description='Top-level namespace')

    declare_use_sim_time_cmd = DeclareLaunchArgument(
        'use_sim_time',
        default_value='false',
        description='Use simulation (Gazebo) clock if true')

    declare_params_file_cmd = DeclareLaunchArgument(
        'params_file',
        default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'),
        description=
        'Full path to the ROS2 parameters file to use for all launched nodes')

    declare_autostart_cmd = DeclareLaunchArgument(
        'autostart',
        default_value='true',
        description='Automatically startup the nav2 stack')

    declare_use_composition_cmd = DeclareLaunchArgument(
        'use_composition',
        default_value='False',
        description='Use composed bringup if True')

    declare_container_name_cmd = DeclareLaunchArgument(
        'container_name',
        default_value='nav2_container',
        description=
        'the name of conatiner that nodes will load in if use composition')

    load_nodes = GroupAction(condition=IfCondition(
        PythonExpression(['not ', use_composition])),
                             actions=[
                                 Node(package='nav2_controller',
                                      executable='controller_server',
                                      output='screen',
                                      parameters=[configured_params],
                                      remappings=remappings),
                                 Node(package='nav2_smoother',
                                      executable='smoother_server',
                                      name='smoother_server',
                                      output='screen',
                                      parameters=[configured_params],
                                      remappings=remappings),
                                 Node(package='nav2_planner',
                                      executable='planner_server',
                                      name='planner_server',
                                      output='screen',
                                      parameters=[configured_params],
                                      remappings=remappings),
                                 Node(package='nav2_behaviors',
                                      executable='behavior_server',
                                      name='behavior_server',
                                      output='screen',
                                      parameters=[configured_params],
                                      remappings=remappings),
                                 Node(package='nav2_bt_navigator',
                                      executable='bt_navigator',
                                      name='bt_navigator',
                                      output='screen',
                                      parameters=[configured_params],
                                      remappings=remappings),
                                 Node(package='nav2_waypoint_follower',
                                      executable='waypoint_follower',
                                      name='waypoint_follower',
                                      output='screen',
                                      parameters=[configured_params],
                                      remappings=remappings),
                                 Node(package='nav2_lifecycle_manager',
                                      executable='lifecycle_manager',
                                      name='lifecycle_manager_navigation',
                                      output='screen',
                                      parameters=[{
                                          'use_sim_time': use_sim_time
                                      }, {
                                          'autostart': autostart
                                      }, {
                                          'node_names': lifecycle_nodes
                                      }]),
                             ])

    load_composable_nodes = LoadComposableNodes(
        condition=IfCondition(use_composition),
        target_container=container_name,
        composable_node_descriptions=[
            ComposableNode(package='nav2_controller',
                           plugin='nav2_controller::ControllerServer',
                           name='controller_server',
                           parameters=[configured_params],
                           remappings=remappings),
            ComposableNode(package='nav2_smoother',
                           plugin='nav2_smoother::SmootherServer',
                           name='smoother_server',
                           parameters=[configured_params],
                           remappings=remappings),
            ComposableNode(package='nav2_planner',
                           plugin='nav2_planner::PlannerServer',
                           name='planner_server',
                           parameters=[configured_params],
                           remappings=remappings),
            ComposableNode(package='nav2_behaviors',
                           plugin='behavior_server::BehaviorServer',
                           name='behavior_server',
                           parameters=[configured_params],
                           remappings=remappings),
            ComposableNode(package='nav2_bt_navigator',
                           plugin='nav2_bt_navigator::BtNavigator',
                           name='bt_navigator',
                           parameters=[configured_params],
                           remappings=remappings),
            ComposableNode(package='nav2_waypoint_follower',
                           plugin='nav2_waypoint_follower::WaypointFollower',
                           name='waypoint_follower',
                           parameters=[configured_params],
                           remappings=remappings),
            ComposableNode(package='nav2_lifecycle_manager',
                           plugin='nav2_lifecycle_manager::LifecycleManager',
                           name='lifecycle_manager_navigation',
                           parameters=[{
                               'use_sim_time': use_sim_time,
                               'autostart': autostart,
                               'node_names': lifecycle_nodes
                           }]),
        ],
    )

    # Create the launch description and populate
    ld = LaunchDescription()

    # Set environment variables
    ld.add_action(stdout_linebuf_envvar)

    # Declare the launch options
    ld.add_action(declare_namespace_cmd)
    ld.add_action(declare_use_sim_time_cmd)
    ld.add_action(declare_params_file_cmd)
    ld.add_action(declare_autostart_cmd)
    ld.add_action(declare_use_composition_cmd)
    ld.add_action(declare_container_name_cmd)

    # Add the actions to launch all of the navigation nodes
    ld.add_action(load_nodes)
    ld.add_action(load_composable_nodes)

    return ld
Exemplo n.º 5
0
def generate_launch_description():

    arguments = [
        # component
        DeclareLaunchArgument("use_intra_process"),
        DeclareLaunchArgument("target_container"),
        # settings
        DeclareLaunchArgument(
            "initial_selector_mode", default_value="local", choices=["local", "remote"]
        ),
        # service
        DeclareLaunchArgument(
            "service/select_external_command", default_value="~/select_external_command"
        ),
        # local input
        DeclareLaunchArgument(
            "input/local/control_cmd", default_value="/api/external/set/command/local/control"
        ),
        DeclareLaunchArgument(
            "input/local/shift_cmd", default_value="/api/external/set/command/local/shift"
        ),
        DeclareLaunchArgument(
            "input/local/turn_signal_cmd",
            default_value="/api/external/set/command/local/turn_signal",
        ),
        DeclareLaunchArgument(
            "input/local/heartbeat", default_value="/api/external/set/command/local/heartbeat"
        ),
        # remote input
        DeclareLaunchArgument(
            "input/remote/control_cmd", default_value="/api/external/set/command/remote/control"
        ),
        DeclareLaunchArgument(
            "input/remote/shift_cmd", default_value="/api/external/set/command/remote/shift"
        ),
        DeclareLaunchArgument(
            "input/remote/turn_signal_cmd",
            default_value="/api/external/set/command/remote/turn_signal",
        ),
        DeclareLaunchArgument(
            "input/remote/heartbeat", default_value="/api/external/set/command/remote/heartbeat"
        ),
        # output
        DeclareLaunchArgument(
            "output/control_cmd", default_value="/external/selected/external_control_cmd"
        ),
        DeclareLaunchArgument("output/gear_cmd", default_value="/external/selected/gear_cmd"),
        DeclareLaunchArgument(
            "output/turn_indicators_cmd", default_value="/external/selected/turn_indicators_cmd"
        ),
        DeclareLaunchArgument(
            "output/hazard_lights_cmd", default_value="/external/selected/hazard_lights_cmd"
        ),
        DeclareLaunchArgument("output/heartbeat", default_value="/external/selected/heartbeat"),
        DeclareLaunchArgument(
            "output/current_selector_mode", default_value="~/current_selector_mode"
        ),
    ]

    component = ComposableNode(
        package="external_cmd_selector",
        plugin="ExternalCmdSelector",
        name="external_cmd_selector",
        remappings=[
            _create_mapping_tuple("service/select_external_command"),
            _create_mapping_tuple("input/local/control_cmd"),
            _create_mapping_tuple("input/local/shift_cmd"),
            _create_mapping_tuple("input/local/turn_signal_cmd"),
            _create_mapping_tuple("input/local/heartbeat"),
            _create_mapping_tuple("input/remote/control_cmd"),
            _create_mapping_tuple("input/remote/shift_cmd"),
            _create_mapping_tuple("input/remote/turn_signal_cmd"),
            _create_mapping_tuple("input/remote/heartbeat"),
            _create_mapping_tuple("output/control_cmd"),
            _create_mapping_tuple("output/gear_cmd"),
            _create_mapping_tuple("output/turn_indicators_cmd"),
            _create_mapping_tuple("output/hazard_lights_cmd"),
            _create_mapping_tuple("output/heartbeat"),
            _create_mapping_tuple("output/current_selector_mode"),
        ],
        parameters=[
            {
                "initial_selector_mode": LaunchConfiguration("initial_selector_mode"),
            }
        ],
        extra_arguments=[
            {
                "use_intra_process_comms": LaunchConfiguration("use_intra_process"),
            }
        ],
    )

    loader = LoadComposableNodes(
        composable_node_descriptions=[component],
        target_container=LaunchConfiguration("target_container"),
    )

    return LaunchDescription(arguments + [loader])
def generate_launch_description():

    arguments = [
        # component
        DeclareLaunchArgument("use_intra_process"),
        DeclareLaunchArgument("target_container"),
        # map file
        DeclareLaunchArgument(
            "csv_path_accel_map",
            default_value=[
                FindPackageShare("raw_vehicle_cmd_converter"),
                "/data/default/accel_map.csv",
            ],  # noqa: E501
            description="csv file path for accel map",
        ),
        DeclareLaunchArgument(
            "csv_path_brake_map",
            default_value=[
                FindPackageShare("raw_vehicle_cmd_converter"),
                "/data/default/brake_map.csv",
            ],  # noqa: E501
            description="csv file path for brake map",
        ),
        # settings
        DeclareLaunchArgument(
            "ref_vel_gain",
            default_value="3.0",
            description="gain for external command accel",
        ),
        DeclareLaunchArgument(
            "wait_for_first_topic",
            default_value="true",
            description=
            "disable topic disruption detection until subscribing first topics",
        ),
        DeclareLaunchArgument(
            "control_command_timeout",
            default_value="1.0",
            description="external control command timeout",
        ),
        DeclareLaunchArgument(
            "emergency_stop_timeout",
            default_value="3.0",
            description="emergency stop timeout for external heartbeat",
        ),
        # input
        DeclareLaunchArgument(
            "in/external_control_cmd",
            default_value="/external/selected/external_control_cmd",
        ),
        DeclareLaunchArgument(
            "in/shift_cmd",
            default_value="/external/selected/gear_cmd",
        ),
        DeclareLaunchArgument(
            "in/emergency_stop",
            default_value="/external/selected/heartbeat",
        ),
        DeclareLaunchArgument(
            "in/current_gate_mode",
            default_value="/control/current_gate_mode",
        ),
        DeclareLaunchArgument(
            "in/odometry",
            default_value="/localization/kinematic_state",
        ),
        # output
        DeclareLaunchArgument(
            "out/control_cmd",
            default_value="/external/selected/control_cmd",
        ),
        DeclareLaunchArgument(
            "out/latest_external_control_cmd",
            default_value="/api/external/get/command/selected/control",
        ),
    ]

    component = ComposableNode(
        package="external_cmd_converter",
        plugin="external_cmd_converter::ExternalCmdConverterNode",
        name="external_cmd_converter",
        remappings=[
            _create_mapping_tuple("in/external_control_cmd"),
            _create_mapping_tuple("in/shift_cmd"),
            _create_mapping_tuple("in/emergency_stop"),
            _create_mapping_tuple("in/current_gate_mode"),
            _create_mapping_tuple("in/odometry"),
            _create_mapping_tuple("out/control_cmd"),
            _create_mapping_tuple("out/latest_external_control_cmd"),
        ],
        parameters=[
            dict(  # noqa: C406 for using helper function
                [
                    _create_mapping_tuple("csv_path_accel_map"),
                    _create_mapping_tuple("csv_path_brake_map"),
                    _create_mapping_tuple("ref_vel_gain"),
                    _create_mapping_tuple("wait_for_first_topic"),
                    _create_mapping_tuple("control_command_timeout"),
                    _create_mapping_tuple("emergency_stop_timeout"),
                ])
        ],
        extra_arguments=[{
            "use_intra_process_comms":
            LaunchConfiguration("use_intra_process"),
        }],
    )

    loader = LoadComposableNodes(
        composable_node_descriptions=[component],
        target_container=LaunchConfiguration("target_container"),
    )

    return LaunchDescription(arguments + [loader])
def launch_setup(context, *args, **kwargs):
    # https://github.com/ros2/launch_ros/issues/156
    def load_composable_node_param(param_path):
        with open(LaunchConfiguration(param_path).perform(context), "r") as f:
            return yaml.safe_load(f)["/**"]["ros__parameters"]

    ns = "euclidean_cluster"
    pkg = "euclidean_cluster"

    # set voxel grid filter as a component
    voxel_grid_filter_component = ComposableNode(
        package="pointcloud_preprocessor",
        plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent",
        name=AnonName("voxel_grid_filter"),
        remappings=[
            ("input", LaunchConfiguration("input_pointcloud")),
            ("output", "voxel_grid_filtered/pointcloud"),
        ],
        parameters=[load_composable_node_param("voxel_grid_param_path")],
    )

    # set compare map filter as a component
    compare_map_filter_component = ComposableNode(
        package="pointcloud_preprocessor",
        plugin="pointcloud_preprocessor::VoxelBasedCompareMapFilterComponent",
        name=AnonName("compare_map_filter"),
        remappings=[
            ("input", "voxel_grid_filtered/pointcloud"),
            ("map", LaunchConfiguration("input_map")),
            ("output", "compare_map_filtered/pointcloud"),
        ],
    )

    use_map_euclidean_cluster_component = ComposableNode(
        package=pkg,
        plugin="euclidean_cluster::EuclideanClusterNode",
        name=AnonName("euclidean_cluster"),
        remappings=[
            ("input", "compare_map_filtered/pointcloud"),
            ("output", LaunchConfiguration("output_clusters")),
        ],
        parameters=[load_composable_node_param("euclidean_param_path")],
    )

    disuse_map_euclidean_cluster_component = ComposableNode(
        package=pkg,
        plugin="euclidean_cluster::EuclideanClusterNode",
        name=AnonName("euclidean_cluster"),
        remappings=[
            ("input", "voxel_grid_filtered/pointcloud"),
            ("output", LaunchConfiguration("output_clusters")),
        ],
        parameters=[load_composable_node_param("euclidean_param_path")],
    )

    container = ComposableNodeContainer(
        name="euclidean_cluster_container",
        namespace=ns,
        package="rclcpp_components",
        executable="component_container",
        composable_node_descriptions=[voxel_grid_filter_component],
        output="screen",
    )

    use_map_loader = LoadComposableNodes(
        composable_node_descriptions=[
            compare_map_filter_component,
            use_map_euclidean_cluster_component,
        ],
        target_container=container,
        condition=IfCondition(LaunchConfiguration("use_pointcloud_map")),
    )

    disuse_map_loader = LoadComposableNodes(
        composable_node_descriptions=[disuse_map_euclidean_cluster_component],
        target_container=container,
        condition=UnlessCondition(LaunchConfiguration("use_pointcloud_map")),
    )

    return [container, use_map_loader, disuse_map_loader]
def generate_launch_description():
    composable_nodes = [
        ComposableNode(
            package='stereo_image_proc',
            plugin='stereo_image_proc::DisparityNode',
            parameters=[{
                'approximate_sync':
                LaunchConfiguration('approximate_sync'),
                'use_system_default_qos':
                LaunchConfiguration('use_system_default_qos'),
                'stereo_algorithm':
                LaunchConfiguration('stereo_algorithm'),
                'prefilter_size':
                LaunchConfiguration('prefilter_size'),
                'prefilter_cap':
                LaunchConfiguration('prefilter_cap'),
                'correlation_window_size':
                LaunchConfiguration('correlation_window_size'),
                'min_disparity':
                LaunchConfiguration('min_disparity'),
                'disparity_range':
                LaunchConfiguration('disparity_range'),
                'texture_threshold':
                LaunchConfiguration('texture_threshold'),
                'speckle_size':
                LaunchConfiguration('speckle_size'),
                'speckle_range':
                LaunchConfiguration('speckle_range'),
                'disp12_max_diff':
                LaunchConfiguration('disp12_max_diff'),
                'uniqueness_ratio':
                LaunchConfiguration('uniqueness_ratio'),
                'P1':
                LaunchConfiguration('P1'),
                'P2':
                LaunchConfiguration('P2'),
                'full_dp':
                LaunchConfiguration('full_dp'),
            }],
            remappings=[
                ('left/image_rect',
                 [LaunchConfiguration('left_namespace'), '/image_rect']),
                ('left/camera_info',
                 [LaunchConfiguration('left_namespace'), '/camera_info']),
                ('right/image_rect',
                 [LaunchConfiguration('right_namespace'), '/image_rect']),
                ('right/camera_info',
                 [LaunchConfiguration('right_namespace'), '/camera_info']),
            ]),
        ComposableNode(
            package='stereo_image_proc',
            plugin='stereo_image_proc::PointCloudNode',
            parameters=[{
                'approximate_sync':
                LaunchConfiguration('approximate_sync'),
                'avoid_point_cloud_padding':
                LaunchConfiguration('avoid_point_cloud_padding'),
                'use_color':
                LaunchConfiguration('use_color'),
                'use_system_default_qos':
                LaunchConfiguration('use_system_default_qos'),
            }],
            remappings=[
                ('left/camera_info',
                 [LaunchConfiguration('left_namespace'), '/camera_info']),
                ('right/camera_info',
                 [LaunchConfiguration('right_namespace'), '/camera_info']),
                ('left/image_rect_color',
                 [LaunchConfiguration('left_namespace'), '/image_rect_color']),
            ]),
    ]

    return LaunchDescription([
        DeclareLaunchArgument(
            name='approximate_sync',
            default_value='False',
            description=
            'Whether to use approximate synchronization of topics. Set to true if '
            'the left and right cameras do not produce exactly synced timestamps.'
        ),
        DeclareLaunchArgument(
            name='avoid_point_cloud_padding',
            default_value='False',
            description='Avoid alignment padding in the generated point cloud.'
            'This reduces bandwidth requirements, as the point cloud size is halved.'
            'Using point clouds without alignment padding might degrade performance '
            'for some algorithms.'),
        DeclareLaunchArgument(
            name='use_color',
            default_value='True',
            description='Generate point cloud with rgb data.'),
        DeclareLaunchArgument(
            name='use_system_default_qos',
            default_value='False',
            description=
            'Use the RMW QoS settings for the image and camera info subscriptions.'
        ),
        DeclareLaunchArgument(
            name='launch_image_proc',
            default_value='True',
            description=
            'Whether to launch debayer and rectify nodes from image_proc.'),
        DeclareLaunchArgument(name='left_namespace',
                              default_value='left',
                              description='Namespace for the left camera'),
        DeclareLaunchArgument(name='right_namespace',
                              default_value='right',
                              description='Namespace for the right camera'),
        DeclareLaunchArgument(
            name='container',
            default_value='',
            description=
            ('Name of an existing node container to load launched nodes into. '
             'If unset, a new container will be created.')),
        # Stereo algorithm parameters
        DeclareLaunchArgument(
            name='stereo_algorithm',
            default_value='0',
            description=
            'Stereo algorithm: Block Matching (0) or Semi-Global Block Matching (1)'
        ),
        DeclareLaunchArgument(
            name='prefilter_size',
            default_value='9',
            description='Normalization window size in pixels (must be odd)'),
        DeclareLaunchArgument(name='prefilter_cap',
                              default_value='31',
                              description='Bound on normalized pixel values'),
        DeclareLaunchArgument(
            name='correlation_window_size',
            default_value='15',
            description='SAD correlation window width in pixels (must be odd)'
        ),
        DeclareLaunchArgument(
            name='min_disparity',
            default_value='0',
            description='Disparity to begin search at in pixels'),
        DeclareLaunchArgument(
            name='disparity_range',
            default_value='64',
            description=
            'Number of disparities to search in pixels (must be a multiple of 16)'
        ),
        DeclareLaunchArgument(
            name='texture_threshold',
            default_value='10',
            description=
            'Filter out if SAD window response does not exceed texture threshold'
        ),
        DeclareLaunchArgument(
            name='speckle_size',
            default_value='100',
            description='Reject regions smaller than this size in pixels'),
        DeclareLaunchArgument(
            name='speckle_range',
            default_value='4',
            description=
            'Maximum allowed difference between detected disparities'),
        DeclareLaunchArgument(
            name='disp12_max_diff',
            default_value='0',
            description=
            'Maximum allowed difference in the left-right disparity check in pixels '
            '(Semi-Global Block Matching only)'),
        DeclareLaunchArgument(
            name='uniqueness_ratio',
            default_value='15.0',
            description=
            'Filter out if best match does not sufficiently exceed the next-best match'
        ),
        DeclareLaunchArgument(
            name='P1',
            default_value='200.0',
            description=
            'The first parameter ccontrolling the disparity smoothness '
            '(Semi-Global Block Matching only)'),
        DeclareLaunchArgument(
            name='P2',
            default_value='400.0',
            description=
            'The second parameter ccontrolling the disparity smoothness '
            '(Semi-Global Block Matching only)'),
        DeclareLaunchArgument(
            name='full_dp',
            default_value='False',
            description=
            'Run the full variant of the algorithm (Semi-Global Block Matching only)'
        ),
        ComposableNodeContainer(
            condition=LaunchConfigurationEquals('container', ''),
            package='rclcpp_components',
            executable='component_container',
            name='stereo_image_proc_container',
            namespace='',
            composable_node_descriptions=composable_nodes,
        ),
        LoadComposableNodes(
            condition=LaunchConfigurationNotEquals('container', ''),
            composable_node_descriptions=composable_nodes,
            target_container=LaunchConfiguration('container'),
        ),
        # If a container name is not provided,
        # set the name of the container launched above for image_proc nodes
        SetLaunchConfiguration(condition=LaunchConfigurationEquals(
            'container', ''),
                               name='container',
                               value='stereo_image_proc_container'),
        GroupAction(
            [
                PushRosNamespace(LaunchConfiguration('left_namespace')),
                IncludeLaunchDescription(PythonLaunchDescriptionSource([
                    FindPackageShare('image_proc'),
                    '/launch/image_proc.launch.py'
                ]),
                                         launch_arguments={
                                             'container':
                                             LaunchConfiguration('container')
                                         }.items()),
            ],
            condition=IfCondition(LaunchConfiguration('launch_image_proc')),
        ),
        GroupAction(
            [
                PushRosNamespace(LaunchConfiguration('right_namespace')),
                IncludeLaunchDescription(PythonLaunchDescriptionSource([
                    FindPackageShare('image_proc'),
                    '/launch/image_proc.launch.py'
                ]),
                                         launch_arguments={
                                             'container':
                                             LaunchConfiguration('container')
                                         }.items()),
            ],
            condition=IfCondition(LaunchConfiguration('launch_image_proc')),
        )
    ])
def generate_launch_description():
    def add_launch_arg(name: str, default_value=None):
        return DeclareLaunchArgument(name, default_value=default_value)

    set_container_executable = SetLaunchConfiguration(
        "container_executable",
        "component_container",
        condition=UnlessCondition(LaunchConfiguration("use_multithread")),
    )

    set_container_mt_executable = SetLaunchConfiguration(
        "container_executable",
        "component_container_mt",
        condition=IfCondition(LaunchConfiguration("use_multithread")),
    )

    composable_nodes = [
        ComposableNode(
            package="pointcloud_to_laserscan",
            plugin="pointcloud_to_laserscan::PointCloudToLaserScanNode",
            name="pointcloud_to_laserscan_node",
            remappings=[
                ("~/input/pointcloud", LaunchConfiguration("input/obstacle_pointcloud")),
                ("~/output/laserscan", LaunchConfiguration("output/laserscan")),
                ("~/output/pointcloud", LaunchConfiguration("output/pointcloud")),
                ("~/output/ray", LaunchConfiguration("output/ray")),
                ("~/output/stixel", LaunchConfiguration("output/stixel")),
            ],
            parameters=[
                {
                    "target_frame": "base_link",  # Leave disabled to output scan in pointcloud frame
                    "transform_tolerance": 0.01,
                    "min_height": 0.0,
                    "max_height": 2.0,
                    "angle_min": -3.141592,  # -M_PI
                    "angle_max": 3.141592,  # M_PI
                    "angle_increment": 0.00436332222,  # 0.25*M_PI/180.0
                    "scan_time": 0.0,
                    "range_min": 1.0,
                    "range_max": 200.0,
                    "use_inf": True,
                    "inf_epsilon": 1.0,
                    # Concurrency level, affects number of pointclouds queued for processing
                    # and number of threads used
                    # 0 : Detect number of cores
                    # 1 : Single threaded
                    # 2->inf : Parallelism level
                    "concurrency_level": 1,
                }
            ],
            extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
        ),
        ComposableNode(
            package="laserscan_to_occupancy_grid_map",
            plugin="occupancy_grid_map::OccupancyGridMapNode",
            name="occupancy_grid_map_node",
            remappings=[
                ("~/input/laserscan", LaunchConfiguration("output/laserscan")),
                ("~/input/obstacle_pointcloud", LaunchConfiguration("input/obstacle_pointcloud")),
                ("~/input/raw_pointcloud", LaunchConfiguration("input/raw_pointcloud")),
                ("~/output/occupancy_grid_map", LaunchConfiguration("output")),
            ],
            parameters=[
                {
                    "map_resolution": 0.5,
                    "use_height_filter": True,
                    "input_obstacle_pointcloud": LaunchConfiguration("input_obstacle_pointcloud"),
                    "input_obstacle_and_raw_pointcloud": LaunchConfiguration(
                        "input_obstacle_and_raw_pointcloud"
                    ),
                }
            ],
            extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
        ),
    ]

    occupancy_grid_map_container = ComposableNodeContainer(
        condition=LaunchConfigurationEquals("container", ""),
        name="occupancy_grid_map_container",
        namespace="",
        package="rclcpp_components",
        executable=LaunchConfiguration("container_executable"),
        composable_node_descriptions=composable_nodes,
        output="screen",
    )

    load_composable_nodes = LoadComposableNodes(
        condition=LaunchConfigurationNotEquals("container", ""),
        composable_node_descriptions=composable_nodes,
        target_container=LaunchConfiguration("container"),
    )

    return LaunchDescription(
        [
            add_launch_arg("container", ""),
            add_launch_arg("use_multithread", "false"),
            add_launch_arg("use_intra_process", "false"),
            add_launch_arg("input/obstacle_pointcloud", "no_ground/oneshot/pointcloud"),
            add_launch_arg("input/raw_pointcloud", "concatenated/pointcloud"),
            add_launch_arg("output", "occupancy_grid"),
            add_launch_arg("output/laserscan", "virtual_scan/laserscan"),
            add_launch_arg("output/pointcloud", "virtual_scan/pointcloud"),
            add_launch_arg("output/ray", "virtual_scan/ray"),
            add_launch_arg("output/stixel", "virtual_scan/stixel"),
            add_launch_arg("input_obstacle_pointcloud", "false"),
            add_launch_arg("input_obstacle_and_raw_pointcloud", "true"),
            set_container_executable,
            set_container_mt_executable,
            occupancy_grid_map_container,
            load_composable_nodes,
        ]
    )