5 Commits
8 changed files with 1017 additions and 87 deletions
+4 -1
View File
@@ -230,7 +230,10 @@
除非用户明确要求,否则不要自动提交、推送或修改远程仓库。 除非用户明确要求,否则不要自动提交、推送或修改远程仓库。
使用 Superpowers 执行计划时,允许 subagent 按相关 skill 创建和使用独立 worktree 及其配套本地分支;其他情况下,除非用户明确要求,不要自动创建分支。 使用 Superpowers 执行任务时,允许按相关 skill 工作流创建本地 Git 提交;
禁止执行 `git push`、合并本地分支、合并 PR 或其他远程写操作。相关 skill
如需独立 worktree 或配套本地分支,可以创建,但不得将其合并到其他分支。
其他情况下,除非用户明确要求,不要自动创建分支。
如果用户要求生成提交信息,提交信息应: 如果用户要求生成提交信息,提交信息应:
+64 -55
View File
@@ -24,7 +24,7 @@ PICO/XR 双手柄 UDP JSON
- Placo 0.9.4 RM75 QP 逆解;收到首帧有效关节反馈后才启用,求解失败时保留上一组有效关节目标。 - Placo 0.9.4 RM75 QP 逆解;收到首帧有效关节反馈后才启用,求解失败时保留上一组有效关节目标。
- 真机模式下,点击对应手柄 `trigger` 可切换并保持对应夹爪开/关状态。 - 真机模式下,点击对应手柄 `trigger` 可切换并保持对应夹爪开/关状态。
- Tkinter 启动面板 `launcher_ui.py`,用于现场快速启动、监控 topic、检查环境和清理进程。 - 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 ```text
src/ src/
├── README.md # 项目主文档 ├── README.md # 项目主文档
├── CODEX.md # Codex/Claude Code 项目工作流和安全规则 ├── AGENTS.md # Codex 项目工作流和安全规则
├── docs/ ├── docs/superpowers/ # Superpowers 设计与实施计划
│ └── 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/
├── xr_rm_bringup/ ├── xr_rm_bringup/
│ ├── config/ │ ├── config/
│ │ ├── dual_arm_rm75.yaml # 双臂配置:left_arm_teleop 与 right_arm_teleop │ │ ├── dual_arm_rm75.yaml # 双臂配置:left_arm_teleop 与 right_arm_teleop
@@ -62,12 +53,15 @@ src/
├── xr_rm_input/ ├── xr_rm_input/
│ ├── launch/ │ ├── launch/
│ │ └── udp_receiver.launch.py # 低层 UDP 接收测试入口 │ │ └── udp_receiver.launch.py # 低层 UDP 接收测试入口
│ ├── test/
│ │ └── test_controller_fields.py
│ └── xr_rm_input/ │ └── xr_rm_input/
│ ├── udp_controller_receiver.py │ ├── udp_controller_receiver.py
│ ├── xrobotoolkit_to_udp_bridge.py
│ └── sample_udp_sender.py # 本机扫轴/正弦模拟手柄 UDP 数据 │ └── sample_udp_sender.py # 本机扫轴/正弦模拟手柄 UDP 数据
├── xr_rm_interfaces/ ├── xr_rm_interfaces/
│ └── msg/ │ └── msg/
│ └── XrController.msg # hand/grip/trigger/pose │ └── XrController.msg # 手柄状态与位姿
└── xr_rm_teleop/ └── xr_rm_teleop/
├── models/ ├── models/
│ ├── rm75/ # 旧 RM75 模型资源(launch 不再选用) │ ├── rm75/ # 旧 RM75 模型资源(launch 不再选用)
@@ -81,6 +75,12 @@ src/
`single_arm_velocity_teleop` 这个名字保留是有意的:双臂模式不是一个大节点直接控制两台机械臂,而是启动两个相同的单臂控制节点,分别命名为 `left_arm_teleop``right_arm_teleop` `single_arm_velocity_teleop` 这个名字保留是有意的:双臂模式不是一个大节点直接控制两台机械臂,而是启动两个相同的单臂控制节点,分别命名为 `left_arm_teleop``right_arm_teleop`
## Superpowers Git 约束
使用 Superpowers 执行任务时,只允许按 skill 工作流创建本地 Git 提交。
禁止执行 `git push`、合并本地分支、合并 PR 或其他远程写操作。skill 如需
独立 worktree 或配套本地分支,可以创建,但不得将其合并到其他分支。
## 环境准备 ## 环境准备
在工作空间根目录,也就是包含 `src/` 的目录执行: 在工作空间根目录,也就是包含 `src/` 的目录执行:
@@ -268,15 +268,14 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false
- 当前三份 YAML 默认均使用 `follow: false` 完成安全基线验证;确认关节加速度与反馈稳定后,再单独测试高跟随。 - 当前三份 YAML 默认均使用 `follow: false` 完成安全基线验证;确认关节加速度与反馈稳定后,再单独测试高跟随。
- `initial_joint_pose`:mock 的初始关节反馈,以及显式开启初始化移动时的真机初始关节角。 - `initial_joint_pose`:mock 的初始关节反馈,以及显式开启初始化移动时的真机初始关节角。
当前 `/xr/*_controller` Project 坐标约定 当前 `/xr/*_controller`坐标处理
- Project`+X` 向右,`+Y` 向上,`+Z` 向后 - XRoboToolkit bridge 原样转发 SDK 的手柄位置和四元数,不额外转换坐标轴
- Unity APK 的 `Project (+Z back)` 会把 PXR `pxr_predict` 原始坐标转换为 `project.x=native.z``project.y=native.y``project.z=-native.x` - receiver 默认按 `xyzw` 解析四元数,也可通过 `quat_order:=wxyz` 切换
- `Source raw` 模式保留原始 pose source 坐标,只用于现场对照。
- 左臂映射:机器人位移增量 = `[-手柄y, 手柄z, -手柄x]` - 左臂映射:机器人位移增量 = `[-手柄y, 手柄z, -手柄x]`
- 右臂映射:机器人位移增量 = `[手柄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 数据格式 ## UDP 数据格式
当前 Unity APK 每个周期发送一个双手柄 JSON 包: 当前 XRoboToolkit bridge 每个周期发送一个双手柄 JSON 包:
```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", "frame_id": "xr_world",
"controllers": { "controllers": {
"left": { "left": {
"hand": "left",
"grip": true, "grip": true,
"trigger": 0.0, "trigger": 0.0,
"axis": [0.2, -0.4],
"buttons": {
"primary": true,
"secondary": false
},
"pos": [-0.12, 1.05, 0.30], "pos": [-0.12, 1.05, 0.30],
"quat": [0.0, 0.0, 0.0, 1.0], "quat": [0.0, 0.0, 0.0, 1.0],
"pose_valid": true, "pose_valid": true,
"pose_source": "pxr_predict", "pose_source": "xrobotoolkit"
"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
}
}, },
"right": { "right": {
"hand": "right",
"grip": true, "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], "pos": [0.12, 1.05, 0.30],
"quat": [0.0, 0.0, 0.0, 1.0], "quat": [0.0, 0.0, 0.0, 1.0],
"pose_valid": true, "pose_valid": true,
"pose_source": "unity_xr", "pose_source": "xrobotoolkit"
"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
}
} }
} }
} }
@@ -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`,用于后续延迟分析 - `t` / `source_time`bridge 的 PC 单调时间,用于诊断发送周期
- `seq`Unity 端递增包序号,用于后续丢包分析 - `seq`bridge 递增的 UDP 包序号,bridge 重启后重新计数
- `frame_id`:默认 `xr_world`,会写入 `XrController.header.frame_id` - `frame_id`:默认 `xr_world`,会写入 `XrController.header.frame_id`
- `grip`:运动使能。`true` 时进入相对位姿控制,`false` 时停止。 - `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。 - `pos`:手柄位置,长度 3。
- `quat`:手柄姿态四元数,默认按 `xyzw` 解析;遥操作节点会用 grip 锁定后的相对旋转控制 TCP 姿态 - `quat`:手柄姿态四元数,默认按 `xyzw` 解析。
- `pose_valid`:姿态是否可信。ROS 接收端看到 `false` 强制 `grip=false` - `pose_valid`:姿态是否可信`false` 时接收端强制 `grip=false`
- `pose_source``pxr_predict``unity_xr``xrobotoolkit``none`,用于判断姿态来自 PICO 预测接口、Unity XR fallback 还是官方 XRoboToolkit SDK bridge - `pose_source`当前 bridge 使用 `xrobotoolkit`
- `tracking_state` / `controller_status`Unity/PICO 侧追踪诊断值,只用于日志和排查。
- `grip_value``axis``buttons`:PICO 端输入诊断字段,当前不会写入 `XrController` 消息。
`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 ## 官方 XRoboToolkit bridge
@@ -466,7 +474,8 @@ Controller topic 没有数据:
- 确认 UDP 发送端目标 IP 是运行 ROS2 的主机 IP。 - 确认 UDP 发送端目标 IP 是运行 ROS2 的主机 IP。
- 确认端口是 `15000`,或 launch 与发送端端口一致。 - 确认端口是 `15000`,或 launch 与发送端端口一致。
-`sample_udp_sender` 在本机验证接收链路。 -`sample_udp_sender` 在本机验证接收链路。
- 如果 Unity HUD 显示某个手柄 `invalid none`ROS 侧会把该手柄 `grip` 强制置为 `false` - 确认 `xrobotoolkit_to_udp_bridge` 没有持续打印 SDK read failedSDK
读取失败时 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 failedSDK
读取失败时 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、合并本地分支以及任何远程写操作。
+122
View File
@@ -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 接收节点。 """XR 手柄 UDP 接收节点。
从 UDP JSON 数据包中解析左右手柄位姿、握持键和扳机值,并发布为 从 UDP JSON 数据包中解析左右手柄位姿、Grip、Trigger、摇杆和主副按键,
`xr_rm_interfaces/XrController` 消息,供遥操作和夹爪节点订阅。 并发布为 `xr_rm_interfaces/XrController` 消息,供遥操作和夹爪节点订阅。
""" """
import json import json
import math
import socket import socket
from collections.abc import Iterable, Mapping from collections.abc import Iterable, Mapping
from typing import Any from typing import Any
@@ -124,6 +125,8 @@ class UdpControllerReceiver(Node):
pos, quat = self._extract_pose(payload) pos, quat = self._extract_pose(payload)
if len(pos) != 3 or len(quat) != 4: if len(pos) != 3 or len(quat) != 4:
raise ValueError("expected pos[3] and 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 = XrController()
msg.header.stamp = self.get_clock().now().to_msg() msg.header.stamp = self.get_clock().now().to_msg()
@@ -144,6 +147,9 @@ class UdpControllerReceiver(Node):
msg.grip = grip msg.grip = grip
msg.trigger = self._clamp_float(payload.get("trigger", 0.0), 0.0, 1.0) 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.x = float(pos[0])
msg.pose.position.y = float(pos[1]) msg.pose.position.y = float(pos[1])
msg.pose.position.z = float(pos[2]) msg.pose.position.z = float(pos[2])
@@ -213,6 +219,28 @@ class UdpControllerReceiver(Node):
raise ValueError("expected 3D position") raise ValueError("expected 3D position")
return [float(item) for item in vector] 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]: def _quaternion(self, value: Any) -> list[float]:
if isinstance(value, Mapping): if isinstance(value, Mapping):
if self._quat_order == "wxyz": if self._quat_order == "wxyz":
@@ -1,8 +1,8 @@
"""XRoboToolkit SDK 到当前 UDP controller JSON 协议的桥接脚本。 """XRoboToolkit SDK 到当前 UDP controller JSON 协议的桥接脚本。
该脚本运行在安装了 `xrobotoolkit_sdk` Python 环境中从官方 该脚本运行在安装了 `xrobotoolkit_sdk` Python 环境中从官方
XRoboToolkit PC-Service SDK 读取 PICO 左右手柄 pose / grip / trigger XRoboToolkit PC-Service SDK 读取 PICO 左右手柄 poseGripTrigger
再发送现有 `udp_controller_receiver` 兼容的 UDP JSON 摇杆和主副按键再发送 `udp_controller_receiver` 兼容的 UDP JSON
""" """
import argparse import argparse
@@ -94,8 +94,6 @@ def _controller_payload(
*, *,
hand: str, hand: str,
pose: Any, pose: Any,
grip_value: Any,
trigger_value: Any,
axis: Any, axis: Any,
buttons: dict[str, bool], buttons: dict[str, bool],
grip_pressed: bool, grip_pressed: bool,
@@ -103,8 +101,6 @@ def _controller_payload(
pose_valid: bool = True, pose_valid: bool = True,
) -> dict[str, Any]: ) -> dict[str, Any]:
pos, quat = _pose_to_pos_quat(pose) if pose_valid else (ZERO_POS.copy(), IDENTITY_QUAT.copy()) 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 { return {
"hand": hand, "hand": hand,
"grip": pose_valid and grip_pressed, "grip": pose_valid and grip_pressed,
@@ -113,8 +109,6 @@ def _controller_payload(
"quat": quat, "quat": quat,
"pose_valid": pose_valid, "pose_valid": pose_valid,
"pose_source": POSE_SOURCE, "pose_source": POSE_SOURCE,
"grip_value": grip_float,
"trigger_value": trigger_float,
"axis": _safe_axis(axis), "axis": _safe_axis(axis),
"buttons": buttons, "buttons": buttons,
} }
@@ -129,15 +123,10 @@ def _stop_controller_payload(hand: str) -> dict[str, Any]:
"quat": IDENTITY_QUAT.copy(), "quat": IDENTITY_QUAT.copy(),
"pose_valid": False, "pose_valid": False,
"pose_source": POSE_SOURCE, "pose_source": POSE_SOURCE,
"grip_value": 0.0,
"trigger_value": 0.0,
"axis": [0.0, 0.0], "axis": [0.0, 0.0],
"buttons": { "buttons": {
"grip": False,
"primary": False, "primary": False,
"secondary": False, "secondary": False,
"menu": False,
"axis_click": False,
}, },
} }
@@ -181,18 +170,12 @@ def _send_stop_packets(
def _buttons_payload( def _buttons_payload(
*, *,
grip: bool,
primary: Callable[[], Any], primary: Callable[[], Any],
secondary: Callable[[], Any], secondary: Callable[[], Any],
menu: Callable[[], Any],
axis_click: Callable[[], Any],
) -> dict[str, bool]: ) -> dict[str, bool]:
return { return {
"grip": grip,
"primary": _safe_bool(primary), "primary": _safe_bool(primary),
"secondary": _safe_bool(secondary), "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( "left": _controller_payload(
hand="left", hand="left",
pose=xrt.get_left_controller_pose(), pose=xrt.get_left_controller_pose(),
grip_value=left_grip_value,
trigger_value=left_trigger_value,
axis=xrt.get_left_axis(), axis=xrt.get_left_axis(),
buttons=_buttons_payload( buttons=_buttons_payload(
grip=left_grip,
primary=xrt.get_X_button, primary=xrt.get_X_button,
secondary=xrt.get_Y_button, secondary=xrt.get_Y_button,
menu=xrt.get_left_menu_button,
axis_click=xrt.get_left_axis_click,
), ),
grip_pressed=left_grip, grip_pressed=left_grip,
trigger_pressed=left_trigger, trigger_pressed=left_trigger,
@@ -337,15 +315,10 @@ def main(argv: Sequence[str] | None = None) -> None:
"right": _controller_payload( "right": _controller_payload(
hand="right", hand="right",
pose=xrt.get_right_controller_pose(), pose=xrt.get_right_controller_pose(),
grip_value=right_grip_value,
trigger_value=right_trigger_value,
axis=xrt.get_right_axis(), axis=xrt.get_right_axis(),
buttons=_buttons_payload( buttons=_buttons_payload(
grip=right_grip,
primary=xrt.get_A_button, primary=xrt.get_A_button,
secondary=xrt.get_B_button, secondary=xrt.get_B_button,
menu=xrt.get_right_menu_button,
axis_click=xrt.get_right_axis_click,
), ),
grip_pressed=right_grip, grip_pressed=right_grip,
trigger_pressed=right_trigger, trigger_pressed=right_trigger,
+5
View File
@@ -1,5 +1,10 @@
std_msgs/Header header std_msgs/Header header
string hand string hand
bool grip bool grip
float32 trigger float32 trigger
bool primary
bool secondary
float32[2] axis
geometry_msgs/Pose pose geometry_msgs/Pose pose