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",
                respawn=True,        
                respawn_delay=2      
            ),
            
            # Node(
            #     package="radio_ctrl",
            #     executable="radio_ctrl_node",
            #     name="radio_ctrl_node",
            #     output="screen",
            #     respawn=True,
            #     respawn_delay=2
            # ),

            Node(
                package="can_radio_ctrl",
                executable="can_radio_ctrl_node",
                name="can_radio_ctrl_node",
                output="screen",
                respawn=True,        
                respawn_delay=2      
            ),
            
            Node(
                package="remote_ctrl",
                executable="remote_ctrl_node",
                name="remote_ctrl_node",
                output="screen",
                respawn=True,
                respawn_delay=2
            ),
            
            Node(
                package="mc",
                executable="mc_node",
                name="mc_node",
                output="screen",
                respawn=True,
                respawn_delay=2
            ),
            
            Node(
                package="ctrl_arbiter",
                executable="ctrl_arbiter_node",
                name="ctrl_arbiter_node",
                output="screen",
                respawn=True,
                respawn_delay=2
            ),
            
            Node(
                package="mqtt_report",
                executable="mqtt_report_node",
                name="mqtt_report_node",
                output="screen",
                respawn=True,
                respawn_delay=2
            ),
            
            IncludeLaunchDescription(
                PythonLaunchDescriptionSource(
                    os.path.join(rslidar_sdk_path, "launch", "start_double.launch.py")
                )
            ),
            
            Node(
                package="rtk",
                executable="rtk_node",
                name="rtk_node",
                parameters=[
                    os.path.join(
                        get_package_share_directory("rtk"),
                        "config",
                        "rtk_params.yaml"
                    )
                ],  # 从YAML文件加载参数
                output="screen"
            ),
            
            Node(
                package="route",
                executable="route_node",
                name="route_node",
                output="screen",
                respawn=True,
                respawn_delay=2
            ),
            
            Node(
                package="sub",
                executable="sub_node",
                name="sub_node",
                output="screen",
                respawn=True,
                respawn_delay=2
            ),
            
            Node(
                package="task_manager",
                executable="task_manager_node",
                name="task_manager_node",
                output="screen",
                respawn=True,
                respawn_delay=2
            ),
            
            Node(
                package="pl",
                executable="pl_node",
                name="pl_node",
                output="screen",
                respawn=True,
                respawn_delay=2
            ),
            
            IncludeLaunchDescription(
                PythonLaunchDescriptionSource(
                    os.path.join(fu_path, "launch", "fu.launch.py")
                )
            ),
        ]
    )