#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# ============================================================================
# master_launch.py — 机器人系统总启动文件
# 启动顺序: origincar_base → LiDAR → EKF → SLAM → Nav2 → VLM_Detect
# 用法:
#   ros2 launch my_robot_bringup master_launch.py
#   ros2 launch my_robot_bringup master_launch.py use_vlm:=false
#   ros2 launch my_robot_bringup master_launch.py vlm_host:=http://X.X.X.X:8000
# ============================================================================
## 障碍物检测 路径规划 轨迹跟随 重定位
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import (
    DeclareLaunchArgument,
    IncludeLaunchDescription,
    TimerAction,
    LogInfo,
)
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():

    # ================================================================
    # 包路径
    # ================================================================
    origincar_dir = get_package_share_directory('origincar_base')
    lslidar_dir = get_package_share_directory('lslidar_driver')
    cyy_slam_dir = get_package_share_directory('cyy_slamtoolbox')
    gc_nav_dir = get_package_share_directory('gc_navigation2_slamtoolbox')
    vlm_dir = get_package_share_directory('vlm_detect')

    # ================================================================
    # Launch 参数 — 可通过命令行覆盖
    # ================================================================

    # 模块开关
    use_base = LaunchConfiguration('use_base', default='true')
    use_lidar = LaunchConfiguration('use_lidar', default='true')
    use_slam = LaunchConfiguration('use_slam', default='true')
    use_nav2 = LaunchConfiguration('use_nav2', default='true')
    use_vlm = LaunchConfiguration('use_vlm', default='true')
    use_tts = LaunchConfiguration('use_tts', default='true')

    # 底盘模式
    akmcar = LaunchConfiguration('akmcar', default='true')
    carto_slam = LaunchConfiguration('carto_slam', default='false')

    # SLAM / Nav2 配置文件
    slam_params_file = LaunchConfiguration('slam_params_file', default=os.path.join(
        cyy_slam_dir, 'config', 'mapper_params_online_async.yaml'))
    nav2_params_file = LaunchConfiguration('nav2_params_file', default=os.path.join(
        gc_nav_dir, 'params', 'gc_navigation_slam.yaml'))

    # VLM 推理服务地址 (Windows WSL)
    vlm_host = LaunchConfiguration('vlm_host', default='http://192.168.10.189:8000')

    # 通用
    use_sim_time = LaunchConfiguration('use_sim_time', default='false')

    # ================================================================
    # 阶段 1: 底盘驱动 + EKF + TF + IMU  (t=0s)
    #
    # origincar_bringup 内部已包含:
    #   - base_serial.launch.py      底盘驱动 (origincar_base_node)
    #   - robot_mode_description     robot_state_publisher (TF from URDF)
    #   - imu_filter_madgwick        IMU 姿态滤波
    #   - robot_localization ekf_node EKF 融合 (odom + IMU → odom_combined)
    #   - joint_state_publisher      关节状态发布
    #   - static TF: base_footprint→gyro_link, base_link→laser
    # ================================================================
    origincar_bringup = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            origincar_dir, '/launch', '/origincar_bringup.launch.py'
        ]),
        condition=IfCondition(use_base),
        launch_arguments={
            'akmcar': akmcar,
            'carto_slam': carto_slam,
        }.items(),
    )

    # ================================================================
    # 阶段 2: 激光雷达  (t=2s)
    # lsn10_launch.py 启动 LS-N10 型号激光雷达驱动
    # 如有其他型号，改包名和 launch 文件名即可
    # ================================================================
    lidar_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            lslidar_dir, '/launch', '/lsn10_launch.py'
        ]),
        condition=IfCondition(use_lidar),
    )

    # ================================================================
    # 阶段 3: SLAM Toolbox — 实时建图  (t=5s)
    # 需要: /scan (LiDAR) + odom→base_link TF (EKF)
    # 输出: map→odom TF + /map topic
    # ================================================================
    slam_toolbox_node = Node(
        package='slam_toolbox',
        executable='async_slam_toolbox_node',
        name='slam_toolbox',
        output='screen',
        condition=IfCondition(use_slam),
        parameters=[slam_params_file,
                     {'use_sim_time': use_sim_time}],
    )

    # ================================================================
    # 阶段 4: Nav2 导航栈  (t=8s)
    # 需要: /map (SLAM) + /scan + 完整 TF 树 (map→odom→base_link→laser)
    # 输出: /cmd_vel → origincar_base → 底盘运动
    # ================================================================
    nav2_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            FindPackageShare('nav2_bringup'), '/launch', '/navigation_launch.py'
        ]),
        condition=IfCondition(use_nav2),
        launch_arguments={
            'use_sim_time': use_sim_time,
            'params_file': nav2_params_file,
        }.items(),
    )

    # ================================================================
    # 阶段 5: VLM 图生文 + TTS 语音播报  (t=10s)
    # 完全独立于导航栈，只依赖 WSL 上 vlm_server.py 已启动
    # ================================================================
    vlm_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            vlm_dir, '/launch', '/vlm_detect.launch.py'
        ]),
        condition=IfCondition(use_vlm),
        launch_arguments={
            'vlm_host': vlm_host,
            'use_tts': use_tts,
        }.items(),
    )

    # ================================================================
    # 组装 — 按时间线顺序，TimerAction 保证先后
    # ================================================================
    return LaunchDescription([

        # —— 参数声明 ——
        DeclareLaunchArgument('use_base', default_value='true',
                              description='底盘驱动 + EKF + TF'),
        DeclareLaunchArgument('use_lidar', default_value='true',
                              description='激光雷达 (lsn10)'),
        DeclareLaunchArgument('use_slam', default_value='true',
                              description='slam_toolbox 建图'),
        DeclareLaunchArgument('use_nav2', default_value='true',
                              description='Nav2 导航栈'),
        DeclareLaunchArgument('use_vlm', default_value='true',
                              description='VLM 图生文 + TTS'),
        DeclareLaunchArgument('use_tts', default_value='true',
                              description='TTS 语音播报开关'),
        DeclareLaunchArgument('akmcar', default_value='true',
                              description='阿克曼底盘 (true=阿克曼, false=差速)'),
        DeclareLaunchArgument('carto_slam', default_value='false',
                              description='使用 Cartographer 替代 EKF'),
        DeclareLaunchArgument('slam_params_file', default_value=os.path.join(
            cyy_slam_dir, 'config', 'mapper_params_online_async.yaml'),
                              description='SLAM 参数文件'),
        DeclareLaunchArgument('nav2_params_file', default_value=os.path.join(
            gc_nav_dir, 'params', 'gc_navigation_slam.yaml'),
                              description='Nav2 参数文件'),
        DeclareLaunchArgument('vlm_host', default_value='http://192.168.10.189:8000',
                              description='VLM 推理服务地址'),
        DeclareLaunchArgument('use_sim_time', default_value='false',
                              description='使用仿真时间'),

        # —— 时间线日志 ——
        LogInfo(msg='========================================'),
        LogInfo(msg='  Robot System Bringup'),
        LogInfo(msg='========================================'),

        # t=0s: 底盘 + EKF
        TimerAction(period=0.0, actions=[
            LogInfo(msg='[1/5] Starting base driver + EKF + TF + IMU ...'),
            origincar_bringup,
        ]),

        # t=2s: 激光雷达
        TimerAction(period=2.0, actions=[
            LogInfo(msg='[2/5] Starting LiDAR (lsn10) ...'),
            lidar_launch,
        ]),

        # t=5s: SLAM
        TimerAction(period=5.0, actions=[
            LogInfo(msg='[3/5] Starting slam_toolbox ...'),
            slam_toolbox_node,
        ]),

        # t=8s: Nav2
        TimerAction(period=8.0, actions=[
            LogInfo(msg='[4/5] Starting Nav2 navigation ...'),
            nav2_launch,
        ]),

        # t=10s: VLM
        TimerAction(period=10.0, actions=[
            LogInfo(msg='[5/5] Starting VLM + TTS ...'),
            vlm_launch,
        ]),

        LogInfo(msg='========================================'),
        LogInfo(msg='  All modules started!'),
        LogInfo(msg='========================================'),
    ])
