from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription
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")
    
    return LaunchDescription(
        [
            # 启动车辆参数节点
            Node(
                package="vehicle_params",
                executable="vehicle_params_node",
                name="vehicle_params_node",
                output="screen",
            ),
            
            # 启动无线电控制节点
            Node(
                package="radio_ctrl",
                executable="radio_ctrl_node",
                name="radio_ctrl_node",
                output="screen",
            ),
            
            # 启动远程控制节点
            Node(
                package="remote_ctrl",
                executable="remote_ctrl_node",
                name="remote_ctrl_node",
                output="screen",
            ),
            
            # 启动MC节点
            Node(
                package="mc",
                executable="mc_node",
                name="mc_node",
                output="screen",
            ),
            
            # 启动控制仲裁节点
            Node(
                package="ctrl_arbiter",
                executable="ctrl_arbiter_node",
                name="ctrl_arbiter_node",
                output="screen",
            ),
            
            # 启动MQTT报告节点
            Node(
                package="mqtt_report",
                executable="mqtt_report_node",
                name="mqtt_report_node",
                output="screen",
            ),
            
            # 启动激光雷达节点（使用现有launch文件）
            IncludeLaunchDescription(
                PythonLaunchDescriptionSource(
                    os.path.join(rslidar_sdk_path, "launch", "start_double.launch.py")
                )
            ),
            
            # 启动RTK节点
            Node(
                package="rtk",
                executable="rtk_node",
                name="rtk_node",
                output="screen",
            ),
            
            # 启动路径节点
            Node(
                package="route",
                executable="route_node",
                name="route_node",
                output="screen",
            ),
            
            # 启动SUB节点
            Node(
                package="sub",
                executable="sub_node",
                name="sub_node",
                output="screen",
            ),
            
            # 启动任务管理器节点
            Node(
                package="task_manager",
                executable="task_manager_node",
                name="task_manager_node",
                output="screen",
            ),
            
            # 启动PL节点
            Node(
                package="pl",
                executable="pl_node",
                name="pl_node",
                output="screen",
            ),
            
            # 启动FU节点（使用现有launch文件）
            IncludeLaunchDescription(
                PythonLaunchDescriptionSource(
                    os.path.join(fu_path, "launch", "fu.launch.py")
                )
            ),
        ]
    )