optimze unity project for xr_rm

xr_rm_input/xr_rm_input/udp_controller_receiver.py

@@ -18,6 +18,8 @@ from xr_rm_interfaces.msg import XrController
 class UdpControllerReceiver(Node):
     """将轻量级 XR UDP 数据包转换成 ROS2 左/右手柄消息。"""
 
+    _VALID_POSE_SOURCES = {"pxr_predict", "unity_xr", "none"}
+
     def __init__(self) -> None:
         super().__init__("udp_controller_receiver")
 
@@ -127,7 +129,20 @@ class UdpControllerReceiver(Node):
         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)))
+        grip = self._as_bool(payload.get("grip", payload.get("grip_button", False)))
+        pose_valid = self._optional_bool(payload.get("pose_valid"), default=True)
+        pose_source = self._validate_pose_diagnostics(payload, hand)
+        if not pose_valid:
+            self.get_logger().warn(
+                f"{hand} XR pose_valid=false，强制 grip=false"
+                f" source={pose_source or 'missing'}"
+                f" tracking_state={payload.get('tracking_state', 'missing')}"
+                f" controller_status={payload.get('controller_status', 'missing')}",
+                throttle_duration_sec=1.0,
+            )
+            grip = False
+
+        msg.grip = grip
         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])
@@ -144,6 +159,30 @@ class UdpControllerReceiver(Node):
         msg.pose.orientation.w = float(w)
         return msg
 
+    def _validate_pose_diagnostics(self, payload: Mapping[str, Any], hand: str) -> str:
+        has_any_diagnostic = any(
+            key in payload for key in ("pose_valid", "pose_source", "tracking_state", "controller_status")
+        )
+        if has_any_diagnostic and "pose_valid" not in payload:
+            self.get_logger().warn(
+                f"{hand} XR 诊断字段缺少 pose_valid，按旧协议处理 grip",
+                throttle_duration_sec=5.0,
+            )
+        if has_any_diagnostic and "pose_source" not in payload:
+            self.get_logger().warn(
+                f"{hand} XR 诊断字段缺少 pose_source",
+                throttle_duration_sec=5.0,
+            )
+
+        pose_source_value = payload.get("pose_source", "")
+        pose_source = str(pose_source_value).strip()
+        if pose_source and pose_source not in self._VALID_POSE_SOURCES:
+            self.get_logger().warn(
+                f"{hand} XR 未知 pose_source={pose_source!r}",
+                throttle_duration_sec=5.0,
+            )
+        return pose_source
+
     @staticmethod
     def _contains_pose(payload: Mapping[str, Any]) -> bool:
         return any(key in payload for key in ("pos", "position", "p", "pose"))
@@ -203,6 +242,12 @@ class UdpControllerReceiver(Node):
             return value.lower() in ("1", "true", "yes", "on")
         return bool(value)
 
+    @classmethod
+    def _optional_bool(cls, value: Any, default: bool) -> bool:
+        if value is None:
+            return default
+        return cls._as_bool(value)
+
     @staticmethod
     def _clamp_float(value: Any, low: float, high: float) -> float:
         number = float(value)