diff --git a/docs/superpowers/plans/2026-07-30-xrobotoolkit-controller-inputs.md b/docs/superpowers/plans/2026-07-30-xrobotoolkit-controller-inputs.md new file mode 100644 index 0000000..cc24d49 --- /dev/null +++ b/docs/superpowers/plans/2026-07-30-xrobotoolkit-controller-inputs.md @@ -0,0 +1,650 @@ +# XRoboToolkit 手柄输入扩展 Implementation Plan + +> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking. + +**Goal:** 将 XRoboToolkit 左右手柄摇杆、主键和副键加入现有 `XrController` 链路,同时保持 Grip、Trigger、位姿和遥操作行为不变。 + +**Architecture:** 直接扩展现有 ROS2 消息,继续使用当前 bridge 的嵌套 UDP `buttons` 结构,由 receiver 将按钮展平到消息字段。新增字段按可选输入解析,旧数据包或非法新增字段回退到安全默认值,不新增话题、依赖或控制分支。 + +**Tech Stack:** Ubuntu 22.04、ROS2 Humble、Python 3.10、ament/colcon、pytest、XRoboToolkit PC-Service Python binding。 + +## Global Constraints + +- 所有构建、测试和运行命令在 `/home/robot/WS_xr` 执行,并先运行 `source /opt/ros/humble/setup.bash`。 +- `XrController.msg` 的字段和顺序必须与批准的设计完全一致,不增加菜单键、摇杆按键、模拟 Grip/Trigger、SDK 时间戳或序号。 +- 保持现有 `grip`、`trigger`、`pose` 语义及控制行为不变。 +- 不新增依赖、ROS2 话题、节点或 LeRobot 录制实现。 +- 不修改 `xr_rm_teleop` 控制代码、机械臂 YAML、安全限位、超时或停止逻辑。 +- 启动验证只使用 `use_mock:=true`,不得连接真机、移动机械臂或操作夹爪。 +- 新增和修改的 Markdown 文档使用中文。 +- Superpowers 工作流只允许创建本地 Git 提交;禁止 push、合并本地分支、合并 PR 或执行任何远程写操作。 +- 每次提交只暂存当前任务列出的文件,不包含用户的其他工作区改动。 + +--- + +### Task 1: 扩展 `XrController` 消息接口 + +**Files:** +- Modify: `src/xr_rm_interfaces/msg/XrController.msg` + +**Interfaces:** +- Consumes: 现有 `std_msgs/Header`、`geometry_msgs/Pose` 和 `hand/grip/trigger/pose` 字段。 +- Produces: `XrController.primary: bool`、`secondary: bool`、`axis: float32[2]`,供 Task 2 的 receiver 赋值。 + +- [ ] **Step 1: 记录旧接口缺少新增字段** + +在工作空间根目录执行: + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +source install/setup.bash +python3 - <<'PY' +from xr_rm_interfaces.msg import XrController + +expected = [ + "header", + "hand", + "grip", + "trigger", + "primary", + "secondary", + "axis", + "pose", +] +actual = list(XrController.get_fields_and_field_types()) +assert actual == expected, actual +PY +``` + +Expected: FAIL;旧接口输出 +`['header', 'hand', 'grip', 'trigger', 'pose']`。 + +- [ ] **Step 2: 用批准的顺序修改消息定义** + +将 `src/xr_rm_interfaces/msg/XrController.msg` 完整替换为: + +```text +std_msgs/Header header +string hand + +bool grip +float32 trigger +bool primary +bool secondary +float32[2] axis + +geometry_msgs/Pose pose +``` + +- [ ] **Step 3: 构建消息包** + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +colcon build --symlink-install --packages-select xr_rm_interfaces +``` + +Expected: `xr_rm_interfaces` 构建成功,无 rosidl 错误。 + +- [ ] **Step 4: 验证生成接口字段和顺序** + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +source install/setup.bash +python3 - <<'PY' +from xr_rm_interfaces.msg import XrController + +expected = [ + "header", + "hand", + "grip", + "trigger", + "primary", + "secondary", + "axis", + "pose", +] +actual = list(XrController.get_fields_and_field_types()) +assert actual == expected, actual +assert XrController.get_fields_and_field_types()["axis"] == "float[2]" +PY +``` + +Expected: PASS,无输出。 + +- [ ] **Step 5: 提交消息接口** + +```bash +cd /home/robot/WS_xr/src +git add xr_rm_interfaces/msg/XrController.msg +git commit -m "feat: 扩展 XR 手柄消息" +``` + +Expected: 只提交 `XrController.msg`;不执行 push 或 merge。 + +--- + +### Task 2: 透传摇杆与主副按键 + +**Files:** +- Create: `src/xr_rm_input/test/test_controller_fields.py` +- Modify: `src/xr_rm_input/xr_rm_input/xrobotoolkit_to_udp_bridge.py` +- Modify: `src/xr_rm_input/xr_rm_input/udp_controller_receiver.py` + +**Interfaces:** +- Consumes: Task 1 生成的 `XrController.primary`、`secondary`、`axis`。 +- Produces: + - `_buttons_payload(*, primary: Callable[[], Any], secondary: Callable[[], Any]) -> dict[str, bool]` + - `_controller_payload(*, hand: str, pose: Any, axis: Any, buttons: dict[str, bool], grip_pressed: bool, trigger_pressed: bool, pose_valid: bool = True) -> dict[str, Any]` + - `UdpControllerReceiver._optional_axis(value: Any) -> list[float]` + - `UdpControllerReceiver._optional_buttons(value: Any) -> tuple[bool, bool]` + +- [ ] **Step 1: 新增失败测试** + +创建 `src/xr_rm_input/test/test_controller_fields.py`: + +```python +import math +from types import SimpleNamespace + +from builtin_interfaces.msg import Time +from xr_rm_input.udp_controller_receiver import UdpControllerReceiver +from xr_rm_input.xrobotoolkit_to_udp_bridge import ( + _buttons_payload, + _controller_payload, + _stop_controller_payload, +) + + +def _receiver_without_socket() -> UdpControllerReceiver: + receiver = object.__new__(UdpControllerReceiver) + receiver._quat_order = "xyzw" + receiver.get_clock = lambda: SimpleNamespace( + now=lambda: SimpleNamespace(to_msg=lambda: Time()) + ) + return receiver + + +def test_bridge_payload_contains_only_selected_controller_inputs() -> None: + buttons = _buttons_payload( + primary=lambda: True, + secondary=lambda: False, + ) + payload = _controller_payload( + hand="left", + pose=[1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0], + axis=[2.0, -2.0], + buttons=buttons, + grip_pressed=True, + trigger_pressed=False, + ) + + assert payload == { + "hand": "left", + "grip": True, + "trigger": 0.0, + "pos": [1.0, 2.0, 3.0], + "quat": [0.0, 0.0, 0.0, 1.0], + "pose_valid": True, + "pose_source": "xrobotoolkit", + "axis": [1.0, -1.0], + "buttons": { + "primary": True, + "secondary": False, + }, + } + + +def test_stop_payload_uses_neutral_selected_inputs() -> None: + payload = _stop_controller_payload("right") + + assert payload["axis"] == [0.0, 0.0] + assert payload["buttons"] == { + "primary": False, + "secondary": False, + } + assert "grip_value" not in payload + assert "trigger_value" not in payload + + +def test_receiver_publishes_selected_controller_inputs() -> None: + msg = _receiver_without_socket()._payload_to_msg( + { + "grip": True, + "trigger": 1.0, + "axis": [2.0, -2.0], + "buttons": { + "primary": True, + "secondary": False, + }, + "pos": [1.0, 2.0, 3.0], + "quat": [0.0, 0.0, 0.0, 1.0], + }, + "left", + ) + + assert msg.primary is True + assert msg.secondary is False + assert list(msg.axis) == [1.0, -1.0] + + +def test_receiver_defaults_invalid_optional_inputs() -> None: + msg = _receiver_without_socket()._payload_to_msg( + { + "grip": True, + "trigger": 0.4, + "axis": [math.nan, 0.0], + "buttons": [], + "pos": [1.0, 2.0, 3.0], + "quat": [0.0, 0.0, 0.0, 1.0], + }, + "right", + ) + + assert msg.primary is False + assert msg.secondary is False + assert list(msg.axis) == [0.0, 0.0] + assert msg.grip is True + assert abs(msg.trigger - 0.4) < 1e-6 + assert msg.pose.position.x == 1.0 + assert msg.pose.position.y == 2.0 + assert msg.pose.position.z == 3.0 + + +def test_receiver_defaults_missing_legacy_optional_inputs() -> None: + msg = _receiver_without_socket()._payload_to_msg( + { + "grip": True, + "trigger": 0.0, + "pos": [0.0, 1.0, 0.0], + "quat": [0.0, 0.0, 0.0, 1.0], + }, + "left", + ) + + assert msg.primary is False + assert msg.secondary is False + assert list(msg.axis) == [0.0, 0.0] + assert msg.grip is True +``` + +- [ ] **Step 2: 运行测试并确认失败** + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +source install/setup.bash +pytest src/xr_rm_input/test/test_controller_fields.py -v +``` + +Expected: FAIL;旧 `_buttons_payload` 仍要求 `grip/menu/axis_click`,且 receiver +尚无 `_optional_axis` 和 `_optional_buttons`。 + +- [ ] **Step 3: 精简 bridge payload** + +在 `xrobotoolkit_to_udp_bridge.py` 中将 `_controller_payload` 改为: + +```python +def _controller_payload( + *, + hand: str, + pose: 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()) + ) + 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, + "axis": _safe_axis(axis), + "buttons": buttons, + } +``` + +将 `_stop_controller_payload` 的输入部分改为: + +```python + "axis": [0.0, 0.0], + "buttons": { + "primary": False, + "secondary": False, + }, +``` + +并删除 `grip_value`、`trigger_value`、`buttons.grip`、`buttons.menu` 和 +`buttons.axis_click` 输出。 + +将 `_buttons_payload` 改为: + +```python +def _buttons_payload( + *, + primary: Callable[[], Any], + secondary: Callable[[], Any], +) -> dict[str, bool]: + return { + "primary": _safe_bool(primary), + "secondary": _safe_bool(secondary), + } +``` + +修改主循环的左手调用: + +```python + "left": _controller_payload( + hand="left", + pose=xrt.get_left_controller_pose(), + axis=xrt.get_left_axis(), + buttons=_buttons_payload( + primary=xrt.get_X_button, + secondary=xrt.get_Y_button, + ), + grip_pressed=left_grip, + trigger_pressed=left_trigger, + ), +``` + +修改主循环的右手调用: + +```python + "right": _controller_payload( + hand="right", + pose=xrt.get_right_controller_pose(), + axis=xrt.get_right_axis(), + buttons=_buttons_payload( + primary=xrt.get_A_button, + secondary=xrt.get_B_button, + ), + grip_pressed=right_grip, + trigger_pressed=right_trigger, + ), +``` + +保留主循环中 `get_left/right_grip()`、`get_left/right_trigger()` 和现有滞回 +开关;仅从 `_controller_payload` 参数及 UDP 输出中删除原始模拟量。 + +- [ ] **Step 4: 为 receiver 增加容错解析** + +在 `udp_controller_receiver.py` 导入区增加: + +```python +import math +``` + +在 `_payload_to_msg` 中读取新增可选字段: + +```python + axis = self._optional_axis(payload.get("axis")) + primary, secondary = self._optional_buttons(payload.get("buttons")) +``` + +在现有 `msg.grip` 和 `msg.trigger` 赋值后加入: + +```python + msg.primary = primary + msg.secondary = secondary + msg.axis = axis +``` + +在 `_vector3` 附近增加两个无状态解析方法: + +```python + @staticmethod + def _optional_axis(value: Any) -> list[float]: + try: + axis = [float(item) for item in value] + except (TypeError, ValueError): + return [0.0, 0.0] + if len(axis) != 2 or not all(math.isfinite(item) for item in axis): + return [0.0, 0.0] + return [ + min(max(axis[0], -1.0), 1.0), + min(max(axis[1], -1.0), 1.0), + ] + + @classmethod + def _optional_buttons(cls, value: Any) -> tuple[bool, bool]: + if not isinstance(value, Mapping): + return False, False + return ( + cls._as_bool(value.get("primary", False)), + cls._as_bool(value.get("secondary", False)), + ) +``` + +不要把新增字段加入现有 pose 诊断条件;它们无效时不得改变 `grip`。 + +- [ ] **Step 5: 运行新增测试** + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +source install/setup.bash +pytest src/xr_rm_input/test/test_controller_fields.py -v +``` + +Expected: `5 passed`。 + +- [ ] **Step 6: 运行 Python 语法检查** + +```bash +cd /home/robot/WS_xr +python3 -m py_compile \ + src/xr_rm_input/xr_rm_input/xrobotoolkit_to_udp_bridge.py \ + src/xr_rm_input/xr_rm_input/udp_controller_receiver.py \ + src/xr_rm_input/test/test_controller_fields.py +``` + +Expected: PASS,无输出。 + +- [ ] **Step 7: 提交 bridge、receiver 和测试** + +```bash +cd /home/robot/WS_xr/src +git add \ + xr_rm_input/xr_rm_input/xrobotoolkit_to_udp_bridge.py \ + xr_rm_input/xr_rm_input/udp_controller_receiver.py \ + xr_rm_input/test/test_controller_fields.py +git commit -m "feat: 发布 XR 手柄摇杆与按键" +``` + +Expected: 只提交列出的三个文件;不执行 push 或 merge。 + +--- + +### Task 3: 更新文档并完成工作空间验证 + +**Files:** +- Modify: `src/README.md` +- Modify: `src/AGENTS.md` + +**Interfaces:** +- Consumes: Task 1 的最终 `XrController` 格式和 Task 2 的 UDP JSON。 +- Produces: 当前手柄接口说明,以及对后续 Superpowers 任务生效的本地 Git 边界。 + +- [ ] **Step 1: 更新 README 的 Git 约束** + +在 README 的环境准备之前增加: + +```markdown +## Superpowers Git 约束 + +使用 Superpowers 执行任务时,只允许按 skill 工作流创建本地 Git 提交。 +禁止执行 `git push`、合并本地分支、合并 PR 或其他远程写操作。skill 如需 +独立 worktree 或配套本地分支,可以创建,但不得将其合并到其他分支。 +``` + +- [ ] **Step 2: 更新 README 的当前 UDP 示例** + +先修正 README 顶部的当前范围和项目结构: + +- 将“自定义 PICO 4 Ultra UDP Sender Unity 工程”完成项替换为 + “XRoboToolkit bridge 读取左右手柄 pose、Grip、Trigger、摇杆和主副按键”。 +- 从项目结构树删除当前仓库中不存在的 + `docs/pico_udp_sender_ubuntu22_setup.md` 和整个 `unity/` 子树。 +- 保留官方 XRoboToolkit APK、PC-Service、`PXREAClientUnity` 和 + `RobotLinuxDemo` 的运行说明;这些是外部工具,不是仓库内已删除的 Unity 工程。 + +将“UDP 数据格式”开头改为“当前 XRoboToolkit bridge 每个周期发送一个双手柄 +JSON 包”,并将示例替换为: + +```json +{ + "t": 12.345, + "source_time": 12.345, + "seq": 42, + "frame_id": "xr_world", + "controllers": { + "left": { + "hand": "left", + "grip": true, + "trigger": 0.0, + "axis": [0.2, -0.4], + "buttons": { + "primary": true, + "secondary": false + }, + "pos": [-0.12, 1.05, 0.30], + "quat": [0.0, 0.0, 0.0, 1.0], + "pose_valid": true, + "pose_source": "xrobotoolkit" + }, + "right": { + "hand": "right", + "grip": true, + "trigger": 1.0, + "axis": [-0.1, 0.3], + "buttons": { + "primary": false, + "secondary": true + }, + "pos": [0.12, 1.05, 0.30], + "quat": [0.0, 0.0, 0.0, 1.0], + "pose_valid": true, + "pose_source": "xrobotoolkit" + } + } +} +``` + +字段说明更新为: + +```markdown +- `t` / `source_time`:bridge 的 PC 单调时间,用于诊断发送周期。 +- `seq`:bridge 递增的 UDP 包序号,bridge 重启后重新计数。 +- `frame_id`:默认 `xr_world`,会写入 `XrController.header.frame_id`。 +- `grip`:运动使能。`true` 时进入相对位姿控制,`false` 时停止。 +- `trigger`:经过 bridge 滞回处理的 `0.0/1.0` 值;上升沿切换对应夹爪状态。 +- `axis`:摇杆 `[x, y]`,每个分量限制在 `-1.0` 到 `1.0`。 +- `buttons.primary`:左手 X 键或右手 A 键。 +- `buttons.secondary`:左手 Y 键或右手 B 键。 +- `pos`:手柄位置,长度 3。 +- `quat`:手柄姿态四元数,默认按 `xyzw` 解析。 +- `pose_valid`:姿态是否可信;`false` 时接收端强制 `grip=false`。 +- `pose_source`:当前 bridge 使用 `xrobotoolkit`。 +``` + +补充说明:`axis`、`buttons.primary` 和 `buttons.secondary` 会进入 +`XrController`;旧 UDP 包缺少这些字段时分别回退为 `[0,0]`、`false` 和 +`false`。删除已经不存在的自定义 Unity 工程和安装文档链接,但保留 receiver +对旧格式字段的兼容说明。 + +将故障排查中的旧 Unity HUD 提示替换为: + +```markdown +- 确认 `xrobotoolkit_to_udp_bridge` 没有持续打印 SDK read failed;SDK + 读取失败时 bridge 会发送 `pose_valid=false` 的停止包。 +``` + +- [ ] **Step 3: 更新 AGENTS 的 Superpowers Git 规则** + +将 AGENTS“Git 与提交”中的 Superpowers 段落改为: + +```markdown +使用 Superpowers 执行任务时,只允许按相关 skill 工作流创建本地 Git 提交; +禁止执行 `git push`、合并本地分支、合并 PR 或其他远程写操作。相关 skill +如需独立 worktree 或配套本地分支,可以创建,但不得将其合并到其他分支。 +其他情况下,除非用户明确要求,不要自动创建分支。 +``` + +- [ ] **Step 4: 运行输入包测试** + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +source install/setup.bash +pytest src/xr_rm_input/test/test_controller_fields.py -v +``` + +Expected: `5 passed`。 + +- [ ] **Step 5: 运行完整工作空间构建** + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +colcon build --symlink-install +``` + +Expected: `xr_rm_interfaces`、`xr_rm_input`、`xr_rm_teleop` 和 +`xr_rm_bringup` 全部构建成功。 + +- [ ] **Step 6: 重新 source 后验证接口和遥操作回归** + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +source install/setup.bash +ros2 interface show xr_rm_interfaces/msg/XrController +pytest src/xr_rm_teleop/test/test_orientation_control.py -v +``` + +Expected: 接口按 `header/hand/grip/trigger/primary/secondary/axis/pose` 顺序 +显示;姿态控制测试全部通过。 + +- [ ] **Step 7: 使用 mock 验证统一启动入口** + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +source install/setup.bash +timeout --signal=INT 8s ros2 launch xr_rm_bringup arm_debug.launch.py \ + arm:=right use_mock:=true +``` + +Expected: `udp_controller_receiver` 和 `single_arm_velocity_teleop` 正常启动; +不出现消息类型、Placo 或 traceback 错误。`timeout` 到期退出属于预期。 + +- [ ] **Step 8: 检查 diff 和格式** + +```bash +cd /home/robot/WS_xr/src +git diff --check +git status --short +``` + +Expected: `git diff --check` 无输出;只剩 README、AGENTS 的计划内文档改动。 + +- [ ] **Step 9: 提交文档** + +```bash +cd /home/robot/WS_xr/src +git add README.md AGENTS.md +git commit -m "docs: 更新手柄输入与 Superpowers 规则" +``` + +Expected: 只提交 README 和 AGENTS;不执行 push 或 merge。