#!/usr/bin/env python3
"""
小车导航系统启动文件
启动所有必要的硬件和导航节点
"""

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, TimerAction, LogInfo
from launch.launch_description_sources import PythonLaunchDescriptionSource, FrontendLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch.conditions import IfCondition, UnlessCondition

def generate_launch_description():
    # 获取包路径
    cyy_navigation2_dir = get_package_share_directory('cyy_navigation2')
    
    # 声明启动参数
    use_base = LaunchConfiguration('use_base', default='true')
    use_lidar = LaunchConfiguration('use_lidar', default='true')
    use_description = LaunchConfiguration('use_description', default='true')
    use_bridge = LaunchConfiguration('use_bridge', default='true')
    
    # 启动参数列表
    launch_arguments = [
        DeclareLaunchArgument(
            'use_base',
            default_value='true',
            description='启动底盘控制'
        ),
        DeclareLaunchArgument(
            'use_lidar',
            default_value='true',
            description='启动激光雷达'
        ),
        DeclareLaunchArgument(
            'use_description',
            default_value='true',
            description='启动URDF显示'
        ),
        DeclareLaunchArgument(
            'use_bridge',
            default_value='true',
            description='启动ROS Bridge'
        ),
    ]
    
    # 创建启动描述
    ld = LaunchDescription(launch_arguments)
    
    # 1. 启动底盘控制
    base_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            FindPackageShare('origincar_base'), '/launch/base_serial.launch.py'
        ]),
        condition=IfCondition(use_base)
    )
    
    # 添加延迟，确保底盘先启动
    ld.add_action(TimerAction(
        period=1.0,
        actions=[LogInfo(msg='启动底盘控制...'), base_launch]
    ))
    
    # 2. 启动激光雷达
    lidar_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            FindPackageShare('lslidar_driver'), '/launch/lsn10_launch.py'
        ]),
        condition=IfCondition(use_lidar)
    )
    
    ld.add_action(TimerAction(
        period=2.0,
        actions=[LogInfo(msg='启动激光雷达...'), lidar_launch]
    ))
    
    # 3. 启动URDF显示
    description_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            FindPackageShare('origincar_description'), '/launch/display.launch.py'
        ]),
        condition=IfCondition(use_description)
    )
    
    ld.add_action(TimerAction(
        period=3.0,
        actions=[LogInfo(msg='启动URDF显示...'), description_launch]
    ))
    
    # 4. 启动ROS Bridge
    bridge_launch = IncludeLaunchDescription(
        FrontendLaunchDescriptionSource([
            FindPackageShare('rosbridge_server'), '/launch/rosbridge_websocket_launch.xml'
        ]),
        condition=IfCondition(use_bridge)
    )
    
    ld.add_action(TimerAction(
        period=4.0,
        actions=[LogInfo(msg='启动ROS Bridge...'), bridge_launch]
    ))
    
    
    return ld