"""单臂/双臂通用调试入口。 该 launch 文件用于现场调试阶段按需启动左臂、右臂或双臂，并可通过 `use_mock` 在 mock 模式和 RM75 真机模式之间切换。它会固定启动 UDP 手柄接收节点，再根据 `arm:=left|right|both` 选择对应的遥操作节点。 """ from pathlib import Path from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, OpaqueFunction, Shutdown from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare XR_PYTHON = "/home/robot/miniconda3/envs/xr/bin/python" def _as_bool(value: str) -> bool: """把 launch 字符串参数转换成 Python bool，便于在 OpaqueFunction 中分支。""" return value.strip().lower() in ("1", "true", "yes", "on") def _config_file(name: str) -> PathJoinSubstitution: """生成 xr_rm_bringup/config 下配置文件的可安装路径。""" return PathJoinSubstitution([ FindPackageShare("xr_rm_bringup"), "config", name, ]) def _dual_rm75_urdf() -> PathJoinSubstitution: return PathJoinSubstitution([ FindPackageShare("xr_rm_teleop"), "models", "dual_rm75", "Dual_arm.urdf", ]) def _udp_receiver_node() -> Node: """接收 PICO/XR UDP 数据，并发布左右手柄 ROS2 话题。""" return Node( package="xr_rm_input", executable="udp_controller_receiver", name="udp_controller_receiver", output="screen", on_exit=Shutdown( reason="XR UDP receiver exited; stopping arm_debug launch" ), parameters=[{ "udp_host": LaunchConfiguration("udp_host"), "udp_port": LaunchConfiguration("udp_port"), "timer_hz": LaunchConfiguration("udp_timer_hz"), "left_topic": "/xr/left_controller", "right_topic": "/xr/right_controller", }], ) def _mujoco_node() -> Node: """启动只读双臂 MuJoCo 运动学显示节点。""" return Node( package="xr_rm_mujoco", executable="dual_arm_simulator", name="dual_arm_simulator", output="screen", prefix=[XR_PYTHON], parameters=[ _config_file("dual_arm_mujoco.yaml"), {"robot_urdf_path": _dual_rm75_urdf()}, ], ) def _validate_mujoco_mode(arm: str, use_mujoco: bool) -> None: if use_mujoco and arm != "both": raise ValueError("use_mujoco:=true requires arm:=both") def _validate_act_mode( arm: str, use_mock: bool, record_act: bool, ) -> None: if record_act and (arm != "right" or use_mock): raise ValueError( "record_act:=true requires arm:=right use_mock:=false" ) def _act_recorder_node() -> Node: """创建独立 ACT 数据采集节点；退出时不终止遥操作。""" return Node( package="xr_rm_teleop", executable="act_episode_recorder", name="act_episode_recorder", output="screen", prefix=[XR_PYTHON], parameters=[_config_file("act_tomato_pick.yaml")], ) def _single_arm_node( arm: str, use_mock: bool, ) -> Node: """创建单臂调试节点；左/右臂分别使用独立 YAML，节点名保持单臂默认名。""" config_name = "left_arm_rm75.yaml" if arm == "left" else "right_arm_rm75.yaml" arm_name = _arm_name(arm) return Node( package="xr_rm_teleop", executable="single_arm_velocity_teleop", name="single_arm_velocity_teleop", output="screen", prefix=[XR_PYTHON], parameters=[ _config_file(config_name), { "use_mock": use_mock, "robot_urdf_path": _dual_rm75_urdf(), "peripheral_config_file": _config_file("peripherals_rm75.yaml"), "peripheral_arm": arm, "tool_command_topic": f"/xr_rm/{arm_name}/tool_enable", }, ], ) def _arm_name(arm: str) -> str: return "left_rm75" if arm == "left" else "right_rm75" def _dual_arm_nodes(use_mock: bool) -> list[Node]: """创建双臂节点；两个节点共用双臂 YAML，但节点名区分左右臂参数命名空间。""" config_file = _config_file("dual_arm_rm75.yaml") return [ Node( package="xr_rm_teleop", executable="single_arm_velocity_teleop", name="left_arm_teleop", output="screen", prefix=[XR_PYTHON], parameters=[ config_file, { "use_mock": use_mock, "robot_urdf_path": _dual_rm75_urdf(), "peripheral_config_file": _config_file("peripherals_rm75.yaml"), "peripheral_arm": "left", "tool_command_topic": "/xr_rm/left_rm75/tool_enable", }, ], ), Node( package="xr_rm_teleop", executable="single_arm_velocity_teleop", name="right_arm_teleop", output="screen", prefix=[XR_PYTHON], parameters=[ config_file, { "use_mock": use_mock, "robot_urdf_path": _dual_rm75_urdf(), "peripheral_config_file": _config_file("peripherals_rm75.yaml"), "peripheral_arm": "right", "tool_command_topic": "/xr_rm/right_rm75/tool_enable", }, ], ), ] def _launch_setup(context, *args, **kwargs): """运行时读取 launch 参数，决定启动单臂还是双臂。""" del args, kwargs if not Path(XR_PYTHON).is_file(): raise RuntimeError( f"XR Python not found: {XR_PYTHON}; " "Placo 0.9.4 must not be installed globally" ) arm = LaunchConfiguration("arm").perform(context).strip().lower() use_mock = _as_bool(LaunchConfiguration("use_mock").perform(context)) use_mujoco = _as_bool( LaunchConfiguration("use_mujoco").perform(context) ) record_act = _as_bool( LaunchConfiguration("record_act").perform(context) ) if arm not in ("left", "right", "both"): raise ValueError("arm must be one of: left, right, both") _validate_mujoco_mode(arm, use_mujoco) _validate_act_mode(arm, use_mock, record_act) nodes = [_udp_receiver_node()] if arm == "both": nodes.extend(_dual_arm_nodes(use_mock)) else: nodes.append(_single_arm_node(arm, use_mock)) if use_mujoco: nodes.append(_mujoco_node()) if record_act: nodes.append(_act_recorder_node()) return nodes def generate_launch_description() -> LaunchDescription: return LaunchDescription([ # 调试目标：left/right/both；默认右臂方便单臂逐步上真机。 DeclareLaunchArgument("arm", default_value="right"), # true 时只跑 mock，不连接 RM75；false 时通过 RealMan SDK 连接真机。 DeclareLaunchArgument("use_mock", default_value="true"), # true 时额外启动只读 MuJoCo 双臂显示，默认不改变现有启动行为。 DeclareLaunchArgument("use_mujoco", default_value="false"), # true 时只允许右臂真机，并启动独立 ACT 数据采集节点。 DeclareLaunchArgument("record_act", default_value="false"), # UDP 监听参数，需要与 PICO 端或 sample_udp_sender 保持一致。 DeclareLaunchArgument("udp_host", default_value="0.0.0.0"), DeclareLaunchArgument("udp_port", default_value="15000"), # UDP receiver 轮询频率高于 PICO 发送频率，减少 socket 中等待时间。 DeclareLaunchArgument("udp_timer_hz", default_value="200.0"), # OpaqueFunction 允许根据 arm/use_mock 等运行时参数动态生成节点。 OpaqueFunction(function=_launch_setup), ])