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

def generate_launch_description():
    # 定义 leg_ctrl 的四个容器节点
    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_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_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_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',
    )

    # 定义 ahrs_driver 节点
    ahrs_driver = Node(
        package="fdilink_ahrs",
        executable="ahrs_driver_node",
        parameters=[{
            'if_debug_': False,
            'serial_port_': '/dev/ttyUSB0',
            'serial_baud_': 921600,
            'imu_topic': '/imu',
            'imu_frame_id_': 'gyro_link',
            'mag_pose_2d_topic': '/mag_pose_2d',
            'Magnetic_topic': '/magnetic',
            'Euler_angles_topic': '/euler_angles',
            'gps_topic': '/gps/fix',
            'twist_topic': '/system_speed',
            'NED_odom_topic': '/NED_odometry',
            'device_type_': 1
        }],
        output="screen"
    )

    # 返回主启动文件的描述
    return LaunchDescription([
        lf_container,
        rf_container,
        lr_container,
        rr_container,
        ahrs_driver
    ])