Files

T

YikaiFu-cart d84393f5cb docs: 添加手柄输入扩展实施计划

2026-07-30 20:13:47 +08:00

19 KiB

Raw Permalink Blame History

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: 记录旧接口缺少新增字段

在工作空间根目录执行：

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 完整替换为：

std_msgs/Header header
string hand

bool grip
float32 trigger
bool primary
bool secondary
float32[2] axis

geometry_msgs/Pose pose

Step 3: 构建消息包

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: 验证生成接口字段和顺序

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: 提交消息接口

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：

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: 运行测试并确认失败

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 改为：

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 的输入部分改为：

        "axis": [0.0, 0.0],
        "buttons": {
            "primary": False,
            "secondary": False,
        },

并删除 grip_value、trigger_value、buttons.grip、buttons.menu 和 buttons.axis_click 输出。

将 _buttons_payload 改为：

def _buttons_payload(
    *,
    primary: Callable[[], Any],
    secondary: Callable[[], Any],
) -> dict[str, bool]:
    return {
        "primary": _safe_bool(primary),
        "secondary": _safe_bool(secondary),
    }

修改主循环的左手调用：

                    "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,
                    ),

修改主循环的右手调用：

                    "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 导入区增加：

import math

在 _payload_to_msg 中读取新增可选字段：

        axis = self._optional_axis(payload.get("axis"))
        primary, secondary = self._optional_buttons(payload.get("buttons"))

在现有 msg.grip 和 msg.trigger 赋值后加入：

        msg.primary = primary
        msg.secondary = secondary
        msg.axis = axis

在 _vector3 附近增加两个无状态解析方法：

    @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: 运行新增测试

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 语法检查

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 和测试

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 的环境准备之前增加：

## 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 包”，并将示例替换为：

{
  "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"
    }
  }
}

字段说明更新为：

- `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 提示替换为：

- 确认 `xrobotoolkit_to_udp_bridge` 没有持续打印 SDK read failed；SDK
  读取失败时 bridge 会发送 `pose_valid=false` 的停止包。

Step 3: 更新 AGENTS 的 Superpowers Git 规则

将 AGENTS“Git 与提交”中的 Superpowers 段落改为：

使用 Superpowers 执行任务时，只允许按相关 skill 工作流创建本地 Git 提交；
禁止执行 `git push`、合并本地分支、合并 PR 或其他远程写操作。相关 skill
如需独立 worktree 或配套本地分支，可以创建，但不得将其合并到其他分支。
其他情况下，除非用户明确要求，不要自动创建分支。

Step 4: 运行输入包测试

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: 运行完整工作空间构建

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 后验证接口和遥操作回归

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 验证统一启动入口

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 和格式

cd /home/robot/WS_xr/src
git diff --check
git status --short

Expected: git diff --check 无输出；只剩 README、AGENTS 的计划内文档改动。

Step 9: 提交文档

cd /home/robot/WS_xr/src
git add README.md AGENTS.md
git commit -m "docs: 更新手柄输入与 Superpowers 规则"

Expected: 只提交 README 和 AGENTS；不执行 push 或 merge。

19 KiB Raw Permalink Blame History Unescape Escape

XRoboToolkit 手柄输入扩展 Implementation Plan

Global Constraints

Task 1: 扩展 XrController 消息接口

Task 2: 透传摇杆与主副按键

Task 3: 更新文档并完成工作空间验证

19 KiB

Raw Permalink Blame History

Task 1: 扩展 `XrController` 消息接口