#!/usr/bin/env python3
"""
Launch localization with automatic AMCL initial pose setting.
No need to manually click 2D Pose Estimate in RViz.
"""

from launch import LaunchDescription
from launch.actions import ExecuteProcess, IncludeLaunchDescription, TimerAction
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
import os

from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
    bringup_dir = get_package_share_directory('nav2_bringup')
    script_path = os.path.join(
        get_package_share_directory('nav2_rrtstar_planner'),
        'scripts',
        'publish_initial_pose.py'
    )

    # Launch arguments
    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
    map_yaml_file = LaunchConfiguration('map')

    # Include original localization_launch.py
    localization_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(bringup_dir, 'launch', 'localization_launch.py')
        ),
        launch_arguments={
            'use_sim_time': use_sim_time,
            'map': map_yaml_file
        }.items()
    )

    # Auto-publish initial pose (0, 0) after 3 seconds delay
    auto_initial_pose = TimerAction(
        period=3.0,
        actions=[
            ExecuteProcess(
                cmd=['python3', script_path],
                output='screen'
            )
        ]
    )

    ld = LaunchDescription()
    ld.add_action(localization_launch)
    ld.add_action(auto_initial_pose)

    return ld
