from launch import LaunchDescription
from launch_ros.actions import ComposableNodeContainer
from launch_ros.descriptions import ComposableNode

def generate_launch_description():
    # 左前腿容器 - LF (Left Front)
    lf_container = ComposableNodeContainer(
        name='lf_motor_container',
        namespace='',
        package='rclcpp_components',
        executable='component_container',
        composable_node_descriptions=[
            ComposableNode(
                package='unitree_motor_serial_driver',
                plugin='unitree_motor_serial_driver::MotorControlNode',
                name='unitree_motor_serial_driver_lf',
                parameters=[
                    {'serial_port': '/dev/ttyACM0'}
                ],
                remappings=[
                    ('motor_cmds', 'LF/motor_cmds'),
                    ('data_motors', 'LF/data_motors')
                ]
            )
        ],
        output='screen',
    )
    
    # 右前腿容器 - RF (Right Front)
    rf_container = ComposableNodeContainer(
        name='rf_motor_container',
        namespace='',
        package='rclcpp_components',
        executable='component_container',
        composable_node_descriptions=[
            ComposableNode(
                package='unitree_motor_serial_driver',
                plugin='unitree_motor_serial_driver::MotorControlNode',
                name='unitree_motor_serial_driver_rf',
                parameters=[
                    {'serial_port': '/dev/ttyACM1'}
                ],
                remappings=[
                    ('motor_cmds', 'RF/motor_cmds'),
                    ('data_motors', 'RF/data_motors')
                ]
            )
        ],
        output='screen',
    )
    
    # 左后腿容器 - LR (Left Rear)
    lr_container = ComposableNodeContainer(
        name='lr_motor_container',
        namespace='',
        package='rclcpp_components',
        executable='component_container',
        composable_node_descriptions=[
            ComposableNode(
                package='unitree_motor_serial_driver',
                plugin='unitree_motor_serial_driver::MotorControlNode',
                name='unitree_motor_serial_driver_lr',
                parameters=[
                    {'serial_port': '/dev/ttyACM2'}
                ],
                remappings=[
                    ('motor_cmds', 'LR/motor_cmds'),
                    ('data_motors', 'LR/data_motors')
                ]
            )
        ],
        output='screen',
    )
    
    # 右后腿容器 - RR (Right Rear)
    rr_container = ComposableNodeContainer(
        name='rr_motor_container',
        namespace='',
        package='rclcpp_components',
        executable='component_container',
        composable_node_descriptions=[
            ComposableNode(
                package='unitree_motor_serial_driver',
                plugin='unitree_motor_serial_driver::MotorControlNode',
                name='unitree_motor_serial_driver_rr',
                parameters=[
                    {'serial_port': '/dev/ttyACM3'}
                ],
                remappings=[
                    ('motor_cmds', 'RR/motor_cmds'),
                    ('data_motors', 'RR/data_motors')
                ]
            )
        ],
        output='screen',
    )

    return LaunchDescription([
        lf_container,
        rf_container,
        lr_container,
        rr_container
    ])