# PICO 4 Ultra UDP 手柄接入与调试教程

本文档用于把 PICO 4 Ultra 左右手柄接入当前 XR-RM75 ROS2 工作空间。目标是让 PICO 端发送符合本项目约定的 UDP JSON，ROS 端发布：

```text
/xr/left_controller
/xr/right_controller
```

然后由 `single_arm_velocity_teleop` 把手柄相对位移转换成 RM75 TCP 运动。

## 1. 当前项目约定

当前链路如下：

```text
PICO 4 Ultra Unity 应用
  -> UDP JSON, 默认端口 15000
  -> xr_rm_input/udp_controller_receiver
  -> /xr/left_controller 与 /xr/right_controller
  -> xr_rm_teleop/single_arm_velocity_teleop
  -> RM75 笛卡尔相对位移控制
```

ROS 端默认参数：

| 项目 | 默认值 |
| --- | --- |
| UDP 监听地址 | `0.0.0.0` |
| UDP 端口 | `15000` |
| 左手柄话题 | `/xr/left_controller` |
| 右手柄话题 | `/xr/right_controller` |
| 四元数顺序 | `xyzw` |
| 默认坐标 | PICO/OpenXR: `+X` 向右, `+Y` 向上, `+Z` 向后 |

控制语义：

- `grip=false`: 机械臂停止，退出相对位移遥操作。
- `grip=true`: 第一帧锁定手柄起点和当前 TCP 起点，之后跟随手柄相对位移。
- `trigger`: 当前主运动链路不使用，范围 `0.0-1.0`，预留给夹爪。
- UDP 超过 `command_timeout_sec=0.12` 秒未更新时，机械臂停止。

因此 PICO 端建议稳定发送 `60 Hz` 或 `90 Hz`。不要低于 `20 Hz`。

## 2. UDP JSON 协议

推荐 PICO 端每包同时发送左右手柄：

```json
{
  "t": 12.345,
  "frame_id": "xr_world",
  "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]
    }
  }
}
```

也兼容单手柄包：

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

字段要求：

| 字段 | 类型 | 说明 |
| --- | --- | --- |
| `hand` | string | 单手柄包使用，`left` 或 `right` |
| `controllers.left` | object | 左手柄数据 |
| `controllers.right` | object | 右手柄数据 |
| `grip` | bool | 运动使能 |
| `trigger` | float | `0.0-1.0` |
| `pos` | float[3] | 手柄位置 `[x, y, z]` |
| `quat` | float[4] | 手柄姿态 `[qx, qy, qz, qw]` |
| `frame_id` | string | 可选，默认 `xr_world` |

## 3. PICO 端到底安装什么

根据官方视频 `XRoboToolkit Unity Client Setup Tutorial` 和当前项目现状，PICO 端有两条路线。先把选择说清楚，这一步很重要。

| 路线 | PICO 端安装 | PC 端安装 | 是否直接适配当前 ROS 项目 | 适合用途 |
| --- | --- | --- | --- | --- |
| 当前项目直连路线 | 自定义 `XR-RM UDP Sender` APK | 不需要 XRoboToolkit PC-Service | 是 | 直接给 `/xr/left_controller` 和 `/xr/right_controller` 发当前项目 JSON |
| 官方 XRoboToolkit 路线 | 官方 `XRoboToolkit-PICO-1.1.1.apk` | 官方 `XRoboToolkit-PC-Service` | 否，需要额外 bridge | 复现论文官方 App、验证 PICO 手柄/手势/数据采集 UI |

如果你的目标是现在让 RM75 跟随 PICO 手柄，优先走“当前项目直连路线”。官方 APK 的 `Send` 会把数据发给 XRoboToolkit PC-Service，不会直接发送本项目的 UDP JSON。

## 4. 官方视频路线：安装 XRoboToolkit App

这一节对应官方视频里的操作。它可以帮你确认 PICO 4 Ultra、手柄追踪、官方 App 和 PC-Service 都正常，但它不是当前 ROS 项目的直接输入源。

### 4.1 PC 端安装 PC-Service

官方视频使用 Windows 演示：

1. 打开 XRoboToolkit 主页：`https://xr-robotics.github.io/`。
2. 点击 `Github`，进入 `XR-Robotics` 组织。
3. 打开 `XRoboToolkit-PC-Service` 仓库的 `Releases`。
4. Windows 下载 `XRoboToolkit-PC-Service.win.zip`。
5. 解压后进入 `bin` 目录。
6. 视频里运行的是 `run3D.bat`。如果只需要后台服务，也可以按官方 README 使用 `runService.bat`。

如果 PC 是 Ubuntu 22.04，可下载 release 里的：

```text
XRoboToolkit_PC_Service_1.0.0_ubuntu_22.04_amd64.deb
```

安装并启动：

```bash
sudo apt install ./XRoboToolkit_PC_Service_1.0.0_ubuntu_22.04_amd64.deb
/opt/apps/roboticsservice/runService.sh
```

服务启动后，确保 PICO 和这台 PC 在同一个局域网。

### 4.2 PICO 端安装官方 APK

官方视频下载的是 Unity Client release 里的 APK。当前可用 release 名称为：

```text
XRoboToolkit-PICO-1.1.1.apk
```

安装步骤：

1. 在 PICO 上开启开发者模式和 USB 调试。
2. 用 USB-C 线连接 PICO 和 PC。
3. PC 上安装 ADB：

```bash
sudo apt update
sudo apt install android-tools-adb
```

4. 查看设备：

```bash
adb devices
```

PICO 内会弹出 USB 调试授权，选择允许。再次执行 `adb devices`，应看到 `device` 状态。

5. 安装 APK：

```bash
adb install -r XRoboToolkit-PICO-1.1.1.apk
```

看到 `Success` 表示安装完成。

### 4.3 PICO 里如何配置官方 App

戴上 PICO：

1. 打开 `Library`。
2. 找到 `XRoboToolkit` 应用。如果它不在普通应用列表里，到未知来源/开发者应用区域找。
3. 打开应用。
4. 如果 PC-Service 和 PICO 在同一 Wi-Fi，应用会自动弹出 PC-Service 主机列表。
5. 用手柄扳机点击运行 PC-Service 的那台 PC。
6. 如果没有自动弹窗，在主面板 `Network -> PC Service` 或 `Enter` 里手动输入 PC-Service 的 IP。
7. 连接成功后，主面板状态应显示类似 `WORKING`。

官方 App 主面板建议这样设置：

| 面板 | 选项 | 推荐设置 |
| --- | --- | --- |
| `Tracking` | `Head` | 可关，当前 RM75 项目不用头显位姿 |
| `Tracking` | `Controller` | 必须开，用于左右手柄 6DoF pose 和按键 |
| `Tracking` | `Hand` | 可关，除非你要验证裸手追踪 |
| `PICO Motion Tracker` | `Mode` | `None`，当前项目不用全身/物体 tracker |
| `Data & Control` | `Send` | 开，开始向 PC-Service 同步追踪数据 |
| `Data & Control` | `Switch w/ A Button` | 可选，开后可用右手 A 键暂停/恢复发送 |
| `Remote Vision` | video source | 当前项目先不用，保持关闭 |
| `Data Collection` | `Record` | 当前项目先不用，保持关闭 |

官方视频里还演示了：

- 勾选 `Controller` 后点击 `Send`，PC-Service 窗口能看到 controller tracking 状态。
- 放下手柄、伸出手，可验证 `Hand` tracking。
- `Remote Vision` 可选择 `ZEDMINI` 并输入相机端 IP，但这属于视频回传链路，当前 RM75 手柄遥操作不需要。
- `Data Collection -> Tracking -> Record` 会把记录文件保存在 PICO 的 `/sdcard/Download`，可用 `adb pull` 取回。

取回官方 App 录制文件示例：

```bash
adb shell ls /sdcard/Download
adb pull /sdcard/Download/<record_file_name> .
```

### 4.4 官方 App 与当前项目的关系

官方 `XRoboToolkit-PICO` App 发给的是 `XRoboToolkit-PC-Service`，而当前项目的 `udp_controller_receiver` 监听的是轻量 UDP JSON：

```text
PICO UDP JSON -> udp_controller_receiver -> /xr/left_controller, /xr/right_controller
```

所以：

- 只安装官方 APK，当前项目不会收到 `/xr/*_controller` 数据。
- 如果要使用官方 APK 控制当前项目，需要额外写一个 bridge，从 PC-Service/PC-Service-Pybind 读取 controller tracking，再转成本项目 UDP JSON 或直接发布 `xr_rm_interfaces/XrController`。
- 当前仓库现在没有这个 bridge，因此现场调试建议先用下面的自定义 UDP Sender APK。

## 5. 当前项目直连路线：安装自定义 UDP Sender APK

这条路线最贴合当前代码。PICO 端 App 的职责很简单：

```text
读取左右手柄 pose / grip / trigger
  -> 组成本项目 JSON
  -> UDP 发到 Ubuntu ROS 主机:15000
```

当前仓库进度，2026-05-28 检查：

- Unity 工程已存在：`unity/XR_RM_PICO_UDP_Sender`。
- PICO SDK 已放在：`unity/PICO-Unity-Integration-SDK-release_3.4.0`。
- 5.2 的本地 Unity 工程配置基本完成：项目版本是 Unity `2022.3.16f1c1`，Android 包名是 `com.local.xr_rm_udp_sender`，最小 API 是 29，网络权限 `ForceInternetPermission` 已开启，PICO XR Loader 已配置，`Assets/Scenes/Main.unity` 已加入 build scenes。
- 5.5 的最小 UDP 发送脚本已经完成：`Assets/Scripts/PicoControllerUdpSender.cs` 会读取 `XRNode.LeftHand/RightHand`，发送 `controllers.left/right` JSON，默认 `host=192.168.9.89`、`port=15000`、`sendHz=60`、`convertUnityToProjectCoordinates=true`。
- 场景中已经挂载 `PicoControllerUdpSender` 对象，序列化参数与脚本默认值一致。
- 本机已经生成过 APK：`unity/XR_RM_PICO_UDP_Sender/Builds/XR-RM-PICO-UDP-Sender.apk`。这是 Unity 生成产物，通常不提交到 git。

仍需现场确认：

- `192.168.9.89` 是否仍然是 Ubuntu ROS 主机当前局域网 IP。如果不是，先在 Unity Inspector 或 `Assets/Editor/XrRmUdpSenderProjectSetup.cs` 中改成新的 IP，再重新 build APK。
- 还没有证据表明 APK 已安装到 PICO。
- 还没有证据表明 PICO 真机运行时已经向 Ubuntu 发 UDP 包。
- 还没有证据表明 ROS 端 `/xr/left_controller` 和 `/xr/right_controller` 已收到 PICO 真机数据。

因此当前应从 5.4 继续：先把已经生成的 APK 安装到 PICO。如果你已经用 Unity `Build And Run` 安装成功，则跳过 5.4，从 5.3 和第 6 节的 ROS 端验证继续。

### 5.1 PICO 和 Ubuntu 网络配置

1. PICO 4 Ultra 和 Ubuntu ROS 主机连接到同一个局域网。
2. 在 Ubuntu 上查看主机 IP：


```bash
hostname -I
```

假设输出里有 `192.168.1.42`，Unity 脚本里的 `host` 就填 `192.168.1.42`。

3. 如果 Ubuntu 开了防火墙，放行 UDP 端口：

```bash
sudo ufw allow 15000/udp
```

4. PICO 上打开开发者模式和 USB 调试，方便用 `adb install` 安装 APK。

### 5.2 在开发电脑上配置 Unity 工程

操作平台：开发电脑，不是在 PICO 头显里操作。可以是 Windows，也可以是 Ubuntu/macOS。建议优先用 Windows，因为 PICO/Unity/ADB 调试资料最多。

本节目标：做出一个能在 PICO 4 Ultra 上运行的 Android APK。这个 APK 读取左右手柄，并把数据 UDP 发给 Ubuntu ROS 主机。

前置条件：

- 已经完成 5.1，知道 Ubuntu ROS 主机 IP，例如 `192.168.1.42`。
- 开发电脑已安装 Unity Hub。
- Unity 已安装 Android Build Support、Android SDK/NDK、OpenJDK。
- 已下载 PICO Unity Integration SDK，或能从 PICO 官方资源页导入。
- PICO 4 Ultra 已开启开发者模式，后面可以用 ADB 安装 APK。

推荐环境参考官方 Unity Client：

| 项目 | 推荐 |
| --- | --- |
| Unity | `2022.3.16f1` 或同系列 LTS |
| Build Target | Android |
| Android SDK | 29 或 Unity LTS 自带 SDK |
| Android NDK | 21.4.7075529 或 Unity LTS 自带 NDK |
| PICO SDK | PICO Unity Integration SDK |

详细步骤：

1. 打开 Unity Hub，新建一个 `3D Core` 或空白 3D 项目，例如 `XR_RM_PICO_UDP_Sender`。
2. 进入 `File -> Build Settings`。
3. 选择 `Android`。
4. 点击 `Switch Platform`。如果按钮灰色，说明已经是 Android 平台。
5. 打开 `Edit -> Project Settings -> Player -> Android -> Other Settings`。
6. 设置 `Package Name`，例如：

```text
com.local.xr_rm_udp_sender
```

7. 设置 `Minimum API Level`。一般使用 Unity/PICO SDK 推荐值即可；如果不确定，用 Android 10/API 29。
8. 设置 `Target API Level`。一般选择 `Automatic` 或 Unity 当前安装的最高 Android API。
9. 找到 `Internet Access`，设为 `Require`。这是必须项，否则 Android 可能不给 UDP 网络权限。
10. 导入 PICO Unity Integration SDK。
11. 打开 `Edit -> Project Settings -> XR Plug-in Management`。
12. 在 Android 选项卡中启用 PICO XR Loader，或按你当前 PICO SDK 文档启用对应 XR 插件。
13. 确认项目能读取 Unity XR 输入。后面的脚本会使用 `UnityEngine.XR.InputDevices` 和 `CommonUsages`。
14. 在场景中新建空物体：`GameObject -> Create Empty`。
15. 命名为 `PicoControllerUdpSender`。
16. 按 5.5 创建并挂载 `PicoControllerUdpSender.cs` 脚本。
17. 在 Inspector 中把脚本参数设置为：

```text
Host = Ubuntu ROS 主机 IP，例如 192.168.1.42
Port = 15000
Send Hz = 60
Convert Unity To Project Coordinates = true
```

完成本节后，Unity 工程应该具备：

- Android 构建目标。
- Android 网络权限。
- PICO/XR 插件。
- 一个会发送 UDP 的场景对象。
- 目标 IP/端口已经指向 Ubuntu ROS 主机。

如果使用 Unity OpenXR 输入路径，常用映射是：

| 功能 | Unity XR `CommonUsages` | OpenXR 语义 |
| --- | --- | --- |
| 手柄位置 | `devicePosition` | `/input/grip/pose` position |
| 手柄姿态 | `deviceRotation` | `/input/grip/pose` rotation |
| 扳机按钮 | `triggerButton` | `/input/trigger/click` |
| 扳机模拟量 | `trigger` | `/input/trigger/value` |
| 抓握按钮 | `gripButton` | squeeze click/button |
| 抓握模拟量 | `grip` | `/input/squeeze/value` |

### 5.3 在 PICO 头显里配置 UDP Sender App

操作平台：PICO 4 Ultra 头显内。不是 Ubuntu 终端，也不是 Unity 编辑器。

本节目标：打开你安装好的 UDP Sender App，确认它发送到正确的 Ubuntu ROS 主机。

前置条件：

- 已经按 5.2 做好 Unity 工程。
- 已经按 5.4 把 APK 安装到 PICO，或已经通过 Unity `Build And Run` 安装到 PICO。
- PICO 和 Ubuntu ROS 主机在同一个局域网。
- Ubuntu ROS 主机已启动 UDP 接收器，或准备按第 6 节启动。

打开 App：

1. 戴上 PICO 4 Ultra。
2. 打开 `Library`。
3. 如果普通应用列表里没有你的 App，进入未知来源/开发者应用区域。
4. 打开 `XR-RM-PICO-UDP-Sender`，或你自己设置的应用名称。

如果你的 UDP Sender App 有 UI，PICO 端推荐配置为：

| 配置项 | 值 |
| --- | --- |
| `Target IP` / `Host` | Ubuntu ROS 主机 IP，例如 `192.168.1.42` |
| `Target Port` | `15000` |
| `Send Rate` | `60 Hz`，稳定后可用 `90 Hz` |
| `Controller Tracking` | `On` |
| `Head Tracking` | `Off`，当前项目不用 |
| `Hand Tracking` | `Off`，当前项目不用 |
| `Motion Tracker` | `None` |
| `Coordinate Mode` | 发送 PICO/OpenXR 风格：`+X` 右，`+Y` 上，`+Z` 后 |
| `Send` | ROS 端接收器启动后再打开 |

推荐操作顺序：

1. 先在 Ubuntu 端启动第 6 节的 UDP 接收器。
2. 再在 PICO App 里打开 `Send`。
3. 先不要按左右手 `grip`。
4. 在 Ubuntu 上观察 `/xr/left_controller` 和 `/xr/right_controller` 是否有持续数据。
5. 确认两个 topic 都有 `grip: false` 的数据后，再分别按左右手抓握键。

如果 App 没有 UI，就不需要在 PICO 里填配置。你需要在 Unity Inspector 里预先写死：

```text
host = Ubuntu ROS 主机 IP
port = 15000
sendHz = 60
convertUnityToProjectCoordinates = true
```

无 UI 版本启动后通常会立刻发送 UDP。此时要特别确认 Ubuntu 端已经启动接收器，否则你会以为 App 没工作，其实只是没人接包。

### 5.4 在开发电脑上用 ADB 安装 APK 到 PICO

操作平台：开发电脑终端 + PICO 头显。终端命令在开发电脑上执行，授权弹窗在 PICO 头显里确认。

本节目标：把 Unity 导出的 APK 安装到 PICO 4 Ultra。

前置条件：

- 已经从 Unity 导出了 Android APK。
- PICO 已开启开发者模式和 USB 调试。
- 开发电脑已安装 ADB。
- PICO 用 USB-C 线连接到开发电脑。

假设 Unity 导出的 APK 名为：

```text
XR-RM-PICO-UDP-Sender.apk
```

安装 ADB：

Ubuntu 开发电脑：

```bash
sudo apt update
sudo apt install android-tools-adb
```

Windows 开发电脑：

```text
安装 Android Studio，或单独安装 Android SDK Platform-Tools。
然后在 PowerShell/CMD 中进入 platform-tools 目录，或把该目录加入 PATH。
```

检查 PICO 是否连接：

```bash
adb devices
```

第一次连接时，PICO 头显里会出现 USB 调试授权弹窗。戴上头显，选择允许。然后再次执行：

```bash
adb devices
```

正确结果应类似：

```text
List of devices attached
XXXXXXXXXXXX    device
```

如果显示 `unauthorized`，说明还没有在 PICO 里允许 USB 调试。重新插拔 USB 线，或执行：

```bash
adb kill-server
adb start-server
adb devices
```

安装 APK：

```bash
adb install -r XR-RM-PICO-UDP-Sender.apk
```

看到下面输出表示安装成功：

```text
Success
```

安装完成后，可以查看应用是否在设备上：

```bash
adb shell pm list packages | grep xr_rm
```

Windows PowerShell/CMD 可以用：

```bash
adb shell pm list packages | findstr xr_rm
```

戴上 PICO：

1. 打开 `Library`。
2. 进入未知来源/开发者应用区域。
3. 打开 `XR-RM-PICO-UDP-Sender`。
4. 确认 App 里显示的目标 IP 和端口正确。
5. 先不要按 `grip`，观察 ROS topic 是否有 `grip=false` 的稳定数据。
6. 再按左右手 `grip` 分别验证。

### 5.5 在 Unity 工程中添加最小 UDP 发送脚本

操作平台：开发电脑上的 Unity 编辑器。

本节目标：给 5.2 的 Unity 工程添加一个最小脚本，使 APK 能发送当前 ROS 项目需要的 UDP JSON。

前置条件：

- 已经完成 5.2 的 Unity 工程配置。
- 场景里已经有一个空物体 `PicoControllerUdpSender`，或准备新建。
- 已经知道 Ubuntu ROS 主机 IP。
- Unity 工程可以正常切换到 Android 平台。

创建脚本：

1. 在 Unity `Project` 面板中进入 `Assets`。
2. 新建目录 `Scripts`，如果已有可跳过。
3. 右键 `Scripts -> Create -> C# Script`。
4. 文件名必须写成：

```text
PicoControllerUdpSender.cs
```

5. 双击打开脚本。
6. 删除 Unity 自动生成的内容。
7. 粘贴下面完整代码。
8. 保存脚本，回到 Unity，等待编译完成。

挂载脚本：

1. 在 `Hierarchy` 面板中选择 5.2 创建的空物体 `PicoControllerUdpSender`。
2. 如果还没有空物体，执行 `GameObject -> Create Empty`，命名为 `PicoControllerUdpSender`。
3. 把 `Assets/Scripts/PicoControllerUdpSender.cs` 拖到这个空物体的 Inspector 上。
4. 在 Inspector 中设置：

```text
Host = Ubuntu ROS 主机 IP，例如 192.168.1.42
Port = 15000
Send Hz = 60
Convert Unity To Project Coordinates = true
```

5. 保存场景：`File -> Save As`，例如保存为 `Assets/Scenes/Main.unity`。
6. 打开 `File -> Build Settings`。
7. 确认 `Scenes In Build` 里包含当前场景。如果没有，点击 `Add Open Scenes`。
8. 点击 `Build` 导出 APK，或在 PICO 已经 USB 连接且 ADB 授权后点击 `Build And Run`。

导出的 APK 可命名为：

```text
XR-RM-PICO-UDP-Sender.apk
```

```csharp
using System;
using System.Net;
using System.Net.Sockets;
using System.Text;
using UnityEngine;
using UnityEngine.XR;

public class PicoControllerUdpSender : MonoBehaviour
{
    [Header("ROS UDP Target")]
    public string host = "192.168.1.42";
    public int port = 15000;

    [Header("Send")]
    public float sendHz = 60.0f;
    public bool convertUnityToProjectCoordinates = true;

    private UdpClient client;
    private IPEndPoint endPoint;
    private float nextSendTime;
    private readonly Packet packet = new Packet();

    [Serializable]
    private class Packet
    {
        public double t;
        public string frame_id = "xr_world";
        public Controllers controllers = new Controllers();
    }

    [Serializable]
    private class Controllers
    {
        public ControllerPayload left = new ControllerPayload();
        public ControllerPayload right = new ControllerPayload();
    }

    [Serializable]
    private class ControllerPayload
    {
        public bool grip;
        public float trigger;
        public float[] pos = new float[] { 0.0f, 1.0f, 0.0f };
        public float[] quat = new float[] { 0.0f, 0.0f, 0.0f, 1.0f };
    }

    private void OnEnable()
    {
        client = new UdpClient();
        endPoint = new IPEndPoint(IPAddress.Parse(host), port);
        nextSendTime = 0.0f;
    }

    private void OnDisable()
    {
        packet.controllers.left.grip = false;
        packet.controllers.right.grip = false;
        SendPacket();
        client?.Close();
        client = null;
    }

    private void Update()
    {
        if (Time.unscaledTime < nextSendTime)
        {
            return;
        }

        nextSendTime = Time.unscaledTime + 1.0f / Mathf.Max(sendHz, 1.0f);
        packet.t = Time.realtimeSinceStartupAsDouble;

        FillController(XRNode.LeftHand, packet.controllers.left);
        FillController(XRNode.RightHand, packet.controllers.right);
        SendPacket();
    }

    private void FillController(XRNode node, ControllerPayload payload)
    {
        InputDevice device = InputDevices.GetDeviceAtXRNode(node);
        if (!device.isValid)
        {
            payload.grip = false;
            payload.trigger = 0.0f;
            return;
        }

        Vector3 position = Vector3.zero;
        Quaternion rotation = Quaternion.identity;
        float trigger = 0.0f;
        float gripValue = 0.0f;
        bool gripButton = false;

        device.TryGetFeatureValue(CommonUsages.devicePosition, out position);
        device.TryGetFeatureValue(CommonUsages.deviceRotation, out rotation);
        device.TryGetFeatureValue(CommonUsages.trigger, out trigger);
        device.TryGetFeatureValue(CommonUsages.grip, out gripValue);
        device.TryGetFeatureValue(CommonUsages.gripButton, out gripButton);

        payload.grip = gripButton || gripValue > 0.5f;
        payload.trigger = Mathf.Clamp01(trigger);

        if (convertUnityToProjectCoordinates)
        {
            // Unity scene coordinates are commonly +Z forward. This project expects
            // OpenXR-style controller positions with +Z backward.
            payload.pos[0] = position.x;
            payload.pos[1] = position.y;
            payload.pos[2] = -position.z;

            payload.quat[0] = -rotation.x;
            payload.quat[1] = -rotation.y;
            payload.quat[2] = rotation.z;
            payload.quat[3] = rotation.w;
        }
        else
        {
            payload.pos[0] = position.x;
            payload.pos[1] = position.y;
            payload.pos[2] = position.z;

            payload.quat[0] = rotation.x;
            payload.quat[1] = rotation.y;
            payload.quat[2] = rotation.z;
            payload.quat[3] = rotation.w;
        }
    }

    private void SendPacket()
    {
        if (client == null || endPoint == null)
        {
            return;
        }

        string json = JsonUtility.ToJson(packet);
        byte[] bytes = Encoding.UTF8.GetBytes(json);
        client.Send(bytes, bytes.Length, endPoint);
    }
}
```

说明：

- 如果 `/xr/left_controller` 和 `/xr/right_controller` 里 `pos.z` 与实际前后方向相反，优先切换 `convertUnityToProjectCoordinates` 后再验证。
- 当前项目的主控制只用位置相对位移，姿态 `quat` 会发布出来，但暂不参与机械臂姿态控制。
- `OnDisable()` 会补发一次 `grip=false`，避免退出 PICO 应用时机械臂保持 active 状态。

## 6. ROS 端先做低层 UDP 验证

在工作空间根目录 `/home/robot/WS_xr` 执行：

```bash
source /opt/ros/humble/setup.bash
source install/setup.bash
ros2 launch xr_rm_input udp_receiver.launch.py udp_port:=15000
```

另开终端查看左右手柄：

```bash
source /opt/ros/humble/setup.bash
source install/setup.bash
ros2 topic echo /xr/left_controller
```

```bash
source /opt/ros/humble/setup.bash
source install/setup.bash
ros2 topic echo /xr/right_controller
```

再查看频率：

```bash
ros2 topic hz /xr/left_controller
ros2 topic hz /xr/right_controller
```

期望现象：

- PICO 应用启动后，两个 topic 都持续刷新。
- 按下左手抓握键时，`/xr/left_controller` 的 `grip` 变成 `true`。
- 按下右手抓握键时，`/xr/right_controller` 的 `grip` 变成 `true`。
- 扳机从松开到按下时，`trigger` 从 `0.0` 附近变到 `1.0` 附近。
- 平移手柄时，`pose.position` 连续变化。

如果收不到包，先在 Ubuntu 上抓 UDP：

```bash
sudo tcpdump -ni any udp port 15000
```

能看到 UDP 但 ROS topic 没数据，说明 JSON 字段不符合协议。看 `udp_controller_receiver` 终端里的 `XR 数据包格式错误` 或 `XR 手柄字段错误`。

## 7. 用 sample_udp_sender 排除 ROS 端问题

在接 PICO 前，先确认 ROS 端链路是通的。

终端 1：

```bash
source /opt/ros/humble/setup.bash
source install/setup.bash
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true
```

终端 2：

```bash
source /opt/ros/humble/setup.bash
source install/setup.bash
ros2 run xr_rm_input sample_udp_sender --hand both --host 127.0.0.1 --port 15000 --seconds 20
```

终端 3：

```bash
ros2 topic echo /xr/left_controller
ros2 topic echo /xr/right_controller
ros2 topic echo /xr_rm/left_rm75/cmd_vel
ros2 topic echo /xr_rm/right_rm75/cmd_vel
```

如果 sample sender 正常，而 PICO 不正常，问题在 PICO 端 IP、端口、权限、JSON 或坐标转换。

## 8. PICO 端 mock 闭环调试流程

先不要连接真机，使用 mock 模式验证完整控制链。

1. 启动双臂 mock：

```bash
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true udp_port:=15000
```

2. 启动 PICO Unity 应用。
3. 确认 `/xr/left_controller` 和 `/xr/right_controller` 正常刷新。
4. 左手按住 `grip`，只移动左手一小段，观察：

```bash
ros2 topic echo /xr_rm/left_rm75/cmd_vel
```

5. 松开左手 `grip`，确认 `cmd_vel` 回到 0。
6. 右手重复同样流程。
7. 用 `ros2 topic hz` 确认频率稳定，建议 `50 Hz` 以上。

## 9. 坐标方向检查

当前配置使用的 PICO/OpenXR 位置坐标：

```text
+X: 向右
+Y: 向上
+Z: 向后
```

双臂配置中的映射关系：

```text
左臂机器人位移增量 = [-手柄y, -手柄z,  手柄x]
右臂机器人位移增量 = [ 手柄y, -手柄z, -手柄x]
```

建议现场按下面顺序验证：

1. 只启动 `use_mock:=true`。
2. 按住左手 `grip`，沿 PICO 的 `+X/-X`、`+Y/-Y`、`+Z/-Z` 每次只动一个轴。
3. 记录 `/xr/left_controller.pose.position` 的变化方向。
4. 记录 `/xr_rm/left_rm75/cmd_vel` 的方向。
5. 右手重复。

如果两个手柄在 ROS topic 里的某个轴都反了，优先检查 Unity 的坐标转换。

如果 ROS topic 正确，但某一只机械臂运动方向不符合现场坐标，优先只改对应 YAML 的 `xr_to_robot_matrix`，不要同时改 Unity 坐标和机器人映射。

## 10. 单臂真机小幅调试

真机前检查：

- 急停可用。
- 机械臂工作区清空。
- PICO topic 在 mock 下已经稳定。
- `grip=false` 时 `/xr_rm/<arm>/cmd_vel` 为 0。
- `move_to_initial_pose_on_connect` 保持 `false`。

左臂：

```bash
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=false udp_port:=15000
```

右臂：

```bash
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=false udp_port:=15000
```

调试动作：

1. 手柄保持静止。
2. 按住 `grip`，第一帧只锁定起点，机械臂不应突然运动。
3. 单轴移动手柄 `2-3 cm`。
4. 松开 `grip`，确认机械臂停止。
5. 每次只验证一个方向。

## 11. 双臂真机调试

只有在左右单臂都通过后，再启动双臂：

```bash
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
  left_robot_ip:=192.168.192.18 \
  right_robot_ip:=192.168.192.19 \
  udp_port:=15000
```

双臂第一轮建议：

1. 左右手 `grip` 都不按，确认双臂静止。
2. 只按左手 `grip`，确认只有左臂响应。
3. 只按右手 `grip`，确认只有右臂响应。
4. 左右同时按住 `grip`，做小幅、慢速、单轴运动。

## 12. 常见问题

| 现象 | 排查 |
| --- | --- |
| PICO 启动后 ROS topic 没数据 | 检查 Unity `host` 是否是 Ubuntu IP；检查 PICO 和 Ubuntu 是否同网段；检查 `sudo tcpdump -ni any udp port 15000` |
| tcpdump 有包但 topic 没数据 | JSON 字段不对；确认包含 `controllers.left/right` 或单包 `hand`；确认 `pos` 长度 3、`quat` 长度 4 |
| topic 有数据但机械臂不动 | 检查 `grip` 是否为 `true`；检查 teleop 节点是否订阅对应话题；检查是否超过工作空间限幅 |
| 按左手右臂动 | PICO 端 left/right 填反，或 Unity `XRNode.LeftHand/RightHand` 获取错误 |
| 松开 grip 后仍有速度 | 确认 PICO 持续发送 `grip=false`，并检查 teleop 终端是否收到超时停止 |
| 经常提示手柄数据超时 | 发送频率太低、网络丢包、PICO 应用后台暂停；提高 `sendHz`，保持应用前台运行 |
| 前后方向反了 | 先切换 Unity 脚本里的 `convertUnityToProjectCoordinates`，再验证 topic 方向 |
| 某一只臂方向反了 | 修改对应 YAML 的 `xr_to_robot_matrix` 符号 |
| 抖动明显 | 降低 `scale` 或 `kp_linear`，提高 `deadband_m`，保持 `low_pass_alpha` 不要过大 |

## 13. 参考入口

- XRoboToolkit Unity Client Setup Tutorial: https://www.youtube.com/watch?v=Yhcm72h3ir4
- XRoboToolkit homepage: https://xr-robotics.github.io/
- XRoboToolkit Unity Client release: https://github.com/XR-Robotics/XRoboToolkit-Unity-Client/releases
- XRoboToolkit PC-Service release: https://github.com/XR-Robotics/XRoboToolkit-PC-Service/releases
- PICO Controller & HMD input mapping: https://developer-cn.picoxr.com/en/document/unity/input-mapping/
- PICO Unity Integration SDK: https://github.com/Pico-Developer/PICO-Unity-Integration-SDK
- PICO `PXR_Input` API: https://developer-cn.picoxr.com/en/reference/unity/client-api/PXR_Input/
- Unity XR `InputDevice.TryGetFeatureValue`: https://docs.unity3d.com/ScriptReference/XR.InputDevice.TryGetFeatureValue.html
- Unity XR `CommonUsages`: https://docs.unity3d.com/ScriptReference/XR.CommonUsages.html
- Unity OpenXR input: https://docs.unity.cn/Packages/com.unity.xr.openxr%401.9/manual/input.html