diff --git a/xr_rm_input/test/test_controller_fields.py b/xr_rm_input/test/test_controller_fields.py new file mode 100644 index 0000000..9ce914a --- /dev/null +++ b/xr_rm_input/test/test_controller_fields.py @@ -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 diff --git a/xr_rm_input/xr_rm_input/udp_controller_receiver.py b/xr_rm_input/xr_rm_input/udp_controller_receiver.py index d4d6c06..f79370e 100755 --- a/xr_rm_input/xr_rm_input/udp_controller_receiver.py +++ b/xr_rm_input/xr_rm_input/udp_controller_receiver.py @@ -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": diff --git a/xr_rm_input/xr_rm_input/xrobotoolkit_to_udp_bridge.py b/xr_rm_input/xr_rm_input/xrobotoolkit_to_udp_bridge.py index 869dd33..4624cd1 100644 --- a/xr_rm_input/xr_rm_input/xrobotoolkit_to_udp_bridge.py +++ b/xr_rm_input/xr_rm_input/xrobotoolkit_to_udp_bridge.py @@ -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,