ndt_mapping.launch.py

"""Launch P2D NDT mapper."""

from ament_index_python import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

import os


def generate_launch_description():
    """
    Launch all nodes required for mapping. This launch file is for pure ndt-mapping.

    If odometry is available, remove the static tf publication and add the odometry node(s)
    to this launch file.
    """
    ndt_mapper_param_file = os.path.join(
        get_package_share_directory('carla_tc'),
        'param/ndt_mapper.param.yaml')

    scan_downsampler_param_file = os.path.join(
        get_package_share_directory('carla_tc'),
        'param/scan_downsampler.param.yaml')

    # Arguments

    ndt_mapper_param = DeclareLaunchArgument(
        'ndt_param_param_file',
        default_value=ndt_mapper_param_file,
        description='Path to config file for ndt mapper'
    )
    scan_downsampler_param = DeclareLaunchArgument(
        'scan_downsampler_param_file',
        default_value=scan_downsampler_param_file,
        description='Path to config file for lidar scan downsampler'
    )

    # Nodes


    scan_downsampler = Node(
        package='voxel_grid_nodes',
        executable='voxel_grid_node_exe',
        name='voxel_grid_cloud_node',
        namespace='lidar_front',
        parameters=[LaunchConfiguration('scan_downsampler_param_file')],
        remappings=[
            ("points_in", "/lidar_front/points_filtered"),
            ("points_downsampled", "/lidar_front/points_filtered_downsampled")
        ]
    )
    ndt_mapper = Node(
        package='ndt_mapping_nodes',
        executable='ndt_mapper_node_exe',
        name='ndt_mapper_node',
        namespace='mapper',
        output='screen',
        parameters=[LaunchConfiguration('ndt_param_param_file')],
        remappings=[
            ("points_in", "/lidar_front/points_filtered_downsampled"),
            ("points_registered", "/lidar_front/points_registered")
        ]
    )

    # This is a hack to make the mapper purely rely on the ndt localizer without using any
    # odometry source.
    # TODO(yunus.caliskan): Revisit after #476
    odom_bl_publisher = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        arguments=["0", "0", "0", "0", "0", "0", "odom", "ego_vehicle"]
    )

    return LaunchDescription([
        scan_downsampler_param,
        ndt_mapper_param,
        scan_downsampler,
        odom_bl_publisher,
        ndt_mapper
    ])