"""双臂 mock 遥操作启动入口。

该 launch 文件用于不连接真实 RM75 的离线闭环验证：启动 UDP 手柄接收节点，
并分别启动左、右两个 `single_arm_velocity_teleop` 节点。两个遥操作节点使用
同一份双臂配置文件，但通过节点名读取各自的参数命名空间。
"""

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare


def generate_launch_description() -> LaunchDescription:
    # 通过 robot_config 参数选择 xr_rm_bringup/config 下的双臂配置文件。
    config_file = PathJoinSubstitution([
        FindPackageShare("xr_rm_bringup"),
        "config",
        LaunchConfiguration("robot_config"),
    ])

    return LaunchDescription([
        # UDP 监听参数，需要和 PICO 端或 sample_udp_sender 的目标地址保持一致。
        DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
        DeclareLaunchArgument("udp_port", default_value="15000"),
        DeclareLaunchArgument("robot_config", default_value="dual_arm_rm75.yaml"),
        # 这些开关便于单独验证 UDP、左臂或右臂链路。
        DeclareLaunchArgument("run_udp", default_value="true"),
        DeclareLaunchArgument("run_left_arm", default_value="true"),
        DeclareLaunchArgument("run_right_arm", default_value="true"),
        # 接收 PICO/XR 侧 UDP 数据，并按左右手字段分发到对应手柄话题。
        Node(
            package="xr_rm_input",
            executable="udp_controller_receiver",
            name="udp_controller_receiver",
            output="screen",
            condition=IfCondition(LaunchConfiguration("run_udp")),
            parameters=[{
                "udp_host": LaunchConfiguration("udp_host"),
                "udp_port": LaunchConfiguration("udp_port"),
                "left_topic": "/xr/left_controller",
                "right_topic": "/xr/right_controller",
            }],
        ),
        # 左臂模拟节点：不连接真实 RM75，只积分速度命令用于验证坐标方向。
        Node(
            package="xr_rm_teleop",
            executable="single_arm_velocity_teleop",
            name="left_arm_teleop",
            output="screen",
            condition=IfCondition(LaunchConfiguration("run_left_arm")),
            parameters=[config_file, {"use_mock": True}],
        ),
        # 右臂模拟节点：与左臂共用同一配置文件，但读取右手柄话题。
        Node(
            package="xr_rm_teleop",
            executable="single_arm_velocity_teleop",
            name="right_arm_teleop",
            output="screen",
            condition=IfCondition(LaunchConfiguration("run_right_arm")),
            parameters=[config_file, {"use_mock": True}],
        ),
    ])