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

from launch.actions import IncludeLaunchDescription

from launch.launch_description_sources import PythonLaunchDescriptionSource

import os

​

​

def generate_launch_description():

    ndt_mapper_param_file = os.path.join(

        get_package_share_directory('ndt_mapping_nodes'),

        'param/ndt_mapper.param.yaml')

​

    scan_downsampler_param_file = os.path.join(

        get_package_share_directory('ndt_mapping_nodes'),

        'param/scan_downsampler.param.yaml')

​

    pc_filter_transform_param_file = os.path.join(

        get_package_share_directory('point_cloud_filter_transform_nodes'),

        'param/vlp16_sim_lexus_filter_transform.param.yaml')

​

    # Arguments

    pc_filter_transform_param = DeclareLaunchArgument(

        'pc_filter_transform_param_file',

        default_value=pc_filter_transform_param_file,

        description='Path to config file for Point Cloud Filter/Transform Nodes'

    )

    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

    filter_transform_vlp16_front = Node(

        package='point_cloud_filter_transform_nodes',

        executable='point_cloud_filter_transform_node_exe',

        name='filter_transform_vlp16_front',

        namespace='lidar_front',

        parameters=[LaunchConfiguration('pc_filter_transform_param_file')],

        remappings=[("points_in", "points_xyzi")]

    )

​

    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", "points_filtered"),

            ("points_downsampled", "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")

        ]

    )

    odom_bl_publisher = Node(

        package='tf2_ros',

        executable='static_transform_publisher',

        arguments=["0", "0", "0", "0", "0", "0", "odom", "base_link"]

    )

​

    point_type_adapter_pkg_prefix = get_package_share_directory(

        'point_type_adapter')

    adapter_launch = IncludeLaunchDescription(

        PythonLaunchDescriptionSource(

            os.path.join(point_type_adapter_pkg_prefix,

                         'launch/point_type_adapter.launch.py'))

    )

​

    return LaunchDescription([

        pc_filter_transform_param,

        scan_downsampler_param,

        ndt_mapper_param,

        filter_transform_vlp16_front,

        scan_downsampler,

        ndt_mapper,

        adapter_launch

    ])

  m_pcl_file_name(declare_parameter("map_pcd_file").get<std::string>()),

  m_yaml_file_name(declare_parameter("map_yaml_file").get<std::string>()),

  m_viz_map(declare_parameter("viz_map", false))

# map_config:

#  latitude: 37.380811523812845

#  longitude: -121.90840595108715

#  elevation: 16.0

#  roll: 0.0

#  pitch: 0.0

#  yaw: 0.0

 if (has_parameter("latitude") && has_parameter("longitude") && has_parameter("elevation")) {

    const float64_t origin_lat = declare_parameter("latitude").get<float64_t>();

    const float64_t origin_lon = declare_parameter("longitude").get<float64_t>();

    const float64_t origin_alt = declare_parameter("elevation").get<float64_t>();

    LatLonAlt map_origin{origin_lat, origin_lon, origin_alt};

    m_map_provider = std::make_unique<Lanelet2MapProvider>(

      map_filename, map_origin, origin_offset_lat, origin_offset_lon);

  } else {

    /// This could potentially also read the same yaml that the ndt map publisher reads

    auto earth_from_map = get_map_origin();

    m_map_provider = std::make_unique<Lanelet2MapProvider>(

      map_filename, std::move(

        earth_from_map), origin_offset_lat, origin_offset_lon);

  }

  m_map_service =

    this->create_service<autoware_auto_mapping_msgs::srv::HADMapService>(

    "HAD_Map_Service", std::bind(

      &Lanelet2MapProviderNode::handle_request, this,

      std::placeholders::_1, std::placeholders::_2));