from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
import os
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    """
    SkyLink Bridge 启动配置
    
    用法:
        ros2 launch skylink_bridge_python bridge.launch.py
        ros2 launch skylink_bridge_python bridge.launch.py target_ip:=192.168.1.100 jpeg_quality:=80
    """
    
    # 获取包共享目录
    pkg_share_dir = get_package_share_directory('skylink_bridge_python')
    config_file = os.path.join(pkg_share_dir, 'config', 'params.yaml')
    
    # ========================================================================
    # 启动参数声明 (可通过命令行覆盖)
    # ========================================================================
    
    target_ip_arg = DeclareLaunchArgument(
        'target_ip',
        default_value='255.255.255.255',
        description='目标 IP 地址 (广播)'
    )
    
    target_port_arg = DeclareLaunchArgument(
        'target_port',
        default_value='9999',
        description='UDP 目标端口'
    )
    
    gps_topic_arg = DeclareLaunchArgument(
        'gps_topic',
        default_value='/mavros/global_position/global',
        description='GPS Topic 名称'
    )
    # ========================================================================
    # 相机 节点 (OpenCV 驱动)
    # ========================================================================

    camera_driver_node = Node(
        package='skylink_bridge_python',
        executable='camera_driver_node',
        name='camera_driver',
        output='screen',
        parameters=[
            config_file,  # 从 YAML 配置文件加载参数
        ],
    )
    
    # ========================================================================
    # UDP Sender 节点
    # ========================================================================
    
    udp_sender_node = Node(
        package='skylink_bridge_python',
        executable='udp_sender_node',
        name='udp_sender',
        output='screen',
        parameters=[
            config_file,  # 从 YAML 配置文件加载参数
            {
                'target_ip': LaunchConfiguration('target_ip'),
                'target_port': LaunchConfiguration('target_port'),
                'jpeg_quality': LaunchConfiguration('jpeg_quality'),
                'camera_topic': LaunchConfiguration('camera_topic'),
                'gps_topic': LaunchConfiguration('gps_topic'),
            }
        ],
        remappings=[
            # 如果需要重映射 Topic，在此添加
            # ('/camera/image_raw', '/my_camera/image'),
        ]
    )
    
    # ========================================================================
    # 构造启动描述
    # ========================================================================
    
    return LaunchDescription([
        target_ip_arg,
        target_port_arg,
        gps_topic_arg,
        camera_driver_node,
        # udp_sender_node,
    ])