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

2026-07-30 20:24:28 +08:00
parent cbc18bed8a
commit 1936adf2fd
3 changed files with 154 additions and 31 deletions
@@ -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