"""
Amadeus 26 哨兵机器人启动文件
启动所有核心节点：IMU接收、中央控制、导航、UART收发
"""

from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration


def generate_launch_description():
    # 声明启动参数
    enemy_color_arg = DeclareLaunchArgument(
        'enemy_color',
        default_value='red',
        description='敌方装甲板颜色 (red/blue)'
    )
    
    # IMU接收节点
    imu_receiver_node = Node(
        package='amadeus_26',
        executable='imu_receiver_node',
        name='imu_receiver_node',
        output='screen',
        parameters=[{
            'serial_port': '/dev/ttyIMU',
            'baudrate': 921600,
            'verbose': False,
        }]
    )
    
    # 中央节点
    central_node = Node(
        package='amadeus_26',
        executable='central_node',
        name='central_node',
        output='screen',
        parameters=[{
            'enemy_color': LaunchConfiguration('enemy_color'),
            'max_hp': 100,
            'low_hp_threshold': 50,
            'control_freq': 50.0,
        }]
    )
    
    # 导航节点
    navigation_node = Node(
        package='amadeus_26',
        executable='navigation_node',
        name='navigation_node',
        output='screen',
        parameters=[{
            'map_width': 12.0,
            'map_height': 8.0,
            'robot_radius': 0.375,
            'max_speed': 2.0,
            'nav_freq': 50.0,
            'init_x': 0.375,
            'init_y': 6.375,
            'init_heading': 0.0,
        }]
    )
    
    # UART收发节点
    uart_transmitter_node = Node(
        package='amadeus_26',
        executable='uart_transmitter_node',
        name='uart_transmitter_node',
        output='screen',
        parameters=[{
            'serial_port': '/dev/ttyCH340',
            'baudrate': 115200,
            'send_frequency': 50.0,
        }]
    )
    
    return LaunchDescription([
        enemy_color_arg,
        imu_receiver_node,
        central_node,
        navigation_node,
        uart_transmitter_node,
    ])