acRealman_xr/docs/pico4_ultra_udp_teleop_setup.md

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

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

/xr/left_controller
/xr/right_controller

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

1. 当前项目约定

当前链路如下：

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 端每包同时发送左右手柄：

{
  "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]
    }
  }
}

也兼容单手柄包：

{
  "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 里的：

XRoboToolkit_PC_Service_1.0.0_ubuntu_22.04_amd64.deb

安装并启动：

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 名称为：

XRoboToolkit-PICO-1.1.1.apk

安装步骤：

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

sudo apt update
sudo apt install android-tools-adb

4. 查看设备：

adb devices

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

5. 安装 APK：

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 录制文件示例：

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：

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 的职责很简单：

读取左右手柄 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：

hostname -I

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

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

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，例如：

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 中把脚本参数设置为：

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 里预先写死：

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 名为：

XR-RM-PICO-UDP-Sender.apk

安装 ADB：

Ubuntu 开发电脑：

sudo apt update
sudo apt install android-tools-adb

Windows 开发电脑：

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

检查 PICO 是否连接：

adb devices

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

adb devices

正确结果应类似：

List of devices attached
XXXXXXXXXXXX    device

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

adb kill-server
adb start-server
adb devices

安装 APK：

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

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

Success

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

adb shell pm list packages | grep xr_rm

Windows PowerShell/CMD 可以用：

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. 文件名必须写成：

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 中设置：

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 可命名为：

XR-RM-PICO-UDP-Sender.apk

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 执行：

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

另开终端查看左右手柄：

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

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

再查看频率：

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：

sudo tcpdump -ni any udp port 15000

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

7. 用 sample_udp_sender 排除 ROS 端问题

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

终端 1：

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：

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：

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：

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，只移动左手一小段，观察：

ros2 topic echo /xr_rm/left_rm75/cmd_vel

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

9. 坐标方向检查

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

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

双臂配置中的映射关系：

左臂机器人位移增量 = [-手柄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。

左臂：

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

右臂：

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. 双臂真机调试

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

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