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

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

xr_rm_input/xr_rm_input/udp_controller_receiver.py

@@ -1,10 +1,11 @@
 """XR 手柄 UDP 接收节点。
 
-从 UDP JSON 数据包中解析左右手柄位姿、握持键和扳机值，并发布为
+从 UDP JSON 数据包中解析左右手柄位姿、Grip、Trigger、摇杆和主副按键，
-`xr_rm_interfaces/XrController` 消息，供遥操作和夹爪节点订阅。
+并发布为 `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

@@ -1,8 +1,8 @@
 """XRoboToolkit SDK 到当前 UDP controller JSON 协议的桥接脚本。
 
 该脚本运行在安装了 `xrobotoolkit_sdk` 的 Python 环境中，从官方
-XRoboToolkit PC-Service SDK 读取 PICO 左右手柄 pose / grip / trigger，
+XRoboToolkit PC-Service SDK 读取 PICO 左右手柄 pose、Grip、Trigger、
-再发送现有 `udp_controller_receiver` 已兼容的 UDP JSON 包。
+摇杆和主副按键，再发送 `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,