#!/usr/bin/env python3
"""
步骤3：Nav2 导航
功能：启动 Gazebo + AMCL + Nav2 + RViz2
用途：使用保存的地图进行自主导航
配置：使用 Task_1 的 Nav2 配置（已验证有效）
"""

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


def generate_launch_description():
    # 包名和路径
    pkg_name = 'final_navigation'
    pkg_share = FindPackageShare(pkg_name).find(pkg_name)
    
    # 文件路径
    xacro_file = os.path.join(pkg_share, 'urdf', 'fusion_robot.urdf.xacro')
    world_file = os.path.join(pkg_share, 'worlds', 'simple_house.world')
    nav2_params_file = os.path.join(pkg_share, 'config', 'nav2_params.yaml')
    
    # 地图文件路径（默认查找顺序）
   
    # 1. 检查 install 目录
    map_file_install = os.path.join(pkg_share, 'maps', 'my_map.yaml')
    # 2. 检查工作空间根目录
    map_file_ws = os.path.expanduser('~/test/task_final/final_navigation/maps/my_map.yaml')
    
    # 选择存在的地图文件
    if os.path.exists(map_file_install):
        map_file = map_file_install
        print(f"\n✅ 找到地图: {map_file_install}\n")
    elif os.path.exists(map_file_ws):
        map_file = map_file_ws
        print(f"\n✅ 找到地图: {map_file_ws}\n")
    else:
        map_file = map_file_install  # 使用默认路径
        print(f"\n⚠️  警告: 地图文件不存在！")
        print(f"   已检查: {map_file_install}")
        print(f"   已检查: {map_file_ws}")
        print(f"   请确保已运行 step2_mapping.launch.py 并保存地图\n")
    
    rviz_config = os.path.join(pkg_share, 'rviz', 'nav_view.rviz')
    
    # Launch 参数
    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
    map_yaml_file = LaunchConfiguration('map', default=map_file)
    
    # 处理 xacro 文件
    robot_description = Command(['xacro ', xacro_file])
    
    # ========================================
    # 1. Gazebo 仿真
    # ========================================
    gazebo = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            PathJoinSubstitution([
                FindPackageShare('gazebo_ros'),
                'launch',
                'gazebo.launch.py'
            ])
        ]),
        launch_arguments={
            'world': world_file,
            'verbose': 'false'
        }.items()
    )
    
    # ========================================
    # 2. 机器人状态发布器
    # ========================================
    robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        name='robot_state_publisher',
        output='screen',
        parameters=[{
            'robot_description': robot_description,
            'use_sim_time': use_sim_time
        }]
    )
    
    # ========================================
    # 3. 在 Gazebo 中生成机器人
    # ========================================
    spawn_entity = Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        arguments=[
            '-entity', 'fusion_robot',
            '-topic', 'robot_description',
            '-x', '0.0',
            '-y', '0.0',
            '-z', '0.1'
        ],
        output='screen'
    )
    
    # ========================================
    # 4. 地图服务器（使用 Nav2 Bringup 的 localization_launch.py）
    # ========================================
    # 注意：不单独启动 map_server 和 amcl，使用 Nav2 的 localization_launch.py
    localization = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            PathJoinSubstitution([
                FindPackageShare('nav2_bringup'),
                'launch',
                'localization_launch.py'
            ])
        ]),
        launch_arguments={
            'map': map_yaml_file,
            'use_sim_time': 'true',
            'params_file': nav2_params_file
        }.items()
    )
    
    # ========================================
    # 5. Nav2 导航栈
    # ========================================
    navigation = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            PathJoinSubstitution([
                FindPackageShare('nav2_bringup'),
                'launch',
                'navigation_launch.py'
            ])
        ]),
        launch_arguments={
            'use_sim_time': 'true',
            'params_file': nav2_params_file
        }.items()
    )
    
    # ========================================
    # 6. RViz2 可视化
    # ========================================
    rviz2 = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        arguments=['-d', rviz_config],
        parameters=[{'use_sim_time': use_sim_time}],
        output='screen'
    )
    
    
    return LaunchDescription([
        DeclareLaunchArgument(
            'use_sim_time',
            default_value='true',
            description='Use simulation time'
        ),
        DeclareLaunchArgument(
            'map',
            default_value=map_file,
            description='Full path to map yaml file'
        ),
        
        gazebo,
        robot_state_publisher,
        spawn_entity,
        localization,
        navigation,
        rviz2,
    ])