initial

xr_rm_input/launch/udp_receiver.launch.py

@@ -0,0 +1,23 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description() -> LaunchDescription:
+    return LaunchDescription([
+        DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
+        DeclareLaunchArgument("udp_port", default_value="15000"),
+        DeclareLaunchArgument("topic", default_value="/xr/right_controller"),
+        Node(
+            package="xr_rm_input",
+            executable="udp_controller_receiver",
+            name="udp_controller_receiver",
+            output="screen",
+            parameters=[{
+                "udp_host": LaunchConfiguration("udp_host"),
+                "udp_port": LaunchConfiguration("udp_port"),
+                "topic": LaunchConfiguration("topic"),
+            }],
+        ),
+    ])

xr_rm_input/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>xr_rm_input</name>
+  <version>0.1.0</version>
+  <description>Receives XR controller pose packets and publishes left/right ROS2 controller messages.</description>
+  <maintainer email="user@example.com">Yikai Fu</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>xr_rm_interfaces</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

xr_rm_input/resource/xr_rm_input

@@ -0,0 +1 @@
+

xr_rm_input/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/xr_rm_input
+[install]
+install_scripts=$base/lib/xr_rm_input

xr_rm_input/setup.py

@@ -0,0 +1,28 @@
+from glob import glob
+from setuptools import setup
+
+package_name = "xr_rm_input"
+
+setup(
+    name=package_name,
+    version="0.1.0",
+    packages=[package_name],
+    data_files=[
+        ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]),
+        (f"share/{package_name}", ["package.xml"]),
+        (f"share/{package_name}/launch", glob("launch/*.launch.py")),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="Yikai Fu",
+    maintainer_email="user@example.com",
+    description="Receives XR controller pose packets and publishes left/right ROS2 messages.",
+    license="Apache-2.0",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [
+            "udp_controller_receiver = xr_rm_input.udp_controller_receiver:main",
+            "sample_udp_sender = xr_rm_input.sample_udp_sender:main",
+        ],
+    },
+)

xr_rm_input/xr_rm_input/__init__.py

@@ -0,0 +1 @@
+

xr_rm_input/xr_rm_input/sample_udp_sender.py

@@ -0,0 +1,58 @@
+import argparse
+import json
+import math
+import socket
+import time
+
+
+def main() -> None:
+    parser = argparse.ArgumentParser(description="通过 UDP 发送一段模拟 XR 左/右手柄数据。")
+    parser.add_argument("--host", default="127.0.0.1")
+    parser.add_argument("--port", type=int, default=15000)
+    parser.add_argument("--seconds", type=float, default=5.0)
+    parser.add_argument("--hz", type=float, default=60.0)
+    parser.add_argument("--amplitude", type=float, default=0.04)
+    parser.add_argument("--hand", choices=("left", "right", "both"), default="right")
+    args = parser.parse_args()
+
+    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    dt = 1.0 / args.hz
+    t0 = time.monotonic()
+
+    hands = ("left", "right") if args.hand == "both" else (args.hand,)
+    packets = {
+        hand: {
+            "t": 0.0,
+            "hand": hand,
+            "grip": False,
+            "trigger": 0.0,
+            "pos": [0.0, 1.0, 0.0],
+            "quat": [0.0, 0.0, 0.0, 1.0],
+        }
+        for hand in hands
+    }
+
+    while time.monotonic() - t0 < args.seconds:
+        t = time.monotonic() - t0
+        # 模拟按住握持键后缓慢移动手柄，用来测试相对位移控制链路。
+        for index, packet in enumerate(packets.values()):
+            phase = index * math.pi
+            packet["t"] = t
+            packet["grip"] = True
+            packet["trigger"] = max(0.0, math.sin(2.0 * math.pi * 0.1 * t))
+            packet["pos"] = [
+                args.amplitude * math.sin(2.0 * math.pi * 0.2 * t + phase),
+                1.0,
+                0.0,
+            ]
+            sock.sendto(json.dumps(packet).encode("utf-8"), (args.host, args.port))
+        time.sleep(dt)
+
+    for packet in packets.values():
+        packet["grip"] = False
+        packet["trigger"] = 0.0
+        sock.sendto(json.dumps(packet).encode("utf-8"), (args.host, args.port))
+
+
+if __name__ == "__main__":
+    main()

xr_rm_input/xr_rm_input/udp_controller_receiver.py

@@ -0,0 +1,215 @@
+import json
+import socket
+from collections.abc import Iterable, Mapping
+from typing import Any
+
+import rclpy
+from rclpy.node import Node
+
+from xr_rm_interfaces.msg import XrController
+
+
+class UdpControllerReceiver(Node):
+    """将轻量级 XR UDP 数据包转换成 ROS2 左/右手柄消息。"""
+
+    def __init__(self) -> None:
+        super().__init__("udp_controller_receiver")
+
+        self.declare_parameter("udp_host", "0.0.0.0")
+        self.declare_parameter("udp_port", 15000)
+        self.declare_parameter("topic", "/xr/right_controller")
+        self.declare_parameter("left_topic", "/xr/left_controller")
+        self.declare_parameter("right_topic", "")
+        self.declare_parameter("default_hand", "right")
+        self.declare_parameter("timer_hz", 100.0)
+        self.declare_parameter("quat_order", "xyzw")
+
+        udp_host = self.get_parameter("udp_host").value
+        udp_port = int(self.get_parameter("udp_port").value)
+        legacy_topic = str(self.get_parameter("topic").value)
+        left_topic = str(self.get_parameter("left_topic").value)
+        right_topic = str(self.get_parameter("right_topic").value) or legacy_topic
+        timer_hz = float(self.get_parameter("timer_hz").value)
+
+        self._default_hand = self._normalize_hand(
+            self.get_parameter("default_hand").value,
+            fallback="right",
+        )
+        self._quat_order = str(self.get_parameter("quat_order").value).lower()
+
+        # UDP 接收使用非阻塞模式，避免没有手柄数据时卡住 ROS2 定时器。
+        self._publishers = {
+            "left": self.create_publisher(XrController, left_topic, 10),
+            "right": self.create_publisher(XrController, right_topic, 10),
+        }
+        self._socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        self._socket.bind((udp_host, udp_port))
+        self._socket.setblocking(False)
+
+        self.create_timer(1.0 / timer_hz, self._poll_socket)
+        self.get_logger().info(
+            "正在监听 XR UDP 数据："
+            f"{udp_host}:{udp_port} -> left={left_topic}, right={right_topic}"
+        )
+
+    def _poll_socket(self) -> None:
+        while True:
+            try:
+                data, _ = self._socket.recvfrom(4096)
+            except BlockingIOError:
+                return
+
+            try:
+                payload = json.loads(data.decode("utf-8"))
+                controller_payloads = list(self._iter_controller_payloads(payload))
+            except (UnicodeDecodeError, json.JSONDecodeError, KeyError, TypeError, ValueError) as exc:
+                self.get_logger().warn(f"XR 数据包格式错误：{exc}", throttle_duration_sec=1.0)
+                continue
+
+            for hand, controller_payload in controller_payloads:
+                try:
+                    msg = self._payload_to_msg(controller_payload, hand)
+                except (KeyError, TypeError, ValueError) as exc:
+                    self.get_logger().warn(f"XR 手柄字段错误：{exc}", throttle_duration_sec=1.0)
+                    continue
+                self._publishers[msg.hand].publish(msg)
+
+    def _iter_controller_payloads(self, payload: Any) -> Iterable[tuple[str, Mapping[str, Any]]]:
+        if isinstance(payload, list):
+            for item in payload:
+                yield from self._iter_controller_payloads(item)
+            return
+
+        if not isinstance(payload, Mapping):
+            raise TypeError("expected JSON object or array")
+
+        if self._contains_pose(payload):
+            hand = self._normalize_hand(payload.get("hand", self._default_hand))
+            yield hand, payload
+            return
+
+        controllers = payload.get("controllers", payload.get("controller"))
+        if isinstance(controllers, Mapping):
+            if self._contains_pose(controllers):
+                hand = self._normalize_hand(controllers.get("hand", self._default_hand))
+                yield hand, controllers
+                return
+            for hand_key, controller_payload in controllers.items():
+                if isinstance(controller_payload, Mapping):
+                    hand = self._normalize_hand(controller_payload.get("hand", hand_key))
+                    yield hand, controller_payload
+            return
+
+        if isinstance(controllers, list):
+            for controller_payload in controllers:
+                yield from self._iter_controller_payloads(controller_payload)
+            return
+
+        for hand_key in ("left", "right", "left_controller", "right_controller"):
+            controller_payload = payload.get(hand_key)
+            if isinstance(controller_payload, Mapping):
+                hand = self._normalize_hand(controller_payload.get("hand", hand_key))
+                yield hand, controller_payload
+
+    def _payload_to_msg(self, payload: Mapping[str, Any], hand: str) -> XrController:
+        pos, quat = self._extract_pose(payload)
+        if len(pos) != 3 or len(quat) != 4:
+            raise ValueError("expected pos[3] and quat[4]")
+
+        msg = XrController()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.header.frame_id = str(payload.get("frame_id", "xr_world"))
+        msg.hand = hand
+        msg.grip = self._as_bool(payload.get("grip", payload.get("grip_button", False)))
+        msg.trigger = self._clamp_float(payload.get("trigger", 0.0), 0.0, 1.0)
+        msg.pose.position.x = float(pos[0])
+        msg.pose.position.y = float(pos[1])
+        msg.pose.position.z = float(pos[2])
+
+        # Unity/OpenXR 项目里的四元数顺序可能不同，这里用参数统一转换成 ROS 的 xyzw。
+        if self._quat_order == "wxyz":
+            w, x, y, z = quat
+        else:
+            x, y, z, w = quat
+        msg.pose.orientation.x = float(x)
+        msg.pose.orientation.y = float(y)
+        msg.pose.orientation.z = float(z)
+        msg.pose.orientation.w = float(w)
+        return msg
+
+    @staticmethod
+    def _contains_pose(payload: Mapping[str, Any]) -> bool:
+        return any(key in payload for key in ("pos", "position", "p", "pose"))
+
+    def _extract_pose(self, payload: Mapping[str, Any]) -> tuple[list[float], list[float]]:
+        pose = payload.get("pose")
+        if isinstance(pose, Mapping):
+            pos = pose.get("position", pose.get("pos", pose.get("p")))
+            quat = pose.get("orientation", pose.get("quat", pose.get("q", [0.0, 0.0, 0.0, 1.0])))
+        elif isinstance(pose, list) and len(pose) >= 7:
+            pos = pose[:3]
+            quat = pose[3:7]
+        else:
+            pos = payload.get("pos", payload.get("position", payload.get("p")))
+            quat = payload.get("quat", payload.get("orientation", payload.get("q", [0.0, 0.0, 0.0, 1.0])))
+
+        if pos is None:
+            raise KeyError("missing pos/position/p")
+        return self._vector3(pos), self._quaternion(quat)
+
+    @staticmethod
+    def _vector3(value: Any) -> list[float]:
+        if isinstance(value, Mapping):
+            return [float(value[key]) for key in ("x", "y", "z")]
+        vector = list(value)
+        if len(vector) != 3:
+            raise ValueError("expected 3D position")
+        return [float(item) for item in vector]
+
+    def _quaternion(self, value: Any) -> list[float]:
+        if isinstance(value, Mapping):
+            if self._quat_order == "wxyz":
+                return [float(value[key]) for key in ("w", "x", "y", "z")]
+            return [float(value[key]) for key in ("x", "y", "z", "w")]
+        quat = list(value)
+        if len(quat) != 4:
+            raise ValueError("expected quaternion")
+        return [float(item) for item in quat]
+
+    def _normalize_hand(self, hand: Any, fallback: str | None = None) -> str:
+        value = str(hand).lower()
+        if value in ("left", "l", "left_controller"):
+            return "left"
+        if value in ("right", "r", "right_controller"):
+            return "right"
+        fallback_hand = fallback or self._default_hand
+        self.get_logger().warn(
+            f"未知手柄标识 {hand!r}，使用默认 {fallback_hand}",
+            throttle_duration_sec=1.0,
+        )
+        return fallback_hand
+
+    @staticmethod
+    def _as_bool(value: Any) -> bool:
+        if isinstance(value, str):
+            return value.lower() in ("1", "true", "yes", "on")
+        return bool(value)
+
+    @staticmethod
+    def _clamp_float(value: Any, low: float, high: float) -> float:
+        number = float(value)
+        return min(max(number, low), high)
+
+
+def main(args: list[str] | None = None) -> None:
+    rclpy.init(args=args)
+    node = UdpControllerReceiver()
+    try:
+        rclpy.spin(node)
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()