feat: Add XRobotoolkit UDP bridge functionality

2026-06-24 21:18:18 +08:00
parent f4e0faf0bc
commit f0c955ae66
6 changed files with 474 additions and 548 deletions
--- a/docs/pico_udp_sender_ubuntu22_setup.md
+++ b/docs/pico_udp_sender_ubuntu22_setup.md
@ -1,545 +0,0 @@
-# Ubuntu 22.04 下 PICO 4 Ultra UDP Sender 配置教程
-
-本文档只记录如何在 **Ubuntu 22.04** 下配置 Unity/PICO 4 Ultra，让头显把左右手柄数据通过 UDP 发给当前 ROS2 工作空间。
-
-目标链路：
-
-```text
-PICO 4 Ultra 双手柄 6DoF pose / grip / trigger / pose 诊断
-  -> Unity Android APK
-  -> UDP JSON, 默认当前 ROS 主机 IP:15000，兜底 192.168.9.99:15000
-  -> xr_rm_input/udp_controller_receiver
-  -> /xr/left_controller 与 /xr/right_controller
-```
-
-## 1. 当前项目事实
-
- Unity 工程路径：`unity/XR_RM_PICO_UDP_Sender`
- Unity 版本：`2022.3.62f3c1`
- Unity Editor 路径：`/home/robot/Unity/Hub/Editor/2022.3.62f3c1/Editor/Unity`
- PICO SDK 路径：`unity/PICO-Unity-Integration-SDK-release_3.4.0`
- PICO package 引用：`Packages/manifest.json` 中的 `file:../../PICO-Unity-Integration-SDK-release_3.4.0`
- Unity UI 依赖：`com.unity.textmeshpro`，运行面板使用预生成的 `Assets/Resources/Fonts/Roboto-Regular SDF.asset` 和 `Roboto-Bold SDF.asset`
- Android 包名：`com.local.xr_rm_udp_sender`
- 默认 UDP 目标：构建时优先使用 `XR_RM_UDP_TARGET_HOST`，否则自动选择本机局域网 IPv4，兜底 `192.168.9.99:15000`
- 默认发送频率：`90 Hz`
- APK 输出：`unity/XR_RM_PICO_UDP_Sender/Builds/XR-RM-PICO-UDP-Sender.apk`
-
-PICO 端建议稳定发送 `90 Hz`，网络或性能不足时可降到 `60 Hz`，不要低于 `20 Hz`。ROS 遥操作节点默认 `command_timeout_sec=0.12`，超时会停止。
-
-## 2. Ubuntu 22.04 环境准备
-
-建议环境：
-
- Ubuntu 22.04 x86_64
- GNOME 桌面，优先 X11 会话
- Unity 2022.3 LTS
- Android Build Support、Android SDK & NDK Tools、OpenJDK
- ADB
-
-检查当前图形会话：
-
-```bash
-echo $XDG_SESSION_TYPE
-```
-
-如果 Unity Editor 或 Hub 在 Wayland 下出现黑屏、鼠标偏移、构建窗口异常，注销后选择 `Ubuntu on Xorg`。
-
-安装 Unity Hub：
-
-```bash
-sudo apt update
-sudo apt install -y curl gpg ca-certificates
-
-sudo install -d /etc/apt/keyrings
-curl -fsSL https://hub.unity3d.com/linux/keys/public \
-  | sudo gpg --dearmor -o /etc/apt/keyrings/unityhub.gpg
-
-echo "deb [arch=amd64 signed-by=/etc/apt/keyrings/unityhub.gpg] https://hub.unity3d.com/linux/repos/deb stable main" \
-  | sudo tee /etc/apt/sources.list.d/unityhub.list
-
-sudo apt update
-sudo apt install -y unityhub
-```
-
-安装 ADB：
-
-```bash
-sudo apt update
-sudo apt install -y android-tools-adb
-adb version
-```
-
-在 Unity Hub 里安装 Unity 2022.3 LTS，并勾选：
-
- `Android Build Support`
- `Android SDK & NDK Tools`
- `OpenJDK`
-
-优先使用 Unity 随 Editor 安装的 embedded Android SDK/NDK/JDK，不要混用系统 Android Studio 的工具链。
-
-## 3. 准备 PICO 4 Ultra
-
-在 PICO 4 Ultra 中：
-
-1. 登录设备。
-2. 开启开发者模式。
-3. 开启 USB 调试。
-4. 用 USB-C 线连接 Ubuntu 主机。
-5. 在 Ubuntu 上执行：
-
-```bash
-adb devices
-```
-
-PICO 内会弹出 USB 调试授权，选择允许。再次执行：
-
-```bash
-adb devices
-```
-
-应看到类似：
-
-```text
-<device_serial>    device
-```
-
-如果是 `unauthorized`，重新插拔 USB 并在 PICO 内确认授权，或执行：
-
-```bash
-adb kill-server
-adb start-server
-adb devices
-```
-
-## 4. 打开 Unity 工程
-
-在 Unity Hub 中：
-
-1. 点击 `Projects`。
-2. 点击 `Add`。
-3. 选择：
-
-```text
-/home/robot/WS_xr/src/unity/XR_RM_PICO_UDP_Sender
-```
-
-4. 用 Unity `2022.3.62f3c1` 或同系列 2022.3 LTS 打开。
-5. 如果提示升级工程版本，先确认 git 状态再继续。
-
-如果 Unity 打开后找不到 PICO package，检查：
-
-```text
-unity/XR_RM_PICO_UDP_Sender/Packages/manifest.json
-../../PICO-Unity-Integration-SDK-release_3.4.0
-```
-
-该相对路径表示从 `unity/XR_RM_PICO_UDP_Sender/Packages` 往上两级到 `unity/`，再进入 `PICO-Unity-Integration-SDK-release_3.4.0`。
-
-## 5. Android 构建设置
-
-当前工程已提供菜单脚本：
-
-```text
-Assets/Editor/XrRmUdpSenderProjectSetup.cs
-```
-
-在 Unity Editor 菜单中依次执行：
-
-```text
-XR-RM -> Apply UDP Sender Android Settings
-XR-RM -> Create Or Refresh UDP Sender Scene
-XR-RM -> Build Android APK
-```
-
-这些菜单会配置或刷新：
-
- `Product Name = XR-RM-PICO-UDP-Sender`
- `Package Name = com.local.xr_rm_udp_sender`
- Android min SDK = API 29
- Android target SDK = Auto
- Internet permission = enabled
- Scripting Backend = IL2CPP
- Target Architectures = ARM64
- Graphics API = OpenGLES3
- XR Management Android loader = PICO `PXR_Loader`
- `Assets/Scenes/Main.unity`
- 场景中的 `PicoControllerUdpSender`、`PicoUdpConfigPanel` 和 `PicoKeepAwake`
- TextMeshPro UI、Roboto SDF 字体、Saved/Favorite IP 面板、PICO predicted pose 优先读取和 Unity XR fallback
-
-也可以使用 batchmode 构建：
-
-```bash
-cd /home/robot/WS_xr/src
-
-UNITY_EDITOR="/home/robot/Unity/Hub/Editor/2022.3.62f3c1/Editor/Unity"
-
-"$UNITY_EDITOR" \
-  -batchmode \
-  -quit \
-  -projectPath "$(pwd)/unity/XR_RM_PICO_UDP_Sender" \
-  -executeMethod XrRmUdpSenderProjectSetup.BuildAndroidApk \
-  -logFile "$(pwd)/unity_build.log"
-```
-
-如果要覆盖自动发现到的 UDP 目标 IP，可在构建命令前设置：
-
-```bash
-export XR_RM_UDP_TARGET_HOST=192.168.9.99
-```
-
-构建成功后 APK 位于：
-
-```text
-unity/XR_RM_PICO_UDP_Sender/Builds/XR-RM-PICO-UDP-Sender.apk
-```
-
-## 6. 安装 APK 到 PICO
-
-确保 PICO 已 USB 连接且 `adb devices` 显示 `device`。
-
-```bash
-cd /home/robot/WS_xr/src
-adb install -r unity/XR_RM_PICO_UDP_Sender/Builds/XR-RM-PICO-UDP-Sender.apk
-```
-
-看到 `Success` 表示安装完成。
-
-检查包是否存在：
-
-```bash
-adb shell pm list packages | grep xr_rm
-```
-
-戴上 PICO：
-
-1. 打开 `Library`。
-2. 进入未知来源/开发者应用区域。
-3. 打开 `XR-RM-PICO-UDP-Sender`。
-
-## 7. 网络配置
-
-PICO 和 Ubuntu ROS 主机必须在同一个局域网。
-
-查看 Ubuntu 主机 IP：
-
-```bash
-hostname -I
-ip -4 addr
-```
-
-假设 Ubuntu 主机 IP 是：
-
-```text
-192.168.9.99
-```
-
-则 PICO App 面板里使用：
-
-| 配置项 | 推荐值 |
-| --- | --- |
-| `Target IP` | `192.168.9.99`，替换为当前 Ubuntu 主机 IP |
-| `Target Port` | `15000` |
-| `Saved IP` | 最近使用或收藏的 IP，可用摇杆左右切换 |
-| `Favorite` | `ON` 表示当前 IP 已收藏 |
-| `Send Rate` | 推荐 `90 Hz`，不稳定时用 `60 Hz` |
-| `Coordinates` | `Project (+Z back)` |
-| `UDP Sending` | `ON` |
-
-如果 Ubuntu 开启了 UFW：
-
-```bash
-sudo ufw allow 15000/udp
-sudo ufw status
-```
-
-## 8. PICO App 操作
-
-面板操作：
-
- 摇杆上/下：选择行
- 摇杆左/右：选择 Saved IP、收藏当前 IP、调整端口/频率、切换坐标模式或发送开关
- Trigger / A / X：编辑当前行或触发当前行
- B / Y：保存并应用
- Menu：在配置面板和运行 HUD 之间切换
- Grip：ROS 侧遥操作使能；未按下时机械臂停止
-
-验证现象：
-
- 面板中 `L valid pxr/unity` 或 `R valid pxr/unity` 表示 Unity 能读到对应手柄姿态；`invalid none` 表示该手柄姿态不可用。
- `UDP Sending ON` 后，ROS2 的 `/xr/left_controller` 与 `/xr/right_controller` 应持续刷新。
- HUD 显示包计数、发送端点、左右姿态状态和 KeepAwake 状态。
- `pose_valid=false` 时，ROS 接收端会强制该手柄 `grip=false`，即使 PICO 端按下了 Grip。
- 按住 `grip` 并移动或小角度转动手柄时，mock 模式下 `/xr_rm/*/target_pose` 应连续变化，`/xr_rm/*/cmd_vel` 会显示目标位姿变化率。
- 真机模式下，点击对应手柄 `trigger` 会切换并保持对应夹爪开/关状态。
- 松开 `grip` 后，机械臂慢停，`cmd_vel` 应回到零。
-
-## 9. ROS2 端验证
-
-先构建并 source 工作空间：
-
-```bash
-cd /home/robot/WS_xr
-source /opt/ros/humble/setup.bash
-colcon build --symlink-install
-source install/setup.bash
-```
-
-只验证 UDP receiver：
-
-```bash
-ros2 launch xr_rm_input udp_receiver.launch.py udp_port:=15000
-```
-
-另开终端查看左右手柄：
-
-```bash
-cd /home/robot/WS_xr
-source /opt/ros/humble/setup.bash
-source install/setup.bash
-
-ros2 topic echo /xr/left_controller
-ros2 topic echo /xr/right_controller
-ros2 topic hz /xr/left_controller
-ros2 topic hz /xr/right_controller
-```
-
-期望现象：
-
- PICO 应用启动并打开 `UDP Sending ON` 后，两个 topic 都持续刷新。
- 按左手 `grip` 时，`/xr/left_controller` 的 `grip` 变成 `true`。
- 按右手 `grip` 时，`/xr/right_controller` 的 `grip` 变成 `true`。
- 扳机从松开到按下时，`trigger` 从 `0.0` 附近变到 `1.0` 附近。
- 单臂或双臂真机 launch 已启动时，对应手柄每次点击 `trigger` 都会切换并保持对应夹爪状态。
- 平移手柄时，`pose.position` 连续变化。
-
-如果收不到包，先抓 UDP：
-
-```bash
-sudo tcpdump -ni any udp port 15000
-```
-
-能看到 UDP 但 ROS topic 没数据，说明 JSON 字段不符合协议。查看 `udp_controller_receiver` 终端里的格式错误提示。
-
-## 10. Mock 闭环验证
-
-先不要连接真机，使用 mock 模式验证完整链路。
-
-终端 1：
-
-```bash
-cd /home/robot/WS_xr
-source /opt/ros/humble/setup.bash
-source install/setup.bash
-ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true udp_port:=15000
-```
-
-终端 2：
-
-```bash
-cd /home/robot/WS_xr
-source /opt/ros/humble/setup.bash
-source install/setup.bash
-ros2 topic echo /xr_rm/left_rm75/cmd_vel
-ros2 topic echo /xr_rm/right_rm75/cmd_vel
-# 需要看实际位姿目标时另开终端：
-ros2 topic echo /xr_rm/left_rm75/target_pose
-ros2 topic echo /xr_rm/right_rm75/target_pose
-```
-
-流程：
-
-1. 启动 PICO Unity 应用。
-2. 确认 `/xr/left_controller` 和 `/xr/right_controller` 正常刷新。
-3. 左手按住 `grip`，只移动左手一小段，观察 `/xr_rm/left_rm75/target_pose` 和 `/xr_rm/left_rm75/cmd_vel`。
-4. 左手保持小角度转动，确认 `target_pose.pose.orientation` 和 `cmd_vel.twist.angular` 会变化。
-5. 松开左手 `grip`，确认 `cmd_vel` 回到 0。
-6. 右手重复同样流程。
-7. 用 `ros2 topic hz` 确认频率稳定，建议接近 PICO 端配置的发送频率。
-
-如果要排除 PICO 端问题，可用本机 sample sender 验证 ROS 端：
-
-```bash
-ros2 run xr_rm_input sample_udp_sender --hand both --host 127.0.0.1 --port 15000 \
-  --pattern axis_sweep --seconds 60 --both-mode staggered
-```
-
-`axis_sweep` 会按 `XR +X/-X/+Y/-Y/+Z/-Z` 打印方向标签。双手 `staggered` 模式先左后右，便于现场逐只确认映射。
-如需同时验证姿态链路，可增加 `--rotation-pattern rpy_steps --rotation-amplitude-deg 25`。
-
-## 11. UDP JSON 协议
-
-Unity APK 每个周期发送一个双手柄 JSON 包：
-
-```json
-{
-  "t": 12.345,
-  "source_time": 12.345,
-  "seq": 42,
-  "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],
-      "pose_valid": true,
-      "pose_source": "pxr_predict",
-      "tracking_state": 3,
-      "controller_status": 2,
-      "grip_value": 1.0,
-      "axis": [0.0, 0.0],
-      "buttons": {
-        "grip": true,
-        "primary": false,
-        "secondary": false,
-        "menu": false,
-        "axis_click": false
-      }
-    },
-    "right": {
-      "grip": true,
-      "trigger": 0.4,
-      "pos": [0.12, 1.05, 0.30],
-      "quat": [0.0, 0.0, 0.0, 1.0],
-      "pose_valid": true,
-      "pose_source": "unity_xr",
-      "tracking_state": 3,
-      "controller_status": -1,
-      "grip_value": 0.8,
-      "axis": [0.0, 0.0],
-      "buttons": {
-        "grip": true,
-        "primary": false,
-        "secondary": false,
-        "menu": false,
-        "axis_click": false
-      }
-    }
-  }
-}
-```
-
-字段要求：
-
-| 字段 | 类型 | 说明 |
-| --- | --- | --- |
-| `t` | number | `Time.realtimeSinceStartupAsDouble` |
-| `source_time` | number | 当前同 `t`，预留给延迟统计 |
-| `seq` | number | 发送端递增包序号，预留给丢包统计 |
-| `frame_id` | string | 默认 `xr_world` |
-| `controllers.left` | object | 左手柄 |
-| `controllers.right` | object | 右手柄 |
-| `grip` | bool | ROS 遥操作启停 |
-| `trigger` | float | `0.0-1.0`；真机模式下跨过 `0.95` 的上升沿切换夹爪开/关状态 |
-| `pos` | float[3] | `[x, y, z]` |
-| `quat` | float[4] | `[qx, qy, qz, qw]`；ROS 遥操作节点会用相对旋转控制 TCP 姿态 |
-| `pose_valid` | bool | 姿态有效性；`false` 时 ROS receiver 强制 `grip=false` |
-| `pose_source` | string | `pxr_predict`、`unity_xr` 或 `none` |
-| `tracking_state` | int | Unity XR tracking state 原始值 |
-| `controller_status` | int | PICO controller status 原始值，Unity XR fallback 时为 `-1` |
-| `grip_value` | float | Grip 模拟量，范围 `0.0-1.0` |
-| `axis` | float[2] | 主摇杆二维轴 |
-| `buttons` | object | `grip`、`primary`、`secondary`、`menu`、`axis_click` |
-
-当前工程的 `Project (+Z back)` 坐标是 ROS 侧使用的统一项目坐标：
-
-```csharp
-project.x = native.z;
-project.y = native.y;
-project.z = -native.x;
-```
-
-这条转换只用于 `pose_source=pxr_predict` 且 `Coordinates=Project (+Z back)` 的情况。`Source raw` 模式保留 PXR native 或 Unity XR fallback 的原始坐标，便于现场对照；`unity_xr` fallback 暂不做 PXR native 转换，避免把已经由 Unity XR 提供的坐标二次转换。
-
-禁用发送、App 暂停或退出时，必须额外发送一次左右手柄 `grip=false`，让 ROS 侧安全停止。
-ROS 侧收到 `grip=false` 时不会主动打开或闭合夹爪，夹爪保持上一次状态。
-
-`udp_controller_receiver` 仍兼容调试脚本的旧格式：单个 object 可以直接带 `hand`、`pos`、`quat`；`controllers` 可以是 object 或 list；位置字段也支持 `position`、`p`、`pose.position`，姿态字段支持 `orientation`、`q`。四元数默认按 `xyzw` 解析，也可用 `quat_order:=wxyz` 覆盖。
-
-## 12. 坐标方向检查
-
-当前 Project 输出使用下面的位置坐标：
-
-```text
-+X: 向右
-+Y: 向上
-+Z: 向后
-```
-
-PXR `pxr_predict` 原始坐标按现场实测通常表现为右移 `native.z+`、前伸 `native.x+`，所以 Unity 在 Project 模式中会输出 `project.x=native.z`、`project.z=-native.x`。如果切到 `Source raw`，应能看到原始 PXR 现象，用于确认当前到底是 PXR native 问题还是 ROS 侧映射问题。
-
-双臂配置中的映射关系：
-
-```text
-左臂机器人位移增量 = [-手柄y,  手柄z, -手柄x]
-右臂机器人位移增量 = [ 手柄y,  手柄z,  手柄x]
-```
-
-建议现场按下面顺序验证：
-
-1. 只启动 `use_mock:=true`。
-2. APK 中选择 `Project (+Z back)`，按住左手 `grip`，每次只沿右/左、上/下、前/后移动一个方向。
-3. 记录 `/xr/left_controller.pose.position` 的变化方向。
-4. 记录 `/xr_rm/left_rm75/target_pose` 的位置方向。
-5. 小角度转动左手手柄，记录 `/xr/left_controller.pose.orientation` 与 `/xr_rm/left_rm75/target_pose.pose.orientation` 是否连续变化。
-6. 右手重复。
-
-如果两个手柄在 ROS topic 里的某个轴都反了，优先检查 Unity/PICO 发送到 `/xr/*_controller.pose.position` 的实际方向。如果 ROS topic 正确，但某一只机械臂运动方向不符合现场坐标，只改对应 YAML 的 `xr_to_robot_matrix`。
-
-`Coordinates=Source raw` 只用于诊断，不建议用于正式遥操作。现场方向修正优先通过 ROS 侧 topic 观察定位：如果 `/xr/*_controller.pose.position` 已经符合 Project 坐标约定，就不要在 Unity 中继续临时改符号。
-
-## 13. 真机前置检查
-
-本文档只负责 PICO UDP 发送配置，但如果继续接真机，必须先确认：
-
- 急停可用。
- 机械臂工作区清空。
- PICO topic 在 mock 下已经稳定。
- `grip=false` 时机械臂慢停，`/xr_rm/<arm>/cmd_vel` 为 0。
- mock 下小角度转动手柄时，`/xr_rm/<arm>/cmd_vel.twist.angular` 有连续小幅变化。
- 夹爪验证时点击对应手柄 `trigger`，确认每次点击都会切换并保持开/关状态。
- `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
-ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=false udp_port:=15000
-```
-
-双臂真机必须在左右单臂都验证通过后再启动：
-
-```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
-```
-
-## 14. 常见问题
-
-| 现象 | 排查 |
-| --- | --- |
-| Unity 找不到 PICO package | 检查 `Packages/manifest.json` 的 `../../PICO-Unity-Integration-SDK-release_3.4.0` 路径 |
-| Gradle 或 Android 构建失败 | 检查 Android Build Support、SDK/NDK/OpenJDK、IL2CPP、ARM64、Android Build Target 和 PICO SDK 编译错误 |
-| PICO 安装失败或 `unauthorized` | 重新授权 USB 调试，执行 `adb kill-server && adb start-server && adb devices` |
-| ROS2 收不到 UDP | 检查同一 Wi-Fi、Target IP、Target Port、UFW、`UDP Sending ON`、左右手柄是否 `valid pxr/unity` |
-| tcpdump 有包但 topic 没数据 | JSON 字段不对，确认包含 `controllers.left/right`，且 `pos` 长度 3、`quat` 长度 4 |
-| 有 topic 但机械臂不动 | 检查 `grip`、`pose_valid` 日志、teleop 节点订阅话题、工作空间限幅、UDP 超时 |
-| trigger 不控制夹爪 | 确认是真机 launch、`enable_tool_control=true`，且 `trigger` 能从低值跨到 0.95 以上 |
-| 按左手右臂动 | PICO 端 left/right 获取或填充反了 |
-| 松开 grip 后仍有速度 | 确认 PICO 持续发送 `grip=false`，并检查 teleop 超时停止 |
-| 经常超时 | 发送频率太低、网络丢包、PICO 应用后台暂停；提高 `sendHz` 并保持应用前台运行 |
-| 前后方向反了 | 先看 `/xr/*_controller.pose.position` 是否符合 `+Z` 向后；topic 正确时修改对应 YAML 的 `xr_to_robot_matrix` |
-| 某一只臂方向反了 | 修改对应 YAML 的 `xr_to_robot_matrix` |
-
-## 15. 参考链接
-
- Unity Hub Linux 安装文档：`https://docs.unity.com/en-us/hub/install-hub-linux`
- Unity 2022.3 系统要求：`https://docs.unity.cn/Documentation/Manual/system-requirements.html`
- PICO 4 Ultra 开发资源：`https://developer.picoxr.com/pico4-ultra/`
- PICO Unity Integration SDK：`https://github.com/Pico-Developer/PICO-Unity-Integration-SDK`
- PICO Controller & HMD input mapping：`https://developer-cn.picoxr.com/en/document/unity/input-mapping/`
- Unity XR `InputDevice.TryGetFeatureValue`：`https://docs.unity3d.com/ScriptReference/XR.InputDevice.TryGetFeatureValue.html`
- Unity XR `CommonUsages`：`https://docs.unity3d.com/ScriptReference/XR.CommonUsages.html`