#!/usr/bin/env python3
"""Launch obstacle_nav2 with the C++ trajectory guard node."""

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():
    pkg_dir = get_package_share_directory("obstacle_nav2")
    base_launch = os.path.join(pkg_dir, "launch", "obstacle_nav2.launch.py")
    guard_params = LaunchConfiguration("guard_params")

    obstacle_nav2 = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(base_launch),
        launch_arguments={
            "use_sim_time": LaunchConfiguration("use_sim_time"),
            "global_frame": LaunchConfiguration("global_frame"),
            "use_static_map": LaunchConfiguration("use_static_map"),
            "map_yaml": LaunchConfiguration("map_yaml"),
            "enable_motion": LaunchConfiguration("enable_motion"),
            "start_base": LaunchConfiguration("start_base"),
            "start_lidar": LaunchConfiguration("start_lidar"),
            "start_obstacle_scanner": LaunchConfiguration("start_obstacle_scanner"),
        }.items(),
    )

    trajectory_guard = Node(
        package="obstacle_nav2",
        executable="trajectory_guard_node",
        name="trajectory_guard_node",
        output="screen",
        parameters=[guard_params],
    )

    return LaunchDescription(
        [
            DeclareLaunchArgument("use_sim_time", default_value="false"),
            DeclareLaunchArgument("global_frame", default_value="odom"),
            DeclareLaunchArgument("use_static_map", default_value="false"),
            DeclareLaunchArgument("map_yaml", default_value=""),
            DeclareLaunchArgument("enable_motion", default_value="true"),
            DeclareLaunchArgument("start_base", default_value="true"),
            DeclareLaunchArgument("start_lidar", default_value="true"),
            DeclareLaunchArgument("start_obstacle_scanner", default_value="true"),
            DeclareLaunchArgument(
                "guard_params",
                default_value=os.path.join(pkg_dir, "config", "trajectory_guard.yaml"),
            ),
            obstacle_nav2,
            trajectory_guard,
        ]
    )
