#!/usr/bin/env python3

import os

from ament_index_python.packages 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


def generate_launch_description():
    pkg_dir = get_package_share_directory('obstacle_nav2')
    profile_10_path = os.path.join(pkg_dir, 'config', 'nav2_profile_10.yaml')
    profile_11_path = os.path.join(pkg_dir, 'config', 'nav2_profile_11.yaml')

    profile_10_yaml = LaunchConfiguration('profile_10_yaml')
    profile_11_yaml = LaunchConfiguration('profile_11_yaml')
    profile_trigger_topic = LaunchConfiguration('profile_trigger_topic')

    nav2_profile_tuner = Node(
        package='obstacle_nav2',
        executable='nav2_profile_tuner',
        name='nav2_profile_tuner',
        output='screen',
        parameters=[{
            'profile_10_yaml': profile_10_yaml,
            'profile_11_yaml': profile_11_yaml,
            'trigger_topic': profile_trigger_topic,
        }],
    )

    return LaunchDescription([
        DeclareLaunchArgument(
            'profile_10_yaml',
            default_value=profile_10_path,
            description='YAML profile applied by default and when sign4return=10'),
        DeclareLaunchArgument(
            'profile_11_yaml',
            default_value=profile_11_path,
            description='YAML profile applied when sign4return=11'),
        DeclareLaunchArgument(
            'profile_trigger_topic',
            default_value='/sign4return',
            description='Int32 topic used to switch Nav2 runtime profiles'),

        nav2_profile_tuner,
    ])
