# 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。