PICO/XR 双手柄 UDP JSON
  -> xr_rm_input/udp_controller_receiver
  -> /xr/left_controller 与 /xr/right_controller
  -> xr_rm_teleop/single_arm_velocity_teleop
  -> 左右 RM75 笛卡尔相对位姿透传控制
  -> /xr_rm/<arm_name>/current_pose、raw_target_pose、target_pose、cmd_vel、target_clamped 调试话题

当前控制方式是“手柄相对位姿透传”遥操作：按住 grip 时锁定当前手柄位置和机械臂 TCP 位置，之后根据手柄相对位移生成目标 TCP，经过工作空间限幅、目标低通和单帧步长限制后，通过 rm_movep_canfd 下发目标位姿。松开 grip、UDP 超时或节点退出时会请求机械臂慢停。

当前范围

已完成：

PICO/XR 手柄 UDP 数据接收，并分发到左右手柄 ROS2 话题。
通过统一的 arm_debug.launch.py 支持左臂、右臂、双臂的 mock 调试和真机调试。
RM75 真机连接适配，包含 rm_movep_canfd 位姿透传、安全速度/加速度配置、可选初始化点位移动。
Tkinter 启动面板 launcher_ui.py，用于现场快速启动、监控 topic、检查环境和清理进程。
自定义 PICO 4 Ultra UDP Sender Unity 工程，负责发送左右手柄 pose、grip 和 trigger。

暂未完成：

D405/D435 视频流、数据记录、相机标定和目标检测链路。
双臂碰撞模型、任务级状态机、自动采摘策略。
PICO 端与 ROS 端的完整时间同步和状态回传。

项目结构

src/
├── README.md                       # 项目主文档
├── CODEX.md                        # Codex/Claude Code 项目工作流和安全规则
├── docs/
│   └── pico_udp_sender_ubuntu22_setup.md
├── unity/
│   ├── XR_RM_PICO_UDP_Sender/      # PICO 4 Ultra UDP Sender Unity 工程
│   └── PICO-Unity-Integration-SDK-release_3.4.0/
├── xr_rm_bringup/
│   ├── config/
│   │   ├── dual_arm_rm75.yaml      # 双臂配置：left_arm_teleop 与 right_arm_teleop
│   │   ├── left_arm_rm75.yaml      # 左臂单独调试配置
│   │   ├── right_arm_rm75.yaml     # 右臂单独调试配置
│   │   └── peripherals_rm75.yaml   # 左右臂末端外设配置
│   ├── launch/
│   │   └── arm_debug.launch.py     # 统一入口：arm:=left/right/both, use_mock:=true/false
│   └── tools/
│       ├── launcher_ui.py          # 图形化调试启动面板
│       └── realman_dual_arm_state_monitor.py
├── xr_rm_input/
│   ├── launch/
│   │   └── udp_receiver.launch.py  # 低层 UDP 接收测试入口
│   └── xr_rm_input/
│       ├── udp_controller_receiver.py
│       └── sample_udp_sender.py    # 本机模拟手柄 UDP 数据
├── xr_rm_interfaces/
│   └── msg/
│       └── XrController.msg        # hand/grip/trigger/pose
└── xr_rm_teleop/
    └── xr_rm_teleop/
        ├── single_arm_velocity_teleop.py
        ├── realman_adapter.py
        └── fun_peripheral.py

single_arm_velocity_teleop 这个名字保留是有意的：双臂模式不是一个大节点直接控制两台机械臂，而是启动两个相同的单臂控制节点，分别命名为 left_arm_teleop 和 right_arm_teleop。

环境准备

在工作空间根目录，也就是包含 src/ 的目录执行：

cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
rosdep update
rosdep install --from-paths src -y --ignore-src
colcon build --symlink-install
source install/setup.bash

真机模式还需要安装睿尔曼 Python API2。若未安装，mock 模式仍可正常使用；真机启动时会提示缺少 Robotic_Arm 包。

如果希望 launcher_ui.py 从任意目录找到工作空间，可以设置：

export XR_RM_WS=/home/robot/WS_xr

使用 launcher_ui.py 调试

推荐现场调试优先使用图形化启动面板。它会自动进入工作空间、source ROS2 与 install/setup.bash，并把每个命令放到独立终端中运行。

源码方式启动：

cd /home/robot/WS_xr
python3 src/xr_rm_bringup/tools/launcher_ui.py

构建后也可以通过 ROS2 入口启动：

source /opt/ros/humble/setup.bash
source install/setup.bash
ros2 run xr_rm_bringup launcher_ui

面板顶部的 Mode 分为五类：

Simulation：左臂 mock、右臂 mock、双臂 mock、sample UDP 发送、one-click mock demo。
Left Arm：左臂网络 ping、左臂真机 launch、左手 sample UDP。
Right Arm：右臂网络 ping、右臂真机 launch、右手 sample UDP。
Dual Arm：左右臂 ping、双臂真机 launch、双手 sample UDP。
Diagnostics：ros2 doctor --report 和核心包的 ros2 pkg prefix 检查。

常用按钮：

Run Selected：运行当前选中的命令。双击列表项也可以运行。
Check Env：检查 ROS2 Humble、工作空间 build、终端、核心 ROS 包、睿尔曼 API2。
Stop All：结束由本工作空间启动的 launch、sample sender、topic monitor、相关 ROS 节点和终端窗口。

每个模式都会附带三个监控入口：

Open Controller Topic Monitor：同时查看 /xr/left_controller 和 /xr/right_controller。
Open Target Velocity Monitor：同时查看 /xr_rm/left_rm75/cmd_vel 和 /xr_rm/right_rm75/cmd_vel；该话题表示目标位姿变化率，仅用于调试。
Open ROS Topic/Node List Monitor：每秒刷新 ros2 topic list 和 ros2 node list。

分屏监控依赖 x-terminal-emulator 指向 Terminator。若提示不支持，可安装并切换：

sudo apt install terminator wmctrl xdotool
sudo update-alternatives --config x-terminal-emulator

Launch 入口说明

arm_debug.launch.py 是当前唯一的遥操作 launch 主入口，launcher_ui.py 中的 mock、单臂真机和双臂真机按钮都调用它。

常用参数：

arm：left、right、both，默认 right。
use_mock：true 不连接真机，false 连接 RM75。
udp_host：UDP 监听地址，默认 0.0.0.0。
udp_port：UDP 监听端口，默认 15000。
udp_timer_hz：UDP receiver 轮询频率，默认 200.0。
left_robot_ip：左臂 IP，默认 192.168.192.18。
right_robot_ip：右臂 IP，默认 192.168.192.19。
robot_port：RM75 TCP 端口，默认 8080。
control_rate_hz：rm_movep_canfd 目标位姿发送频率，默认 90.0。
follow：传给 rm_movep_canfd 的跟随标志，默认 false。
enable_tool_control：是否在遥操作节点内启用末端工具控制 topic，默认 true。
configure_peripheral_on_connect：遥操作节点连接真机后是否配置末端外设，默认 true；工具控制会复用同一个 RealMan 连接，避免两个进程同时抢占同一机械臂。
move_to_initial_pose_on_connect：连接后是否执行 movej/movel 初始化，默认 false。

配置文件说明

xr_rm_bringup/config/dual_arm_rm75.yaml 是双臂配置主文件，包含两个 ROS 节点命名空间：

left_arm_teleop
right_arm_teleop

left_arm_rm75.yaml 和 right_arm_rm75.yaml 用于 arm_debug.launch.py arm:=left/right 的单臂调试，因为单臂节点名是 single_arm_velocity_teleop。

xr_rm_bringup/config/peripherals_rm75.yaml 保存末端工具坐标、负载和左右臂外设选择。当前配置为左臂 scissorgripper=1、右臂 scissorgripper=2，并且只在真机连接阶段初始化外设，不在主遥操作循环里控制开合。

重点控制参数：

controller_topic：订阅的手柄话题。
scale：手柄位移到 TCP 位移的比例。
target_filter_alpha / target_filter_alpha_fast：目标 TCP 低通滤波系数，快速移动时自动使用更大的系数。
target_filter_fast_threshold_m：进入快速滤波区间的目标变化阈值。
max_linear_speed：目标位姿单帧步长限制对应的最大线速度。
workspace_min / workspace_max：笛卡尔工作空间边界。
cyl_radius_limit：基座圆柱半径限制。
xr_to_robot_matrix：PICO/OpenXR 位移到 RM75 base 坐标的映射矩阵。
current_pose_poll_hz：低频读取真机当前 TCP 的频率；控制中不再每帧阻塞读取状态。
mock_initial_pose：mock 模式初始 TCP 位姿。
initial_joint_pose / initial_tcp_pose：可选真机初始化点。

当前坐标约定：

PICO/OpenXR：+X 向右，+Y 向上，+Z 向后。
左臂映射：机器人位移增量 = [-手柄y, 手柄z, -手柄x]。
右臂映射：机器人位移增量 = [手柄y, 手柄z, 手柄x]。

如果某个方向相反，只改对应臂的 xr_to_robot_matrix 符号，不要同时改多个控制参数。

末端工具开合

真机 launch 默认会在遥操作节点内启用工具控制。启动后可以用 Bool 话题控制开合，true 表示打开，false 表示闭合：

ros2 topic pub --once /xr_rm/left_rm75/tool_enable std_msgs/msg/Bool "{data: true}"
ros2 topic pub --once /xr_rm/left_rm75/tool_enable std_msgs/msg/Bool "{data: false}"

ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: true}"
ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: false}"

桌面 UI 的 Left Arm、Right Arm 和 Dual Arm 模式里也有对应的 Tool Open/Close 命令项。

UDP 数据格式

单个手柄：

{
  "hand": "right",
  "grip": true,
  "trigger": 0.2,
  "pos": [0.12, 1.05, 0.30],
  "quat": [0.0, 0.0, 0.0, 1.0]
}

左右手柄一起发送：

{
  "controllers": {
    "left": {
      "grip": true,
      "trigger": 0.0,
      "pos": [-0.12, 1.05, 0.30],
      "quat": [0.0, 0.0, 0.0, 1.0]
    },
    "right": {
      "grip": true,
      "trigger": 0.4,
      "pos": [0.12, 1.05, 0.30],
      "quat": [0.0, 0.0, 0.0, 1.0]
    }
  }
}

字段说明：

hand：left 或 right。
grip：运动使能。true 时进入相对位移控制，false 时停止。
trigger：扳机值，范围 0.0-1.0。当前控制主链路不使用它，预留给后续夹爪集成。
pos：手柄位置，长度 3。
quat：手柄姿态四元数，默认按 xyzw 解析。

PICO 4 Ultra 在 Ubuntu 22.04 下配置 Unity、构建 APK、安装到头显并向 ROS2 主机发送 UDP 的详细步骤见 docs/pico_udp_sender_ubuntu22_setup.md。

真机安全验证

第一次接真机时按这个顺序走：

确认急停、网络、机械臂工作区和人员位置。
launcher_ui.py 中先 Ping Left RM75 或 Ping Right RM75。
单臂启动，move_to_initial_pose_on_connect:=false。
手握急停，按住 grip 后只做小幅单轴移动。
逐个确认上/下、前/后、左/右方向。
确认松开 grip 后机械臂慢停，/xr_rm/<arm>/cmd_vel 回到零。
左右臂都确认后，再进入双臂模式。

当前项目没有双臂碰撞检测。双臂首次联调时，请让两个工作区在物理上分开，低速验证，不要让两臂末端互相靠近。

后续优化路线

为了达到“稳定可用的双臂 XR 遥操作/采摘平台”，建议按下面顺序推进：

稳定 PICO 数据链路：固定 UDP JSON 协议和坐标系，增加频率、延迟、丢包统计，记录 /xr/*_controller、/xr_rm/*/raw_target_pose、/xr_rm/*/target_pose、/xr_rm/*/target_clamped、/xr_rm/*/current_pose。
提升真机安全性：增加启动前安全检查、软件急停 topic、UI Stop 状态提示、双臂中间区域互斥边界和速度/加速度限幅。
集成末端执行器：明确夹爪 topic、力控比例、开合方向和安全上限，把 trigger 从预留字段变成稳定夹爪输入。
接入视觉和数据记录：加入 D405/D435 相机 launch、TF、内外参和 rosbag2 实验记录。
从遥操作走向半自动：先做目标检测和 3D 定位提示，再做单臂辅助，最后做双臂任务分配和任务级状态机。

常见问题

launcher_ui.py 提示找不到 install/setup.bash：

cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
colcon build --symlink-install
source install/setup.bash

真机模式提示缺少 Robotic_Arm：

未安装睿尔曼 Python API2。请安装厂商 SDK，或用 use_mock:=true 先跑模拟模式。

Controller topic 没有数据：

确认 UDP 发送端目标 IP 是运行 ROS2 的主机 IP。
确认端口是 15000，或 launch 与发送端端口一致。
用 sample_udp_sender 在本机验证接收链路。

机械臂不动：

确认 grip=true。
确认 /xr_rm/<arm>/raw_target_pose 与 /xr_rm/<arm>/target_pose 是否在变化。
确认 /xr_rm/<arm>/target_clamped 是否持续为 true，如果是，目标 TCP 可能被工作空间、圆柱半径或单帧步长限制夹住。
确认真机 SDK 连接成功，且 RM75 没有报警或急停。

Languages

C# 91.9%

Python 4.1%

ShaderLab 2.8%

GLSL 0.8%

HLSL 0.4%

README.md Unescape Escape

XR-RM75 双臂遥操作工作空间