docs: 添加手柄输入扩展实施计划

2026-07-30 20:13:47 +08:00
parent 378ba05be8
commit d84393f5cb
1 changed files with 650 additions and 0 deletions
@@ -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。