from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription, TimerAction
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
import os


def generate_launch_description():
    # 获取各包的路径
    rslidar_sdk_path = get_package_share_directory("rslidar_sdk")
    fu_path = get_package_share_directory("fu")

    # ===================== 按顺序延时启动所有节点 =====================
    # 1. 车辆参数节点 (立即启动)
    vehicle_params_node = Node(
        package="vehicle_params",
        executable="vehicle_params_node",
        name="vehicle_params_node",
        output="screen",
        respawn=True,
        respawn_delay=2,
    )

    # 2. CAN遥控节点 (延时0.2s)
    can_radio_ctrl_node = TimerAction(
        period=0.2,
        actions=[Node(
            package="can_radio_ctrl",
            executable="can_radio_ctrl_node",
            name="can_radio_ctrl_node",
            output="screen",
            respawn=True,
            respawn_delay=2,
        )]
    )
    # Node(
    #     package="radio_ctrl",
    #     executable="radio_ctrl_node",
    #     name="radio_ctrl_node",
    #     output="screen",
    #     respawn=True,
    #     respawn_delay=2,
    # ),

    # 3. 远程控制节点 (延时0.4s)
    remote_ctrl_node = TimerAction(
        period=0.4,
        actions=[Node(
            package="remote_ctrl",
            executable="remote_ctrl_node",
            name="remote_ctrl_node",
            output="screen",
            respawn=True,
            respawn_delay=2,
        )]
    )

    # 4. MC电机控制节点 (延时0.6s)
    mc_node = TimerAction(
        period=0.6,
        actions=[Node(
            package="mc",
            executable="mc_node",
            name="mc_node",
            output="screen",
            respawn=True,
            respawn_delay=2,
        )]
    )

    # 5. 控制仲裁节点 (延时0.8s)
    ctrl_arbiter_node = TimerAction(
        period=0.8,
        actions=[Node(
            package="ctrl_arbiter",
            executable="ctrl_arbiter_node",
            name="ctrl_arbiter_node",
            output="screen",
            respawn=True,
            respawn_delay=2,
        )]
    )

    # 6. MQTT上报节点 (延时1.0s) —— 【关键：必须最后启动】
    mqtt_report_node = TimerAction(
        period=1.0,
        actions=[Node(
            package="mqtt_report",
            executable="mqtt_report_node",
            name="mqtt_report_node",
            output="screen",
            respawn=True,
            respawn_delay=2,
        )]
    )

    # 7. 雷达启动 (延时1.5s)
    lidar_launch = TimerAction(
        period=1.5,
        actions=[IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                os.path.join(rslidar_sdk_path, "launch", "start_double.launch.py")
            )
        )]
    )

    # 8. RTK定位节点 (延时2.0s)
    rtk_node = TimerAction(
        period=2.0,
        actions=[Node(
            package="rtk",
            executable="rtk_node",
            name="rtk_node",
            parameters=[
                os.path.join(
                    get_package_share_directory("rtk"), "config", "rtk_params.yaml"
                )
            ],
            output="screen",
        )]
    )

    # 9. 路径节点 (延时2.5s)
    route_node = TimerAction(
        period=2.5,
        actions=[Node(
            package="route",
            executable="route_node",
            name="route_node",
            output="screen",
            respawn=True,
            respawn_delay=2,
        )]
    )

    # 10. 订阅节点 (延时3.0s)
    sub_node = TimerAction(
        period=3.0,
        actions=[Node(
            package="sub",
            executable="sub_node",
            name="sub_node",
            output="screen",
            respawn=True,
            respawn_delay=2,
        )]
    )

    # 11. 任务管理节点 (延时3.5s)
    task_manager_node = TimerAction(
        period=3.5,
        actions=[Node(
            package="task_manager",
            executable="task_manager_node",
            name="task_manager_node",
            output="screen",
            respawn=True,
            respawn_delay=2,
        )]
    )

    # 12. 规划控制节点 (延时4.0s)
    pl_node = TimerAction(
        period=4.0,
        actions=[Node(
            package="pl",
            executable="pl_node",
            name="pl_node",
            output="screen",
            respawn=True,
            respawn_delay=2,
        )]
    )

    # 13. FU功能节点 (延时4.5s)
    fu_launch = TimerAction(
        period=4.5,
        actions=[IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                os.path.join(fu_path, "launch", "fu.launch.py")
            )
        )]
    )

    # ===================== 组装启动列表 =====================
    return LaunchDescription(
        [
            vehicle_params_node,
            can_radio_ctrl_node,
            remote_ctrl_node,
            mc_node,
            ctrl_arbiter_node,
            mqtt_report_node,
            lidar_launch,
            rtk_node,
            route_node,
            sub_node,
            task_manager_node,
            pl_node,
            fu_launch,
        ]
    )