YikaiFu-cart/acRealman_xr
1
1
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
YikaiFu-cart
2026-07-30 20:13:47 +08:00
parent 378ba05be8
commit d84393f5cb
1 changed files with 650 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
docs/superpowers/plans/2026-07-30-xrobotoolkit-controller-inputs.md
+650
View File
@@ -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。