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

feat: 发布 XR 手柄摇杆与按键

Browse Source
This commit is contained in:
YikaiFu-cart
2026-07-30 20:24:28 +08:00
parent cbc18bed8a
commit 1936adf2fd
3 changed files with 154 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
xr_rm_input/test/test_controller_fields.py
+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
xr_rm_input/xr_rm_input/udp_controller_receiver.py
+30 -2
View File
@@ -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":
xr_rm_input/xr_rm_input/xrobotoolkit_to_udp_bridge.py
+2 -29
View File
@@ -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,