Compare commits
5
Commits
bbfb306903
...
d26ce7b945
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
d26ce7b945 | ||
|
|
1936adf2fd | ||
|
|
cbc18bed8a | ||
|
|
d84393f5cb | ||
|
|
378ba05be8 |
@@ -230,7 +230,10 @@
|
||||
|
||||
除非用户明确要求,否则不要自动提交、推送或修改远程仓库。
|
||||
|
||||
使用 Superpowers 执行计划时,允许 subagent 按相关 skill 创建和使用独立 worktree 及其配套本地分支;其他情况下,除非用户明确要求,不要自动创建分支。
|
||||
使用 Superpowers 执行任务时,只允许按相关 skill 工作流创建本地 Git 提交;
|
||||
禁止执行 `git push`、合并本地分支、合并 PR 或其他远程写操作。相关 skill
|
||||
如需独立 worktree 或配套本地分支,可以创建,但不得将其合并到其他分支。
|
||||
其他情况下,除非用户明确要求,不要自动创建分支。
|
||||
|
||||
如果用户要求生成提交信息,提交信息应:
|
||||
|
||||
|
||||
@@ -24,7 +24,7 @@ PICO/XR 双手柄 UDP JSON
|
||||
- Placo 0.9.4 RM75 QP 逆解;收到首帧有效关节反馈后才启用,求解失败时保留上一组有效关节目标。
|
||||
- 真机模式下,点击对应手柄 `trigger` 可切换并保持对应夹爪开/关状态。
|
||||
- Tkinter 启动面板 `launcher_ui.py`,用于现场快速启动、监控 topic、检查环境和清理进程。
|
||||
- 自定义 PICO 4 Ultra UDP Sender Unity 工程,负责发送左右手柄 pose、`grip`、`trigger` 和 pose 诊断字段。
|
||||
- XRoboToolkit bridge 读取左右手柄 pose、Grip、Trigger、摇杆和主副按键。
|
||||
|
||||
暂未完成:
|
||||
|
||||
@@ -37,17 +37,8 @@ PICO/XR 双手柄 UDP JSON
|
||||
```text
|
||||
src/
|
||||
├── README.md # 项目主文档
|
||||
├── CODEX.md # Codex/Claude Code 项目工作流和安全规则
|
||||
├── docs/
|
||||
│ └── pico_udp_sender_ubuntu22_setup.md # Ubuntu 22.04 下 PICO UDP Sender 配置教程
|
||||
├── unity/
|
||||
│ ├── XR_RM_PICO_UDP_Sender/ # PICO 4 Ultra UDP Sender Unity 工程
|
||||
│ │ ├── Assets/Editor/ # Android/PICO 设置与 APK 构建菜单
|
||||
│ │ ├── Assets/Scripts/ # UDP sender、配置面板、KeepAwake
|
||||
│ │ ├── Assets/Resources/ # PICO 资源与 Roboto TMP 字体
|
||||
│ │ ├── Packages/ # Unity package manifest
|
||||
│ │ └── ProjectSettings/
|
||||
│ └── PICO-Unity-Integration-SDK-release_3.4.0/
|
||||
├── AGENTS.md # Codex 项目工作流和安全规则
|
||||
├── docs/superpowers/ # Superpowers 设计与实施计划
|
||||
├── xr_rm_bringup/
|
||||
│ ├── config/
|
||||
│ │ ├── dual_arm_rm75.yaml # 双臂配置:left_arm_teleop 与 right_arm_teleop
|
||||
@@ -62,12 +53,15 @@ src/
|
||||
├── xr_rm_input/
|
||||
│ ├── launch/
|
||||
│ │ └── udp_receiver.launch.py # 低层 UDP 接收测试入口
|
||||
│ ├── test/
|
||||
│ │ └── test_controller_fields.py
|
||||
│ └── xr_rm_input/
|
||||
│ ├── udp_controller_receiver.py
|
||||
│ ├── xrobotoolkit_to_udp_bridge.py
|
||||
│ └── sample_udp_sender.py # 本机扫轴/正弦模拟手柄 UDP 数据
|
||||
├── xr_rm_interfaces/
|
||||
│ └── msg/
|
||||
│ └── XrController.msg # hand/grip/trigger/pose
|
||||
│ └── XrController.msg # 手柄状态与位姿
|
||||
└── xr_rm_teleop/
|
||||
├── models/
|
||||
│ ├── rm75/ # 旧 RM75 模型资源(launch 不再选用)
|
||||
@@ -81,6 +75,12 @@ src/
|
||||
|
||||
`single_arm_velocity_teleop` 这个名字保留是有意的:双臂模式不是一个大节点直接控制两台机械臂,而是启动两个相同的单臂控制节点,分别命名为 `left_arm_teleop` 和 `right_arm_teleop`。
|
||||
|
||||
## Superpowers Git 约束
|
||||
|
||||
使用 Superpowers 执行任务时,只允许按 skill 工作流创建本地 Git 提交。
|
||||
禁止执行 `git push`、合并本地分支、合并 PR 或其他远程写操作。skill 如需
|
||||
独立 worktree 或配套本地分支,可以创建,但不得将其合并到其他分支。
|
||||
|
||||
## 环境准备
|
||||
|
||||
在工作空间根目录,也就是包含 `src/` 的目录执行:
|
||||
@@ -268,15 +268,14 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false
|
||||
- 当前三份 YAML 默认均使用 `follow: false` 完成安全基线验证;确认关节加速度与反馈稳定后,再单独测试高跟随。
|
||||
- `initial_joint_pose`:mock 的初始关节反馈,以及显式开启初始化移动时的真机初始关节角。
|
||||
|
||||
当前 `/xr/*_controller` 的 Project 坐标约定:
|
||||
当前 `/xr/*_controller` 的坐标处理:
|
||||
|
||||
- Project:`+X` 向右,`+Y` 向上,`+Z` 向后。
|
||||
- Unity APK 的 `Project (+Z back)` 会把 PXR `pxr_predict` 原始坐标转换为 `project.x=native.z`、`project.y=native.y`、`project.z=-native.x`。
|
||||
- `Source raw` 模式保留原始 pose source 坐标,只用于现场对照。
|
||||
- XRoboToolkit bridge 原样转发 SDK 的手柄位置和四元数,不额外转换坐标轴。
|
||||
- receiver 默认按 `xyzw` 解析四元数,也可通过 `quat_order:=wxyz` 切换。
|
||||
- 左臂映射:机器人位移增量 = `[-手柄y, 手柄z, -手柄x]`。
|
||||
- 右臂映射:机器人位移增量 = `[手柄y, 手柄z, 手柄x]`。
|
||||
|
||||
如果 `/xr/*_controller.pose.position` 已符合 Project 坐标,但某个机械臂方向相反,只改对应臂的 `xr_to_robot_matrix` 符号,不要同时改多个控制参数。
|
||||
如果某个机械臂方向相反,只改对应臂的 `xr_to_robot_matrix` 符号,不要同时改多个控制参数。
|
||||
|
||||
## 末端工具开合
|
||||
|
||||
@@ -296,7 +295,7 @@ ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: fa
|
||||
|
||||
## UDP 数据格式
|
||||
|
||||
当前 Unity APK 每个周期发送一个双手柄 JSON 包:
|
||||
当前 XRoboToolkit bridge 每个周期发送一个双手柄 JSON 包:
|
||||
|
||||
```json
|
||||
{
|
||||
@@ -306,42 +305,32 @@ ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: fa
|
||||
"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": "pxr_predict",
|
||||
"tracking_state": 3,
|
||||
"controller_status": 2,
|
||||
"grip_value": 1.0,
|
||||
"axis": [0.0, 0.0],
|
||||
"buttons": {
|
||||
"grip": true,
|
||||
"primary": false,
|
||||
"secondary": false,
|
||||
"menu": false,
|
||||
"axis_click": false
|
||||
}
|
||||
"pose_source": "xrobotoolkit"
|
||||
},
|
||||
"right": {
|
||||
"hand": "right",
|
||||
"grip": true,
|
||||
"trigger": 0.4,
|
||||
"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": "unity_xr",
|
||||
"tracking_state": 3,
|
||||
"controller_status": -1,
|
||||
"grip_value": 0.8,
|
||||
"axis": [0.0, 0.0],
|
||||
"buttons": {
|
||||
"grip": true,
|
||||
"primary": false,
|
||||
"secondary": false,
|
||||
"menu": false,
|
||||
"axis_click": false
|
||||
}
|
||||
"pose_source": "xrobotoolkit"
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -349,21 +338,40 @@ ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: fa
|
||||
|
||||
字段说明:
|
||||
|
||||
- `t` / `source_time`:Unity 端 `Time.realtimeSinceStartupAsDouble`,用于后续延迟分析。
|
||||
- `seq`:Unity 端递增包序号,用于后续丢包分析。
|
||||
- `t` / `source_time`:bridge 的 PC 单调时间,用于诊断发送周期。
|
||||
- `seq`:bridge 递增的 UDP 包序号,bridge 重启后重新计数。
|
||||
- `frame_id`:默认 `xr_world`,会写入 `XrController.header.frame_id`。
|
||||
- `grip`:运动使能。`true` 时进入相对位姿控制,`false` 时停止。
|
||||
- `trigger`:扳机值,范围 `0.0-1.0`。真机模式下跨过 `0.95` 的上升沿会切换对应夹爪开/关状态。
|
||||
- `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` 解析;遥操作节点会用 grip 锁定后的相对旋转控制 TCP 姿态。
|
||||
- `pose_valid`:姿态是否可信。ROS 接收端看到 `false` 会强制 `grip=false`。
|
||||
- `pose_source`:`pxr_predict`、`unity_xr`、`xrobotoolkit` 或 `none`,用于判断姿态来自 PICO 预测接口、Unity XR fallback 还是官方 XRoboToolkit SDK bridge。
|
||||
- `tracking_state` / `controller_status`:Unity/PICO 侧追踪诊断值,只用于日志和排查。
|
||||
- `grip_value`、`axis`、`buttons`:PICO 端输入诊断字段,当前不会写入 `XrController` 消息。
|
||||
- `quat`:手柄姿态四元数,默认按 `xyzw` 解析。
|
||||
- `pose_valid`:姿态是否可信;`false` 时接收端强制 `grip=false`。
|
||||
- `pose_source`:当前 bridge 使用 `xrobotoolkit`。
|
||||
|
||||
`udp_controller_receiver` 仍兼容调试用的单手柄包:可以直接发送带 `hand`、`pos`、`quat` 的 JSON object,也可以用 `controllers` list、顶层 `left/right`、`pose.position`、`position`、`p`、`q` 等常见字段。四元数默认按 `xyzw` 解析,也可通过 `quat_order:=wxyz` 切换。
|
||||
`axis`、`buttons.primary` 和 `buttons.secondary` 会进入 `XrController`;旧 UDP
|
||||
包缺少这些字段时分别回退为 `[0,0]`、`false` 和 `false`。
|
||||
|
||||
PICO 4 Ultra 在 Ubuntu 22.04 下配置 Unity、构建 APK、安装到头显并向 ROS2 主机发送 UDP 的详细步骤见 [docs/pico_udp_sender_ubuntu22_setup.md](docs/pico_udp_sender_ubuntu22_setup.md)。
|
||||
接收端发布的消息格式为:
|
||||
|
||||
```text
|
||||
std_msgs/Header header
|
||||
string hand
|
||||
|
||||
bool grip
|
||||
float32 trigger
|
||||
bool primary
|
||||
bool secondary
|
||||
float32[2] axis
|
||||
|
||||
geometry_msgs/Pose pose
|
||||
```
|
||||
|
||||
`udp_controller_receiver` 仍兼容调试用的单手柄包:可以直接发送带 `hand`、`pos`、
|
||||
`quat` 的 JSON object,也可以用 `controllers` list、顶层 `left/right`、
|
||||
`pose.position`、`position`、`p`、`q` 等常见字段。
|
||||
|
||||
## 官方 XRoboToolkit bridge
|
||||
|
||||
@@ -466,7 +474,8 @@ Controller topic 没有数据:
|
||||
- 确认 UDP 发送端目标 IP 是运行 ROS2 的主机 IP。
|
||||
- 确认端口是 `15000`,或 launch 与发送端端口一致。
|
||||
- 用 `sample_udp_sender` 在本机验证接收链路。
|
||||
- 如果 Unity HUD 显示某个手柄 `invalid none`,ROS 侧会把该手柄 `grip` 强制置为 `false`。
|
||||
- 确认 `xrobotoolkit_to_udp_bridge` 没有持续打印 SDK read failed;SDK
|
||||
读取失败时 bridge 会发送 `pose_valid=false` 的停止包。
|
||||
|
||||
机械臂不动:
|
||||
|
||||
|
||||
@@ -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。
|
||||
@@ -0,0 +1,140 @@
|
||||
# XRoboToolkit 手柄输入扩展设计
|
||||
|
||||
## 背景
|
||||
|
||||
当前 `xrobotoolkit_to_udp_bridge` 已从 XRoboToolkit PC-Service SDK 读取左右
|
||||
手柄摇杆、主键和副键,但 `udp_controller_receiver` 只把 `grip`、`trigger`
|
||||
和位姿写入 `XrController`,其余信息在 UDP 到 ROS2 的转换中丢失。
|
||||
|
||||
后续项目会使用 LeRobot 同时记录相机、RM75 状态和手柄输入。本次只补齐当前
|
||||
明确需要的手柄字段,不实现 LeRobot 录制,不改变现有机械臂控制逻辑。
|
||||
|
||||
## 目标
|
||||
|
||||
- 将左右手柄摇杆、主键和副键发布到现有 `XrController` 话题。
|
||||
- 保持现有 `grip`、`trigger` 和 `pose` 的语义及控制行为不变。
|
||||
- 兼容不包含新增字段的旧 UDP 数据包。
|
||||
- 使用现有节点、消息和 UDP 协议,不增加依赖或新话题。
|
||||
- 更新 README 和 AGENTS,记录接口及 Superpowers 的 Git 操作边界。
|
||||
|
||||
## 不在本次范围
|
||||
|
||||
- Grip 和 Trigger 原始模拟量。
|
||||
- 菜单键、摇杆按键、SDK 时间戳和 bridge 序号。
|
||||
- 头显位姿、26 点手部骨骼、身体追踪和 Motion Tracker。
|
||||
- LeRobot 数据集录制、相机同步和 RM75 状态采集。
|
||||
- 任何机械臂控制参数、安全逻辑或真机行为修改。
|
||||
|
||||
## 方案选择
|
||||
|
||||
采用直接扩展 `XrController` 的方案。相比新增 `sensor_msgs/Joy` 话题,该方案
|
||||
不需要额外同步左右手柄话题;相比继续只保留 UDP JSON,它能让 ROS2 和后续
|
||||
LeRobot 适配层直接读取类型明确的数据。
|
||||
|
||||
修改消息定义后必须重新构建并重启相关节点。重新构建后的现有遥操作代码仍只读取
|
||||
原字段,不需要修改控制逻辑。
|
||||
|
||||
## ROS2 消息格式
|
||||
|
||||
`XrController.msg` 使用以下固定顺序:
|
||||
|
||||
```text
|
||||
std_msgs/Header header
|
||||
string hand
|
||||
|
||||
bool grip
|
||||
float32 trigger
|
||||
bool primary
|
||||
bool secondary
|
||||
float32[2] axis
|
||||
|
||||
geometry_msgs/Pose pose
|
||||
```
|
||||
|
||||
字段语义:
|
||||
|
||||
- `primary`:左手 X 键,右手 A 键。
|
||||
- `secondary`:左手 Y 键,右手 B 键。
|
||||
- `axis`:对应手柄摇杆的 `[x, y]`,每个分量限制在 `[-1.0, 1.0]`。
|
||||
|
||||
## 数据流
|
||||
|
||||
正常链路保持不变:
|
||||
|
||||
```text
|
||||
XRoboToolkit PC-Service SDK
|
||||
→ xrobotoolkit_to_udp_bridge
|
||||
→ UDP JSON
|
||||
→ udp_controller_receiver
|
||||
→ /xr/left_controller、/xr/right_controller
|
||||
→ single_arm_velocity_teleop
|
||||
```
|
||||
|
||||
bridge 继续读取 Grip 和 Trigger 模拟量并应用现有滞回,只是不再把未使用的
|
||||
`grip_value`、`trigger_value`、`menu` 和 `axis_click` 放入 UDP JSON。
|
||||
|
||||
UDP 中的按钮继续使用现有嵌套结构:
|
||||
|
||||
```json
|
||||
{
|
||||
"grip": true,
|
||||
"trigger": 0.0,
|
||||
"axis": [0.2, -0.4],
|
||||
"buttons": {
|
||||
"primary": true,
|
||||
"secondary": false
|
||||
},
|
||||
"pos": [0.0, 1.0, 0.0],
|
||||
"quat": [0.0, 0.0, 0.0, 1.0]
|
||||
}
|
||||
```
|
||||
|
||||
`udp_controller_receiver` 将嵌套按钮展平到 ROS2 消息字段。现有遥操作节点忽略
|
||||
新增字段,因此目标位姿、夹爪触发和安全停止路径均不变化。
|
||||
|
||||
## 兼容与异常处理
|
||||
|
||||
- 旧 UDP 包缺少 `axis` 或 `buttons` 时,发布
|
||||
`axis=[0.0, 0.0]`、`primary=false`、`secondary=false`。
|
||||
- 新增可选字段格式错误时使用上述默认值,不丢弃有效的 Grip、Trigger 和位姿。
|
||||
- bridge 和 receiver 均将摇杆分量限制在 `[-1.0, 1.0]`。
|
||||
- 旧包中存在 `menu`、`axis_click` 或其他按钮字段时忽略,不报错。
|
||||
- `sample_udp_sender` 保持旧格式,用它验证向后兼容,不为本次需求增加新参数。
|
||||
|
||||
## 文件范围
|
||||
|
||||
- `xr_rm_interfaces/msg/XrController.msg`
|
||||
- `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/` 下的一份最小兼容性测试
|
||||
- `README.md`
|
||||
- `AGENTS.md`
|
||||
|
||||
不修改 `xr_rm_teleop` 控制实现及三个机械臂 YAML。
|
||||
|
||||
## README 与 AGENTS 规则
|
||||
|
||||
README 增加新的手柄字段、UDP 格式和兼容行为说明。
|
||||
|
||||
AGENTS 和 README 同时明确:使用 Superpowers 执行任务时,只允许按 skill
|
||||
工作流创建本地 Git 提交;不得推送、合并或执行其他远程写操作。skill 如需本地
|
||||
worktree 或配套分支,可以创建,但不得将其合并到其他分支。
|
||||
|
||||
## 验证
|
||||
|
||||
自动验证包括:
|
||||
|
||||
- bridge 生成的 UDP payload 只包含确认保留的按钮和摇杆字段。
|
||||
- 左手 X/Y 与右手 A/B 正确映射到 `primary/secondary`。
|
||||
- receiver 正确发布新增字段。
|
||||
- 旧 UDP 包继续发布,新增字段使用默认值。
|
||||
- 非法新增字段不会阻断现有 Grip、Trigger 和位姿。
|
||||
- 在 `/home/robot/WS_xr` source ROS2 Humble 后运行相关 pytest。
|
||||
- 运行 `colcon build --symlink-install`。
|
||||
|
||||
运行验证只使用 mock,不连接真机、不移动机械臂、不操作夹爪。
|
||||
|
||||
## Git 边界
|
||||
|
||||
本设计和后续实现可以按 Superpowers 流程创建本地提交。禁止执行 `git push`、
|
||||
创建或合并 PR、合并本地分支以及任何远程写操作。
|
||||
@@ -0,0 +1,122 @@
|
||||
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
|
||||
@@ -1,10 +1,11 @@
|
||||
"""XR 手柄 UDP 接收节点。
|
||||
|
||||
从 UDP JSON 数据包中解析左右手柄位姿、握持键和扳机值,并发布为
|
||||
`xr_rm_interfaces/XrController` 消息,供遥操作和夹爪节点订阅。
|
||||
从 UDP JSON 数据包中解析左右手柄位姿、Grip、Trigger、摇杆和主副按键,
|
||||
并发布为 `xr_rm_interfaces/XrController` 消息,供遥操作和夹爪节点订阅。
|
||||
"""
|
||||
|
||||
import json
|
||||
import math
|
||||
import socket
|
||||
from collections.abc import Iterable, Mapping
|
||||
from typing import Any
|
||||
@@ -124,6 +125,8 @@ class UdpControllerReceiver(Node):
|
||||
pos, quat = self._extract_pose(payload)
|
||||
if len(pos) != 3 or len(quat) != 4:
|
||||
raise ValueError("expected pos[3] and quat[4]")
|
||||
axis = self._optional_axis(payload.get("axis"))
|
||||
primary, secondary = self._optional_buttons(payload.get("buttons"))
|
||||
|
||||
msg = XrController()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
@@ -144,6 +147,9 @@ class UdpControllerReceiver(Node):
|
||||
|
||||
msg.grip = grip
|
||||
msg.trigger = self._clamp_float(payload.get("trigger", 0.0), 0.0, 1.0)
|
||||
msg.primary = primary
|
||||
msg.secondary = secondary
|
||||
msg.axis = axis
|
||||
msg.pose.position.x = float(pos[0])
|
||||
msg.pose.position.y = float(pos[1])
|
||||
msg.pose.position.z = float(pos[2])
|
||||
@@ -213,6 +219,28 @@ class UdpControllerReceiver(Node):
|
||||
raise ValueError("expected 3D position")
|
||||
return [float(item) for item in vector]
|
||||
|
||||
@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)),
|
||||
)
|
||||
|
||||
def _quaternion(self, value: Any) -> list[float]:
|
||||
if isinstance(value, Mapping):
|
||||
if self._quat_order == "wxyz":
|
||||
|
||||
@@ -1,8 +1,8 @@
|
||||
"""XRoboToolkit SDK 到当前 UDP controller JSON 协议的桥接脚本。
|
||||
|
||||
该脚本运行在安装了 `xrobotoolkit_sdk` 的 Python 环境中,从官方
|
||||
XRoboToolkit PC-Service SDK 读取 PICO 左右手柄 pose / grip / trigger,
|
||||
再发送现有 `udp_controller_receiver` 已兼容的 UDP JSON 包。
|
||||
XRoboToolkit PC-Service SDK 读取 PICO 左右手柄 pose、Grip、Trigger、
|
||||
摇杆和主副按键,再发送 `udp_controller_receiver` 兼容的 UDP JSON 包。
|
||||
"""
|
||||
|
||||
import argparse
|
||||
@@ -94,8 +94,6 @@ def _controller_payload(
|
||||
*,
|
||||
hand: str,
|
||||
pose: Any,
|
||||
grip_value: Any,
|
||||
trigger_value: Any,
|
||||
axis: Any,
|
||||
buttons: dict[str, bool],
|
||||
grip_pressed: bool,
|
||||
@@ -103,8 +101,6 @@ def _controller_payload(
|
||||
pose_valid: bool = True,
|
||||
) -> dict[str, Any]:
|
||||
pos, quat = _pose_to_pos_quat(pose) if pose_valid else (ZERO_POS.copy(), IDENTITY_QUAT.copy())
|
||||
grip_float = _clamp_float(grip_value, 0.0, 1.0)
|
||||
trigger_float = _clamp_float(trigger_value, 0.0, 1.0)
|
||||
return {
|
||||
"hand": hand,
|
||||
"grip": pose_valid and grip_pressed,
|
||||
@@ -113,8 +109,6 @@ def _controller_payload(
|
||||
"quat": quat,
|
||||
"pose_valid": pose_valid,
|
||||
"pose_source": POSE_SOURCE,
|
||||
"grip_value": grip_float,
|
||||
"trigger_value": trigger_float,
|
||||
"axis": _safe_axis(axis),
|
||||
"buttons": buttons,
|
||||
}
|
||||
@@ -129,15 +123,10 @@ def _stop_controller_payload(hand: str) -> dict[str, Any]:
|
||||
"quat": IDENTITY_QUAT.copy(),
|
||||
"pose_valid": False,
|
||||
"pose_source": POSE_SOURCE,
|
||||
"grip_value": 0.0,
|
||||
"trigger_value": 0.0,
|
||||
"axis": [0.0, 0.0],
|
||||
"buttons": {
|
||||
"grip": False,
|
||||
"primary": False,
|
||||
"secondary": False,
|
||||
"menu": False,
|
||||
"axis_click": False,
|
||||
},
|
||||
}
|
||||
|
||||
@@ -181,18 +170,12 @@ def _send_stop_packets(
|
||||
|
||||
def _buttons_payload(
|
||||
*,
|
||||
grip: bool,
|
||||
primary: Callable[[], Any],
|
||||
secondary: Callable[[], Any],
|
||||
menu: Callable[[], Any],
|
||||
axis_click: Callable[[], Any],
|
||||
) -> dict[str, bool]:
|
||||
return {
|
||||
"grip": grip,
|
||||
"primary": _safe_bool(primary),
|
||||
"secondary": _safe_bool(secondary),
|
||||
"menu": _safe_bool(menu),
|
||||
"axis_click": _safe_bool(axis_click),
|
||||
}
|
||||
|
||||
|
||||
@@ -321,15 +304,10 @@ def main(argv: Sequence[str] | None = None) -> None:
|
||||
"left": _controller_payload(
|
||||
hand="left",
|
||||
pose=xrt.get_left_controller_pose(),
|
||||
grip_value=left_grip_value,
|
||||
trigger_value=left_trigger_value,
|
||||
axis=xrt.get_left_axis(),
|
||||
buttons=_buttons_payload(
|
||||
grip=left_grip,
|
||||
primary=xrt.get_X_button,
|
||||
secondary=xrt.get_Y_button,
|
||||
menu=xrt.get_left_menu_button,
|
||||
axis_click=xrt.get_left_axis_click,
|
||||
),
|
||||
grip_pressed=left_grip,
|
||||
trigger_pressed=left_trigger,
|
||||
@@ -337,15 +315,10 @@ def main(argv: Sequence[str] | None = None) -> None:
|
||||
"right": _controller_payload(
|
||||
hand="right",
|
||||
pose=xrt.get_right_controller_pose(),
|
||||
grip_value=right_grip_value,
|
||||
trigger_value=right_trigger_value,
|
||||
axis=xrt.get_right_axis(),
|
||||
buttons=_buttons_payload(
|
||||
grip=right_grip,
|
||||
primary=xrt.get_A_button,
|
||||
secondary=xrt.get_B_button,
|
||||
menu=xrt.get_right_menu_button,
|
||||
axis_click=xrt.get_right_axis_click,
|
||||
),
|
||||
grip_pressed=right_grip,
|
||||
trigger_pressed=right_trigger,
|
||||
|
||||
@@ -1,5 +1,10 @@
|
||||
std_msgs/Header header
|
||||
string hand
|
||||
|
||||
bool grip
|
||||
float32 trigger
|
||||
bool primary
|
||||
bool secondary
|
||||
float32[2] axis
|
||||
|
||||
geometry_msgs/Pose pose
|
||||
|
||||
Reference in New Issue
Block a user