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

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":

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,