ZhengLiu-cart/acRealman_xr_from_Yikai
1
0
Fork 0
forked from YikaiFu-cart/acRealman_xr
Code Pull Requests Activity

add unity doc.

Browse Source
This commit is contained in:
YikaiFu-cart
2026-05-30 16:04:37 +08:00
parent ce52895ec2
commit f4ee53d9c3
1538 changed files with 221228 additions and 2192 deletions
Download Patch File Download Diff File Expand all files Collapse all files

411
docs/PROMPT.md
View File

@ -1,411 +0,0 @@
# Claude Code 生成提示词：XR-RM75 双臂遥操作 ROS2 项目
下面这份提示词用于交给 Claude Code，让它在一个空的 ROS2 Humble 工作空间中生成一个与当前项目大致等价的代码框架。项目不是完整复刻论文中的 XRoboToolkit，而是参考其 OpenXR 数据约定、模块化分层和 grip 相对位移遥操作思想，落地一个阶段一可调试版本：PICO 4 Ultra 双手柄通过 UDP 控制左右 RM75 机械臂的笛卡尔速度。
## 当前项目整体脉络
项目根目录是 ROS2 workspace 的 `src/` 层，面向 Ubuntu 22.04 + ROS2 Humble。当前核心目标是跑通以下闭环：
```text
PICO 4 Ultra 双手柄位姿/按键
-> UDP JSON 数据包
-> xr_rm_input/udp_controller_receiver
-> /xr/left_controller 与 /xr/right_controller
-> xr_rm_teleop/single_arm_velocity_teleop
-> MockRealManAdapter 或 RealManAdapter
-> /xr_rm/<arm>/current_pose、target_pose、cmd_vel 调试话题
-> 真机模式下调用睿尔曼 RM75 Python API2 的 rm_movev_canfd
```
当前阶段已包含：
- PICO/OpenXR 手柄 UDP 输入解析。
- 左右手柄话题分发。
- 左右 RM75 单臂速度遥操作节点。
- 通过统一的 `arm_debug.launch.py` 支持双臂 mock、双臂真机和单臂调试。
- acRealman 来源的 RM75 IP、工作空间、圆柱半径约束、初始化点位和部分安全配置。
- Tkinter 启动面板，用于打开 mock、真机、诊断和话题监控命令。
当前阶段暂不实现：
- XR 侧 Unity 应用。
- 夹爪 trigger 桥接和末端执行器 launch 集成。
- 论文中的低延迟立体视频流。
- QP IK、灵巧手 retarget、全身追踪、运动捕捉 tracker。
- VLA 数据集记录和训练流程。
- 双臂碰撞模型、自动采摘算法、移动底盘和升降柱。
## 论文 XRoboToolkit 对本项目的启发
论文《XRoboToolkit: A Cross-Platform Framework for Robot Teleoperation》的关键思想如下，生成代码时只吸收与当前阶段相关的部分：
- XR 侧遵循 OpenXR 坐标约定：右手系，`+X` 向右，`+Y` 向上，`+Z` 向后；6DoF pose 使用位置 `[x, y, z]` 和四元数 `[qx, qy, qz, qw]`。
- 实时追踪数据可统一放在一个 JSON 对象里传输，控制器字段包括 pose、grip、trigger、摇杆和按钮等。当前项目只需要 pose、grip、trigger、hand。
- 控制器遥操作使用 grip 作为使能键。为了稳定直观，grip 首次按下时锁定手柄起点和机器人 TCP 起点，之后机器人末端跟随“手柄相对位移”，不是直接跟随绝对 pose。
- XRoboToolkit 是模块化结构：XR 输入、PC 服务、机器人控制、视觉反馈、数据记录彼此解耦。当前项目也按 ROS2 package 拆分：interfaces、input、teleop、bringup。
- 论文完整系统包含 QP IK、立体视觉、运动 tracker、灵巧手 retarget 和数据采集。当前项目的 RM75 阶段一只做笛卡尔速度遥操，后续扩展时再接这些模块。
## 请 Claude Code 生成的项目
请在一个 ROS2 Humble workspace 的 `src/` 目录中生成以下四个 package：
```text
xr_rm_interfaces/
CMakeLists.txt
package.xml
msg/XrController.msg
xr_rm_input/
package.xml
setup.py
setup.cfg
resource/xr_rm_input
launch/udp_receiver.launch.py
xr_rm_input/__init__.py
xr_rm_input/udp_controller_receiver.py
xr_rm_input/sample_udp_sender.py
xr_rm_teleop/
package.xml
setup.py
setup.cfg
resource/xr_rm_teleop
xr_rm_teleop/__init__.py
xr_rm_teleop/realman_adapter.py
xr_rm_teleop/single_arm_velocity_teleop.py
xr_rm_bringup/
CMakeLists.txt
package.xml
config/dual_arm_rm75.yaml
config/left_arm_rm75.yaml
config/right_arm_rm75.yaml
launch/arm_debug.launch.py
tools/launcher_ui.py
```
同时生成根目录 `README.md`，说明构建、mock 验证、真机验证和话题检查方式。
## 接口定义
`xr_rm_interfaces/msg/XrController.msg`：
```text
std_msgs/Header header
string hand
bool grip
float32 trigger
geometry_msgs/Pose pose
```
`package.xml` 和 `CMakeLists.txt` 需要使用 `rosidl_default_generators` 生成该消息，并依赖 `std_msgs`、`geometry_msgs`。
## xr_rm_input 需求
实现 `UdpControllerReceiver` 节点，职责是把轻量级 XR UDP JSON 转成左右手柄 ROS2 消息。
节点名：`udp_controller_receiver`
参数：
- `udp_host`，默认 `0.0.0.0`
- `udp_port`，默认 `15000`
- `topic`，兼容旧版单话题，默认 `/xr/right_controller`
- `left_topic`，默认 `/xr/left_controller`
- `right_topic`，默认空字符串，为空时使用 `topic`
- `default_hand`，默认 `right`
- `timer_hz`，默认 `100.0`
- `quat_order`，默认 `xyzw`，可选 `wxyz`
实现要点：
- UDP socket 使用非阻塞模式，定时器里循环读取直到 `BlockingIOError`。
- 支持单手柄 JSON：
```json
{
"hand": "right",
"grip": true,
"trigger": 0.2,
"pos": [0.12, 1.05, -0.32],
"quat": [0.0, 0.0, 0.0, 1.0]
}
```
- 支持双手柄 JSON：
```json
{
"controllers": {
"left": {
"grip": true,
"trigger": 0.0,
"pos": [-0.12, 1.05, -0.32],
"quat": [0.0, 0.0, 0.0, 1.0]
},
"right": {
"grip": true,
"trigger": 0.4,
"pos": [0.12, 1.05, -0.32],
"quat": [0.0, 0.0, 0.0, 1.0]
}
}
}
```
- 也支持数组、多种字段别名：`pos`/`position`/`p`、`quat`/`orientation`/`q`、`pose` 对象或 `[x,y,z,qx,qy,qz,qw]` 列表。
- hand 归一化：`left`/`l`/`left_controller` 映射为 `left`，`right`/`r`/`right_controller` 映射为 `right`。
- grip 支持 bool 和字符串布尔值。
- trigger clamp 到 `[0, 1]`。
- 异常包只 warn，不让节点崩溃。
实现 `sample_udp_sender.py`：
- 命令行参数：`--host`、`--port`、`--seconds`、`--hz`、`--amplitude`、`--hand left|right|both`
- 按指定频率发送模拟手柄数据。
- 发送期间 `grip=true`，结束时给每只手发送一次 `grip=false`。
`udp_receiver.launch.py` 启动 `udp_controller_receiver`，暴露 `udp_host`、`udp_port`、`topic` 参数。
## xr_rm_teleop 需求
### realman_adapter.py
实现三个对象：
- `ArmPose` dataclass，字段 `x,y,z,rx,ry,rz`，提供 `xyz()`。
- `MockRealManAdapter`：不连接真机，保存当前 pose，`send_cartesian_velocity()` 按控制周期简单积分速度，适合 mock 验证。
- `RealManAdapter`：封装睿尔曼 Python API2。
`RealManAdapter` 要点：
- 延迟导入 `from Robotic_Arm.rm_robot_interface import RoboticArm, rm_thread_mode_e`，未安装时抛出清晰错误。
- `connect()` 中创建机械臂连接，支持 IP 和端口。
- 可选下发安全配置：避奇异、最大线速度、最大角速度、线加速度、角加速度、各关节最大速度和加速度。
- 可选在连接后执行 `rm_movej(initial_joint_pose)` 和 `rm_movel(initial_tcp_pose)`，默认关闭。
- 使用 `rm_set_movev_canfd_init(avoid_singularity, frame_type, dt_ms)` 初始化速度透传。
- `get_current_pose()` 从不同 SDK 返回结构中递归解析常见字段：`pose`、`tool_pose`、`tcp_pose`、`current_pose`。
- `send_cartesian_velocity(velocity, follow)` 调用 `rm_movev_canfd(velocity, follow, 0, 0)`。
- `stop()` 发送零速度并尝试 slow stop。
- `close()` 停止并删除机械臂连接。
### single_arm_velocity_teleop.py
实现 `SingleArmVelocityTeleop` 节点。
节点名：`single_arm_velocity_teleop`
核心参数：
- `arm_name`，默认 `rm75`
- `controller_topic`，默认 `/xr/right_controller`
- `control_rate_hz`，默认 `50.0`
- `command_timeout_sec`，默认 `0.12`
- `scale`，默认 `1.0`
- `kp_linear`，默认 `2.0`
- `deadband_m`，默认 `0.002`
- `low_pass_alpha`，默认 `0.35`
- `max_linear_speed`，默认 `0.05`
- `enable_position_axes`，默认 `[true, true, true]`
- `workspace_min`、`workspace_max`
- `cyl_radius_limit`
- `low_z_threshold`
- `low_z_min_radius`
- `xr_to_robot_matrix`，长度 9
- `use_mock`
- `mock_initial_pose`
- `robot_ip`、`robot_port`
- `avoid_singularity`、`frame_type`、`follow`
- `configure_safety_limits`
- `max_line_speed`、`max_angular_speed`、`max_line_acc`、`max_angular_acc`
- `joint_max_speed`、`joint_max_acc`
- `move_to_initial_pose_on_connect`
- `initial_joint_pose`、`initial_tcp_pose`、`init_move_speed`
- `debug_topic_prefix`，默认 `/xr_rm`
控制流程：
1. 订阅 `controller_topic` 的 `XrController`。
2. 每个控制 tick 检查是否收到消息，未收到或超时则 `_safe_stop()`。
3. grip 未按下时停止输出，并清空 active 状态、手柄起点、机器人起点。
4. grip 首次按下时只锁定当前手柄位置和当前 TCP 位置，不立即运动。
5. grip 持续按下时：
- 计算 `controller_delta = controller_now - controller_start`
- 用 `xr_to_robot_matrix` 映射为机器人坐标增量
- `target = robot_start + scale * robot_delta`
- 先按 `workspace_min/max` 限幅，再按 `cyl_radius_limit` 限制基座圆柱半径，低高度区域使用 `low_z_min_radius`
- `error = target - robot_now`
- 对禁用轴清零
- 小于 `deadband_m` 输出零速度，否则 `velocity = kp_linear * error`
- 按向量范数限制 `max_linear_speed`
- 做一阶低通滤波
- 拼成 `[vx, vy, vz, 0, 0, 0]` 发送给 adapter
6. 发布调试话题：
- `/xr_rm/<arm_name>/current_pose`，`PoseStamped`
- `/xr_rm/<arm_name>/target_pose`，`PoseStamped`
- `/xr_rm/<arm_name>/cmd_vel`，`TwistStamped`
7. 所有异常、断连、松手、退出都要发送零速度。
注意：当前阶段只控制 TCP 平移，不控制姿态。
## bringup 配置需求
### RM75 默认参数
沿用以下现场参数：
- 左臂 IP：`192.168.192.18`
- 右臂 IP：`192.168.192.19`
- TCP 端口：`8080`
- 左臂工作空间：`x[-0.50, 0.50] y[-0.60, -0.20] z[0.10, 0.50]`
- 右臂工作空间：`x[-0.70, 0.50] y[-0.60, 0.40] z[0.10, 0.70]`
- 左右臂圆柱半径限制：`[0.20, 0.60]`
- `low_z_threshold=0.20`
- `low_z_min_radius=0.21`
- 保守速度：`max_linear_speed=0.04`
- `scale=0.75`
- `kp_linear=1.8`
- `low_pass_alpha=0.35`
### 坐标映射
遵循论文中的 PICO/OpenXR 坐标：`+X` 向右，`+Y` 向上，`+Z` 向后。
左臂 base 坐标：`+X` 向下，`+Z` 向右，前方工作区对应 `-Y`。
左臂映射：
```text
robot_delta = [-controller_y, -controller_z, controller_x]
xr_to_robot_matrix:
[0.0, -1.0, 0.0,
0.0, 0.0, -1.0,
1.0, 0.0, 0.0]
```
右臂 base 坐标：`+X` 向上，`+Z` 向左，前方工作区对应 `-Y`。
右臂映射：
```text
robot_delta = [controller_y, -controller_z, -controller_x]
xr_to_robot_matrix:
[0.0, 1.0, 0.0,
0.0, 0.0, -1.0,
-1.0, 0.0, 0.0]
```
### 初始点位
左臂：
```text
mock_initial_pose: [-0.2562, -0.2765, 0.1489, -3.0190, -0.1010, 3.1400]
initial_joint_pose: [-167.21, 28.48, 28.21, 61.35, -14.40, 84.49, -124.51]
initial_tcp_pose: [-0.2562, -0.2765, 0.1489, -3.0190, -0.1010, 3.1400]
```
右臂：
```text
mock_initial_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910]
initial_joint_pose: [-25.60, 34.09, -19.55, 71.59, 16.97, 80.98, 59.67]
initial_tcp_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910]
```
真机 launch 默认不要自动移动到初始化点，只有显式设置 `move_to_initial_pose_on_connect:=true` 才执行。
### launch 文件
`arm_debug.launch.py`：
- 参数 `arm:=left|right|both`
- 参数 `use_mock:=true|false`
- 单臂时读取 `left_arm_rm75.yaml` 或 `right_arm_rm75.yaml`
- 双臂时读取 `dual_arm_rm75.yaml`
- 总是启动 `udp_controller_receiver`
- 这是唯一的遥操作 launch 主入口，UI 和命令行调试都优先使用它。
### 启动面板
`tools/launcher_ui.py` 做一个简单 Tkinter 面板即可：
- 自动寻找 workspace 根目录。
- 每个终端启动前 source `/opt/ros/humble/setup.bash` 和 `install/setup.bash`。
- 模式包括 Simulation、Left Arm、Right Arm、Dual Arm、Diagnostics。
- 提供按钮运行 mock launch、真机 launch、sample UDP sender、topic list、node list、topic echo。
- 提供清理按钮，能关闭由面板启动的终端或进程。
## README 运行流程
README 需要包含以下命令。
构建：
```bash
source /opt/ros/humble/setup.bash
rosdep update
rosdep install --from-paths src -y --ignore-src
colcon build --symlink-install
source install/setup.bash
```
mock 验证：
```bash
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true
ros2 run xr_rm_input sample_udp_sender --hand both --host 127.0.0.1 --port 15000
```
检查话题：
```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
```
单臂调试：
```bash
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=true
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=true
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true
```
真机启动：
```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
```
安全验证说明：
- 首次真机测试保持 `max_linear_speed <= 0.04`。
- 先只上右臂，再上左臂。
- 每次按住 grip 后分别检查上、前后、左右方向。
- 方向反了只改对应臂 YAML 中 `xr_to_robot_matrix` 的符号。
- 当前没有双臂碰撞模型，首次测试要让两臂工作区物理分离。
## 代码风格和验收标准
代码要求：
- Python 使用类型标注，异常信息清晰。
- ROS2 节点退出时必须安全停止机械臂。
- mock 模式不得导入或依赖睿尔曼 SDK。
- 真机 SDK 未安装时要提示使用 `use_mock:=true` 或安装厂商 API2。
- 参数校验要覆盖控制频率、timeout、低通系数、工作空间上下界、圆柱半径。
- 不要引入论文中当前阶段不需要的大型依赖，如 Unity、PlaCo、Pinocchio、MuJoCo、视频流模块。
验收标准：
- `colcon build --symlink-install` 能通过。
- `ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true` 能启动三个节点。
- `sample_udp_sender --hand both` 后能看到左右 `/xr/*_controller` 和 `/xr_rm/*/cmd_vel`。
- 松开 grip、UDP 超时、节点退出时都会发送零速度。
- 修改 YAML 中 `xr_to_robot_matrix` 能改变对应臂方向映射。
- 真机模式只在 `use_mock=false` 时尝试连接 RM75。

900
docs/pico4_ultra_udp_teleop_setup.md
View File

@ -1,900 +0,0 @@
# 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

485
docs/pico_udp_sender_ubuntu22_setup.md Normal file
View File

@ -0,0 +1,485 @@
# 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
-> Unity Android APK
-> UDP JSON, 默认 192.168.9.99:15000 或当前 ROS 主机 IP: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`
- Android 包名：`com.local.xr_rm_udp_sender`
- 默认 UDP 目标：`192.168.9.99:15000`
- 默认发送频率：`60 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`，超时会停止。
## 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`
也可以使用 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` |
| `Send Rate` | `60 Hz` 或 `90 Hz` |
| `Coordinates` | `Project` |
| `UDP Sending` | `ON` |
如果 Ubuntu 开启了 UFW：
```bash
sudo ufw allow 15000/udp
sudo ufw status
```
## 8. PICO App 操作
面板操作：
- 摇杆上/下：选择行
- 摇杆左/右：调整端口、频率、坐标模式或发送开关
- Trigger / A / X：编辑当前行或触发当前行
- B / Y：保存并应用
- Menu：在配置面板和运行 HUD 之间切换
- Grip：ROS 侧遥操作使能；未按下时机械臂停止
验证现象：
- 面板中 `L ok / R ok` 表示 Unity 能读到左右手柄。
- `UDP Sending ON` 后，ROS2 的 `/xr/left_controller` 与 `/xr/right_controller` 应持续刷新。
- HUD 显示包计数、追踪状态、grip 和 KeepAwake 状态。
- 按住 `grip` 并移动手柄时，mock 模式下 `/xr_rm/*/cmd_vel` 应出现非零速度。
- 松开 `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` 附近。
- 平移手柄时，`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
```
流程：
1. 启动 PICO Unity 应用。
2. 确认 `/xr/left_controller` 和 `/xr/right_controller` 正常刷新。
3. 左手按住 `grip`，只移动左手一小段，观察 `/xr_rm/left_rm75/cmd_vel`。
4. 松开左手 `grip`，确认 `cmd_vel` 回到 0。
5. 右手重复同样流程。
6. 用 `ros2 topic hz` 确认频率稳定，建议 `50 Hz` 以上。
如果要排除 PICO 端问题，可用本机 sample sender 验证 ROS 端：
```bash
ros2 run xr_rm_input sample_udp_sender --hand both --host 127.0.0.1 --port 15000 --seconds 20
```
## 11. UDP JSON 协议
Unity APK 每个周期发送一个双手柄 JSON 包：
```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]
}
}
}
```
字段要求：
| 字段 | 类型 | 说明 |
| --- | --- | --- |
| `t` | number | `Time.realtimeSinceStartupAsDouble` |
| `frame_id` | string | 默认 `xr_world` |
| `controllers.left` | object | 左手柄 |
| `controllers.right` | object | 右手柄 |
| `grip` | bool | ROS 遥操作启停 |
| `trigger` | float | `0.0-1.0` |
| `pos` | float[3] | `[x, y, z]` |
| `quat` | float[4] | `[qx, qy, qz, qw]` |
坐标转换应与当前工程保持一致：
```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;
```
禁用发送、App 暂停或退出时，必须额外发送一次左右手柄 `grip=false`，让 ROS 侧安全停止。
## 12. 坐标方向检查
当前配置使用 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`。
## 13. 真机前置检查
本文档只负责 PICO UDP 发送配置，但如果继续接真机，必须先确认：
- 急停可用。
- 机械臂工作区清空。
- 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
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`、`L ok / R ok` |
| tcpdump 有包但 topic 没数据 | JSON 字段不对，确认包含 `controllers.left/right`，且 `pos` 长度 3、`quat` 长度 4 |
| 有 topic 但机械臂不动 | 检查 `grip`、teleop 节点订阅话题、工作空间限幅、UDP 超时 |
| 按左手右臂动 | PICO 端 left/right 获取或填充反了 |
| 松开 grip 后仍有速度 | 确认 PICO 持续发送 `grip=false`，并检查 teleop 超时停止 |
| 经常超时 | 发送频率太低、网络丢包、PICO 应用后台暂停；提高 `sendHz` 并保持应用前台运行 |
| 前后方向反了 | 先切换 Unity 坐标转换，再验证 ROS topic |
| 某一只臂方向反了 | 修改对应 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`

797
docs/ubuntu22_unity_pico_udp_sender_prompt.md
View File

@ -1,797 +0,0 @@
# Ubuntu 22.04 Unity Prompt：复现 XR_RM_PICO_UDP_Sender
本文档用于在 Ubuntu 22.04 下搭建 Unity + PICO 4 Ultra Android 开发环境，并给出一份可以直接交给 Codex、Claude Code 或其他代码代理的 Unity 项目生成提示词。目标是最终做出与当前仓库中 `unity/XR_RM_PICO_UDP_Sender` 等价的 APK：
```text
PICO 4 Ultra 双手柄 6DoF pose / grip / trigger
-> Unity Android APK
-> 60 Hz UDP JSON，默认发往 ROS2 主机 15000 端口
-> xr_rm_input/udp_controller_receiver
-> /xr/left_controller 与 /xr/right_controller
-> xr_rm_teleop/single_arm_velocity_teleop
```
截至 2026-05-29，当前项目事实如下：
- 当前 Unity 工程路径：`unity/XR_RM_PICO_UDP_Sender`
- 当前工程版本：`2022.3.16f1c1`
- 当前 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`
- 当前默认 UDP 目标：`192.168.9.89:15000`
- 当前默认发送频率：`60 Hz`
- 当前 Android 包名：`com.local.xr_rm_udp_sender`
- 当前 APK 名称：`Builds/XR-RM-PICO-UDP-Sender.apk`
## 1. Unity 版本选择建议
优先建议使用 Unity 2022.3 LTS，而不是直接升级到 Unity 6。
原因：
- 当前工程已经用 Unity 2022.3 系列创建。
- PICO 官方 PICO 4 Ultra 资源页说明 PICO Unity Integration SDK 3.0 支持 Unity 2020-23，Unity 6 支持仍属于计划项。
- Unity 6 对 Ubuntu 22.04 的官方 Linux Editor 支持更清晰，但当前 PICO SDK 主线更适合 Unity 2022.3。
- Unity 2022.3 官方系统要求页列出的 Linux Editor 发行版是 Ubuntu 20.04 和 18.04，不是 22.04；因此在 Ubuntu 22.04 上使用 2022.3 时，建议保持 GNOME + X11、使用官方显卡驱动，并避免 Wayland 相关变量干扰。
推荐策略：
1. 如果要最大程度复现当前工程：使用 Unity `2022.3.16f1` 或最接近的 2022.3 LTS 国际版补丁。当前工程的 `2022.3.16f1c1` 是中国版编辑器标识，在 Ubuntu 国际版 Hub 中通常选择不带 `c1` 的对应版本或相近补丁。
2. 如果是新建工程并希望减少 Linux/Android 构建问题：使用 Unity `2022.3.31f1` 或 Unity Hub 中较新的 `2022.3.x LTS` 补丁，但不要升到 Unity 6，除非你已经确认所用 PICO SDK 版本明确支持 Unity 6。
3. 如果首要目标是严格官方支持 Ubuntu 22.04 Linux Editor，而不是 PICO SDK 稳定性，可以评估 Unity 6；但这会偏离当前项目，本文后续步骤不以 Unity 6 为默认路线。
## 2. Ubuntu 22.04 环境准备
### 2.1 系统与图形环境
建议使用：
- Ubuntu 22.04 x86_64
- GNOME 桌面
- X11 会话优先
- NVIDIA 用户使用官方 proprietary driver；AMD 用户使用 Mesa 驱动
- 至少 16 GB RAM，磁盘预留 40 GB 以上
检查当前会话：
```bash
echo $XDG_SESSION_TYPE
```
如果输出是 `wayland` 且 Unity Editor 或 Hub 出现黑屏、鼠标偏移、构建窗口异常，注销后在登录界面选择 `Ubuntu on Xorg`。
如果 Unity Editor 报 `Pipe error !`，先在启动 Unity 的终端里执行：
```bash
ulimit -n 4096
unityhub
```
### 2.2 安装 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
```
启动 Hub：
```bash
unityhub
```
首次启动需要登录 Unity 账号并激活 Personal 或其他可用 license。
### 2.3 安装 Unity Editor 和 Android 模块
在 Unity Hub 中：
1. 打开 `Installs`。
2. 点击 `Install Editor`。
3. 选择 `2022.3 LTS`。
4. 模块必须勾选：
- `Android Build Support`
- `Android SDK & NDK Tools`
- `OpenJDK`
5. 安装完成后，在 `Installs` 页面确认对应 Editor 存在。
不要优先使用系统 Android Studio 的 SDK、NDK、JDK。Unity Android 构建最稳的方式是使用 Unity Hub 随 Editor 安装的 embedded Android SDK/NDK/OpenJDK。
### 2.4 安装 ADB
ADB 用于安装 APK、查看设备和抓取 logcat：
```bash
sudo apt update
sudo apt install -y android-tools-adb
adb version
```
### 2.5 准备 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 内确认授权。
## 3. 打开当前 Unity 工程
如果你是在本仓库中继续工作，目录应类似：
```text
XR_Realman/
docs/
unity/
PICO-Unity-Integration-SDK-release_3.4.0/
XR_RM_PICO_UDP_Sender/
xr_rm_input/
xr_rm_teleop/
```
在 Unity Hub 中：
1. 点击 `Projects`。
2. 点击 `Add`。
3. 选择：
```text
XR_Realman/unity/XR_RM_PICO_UDP_Sender
```
4. 用 Unity 2022.3 LTS 打开。
5. 如果提示升级工程版本，先备份或确认 git 状态，再继续。
本工程的 `Packages/manifest.json` 依赖：
```json
{
"dependencies": {
"com.unity.xr.picoxr": "file:../../PICO-Unity-Integration-SDK-release_3.4.0",
"com.unity.xr.management": "4.4.0",
"com.unity.modules.androidjni": "1.0.0",
"com.unity.modules.animation": "1.0.0",
"com.unity.modules.imageconversion": "1.0.0",
"com.unity.modules.jsonserialize": "1.0.0",
"com.unity.modules.unitywebrequest": "1.0.0",
"com.unity.modules.unitywebrequesttexture": "1.0.0",
"com.unity.modules.xr": "1.0.0"
}
}
```
如果 Unity 打开后找不到 `com.unity.xr.picoxr`，先确认 PICO SDK 文件夹确实位于：
```text
XR_Realman/unity/PICO-Unity-Integration-SDK-release_3.4.0
```
## 4. 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` 对象
- `PicoControllerUdpSender` 和 `PicoUdpConfigPanel` 组件
也可以用 batchmode 构建：
```bash
cd /path/to/XR_Realman
UNITY_EDITOR="$HOME/Unity/Hub/Editor/2022.3.31f1/Editor/Unity"
"$UNITY_EDITOR" \
-batchmode \
-quit \
-projectPath "$(pwd)/unity/XR_RM_PICO_UDP_Sender" \
-executeMethod XrRmUdpSenderProjectSetup.BuildAndroidApk \
-logFile "$(pwd)/unity_build.log"
```
根据你实际安装版本修改 `2022.3.31f1`。构建成功后 APK 在：
```text
unity/XR_RM_PICO_UDP_Sender/Builds/XR-RM-PICO-UDP-Sender.apk
```
安装：
```bash
adb install -r unity/XR_RM_PICO_UDP_Sender/Builds/XR-RM-PICO-UDP-Sender.apk
```
## 5. 网络和 ROS2 联调
### 5.1 找到 Ubuntu 主机 IP
PICO 和 Ubuntu 主机必须在同一个局域网中。查看 Ubuntu IP：
```bash
hostname -I
ip -4 addr
```
假设 Ubuntu 主机 IP 是：
```text
192.168.9.89
```
则 Unity APK 中 Target IP 使用 `192.168.9.89`，Target Port 使用 `15000`。
如果启用了 UFW：
```bash
sudo ufw allow 15000/udp
sudo ufw status
```
### 5.2 启动 ROS2 mock 验证链路
在 Ubuntu 的 ROS2 workspace 根目录：
```bash
source /opt/ros/humble/setup.bash
colcon build --symlink-install
source install/setup.bash
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true
```
另开终端检查话题：
```bash
source /opt/ros/humble/setup.bash
source install/setup.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
```
### 5.3 在 PICO 内配置和操作 APK
戴上 PICO 后打开 `XR-RM-PICO-UDP-Sender`。
面板字段：
| 行 | 含义 | 推荐值 |
| --- | --- | --- |
| `Target IP` | ROS2 Ubuntu 主机 IP | 例如 `192.168.9.89` |
| `Target Port` | ROS2 UDP 接收端口 | `15000` |
| `Send Rate` | UDP 发送频率 | `60 Hz` 或 `90 Hz` |
| `Coordinates` | 是否转换 Unity 坐标 | `Project` |
| `UDP Sending` | 是否发送 | `ON` |
| `Save & Apply` | 保存到 PlayerPrefs 并应用 | 按 trigger |
手柄操作：
- 摇杆上/下：选择行
- 摇杆左/右：调整端口、频率、坐标模式或开关发送
- Trigger / A / X：编辑当前行或触发当前行
- B / Y：保存并应用
- Menu：显示或隐藏面板
- Grip：ROS 侧遥操作使能；未按下时机械臂停止
验证现象：
- 面板中 `L ok / R ok` 表示 Unity 能读到左右手柄。
- `UDP Sending ON` 后，ROS2 的 `/xr/left_controller` 与 `/xr/right_controller` 应持续刷新。
- 按住 grip 并移动手柄时，mock 模式下 `/xr_rm/*/cmd_vel` 应出现非零速度。
- 松开 grip 后，`cmd_vel` 应回到零速度。
## 6. UDP JSON 协议
Unity APK 每个周期发送一个双手柄 JSON 包：
```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]
}
}
}
```
字段要求：
| 字段 | 类型 | 说明 |
| --- | --- | --- |
| `t` | number | `Time.realtimeSinceStartupAsDouble` |
| `frame_id` | string | 固定 `xr_world` |
| `controllers.left` | object | 左手柄 |
| `controllers.right` | object | 右手柄 |
| `grip` | bool | grip 使能，ROS 遥操作使用它启停 |
| `trigger` | float | `0.0-1.0` |
| `pos` | float[3] | `[x, y, z]` |
| `quat` | float[4] | `[qx, qy, qz, qw]` |
坐标转换必须与当前工程保持一致：
```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;
```
当禁用发送或 App 退出时，必须额外发一次左右手柄 `grip=false` 的包，让 ROS 侧安全停止。
## 7. 给代码代理的 Unity 生成 Prompt
下面整段可以直接交给 Codex 或 Claude Code，让它在一个空 Unity 工程中生成与当前 `XR_RM_PICO_UDP_Sender` 等价的项目。
```text
你是一个 Unity + PICO 4 Ultra + ROS2 UDP 遥操作项目的代码代理。请在 Ubuntu 22.04 上可打开和构建的 Unity Android 工程中，实现一个与 XR_RM_PICO_UDP_Sender 等价的 PICO UDP 发送端。
目标效果：
PICO 4 Ultra APK 运行后读取左右手柄 6DoF pose、grip、trigger，以 60 Hz 或用户配置频率向 ROS2 Ubuntu 主机发送 UDP JSON。ROS2 侧的 xr_rm_input/udp_controller_receiver 能直接解析，并发布 /xr/left_controller 与 /xr/right_controller。用户在 PICO 内可以通过世界空间 UI 面板修改 Target IP、Target Port、Send Rate、坐标转换模式、UDP Sending 开关，并保存到 PlayerPrefs。松开 grip、暂停发送或退出 App 时，ROS 侧必须能收到 grip=false 或因超时安全停止。
开发环境约束：
- Unity 版本优先使用 2022.3 LTS。
- Android 平台。
- PICO 4 Ultra。
- PICO Unity Integration SDK 使用本地 package 引用。
- 不引入额外第三方依赖。
- 使用 UnityEngine.XR.InputDevices / CommonUsages 获取手柄输入。
- 使用 System.Net.Sockets.UdpClient 发送 UDP。
- 使用 Unity JsonUtility 序列化 JSON。
- 使用内置 UGUI Text/Image/Canvas 生成配置面板，不依赖 TextMeshPro。
工程结构要求：
Assets/
Scripts/
PicoControllerUdpSender.cs
PicoUdpConfigPanel.cs
Editor/
XrRmUdpSenderProjectSetup.cs
Scenes/
Main.unity
Packages/manifest.json 需要包含：
{
"dependencies": {
"com.unity.xr.picoxr": "file:../../PICO-Unity-Integration-SDK-release_3.4.0",
"com.unity.xr.management": "4.4.0",
"com.unity.modules.androidjni": "1.0.0",
"com.unity.modules.animation": "1.0.0",
"com.unity.modules.imageconversion": "1.0.0",
"com.unity.modules.jsonserialize": "1.0.0",
"com.unity.modules.unitywebrequest": "1.0.0",
"com.unity.modules.unitywebrequesttexture": "1.0.0",
"com.unity.modules.xr": "1.0.0"
}
}
一、实现 PicoControllerUdpSender.cs
public 字段：
- host，默认 "192.168.9.89"
- port，默认 15000
- sendHz，默认 60.0f
- convertUnityToProjectCoordinates，默认 true
- sendOnStart，默认 true
只读状态属性：
- SendEnabled
- HasEndpoint
- LeftDeviceValid
- RightDeviceValid
- LastError
- LastSendTime
PlayerPrefs key：
- xr_rm_udp_sender.host
- xr_rm_udp_sender.port
- xr_rm_udp_sender.send_hz
- xr_rm_udp_sender.convert_coordinates
生命周期：
- OnEnable:
- LoadConfig()
- ReopenClient()
- sendEnabled = sendOnStart
- nextSendTime = 0
- Update:
- 如果 sendEnabled=false，直接 return
- 按 sendHz 节流
- packet.t = Time.realtimeSinceStartupAsDouble
- FillController(XRNode.LeftHand, left payload)
- FillController(XRNode.RightHand, right payload)
- SendPacket()
- OnDisable:
- 将左右 payload.grip=false
- SendPacket()
- 关闭 UdpClient
JSON 数据结构：
Packet:
- double t
- string frame_id = "xr_world"
- Controllers controllers
Controllers:
- ControllerPayload left
- ControllerPayload right
ControllerPayload:
- bool grip
- float trigger
- float[] pos = new float[] {0.0f, 1.0f, 0.0f}
- float[] quat = new float[] {0.0f, 0.0f, 0.0f, 1.0f}
ApplyConfig(newHost, newPort, newSendHz, newConvert, save):
- Trim host
- IPAddress.TryParse 校验 IP
- port 必须在 1-65535
- sendHz clamp 到 1-120
- 更新字段并 ReopenClient(address)
- save=true 时写 PlayerPrefs
- 失败时写 LastError 并返回 false
SetSending(enabled):
- 如果从 true 切到 false，先发送一次左右 grip=false
- 更新 sendEnabled
- nextSendTime = 0
FillController:
- InputDevice device = InputDevices.GetDeviceAtXRNode(node)
- 更新 LeftDeviceValid / RightDeviceValid
- device invalid 时 payload.grip=false, payload.trigger=0, return
- 读取：
- CommonUsages.devicePosition -> Vector3 position
- CommonUsages.deviceRotation -> Quaternion rotation
- CommonUsages.trigger -> float trigger
- CommonUsages.grip -> float gripValue
- CommonUsages.gripButton -> bool gripButton
- payload.grip = gripButton || gripValue > 0.5f
- payload.trigger = Mathf.Clamp01(trigger)
- 如果 convertUnityToProjectCoordinates=true:
- pos = [position.x, position.y, -position.z]
- quat = [-rotation.x, -rotation.y, rotation.z, rotation.w]
- 否则：
- pos = [position.x, position.y, position.z]
- quat = [rotation.x, rotation.y, rotation.z, rotation.w]
SendPacket:
- client 或 endpoint 为空时 return
- JsonUtility.ToJson(packet)
- UTF8 编码
- client.Send(bytes, bytes.Length, endpoint)
- 成功时 LastSendTime=Time.realtimeSinceStartupAsDouble，LastError=""
- 异常时 LastError=exception.Message
二、实现 PicoUdpConfigPanel.cs
功能：
- 和 PicoControllerUdpSender 放在同一个 GameObject。
- Awake 时查找 sender，缺失则 AddComponent。
- Start 时：
- Application.targetFrameRate = targetFrameRate，默认 90
- QualitySettings.vSyncCount = 0
- Screen.sleepTimeout = SleepTimeout.NeverSleep
- XRSettings.eyeTextureResolutionScale = 1.0f
- 创建世界空间配置面板
- 从 sender 加载 draft
- showOnStart 控制面板初始显示
- Update 时处理系统键盘、手柄输入、刷新面板。
public 字段：
- targetFrameRate = 90
- showOnStart = true
- togglePanelWithMenuButton = true
面板：
- 如果没有 Camera.main，创建 "XR-RM Main Camera"，tag=MainCamera，near=0.05，far=100，SolidColor 深色背景，并尽量添加 TrackedPoseDriver。
- 创建 "XR-RM UDP Config Canvas"，WorldSpace，挂在 camera 下，localPosition=(0,0,2)，localScale=0.0014，sizeDelta=(980,640)。
- 背景 Image 深色半透明。
- 标题："XR-RM PICO UDP Sender"
- 状态行显示：
- endpoint
- sending/paused
- L ok / --, R ok / --
- panelMessage 或 sender.LastError
- 6 行配置：
0. Target IP
1. Target Port
2. Send Rate
3. Coordinates
4. UDP Sending
5. Save & Apply
- 页脚：
"Stick Up/Down: select Stick Left/Right: adjust Trigger/A/X: edit B/Y: save Menu: show/hide"
输入：
- CommonUsages.menuButton：显示/隐藏面板。
- CommonUsages.primary2DAxis：
- y > 0.55 上移选择
- y < -0.55 下移选择
- x > 0.65 当前值增加或切换
- x < -0.65 当前值减少或切换
- 需要 AxisRepeatDelay=0.22f 防连发
- Trigger / primaryButton / triggerButton / trigger value > 0.75：编辑或激活当前行。
- secondaryButton：保存并应用。
编辑：
- Target IP 使用 TouchScreenKeyboardType.URL。
- Target Port 使用 NumberPad。
- Send Rate 使用 NumberPad。
- 端口 clamp 1-65535。
- Send Rate clamp 1-120。
- Coordinates 行切换 convertDraft。
- UDP Sending 行调用 sender.SetSending(!sender.SendEnabled)。
- Save & Apply 行调用 sender.ApplyConfig(hostDraft, portDraft, sendHzDraft, convertDraft, true)。
UI 文本：
- 使用 Resources.GetBuiltinResource<Font>("Arial.ttf")。
- Text 支持 wrap、truncate、best fit。
- selected 行用亮蓝色和 bold，普通行用浅色。
三、实现 XrRmUdpSenderProjectSetup.cs
仅在 UNITY_EDITOR 下编译。
菜单：
1. XR-RM/Apply UDP Sender Android Settings
2. XR-RM/Create Or Refresh UDP Sender Scene
3. XR-RM/Build Android APK
Apply UDP Sender Android Settings：
- PlayerSettings.productName = "XR-RM-PICO-UDP-Sender"
- Android application identifier = "com.local.xr_rm_udp_sender"
- minSdkVersion = AndroidApiLevel29
- targetSdkVersion = AndroidApiLevelAuto
- forceInternetPermission = true
- Scripting Backend = IL2CPP
- Android targetArchitectures = ARM64
- Graphics API = OpenGLES3
- SwitchActiveBuildTarget Android
- EnsurePicoSettings()
- EnablePicoLoader()
- CreateOrRefreshScene()
- EditorBuildSettings.scenes = Main.unity
- AssetDatabase.SaveAssets()
Create Or Refresh UDP Sender Scene：
- 确保 Assets/Scenes 存在。
- 打开或新建 Assets/Scenes/Main.unity。
- 找到或创建 GameObject "PicoControllerUdpSender"。
- 添加/配置 PicoControllerUdpSender：
- host="192.168.9.89"
- port=15000
- sendHz=60
- convertUnityToProjectCoordinates=true
- sendOnStart=true
- 添加/配置 PicoUdpConfigPanel：
- targetFrameRate=90
- showOnStart=true
- togglePanelWithMenuButton=true
- 保存 scene。
EnsurePicoSettings：
- 确保 Assets/XR/Settings 存在。
- 创建或加载 Assets/XR/Settings/PXR_Settings.asset。
- 默认 stereoRenderingModeAndroid = MultiPass。
- EditorBuildSettings.AddConfigObject("Unity.XR.PXR.Settings", settings, true)。
EnablePicoLoader：
- 查找或创建 XRGeneralSettingsPerBuildTarget。
- 获取 Android 的 XRGeneralSettings。
- 确保 XRManagerSettings 存在。
- 移除当前 active loaders。
- 使用 XRPackageMetadataStore.AssignLoader(generalSettings.Manager, "PXR_Loader", BuildTargetGroup.Android)。
- 失败时 Debug.LogWarning 提醒检查 PICO Integration package。
Build Android APK：
- 调用 ApplyAndroidSettings()
- 确保 Builds 目录存在。
- BuildPlayerOptions:
- scenes = Assets/Scenes/Main.unity
- locationPathName = Builds/XR-RM-PICO-UDP-Sender.apk
- target = Android
- targetGroup = Android
- options = None
- BuildPipeline.BuildPlayer(options)
- 如果 result 不是 Succeeded，throw Exception。
四、验收标准
1. Unity Editor 打开工程无编译错误。
2. XR-RM/Apply UDP Sender Android Settings 能执行。
3. XR-RM/Create Or Refresh UDP Sender Scene 后，Main.unity 中有 PicoControllerUdpSender 对象和两个组件。
4. XR-RM/Build Android APK 能生成 Builds/XR-RM-PICO-UDP-Sender.apk。
5. adb install -r 安装到 PICO 4 Ultra 后能启动。
6. PICO 内能看到世界空间配置面板。
7. 面板可修改 Target IP、Port、Send Rate、Coordinates、UDP Sending 并保存。
8. 左右手柄有效时面板显示 L ok / R ok。
9. UDP Sending ON 时，ROS2 端 udp_controller_receiver 能收到双手柄 JSON。
10. /xr/left_controller 和 /xr/right_controller 持续刷新。
11. 按住 grip 时 ROS2 遥操作链路输出速度；松开 grip、暂停发送、退出 App 时安全停止。
五、不要实现的内容
- 不实现 XRoboToolkit PC-Service。
- 不实现视频流。
- 不实现夹爪控制。
- 不实现手势识别、hand tracking 或 motion tracker。
- 不引入 XR Interaction Toolkit，除非 PICO SDK 必须要求。
- 不引入 TextMeshPro。
- 不改 ROS2 端协议。
```
## 8. 常见问题排查
### Unity 找不到 PICO package
检查 `Packages/manifest.json` 的相对路径是否和实际目录一致：
```text
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`。
### Gradle 或 Android 构建失败
优先检查：
1. Unity Editor 是否安装了 `Android Build Support`、`Android SDK & NDK Tools`、`OpenJDK`。
2. `Project Settings -> Player -> Android` 是否是 IL2CPP + ARM64。
3. 是否已切换到 Android Build Target。
4. 是否使用 Unity embedded SDK/NDK/JDK。
5. PICO SDK package 是否无编译错误。
### PICO 收不到 APK 或安装失败
```bash
adb kill-server
adb start-server
adb devices
adb install -r unity/XR_RM_PICO_UDP_Sender/Builds/XR-RM-PICO-UDP-Sender.apk
```
如果设备显示 `unauthorized`，在 PICO 内重新授权 USB 调试。
### ROS2 收不到 UDP
按顺序检查：
1. PICO 和 Ubuntu 是否同一个 Wi-Fi。
2. Target IP 是否是 Ubuntu 主机 IP，不是路由器 IP，也不是 PICO IP。
3. Target Port 是否是 `15000`。
4. `udp_controller_receiver` 是否已启动。
5. UFW 是否允许 UDP 15000。
6. PICO 面板是否显示 `UDP Sending ON`。
7. 面板是否显示 `L ok / R ok`。
可以临时关闭 ROS2 receiver，用 netcat 独占端口看是否有 UDP 包：
```bash
nc -ul 15000
```
注意：`nc` 和 ROS2 receiver 不能同时绑定同一个 UDP 端口。
### 有 UDP 但机械臂不动
先在 mock 模式验证：
```bash
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true
```
然后检查：
```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
```
如果 `/xr/*_controller` 有数据但 `cmd_vel` 为零，多半是 grip 没按下、UDP 超时、工作空间限幅、或 `xr_to_robot_matrix` 方向/尺度需要调整。
## 9. 参考链接
- 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`
- Unity 6.0 系统要求：`https://docs.unity3d.com/6000.0/Documentation/Manual/system-requirements.html`
- PICO 4 Ultra 开发资源：`https://developer.picoxr.com/pico4-ultra/`
- PICO Unity Integration SDK GitHub：`https://github.com/Pico-Developer/PICO-Unity-Integration-SDK`