#!/usr/bin/env python3
"""
步骤2：SLAM 建图
功能：启动 Gazebo + SLAM Toolbox + RViz2
用途：手动控制机器人进行 SLAM 建图
配置：使用 Task_3 的 SLAM 配置（已验证稳定）
"""

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')
    slam_params_file = os.path.join(pkg_share, 'config', 'slam_params.yaml')
    rviz_config = os.path.join(pkg_share, 'rviz', 'slam_view.rviz')
    
    # Launch 参数
    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
    
    # 处理 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. SLAM Toolbox（使用 Task_3 的配置）
    # ========================================
    slam_toolbox = Node(
        package='slam_toolbox',
        executable='async_slam_toolbox_node',
        name='slam_toolbox',
        output='screen',
        parameters=[
            slam_params_file,
            {'use_sim_time': use_sim_time}
        ]
    )
    
    # ========================================
    # 5. 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'
        ),
        
        gazebo,
        robot_state_publisher,
        spawn_entity,
        slam_toolbox,
        rviz2,
    ])