From f0c955ae66ebd65e885d4f7a705efd7fc21288bb Mon Sep 17 00:00:00 2001 From: YikaiFu-cart Date: Wed, 24 Jun 2026 21:18:18 +0800 Subject: [PATCH] feat: Add XRobotoolkit UDP bridge functionality --- README.md | 55 +- docs/pico_udp_sender_ubuntu22_setup.md | 545 ------------------ xr_rm_bringup/tools/launcher_ui.py | 17 +- xr_rm_input/setup.py | 1 + .../xr_rm_input/udp_controller_receiver.py | 2 +- .../xr_rm_input/xrobotoolkit_to_udp_bridge.py | 402 +++++++++++++ 6 files changed, 474 insertions(+), 548 deletions(-) delete mode 100644 docs/pico_udp_sender_ubuntu22_setup.md create mode 100644 xr_rm_input/xr_rm_input/xrobotoolkit_to_udp_bridge.py diff --git a/README.md b/README.md index d47f221..b152525 100755 --- a/README.md +++ b/README.md @@ -340,7 +340,7 @@ ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: fa - `pos`：手柄位置，长度 3。 - `quat`：手柄姿态四元数，默认按 `xyzw` 解析；遥操作节点会用 grip 锁定后的相对旋转控制 TCP 姿态。 - `pose_valid`：姿态是否可信。ROS 接收端看到 `false` 会强制 `grip=false`。 -- `pose_source`：`pxr_predict`、`unity_xr` 或 `none`，用于判断姿态来自 PICO 预测接口还是 Unity XR fallback。 +- `pose_source`：`pxr_predict`、`unity_xr`、`xrobotoolkit` 或 `none`，用于判断姿态来自 PICO 预测接口、Unity XR fallback 还是官方 XRoboToolkit SDK bridge。 - `tracking_state` / `controller_status`：Unity/PICO 侧追踪诊断值，只用于日志和排查。 - `grip_value`、`axis`、`buttons`：PICO 端输入诊断字段，当前不会写入 `XrController` 消息。 @@ -348,6 +348,59 @@ ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: fa PICO 4 Ultra 在 Ubuntu 22.04 下配置 Unity、构建 APK、安装到头显并向 ROS2 主机发送 UDP 的详细步骤见 [docs/pico_udp_sender_ubuntu22_setup.md](docs/pico_udp_sender_ubuntu22_setup.md)。 +## 官方 XRoboToolkit bridge + +如果使用官方 XRoboToolkit APK 和 PC-Service，可以用 `xrobotoolkit_to_udp_bridge` 从本机 ROS Python 环境中的 `xrobotoolkit_sdk` 读取左右手柄数据，再转换成当前 `udp_controller_receiver` 支持的 UDP JSON。 + +正式运行时不要同时启动官方 `PXREAClientUnity` / `RobotLinuxDemo` 可视化窗口。`/opt/apps/roboticsservice/run3D.sh` 会启动这个可视化 demo，适合单独确认 PICO 与 PC-Service 已连接；bridge 遥操作链路中只需要 PC-Service。 + +运行前只保留一个 UDP 输入源。先清掉重复 bridge、sample sender 和官方 Unity 可视化 demo，再保留或启动 PC-Service： + +```bash +pkill -f '[x]robotoolkit_to_udp_bridge' +pkill -f '[s]ample_udp_sender' +pkill -f '[R]obotLinuxDemo.x86_64' +pkill -f '[P]XREAClientUnity' +pgrep -af RoboticsServiceProcess || /opt/apps/roboticsservice/runService.sh +``` + +启动 ROS mock 接收链路： + +```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 +``` + +另开终端启动 bridge： + +```bash +cd /home/robot/WS_xr +source ~/.bashrc +source /opt/ros/humble/setup.bash +source install/setup.bash +ros2 run xr_rm_input xrobotoolkit_to_udp_bridge \ + --host 127.0.0.1 --port 15000 --hz 90 +``` + +bridge 默认对 grip/trigger 做轻量滞回：`grip` 按下阈值 `0.90`、松开阈值 `0.75`；`trigger` 按下阈值 `0.95`、松开阈值 `0.75`。启动日志会打印 PID、UDP endpoint 和阈值，便于确认当前只运行了一个 bridge。 + +验证手柄数据是否进入 ROS： + +```bash +ps -ef | grep -E 'xrobotoolkit_to_udp_bridge|sample_udp_sender|RobotLinuxDemo|PXREAClientUnity' | grep -v grep +ros2 topic hz /xr/left_controller +ros2 topic hz /xr/right_controller +ros2 topic echo /xr/left_controller --field pose.position +ros2 topic echo /xr/right_controller --field pose.position +ros2 topic echo /xr/left_controller --field grip +ros2 topic echo /xr/right_controller --field grip +ros2 topic echo /xr/right_controller --field trigger +``` + +`/xr/left_controller` 和 `/xr/right_controller` 持续刷新、位置随手柄移动变化、`grip` 随握持键切换，即表示官方 XRoboToolkit 数据已经进入当前遥操作输入层。 + ## 真机安全验证 第一次接真机时按这个顺序走： diff --git a/docs/pico_udp_sender_ubuntu22_setup.md b/docs/pico_udp_sender_ubuntu22_setup.md deleted file mode 100644 index f5ad30f..0000000 --- a/docs/pico_udp_sender_ubuntu22_setup.md +++ /dev/null @@ -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 -``` - -如果是 `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//cmd_vel` 为 0。 -- mock 下小角度转动手柄时，`/xr_rm//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` diff --git a/xr_rm_bringup/tools/launcher_ui.py b/xr_rm_bringup/tools/launcher_ui.py index 8f63263..cca5d86 100755 --- a/xr_rm_bringup/tools/launcher_ui.py +++ b/xr_rm_bringup/tools/launcher_ui.py @@ -33,6 +33,8 @@ DEFAULT_RIGHT_IP = "192.168.192.19" DEFAULT_PORT = "15000" SAMPLE_SENDER_SECONDS = "30" SAMPLE_SENDER_STAGGERED_SECONDS = "60" +XROBOTOOLKIT_BRIDGE_HZ = "90" +XROBOTOOLKIT_BRIDGE_EXECUTABLE = "xrobotoolkit_to_udp_bridge" SAMPLE_SENDER_ARGS = ( f"--host 127.0.0.1 --port {DEFAULT_PORT} " "--pattern axis_sweep --amplitude 0.5 " @@ -111,6 +113,13 @@ def _sample_udp_sender_command( return command +def _xrobotoolkit_bridge_command() -> str: + return ( + f"ros2 run xr_rm_input {XROBOTOOLKIT_BRIDGE_EXECUTABLE} " + f"--host 127.0.0.1 --port {DEFAULT_PORT} --hz {XROBOTOOLKIT_BRIDGE_HZ}" + ) + + def _tool_command(arm: str, open_tool: bool) -> str: arm_name = "left_rm75" if arm == "left" else "right_rm75" value = "true" if open_tool else "false" @@ -248,6 +257,7 @@ def build_commands_by_mode(mode: str) -> list[tuple[str, str]]: ("Left Arm Mock Launch", "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=true"), ("Right Arm Mock Launch", "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=true"), ("Dual Arm Mock Launch", "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true"), + ("XRobotoolkit UDP Bridge (90 Hz)", _xrobotoolkit_bridge_command()), ( "Sample UDP Sender (Left, 30s)", _sample_udp_sender_command("left"), @@ -282,6 +292,7 @@ def build_commands_by_mode(mode: str) -> list[tuple[str, str]]: f"left_robot_ip:={DEFAULT_LEFT_IP} " "move_to_initial_pose_on_connect:=false", ), + ("XRobotoolkit UDP Bridge (90 Hz)", _xrobotoolkit_bridge_command()), ("Left Tool Open", _tool_command("left", True)), ("Left Tool Close", _tool_command("left", False)), ( @@ -299,6 +310,7 @@ def build_commands_by_mode(mode: str) -> list[tuple[str, str]]: f"right_robot_ip:={DEFAULT_RIGHT_IP} " "move_to_initial_pose_on_connect:=false", ), + ("XRobotoolkit UDP Bridge (90 Hz)", _xrobotoolkit_bridge_command()), ("Right Tool Open", _tool_command("right", True)), ("Right Tool Close", _tool_command("right", False)), ( @@ -317,6 +329,7 @@ def build_commands_by_mode(mode: str) -> list[tuple[str, str]]: f"left_robot_ip:={DEFAULT_LEFT_IP} right_robot_ip:={DEFAULT_RIGHT_IP} " "move_to_initial_pose_on_connect:=false", ), + ("XRobotoolkit UDP Bridge (90 Hz)", _xrobotoolkit_bridge_command()), ( "Sample UDP Sender (Both Staggered, 60s)", _sample_udp_sender_command("both", SAMPLE_SENDER_STAGGERED_SECONDS, "staggered"), @@ -329,6 +342,7 @@ def build_commands_by_mode(mode: str) -> list[tuple[str, str]]: ("XR-RM Bringup Prefix", "ros2 pkg prefix xr_rm_bringup"), ("XR-RM Input Prefix", "ros2 pkg prefix xr_rm_input"), ("XR-RM Teleop Prefix", "ros2 pkg prefix xr_rm_teleop"), + ("XRobotoolkit UDP Bridge (90 Hz)", _xrobotoolkit_bridge_command()), ] one_click = None return _finalize_items(items, one_click) @@ -1277,7 +1291,7 @@ class LauncherApp: def stop_launched_processes(self, *, confirm: bool, notify: bool) -> bool: if confirm and not messagebox.askyesno( "Confirm Stop", - "Stop XR-RM launcher terminals, topic monitors, and ROS nodes started from this workspace?", + "Stop XR-RM launcher terminals, topic monitors, ROS nodes, and bridge processes started from this workspace?", ): return False @@ -1287,6 +1301,7 @@ class LauncherApp: "ros2 run xr_rm_input", "ros2 run xr_rm_teleop", "XR_RM_LAUNCHER_SESSION=1", + XROBOTOOLKIT_BRIDGE_EXECUTABLE, TERMINAL_TITLE_PREFIX, TOPIC_MONITOR_TITLE, CMD_VEL_MONITOR_TITLE, diff --git a/xr_rm_input/setup.py b/xr_rm_input/setup.py index f567f39..db6b8fc 100755 --- a/xr_rm_input/setup.py +++ b/xr_rm_input/setup.py @@ -29,6 +29,7 @@ setup( "console_scripts": [ "udp_controller_receiver = xr_rm_input.udp_controller_receiver:main", "sample_udp_sender = xr_rm_input.sample_udp_sender:main", + "xrobotoolkit_to_udp_bridge = xr_rm_input.xrobotoolkit_to_udp_bridge:main", ], }, ) diff --git a/xr_rm_input/xr_rm_input/udp_controller_receiver.py b/xr_rm_input/xr_rm_input/udp_controller_receiver.py index 4236be1..d4d6c06 100755 --- a/xr_rm_input/xr_rm_input/udp_controller_receiver.py +++ b/xr_rm_input/xr_rm_input/udp_controller_receiver.py @@ -18,7 +18,7 @@ from xr_rm_interfaces.msg import XrController class UdpControllerReceiver(Node): """将轻量级 XR UDP 数据包转换成 ROS2 左/右手柄消息。""" - _VALID_POSE_SOURCES = {"pxr_predict", "unity_xr", "none"} + _VALID_POSE_SOURCES = {"pxr_predict", "unity_xr", "xrobotoolkit", "none"} def __init__(self) -> None: super().__init__("udp_controller_receiver") diff --git a/xr_rm_input/xr_rm_input/xrobotoolkit_to_udp_bridge.py b/xr_rm_input/xr_rm_input/xrobotoolkit_to_udp_bridge.py new file mode 100644 index 0000000..869dd33 --- /dev/null +++ b/xr_rm_input/xr_rm_input/xrobotoolkit_to_udp_bridge.py @@ -0,0 +1,402 @@ +"""XRoboToolkit SDK 到当前 UDP controller JSON 协议的桥接脚本。 + +该脚本运行在安装了 `xrobotoolkit_sdk` 的 Python 环境中，从官方 +XRoboToolkit PC-Service SDK 读取 PICO 左右手柄 pose / grip / trigger， +再发送现有 `udp_controller_receiver` 已兼容的 UDP JSON 包。 +""" + +import argparse +import json +import math +import os +import socket +import sys +import time +from collections.abc import Callable, Sequence +from typing import Any + + +IDENTITY_QUAT = [0.0, 0.0, 0.0, 1.0] +ZERO_POS = [0.0, 0.0, 0.0] +POSE_SOURCE = "xrobotoolkit" + + +def _positive_float(name: str) -> Callable[[str], float]: + def parser(value: str) -> float: + number = float(value) + if number <= 0.0: + raise argparse.ArgumentTypeError(f"{name} must be > 0") + return number + + return parser + + +def _non_negative_int(name: str) -> Callable[[str], int]: + def parser(value: str) -> int: + number = int(value) + if number < 0: + raise argparse.ArgumentTypeError(f"{name} must be >= 0") + return number + + return parser + + +def _clamp_float(value: Any, low: float, high: float) -> float: + number = float(value) + return min(max(number, low), high) + + +class _HysteresisSwitch: + def __init__(self, *, press_threshold: float, release_threshold: float) -> None: + self.press_threshold = press_threshold + self.release_threshold = release_threshold + self.pressed = False + + def update(self, value: Any) -> bool: + analog = _clamp_float(value, 0.0, 1.0) + if self.pressed: + if analog <= self.release_threshold: + self.pressed = False + elif analog >= self.press_threshold: + self.pressed = True + return self.pressed + + +def _pose_to_pos_quat(pose: Any) -> tuple[list[float], list[float]]: + values = list(pose) + if len(values) != 7: + raise ValueError(f"expected XRoboToolkit pose with 7 values, got {len(values)}") + + numbers = [float(value) for value in values] + if not all(math.isfinite(value) for value in numbers): + raise ValueError(f"pose contains non-finite values: {numbers!r}") + return numbers[:3], numbers[3:7] + + +def _safe_axis(value: Any) -> list[float]: + try: + axis = [float(item) for item in list(value)] + except (TypeError, ValueError): + return [0.0, 0.0] + if len(axis) < 2: + return [0.0, 0.0] + return [_clamp_float(axis[0], -1.0, 1.0), _clamp_float(axis[1], -1.0, 1.0)] + + +def _safe_bool(func: Callable[[], Any], default: bool = False) -> bool: + try: + return bool(func()) + except Exception: + return default + + +def _controller_payload( + *, + hand: str, + pose: Any, + grip_value: Any, + trigger_value: Any, + axis: Any, + buttons: dict[str, bool], + grip_pressed: bool, + trigger_pressed: bool, + pose_valid: bool = True, +) -> dict[str, Any]: + pos, quat = _pose_to_pos_quat(pose) if pose_valid else (ZERO_POS.copy(), IDENTITY_QUAT.copy()) + grip_float = _clamp_float(grip_value, 0.0, 1.0) + trigger_float = _clamp_float(trigger_value, 0.0, 1.0) + return { + "hand": hand, + "grip": pose_valid and grip_pressed, + "trigger": 1.0 if pose_valid and trigger_pressed else 0.0, + "pos": pos, + "quat": quat, + "pose_valid": pose_valid, + "pose_source": POSE_SOURCE, + "grip_value": grip_float, + "trigger_value": trigger_float, + "axis": _safe_axis(axis), + "buttons": buttons, + } + + +def _stop_controller_payload(hand: str) -> dict[str, Any]: + return { + "hand": hand, + "grip": False, + "trigger": 0.0, + "pos": ZERO_POS.copy(), + "quat": IDENTITY_QUAT.copy(), + "pose_valid": False, + "pose_source": POSE_SOURCE, + "grip_value": 0.0, + "trigger_value": 0.0, + "axis": [0.0, 0.0], + "buttons": { + "grip": False, + "primary": False, + "secondary": False, + "menu": False, + "axis_click": False, + }, + } + + +def _packet(seq: int, frame_id: str, controllers: dict[str, dict[str, Any]]) -> dict[str, Any]: + now = time.monotonic() + return { + "t": now, + "source_time": now, + "seq": seq, + "frame_id": frame_id, + "controllers": controllers, + } + + +def _send_packet(sock: socket.socket, host: str, port: int, payload: dict[str, Any]) -> None: + sock.sendto(json.dumps(payload, separators=(",", ":")).encode("utf-8"), (host, port)) + + +def _send_stop_packets( + sock: socket.socket, + *, + host: str, + port: int, + frame_id: str, + seq: int, + count: int, + interval_sec: float, +) -> int: + controllers = { + "left": _stop_controller_payload("left"), + "right": _stop_controller_payload("right"), + } + for _ in range(count): + seq += 1 + _send_packet(sock, host, port, _packet(seq, frame_id, controllers)) + if interval_sec > 0.0: + time.sleep(interval_sec) + return seq + + +def _buttons_payload( + *, + grip: bool, + primary: Callable[[], Any], + secondary: Callable[[], Any], + menu: Callable[[], Any], + axis_click: Callable[[], Any], +) -> dict[str, bool]: + return { + "grip": grip, + "primary": _safe_bool(primary), + "secondary": _safe_bool(secondary), + "menu": _safe_bool(menu), + "axis_click": _safe_bool(axis_click), + } + + +def _parse_args(argv: Sequence[str] | None = None) -> argparse.Namespace: + parser = argparse.ArgumentParser( + description="Read PICO controller state from xrobotoolkit_sdk and send UDP JSON packets." + ) + parser.add_argument("--host", default="127.0.0.1", help="UDP receiver host.") + parser.add_argument("--port", type=int, default=15000, help="UDP receiver port.") + parser.add_argument("--hz", type=_positive_float("hz"), default=90.0, help="Send rate in Hz.") + parser.add_argument( + "--grip-threshold", + type=float, + default=0.9, + help="Grip analog press threshold used to set the stable boolean grip field.", + ) + parser.add_argument( + "--grip-release-threshold", + type=float, + default=0.75, + help="Grip analog release threshold. Must be <= --grip-threshold.", + ) + parser.add_argument( + "--trigger-threshold", + type=float, + default=0.95, + help="Trigger analog press threshold used to set the stable trigger field to 1.0.", + ) + parser.add_argument( + "--trigger-release-threshold", + type=float, + default=0.75, + help="Trigger analog release threshold. Must be <= --trigger-threshold.", + ) + parser.add_argument("--frame-id", default="xr_world", help="frame_id written into UDP packets.") + parser.add_argument( + "--stop-packets", + type=_non_negative_int("stop-packets"), + default=5, + help="Number of all-stop packets sent on shutdown or SDK read failure.", + ) + return parser.parse_args(argv) + + +def _import_xrobotoolkit_sdk(): + try: + import xrobotoolkit_sdk as xrt # type: ignore[import-not-found] + except ImportError as exc: + message = ( + "Failed to import xrobotoolkit_sdk. Install the XRoboToolkit SDK into " + "the ROS Python environment and set LD_LIBRARY_PATH, for example:\n" + " export LD_LIBRARY_PATH=/opt/apps/roboticsservice/SDK/x64:${LD_LIBRARY_PATH:-}\n" + " ros2 run xr_rm_input xrobotoolkit_to_udp_bridge" + ) + raise RuntimeError(message) from exc + return xrt + + +def _validate_threshold_pair(parser_name: str, press: float, release: float) -> None: + if release > press: + raise ValueError( + f"{parser_name} release threshold ({release:.2f}) must be <= press threshold ({press:.2f})" + ) + + +def main(argv: Sequence[str] | None = None) -> None: + args = _parse_args(argv) + args.grip_threshold = _clamp_float(args.grip_threshold, 0.0, 1.0) + args.grip_release_threshold = _clamp_float(args.grip_release_threshold, 0.0, 1.0) + args.trigger_threshold = _clamp_float(args.trigger_threshold, 0.0, 1.0) + args.trigger_release_threshold = _clamp_float(args.trigger_release_threshold, 0.0, 1.0) + _validate_threshold_pair("grip", args.grip_threshold, args.grip_release_threshold) + _validate_threshold_pair("trigger", args.trigger_threshold, args.trigger_release_threshold) + + grip_switches = { + "left": _HysteresisSwitch( + press_threshold=args.grip_threshold, + release_threshold=args.grip_release_threshold, + ), + "right": _HysteresisSwitch( + press_threshold=args.grip_threshold, + release_threshold=args.grip_release_threshold, + ), + } + trigger_switches = { + "left": _HysteresisSwitch( + press_threshold=args.trigger_threshold, + release_threshold=args.trigger_release_threshold, + ), + "right": _HysteresisSwitch( + press_threshold=args.trigger_threshold, + release_threshold=args.trigger_release_threshold, + ), + } + + stop_interval_sec = min(1.0 / args.hz, 0.02) + sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + seq = 0 + xrt = _import_xrobotoolkit_sdk() + sdk_initialized = False + + try: + xrt.init() + sdk_initialized = True + print( + "XRoboToolkit UDP bridge started: " + f"pid={os.getpid()}, endpoint={args.host}:{args.port}, hz={args.hz:.1f}, " + f"grip={args.grip_threshold:.2f}/{args.grip_release_threshold:.2f}, " + f"trigger={args.trigger_threshold:.2f}/{args.trigger_release_threshold:.2f}", + flush=True, + ) + + dt = 1.0 / args.hz + next_tick = time.monotonic() + while True: + try: + left_grip_value = xrt.get_left_grip() + right_grip_value = xrt.get_right_grip() + left_trigger_value = xrt.get_left_trigger() + right_trigger_value = xrt.get_right_trigger() + left_grip = grip_switches["left"].update(left_grip_value) + right_grip = grip_switches["right"].update(right_grip_value) + left_trigger = trigger_switches["left"].update(left_trigger_value) + right_trigger = trigger_switches["right"].update(right_trigger_value) + controllers = { + "left": _controller_payload( + hand="left", + pose=xrt.get_left_controller_pose(), + grip_value=left_grip_value, + trigger_value=left_trigger_value, + axis=xrt.get_left_axis(), + buttons=_buttons_payload( + grip=left_grip, + primary=xrt.get_X_button, + secondary=xrt.get_Y_button, + menu=xrt.get_left_menu_button, + axis_click=xrt.get_left_axis_click, + ), + grip_pressed=left_grip, + trigger_pressed=left_trigger, + ), + "right": _controller_payload( + hand="right", + pose=xrt.get_right_controller_pose(), + grip_value=right_grip_value, + trigger_value=right_trigger_value, + axis=xrt.get_right_axis(), + buttons=_buttons_payload( + grip=right_grip, + primary=xrt.get_A_button, + secondary=xrt.get_B_button, + menu=xrt.get_right_menu_button, + axis_click=xrt.get_right_axis_click, + ), + grip_pressed=right_grip, + trigger_pressed=right_trigger, + ), + } + except Exception: + print( + "XRoboToolkit SDK read failed; sending stop packets.", + file=sys.stderr, + flush=True, + ) + seq = _send_stop_packets( + sock, + host=args.host, + port=args.port, + frame_id=args.frame_id, + seq=seq, + count=args.stop_packets, + interval_sec=stop_interval_sec, + ) + raise + + seq += 1 + _send_packet(sock, args.host, args.port, _packet(seq, args.frame_id, controllers)) + next_tick += dt + sleep_sec = next_tick - time.monotonic() + if sleep_sec > 0.0: + time.sleep(sleep_sec) + else: + next_tick = time.monotonic() + + except KeyboardInterrupt: + print("XRoboToolkit UDP bridge interrupted; sending stop packets.", flush=True) + finally: + seq = _send_stop_packets( + sock, + host=args.host, + port=args.port, + frame_id=args.frame_id, + seq=seq, + count=args.stop_packets, + interval_sec=stop_interval_sec, + ) + if sdk_initialized: + try: + xrt.close() + except Exception as exc: + print(f"XRoboToolkit SDK close failed: {exc}", file=sys.stderr, flush=True) + sock.close() + print(f"XRoboToolkit UDP bridge stopped at seq={seq}.", flush=True) + + +if __name__ == "__main__": + main()