optimze unity project for xr_rm

2026-06-03 14:16:52 +08:00
parent 9260e46b3c
commit 3fc8024648
98 changed files with 20352 additions and 1568 deletions
--- a/docs/pico_udp_sender_ubuntu22_setup.md
+++ b/docs/pico_udp_sender_ubuntu22_setup.md
@ -5,9 +5,9 @@
 目标链路：

 ```text
-PICO 4 Ultra 双手柄 6DoF pose / grip / trigger
+PICO 4 Ultra 双手柄 6DoF pose / grip / trigger / pose 诊断
  -> Unity Android APK
-  -> UDP JSON, 默认 192.168.9.99:15000 或当前 ROS 主机 IP:15000
+  -> UDP JSON, 默认当前 ROS 主机 IP:15000，兜底 192.168.9.99:15000
  -> xr_rm_input/udp_controller_receiver
  -> /xr/left_controller 与 /xr/right_controller
 ```
@ -19,12 +19,13 @@ PICO 4 Ultra 双手柄 6DoF pose / grip / trigger
 - 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 目标：`192.168.9.99:15000`
- 默认发送频率：`60 Hz`
+- 默认 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 端建议稳定发送 `60 Hz` 或 `90 Hz`，不要低于 `20 Hz`。ROS 遥操作节点默认 `command_timeout_sec=0.12`，超时会停止。
+PICO 端建议稳定发送 `90 Hz`，网络或性能不足时可降到 `60 Hz`，不要低于 `20 Hz`。ROS 遥操作节点默认 `command_timeout_sec=0.12`，超时会停止。

 ## 2. Ubuntu 22.04 环境准备

@ -164,6 +165,7 @@ XR-RM -> Build Android APK
 - 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 构建：

@ -238,8 +240,10 @@ ip -4 addr
 | --- | --- |
 | `Target IP` | `192.168.9.99`，替换为当前 Ubuntu 主机 IP |
 | `Target Port` | `15000` |
-| `Send Rate` | `60 Hz` 或 `90 Hz` |
-| `Coordinates` | `Project` |
+| `Saved IP` | 最近使用或收藏的 IP，可用摇杆左右切换 |
+| `Favorite` | `ON` 表示当前 IP 已收藏 |
+| `Send Rate` | 推荐 `90 Hz`，不稳定时用 `60 Hz` |
+| `Coordinates` | `Project (+Z back)` |
 | `UDP Sending` | `ON` |

 如果 Ubuntu 开启了 UFW：
@ -254,7 +258,7 @@ sudo ufw status
 面板操作：

 - 摇杆上/下：选择行
- 摇杆左/右：调整端口、频率、坐标模式或发送开关
+- 摇杆左/右：选择 Saved IP、收藏当前 IP、调整端口/频率、切换坐标模式或发送开关
 - Trigger / A / X：编辑当前行或触发当前行
 - B / Y：保存并应用
 - Menu：在配置面板和运行 HUD 之间切换
@ -262,9 +266,10 @@ sudo ufw status

 验证现象：

- 面板中 `L ok / R ok` 表示 Unity 能读到左右手柄。
+- 面板中 `L valid pxr/unity` 或 `R valid pxr/unity` 表示 Unity 能读到对应手柄姿态；`invalid none` 表示该手柄姿态不可用。
 - `UDP Sending ON` 后，ROS2 的 `/xr/left_controller` 与 `/xr/right_controller` 应持续刷新。
- HUD 显示包计数、追踪状态、grip 和 KeepAwake 状态。
+- HUD 显示包计数、发送端点、左右姿态状态和 KeepAwake 状态。
+- `pose_valid=false` 时，ROS 接收端会强制该手柄 `grip=false`，即使 PICO 端按下了 Grip。
 - 按住 `grip` 并移动手柄时，mock 模式下 `/xr_rm/*/target_pose` 应连续变化，`/xr_rm/*/cmd_vel` 会显示目标位姿变化率。
 - 松开 `grip` 后，机械臂慢停，`cmd_vel` 应回到零。

@ -352,9 +357,12 @@ ros2 topic echo /xr_rm/right_rm75/target_pose
 如果要排除 PICO 端问题，可用本机 sample sender 验证 ROS 端：

 ```bash
-ros2 run xr_rm_input sample_udp_sender --hand both --host 127.0.0.1 --port 15000 --seconds 20
+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` 模式先左后右，便于现场逐只确认映射。
+
 ## 11. UDP JSON 协议

 Unity APK 每个周期发送一个双手柄 JSON 包：
@ -362,19 +370,47 @@ 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]
+      "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]
+      "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
+      }
    }
  }
 }
@ -385,6 +421,8 @@ Unity APK 每个周期发送一个双手柄 JSON 包：
 | 字段 | 类型 | 说明 |
 | --- | --- | --- |
 | `t` | number | `Time.realtimeSinceStartupAsDouble` |
+| `source_time` | number | 当前同 `t`，预留给延迟统计 |
+| `seq` | number | 发送端递增包序号，预留给丢包统计 |
 | `frame_id` | string | 默认 `xr_world` |
 | `controllers.left` | object | 左手柄 |
 | `controllers.right` | object | 右手柄 |
@ -392,25 +430,31 @@ Unity APK 每个周期发送一个双手柄 JSON 包：
 | `trigger` | float | `0.0-1.0` |
 | `pos` | float[3] | `[x, y, z]` |
 | `quat` | float[4] | `[qx, qy, qz, qw]` |
+| `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
-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;
+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 侧安全停止。

+`udp_controller_receiver` 仍兼容调试脚本的旧格式：单个 object 可以直接带 `hand`、`pos`、`quat`；`controllers` 可以是 object 或 list；位置字段也支持 `position`、`p`、`pose.position`，姿态字段支持 `orientation`、`q`。四元数默认按 `xyzw` 解析，也可用 `quat_order:=wxyz` 覆盖。
+
 ## 12. 坐标方向检查

-当前配置使用 PICO/OpenXR 位置坐标：
+当前 Project 输出使用下面的位置坐标：

 ```text
 +X: 向右
@ -418,6 +462,8 @@ payload.quat[3] = rotation.w;
 +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
@ -428,12 +474,14 @@ payload.quat[3] = rotation.w;
 建议现场按下面顺序验证：

 1. 只启动 `use_mock:=true`。
-2. 按住左手 `grip`，沿 PICO 的 `+X/-X`、`+Y/-Y`、`+Z/-Z` 每次只动一个轴。
+2. APK 中选择 `Project (+Z back)`，按住左手 `grip`，每次只沿右/左、上/下、前/后移动一个方向。
 3. 记录 `/xr/left_controller.pose.position` 的变化方向。
 4. 记录 `/xr_rm/left_rm75/target_pose` 的方向。
 5. 右手重复。

-如果两个手柄在 ROS topic 里的某个轴都反了，优先检查 Unity 的坐标转换。如果 ROS topic 正确，但某一只机械臂运动方向不符合现场坐标，只改对应 YAML 的 `xr_to_robot_matrix`。
+如果两个手柄在 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. 真机前置检查

@ -468,13 +516,13 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
 | 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`、`L ok / R ok` |
+| 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`、teleop 节点订阅话题、工作空间限幅、UDP 超时 |
+| 有 topic 但机械臂不动 | 检查 `grip`、`pose_valid` 日志、teleop 节点订阅话题、工作空间限幅、UDP 超时 |
 | 按左手右臂动 | PICO 端 left/right 获取或填充反了 |
 | 松开 grip 后仍有速度 | 确认 PICO 持续发送 `grip=false`，并检查 teleop 超时停止 |
 | 经常超时 | 发送频率太低、网络丢包、PICO 应用后台暂停；提高 `sendHz` 并保持应用前台运行 |
-| 前后方向反了 | 先切换 Unity 坐标转换，再验证 ROS topic |
+| 前后方向反了 | 先看 `/xr/*_controller.pose.position` 是否符合 `+Z` 向后；topic 正确时修改对应 YAML 的 `xr_to_robot_matrix` |
 | 某一只臂方向反了 | 修改对应 YAML 的 `xr_to_robot_matrix` |

 ## 15. 参考链接