Add OpenXR controller health monitoring

docs/source/references/device_provider_monitoring.rst

@@ -45,7 +45,8 @@ Identity and inventory
 - A monitored plugin device ID is
   ``<plugin_root_id><manifest device path>``.
 - The live OpenXR provider is ``openxr/runtime`` and owns
-  ``openxr/headset``.
+  ``openxr/headset``. When a controller input source is configured, it also
+  owns ``openxr/controller/left`` and ``openxr/controller/right``.
 - Replay sessions expose neither a live OpenXR provider nor configured plugin
   providers.
 

src/python/isaaccapture/teleop_session_manager/status_monitor.py

@@ -24,9 +24,20 @@
 REPORT_STALE_AFTER_NS = 3_000_000_000
 OPENXR_PROVIDER_ID = "openxr/runtime"
 OPENXR_DEVICE_ID = "openxr/headset"
+OPENXR_LEFT_CONTROLLER_DEVICE_ID = "openxr/controller/left"
+OPENXR_RIGHT_CONTROLLER_DEVICE_ID = "openxr/controller/right"
+OPENXR_CONTROLLER_TRACKING_LOSS_DELAY_NS = 1_000_000_000
 PLUGIN_STATUS_COLLECTION_SUFFIX = "/device_status"
 OPENXR_IDENTIFIER_MAX_BYTES = 255
 
+_OPENXR_CONTROLLER_DEVICES = (
+    ("left", OPENXR_LEFT_CONTROLLER_DEVICE_ID, "/controller/left"),
+    ("right", OPENXR_RIGHT_CONTROLLER_DEVICE_ID, "/controller/right"),
+)
+_OPENXR_CONTROLLER_SIDE_BY_ID = {
+    device_id: side for side, device_id, _path in _OPENXR_CONTROLLER_DEVICES
+}
+
 _PLUGIN_DEVICE_REASON_MAP = {
     "NONE": StatusReason.NONE,
     "NO_HARDWARE_SIGNAL": StatusReason.NO_HARDWARE_SIGNAL,
@@ -393,6 +404,10 @@ def __init__(self) -> None:
         self._plugin_runtimes: tuple[_PluginRuntime, ...] = ()
         self._external_openxr = False
         self._runtime_failed = False
+        self._controller_untracked_since_ns: dict[str, int | None] = {
+            "left": None,
+            "right": None,
+        }
 
     def get_status(self) -> StatusSnapshot:
         with self._lock:
@@ -412,6 +427,7 @@ def rebuild(
         *,
         include_openxr: bool,
         external_openxr: bool,
+        include_openxr_controllers: bool = False,
         now_ns: int | None = None,
     ) -> None:
         """Validate and install a fresh INITIALIZING startup inventory."""
@@ -475,7 +491,12 @@ def rebuild(
                 )
 
         if include_openxr:
-            if OPENXR_PROVIDER_ID in provider_ids or OPENXR_DEVICE_ID in device_ids:
+            openxr_device_ids = {OPENXR_DEVICE_ID}
+            if include_openxr_controllers:
+                openxr_device_ids.update(
+                    device_id for _side, device_id, _path in _OPENXR_CONTROLLER_DEVICES
+                )
+            if OPENXR_PROVIDER_ID in provider_ids or openxr_device_ids & device_ids:
                 raise ValueError(
                     "OpenXR provider or device ID conflicts with plugin inventory"
                 )
@@ -501,10 +522,25 @@ def rebuild(
                     error="",
                 )
             )
+            if include_openxr_controllers:
+                devices.extend(
+                    DeviceStatus(
+                        id=device_id,
+                        path=path,
+                        type="controller",
+                        description=f"{side.title()} OpenXR controller",
+                        provider_id=OPENXR_PROVIDER_ID,
+                        status=DeviceState.INITIALIZING,
+                        reason=StatusReason.PROVIDER_INITIALIZING,
+                        error="",
+                    )
+                    for side, device_id, path in _OPENXR_CONTROLLER_DEVICES
+                )
 
         self._plugin_runtimes = tuple(_PluginRuntime(spec) for spec in plugin_specs)
         self._external_openxr = external_openxr
         self._runtime_failed = False
+        self._controller_untracked_since_ns = {"left": None, "right": None}
         self._replace_snapshot(
             StatusSnapshot(
                 time.monotonic_ns() if now_ns is None else now_ns,
@@ -535,6 +571,16 @@ def mark_available(self, *, now_ns: int | None = None) -> None:
             )
         for device in snapshot.devices:
             if device.provider_id == OPENXR_PROVIDER_ID:
+                if device.id != OPENXR_DEVICE_ID:
+                    devices.append(
+                        replace(
+                            device,
+                            status=DeviceState.UNKNOWN,
+                            reason=StatusReason.NO_CURRENT_DATA,
+                            error="waiting for the first controller tracking sample",
+                        )
+                    )
+                    continue
                 devices.append(
                     replace(
                         device,
@@ -573,13 +619,15 @@ def refresh(
         deviceio_session: Any,
         openxr_session: Any = None,
         *,
+        openxr_controllers: dict[str, Any] | None = None,
         now_ns: int | None = None,
     ) -> None:
         """Poll owned provider inputs and publish one reduced snapshot."""
         with self._update_lock:
             self._refresh(
                 deviceio_session,
                 openxr_session,
+                openxr_controllers=openxr_controllers,
                 now_ns=now_ns,
             )
 
@@ -588,6 +636,7 @@ def _refresh(
         deviceio_session: Any,
         openxr_session: Any,
         *,
+        openxr_controllers: dict[str, Any] | None,
         now_ns: int | None,
     ) -> None:
         if self._runtime_failed:
@@ -632,7 +681,10 @@ def _refresh(
 
         if OPENXR_PROVIDER_ID in providers_by_id and not self._external_openxr:
             provider = providers_by_id[OPENXR_PROVIDER_ID]
-            headset = devices_by_provider[OPENXR_PROVIDER_ID][0]
+            openxr_devices = devices_by_provider[OPENXR_PROVIDER_ID]
+            headset = next(
+                device for device in openxr_devices if device.id == OPENXR_DEVICE_ID
+            )
             if provider.status != ProviderState.FAILED:
                 try:
                     if openxr_session is None:
@@ -655,7 +707,33 @@ def _refresh(
                         error=str(error),
                     )
                 providers_by_id[OPENXR_PROVIDER_ID] = provider
-                devices_by_provider[OPENXR_PROVIDER_ID] = (headset,)
+                openxr_devices = tuple(
+                    headset if device.id == OPENXR_DEVICE_ID else device
+                    for device in openxr_devices
+                )
+            devices_by_provider[OPENXR_PROVIDER_ID] = openxr_devices
+
+        if OPENXR_PROVIDER_ID in providers_by_id:
+            provider = providers_by_id[OPENXR_PROVIDER_ID]
+            openxr_devices = devices_by_provider[OPENXR_PROVIDER_ID]
+            if provider.status == ProviderState.FAILED:
+                devices_by_provider[OPENXR_PROVIDER_ID] = tuple(
+                    replace(
+                        device,
+                        status=DeviceState.UNAVAILABLE,
+                        reason=provider.reason,
+                        error=provider.error,
+                    )
+                    for device in openxr_devices
+                )
+            elif openxr_controllers is not None:
+                devices_by_provider[OPENXR_PROVIDER_ID] = (
+                    self._reduce_openxr_controllers(
+                        openxr_devices,
+                        openxr_controllers,
+                        now_ns,
+                    )
+                )
 
         providers = tuple(
             providers_by_id[provider.id] for provider in snapshot.providers
@@ -667,6 +745,77 @@ def _refresh(
         )
         self._replace_snapshot(StatusSnapshot(now_ns, providers, devices))
 
+    def _reduce_openxr_controllers(
+        self,
+        devices: tuple[DeviceStatus, ...],
+        snapshots: dict[str, Any],
+        now_ns: int,
+    ) -> tuple[DeviceStatus, ...]:
+        reduced = []
+        for device in devices:
+            side = _OPENXR_CONTROLLER_SIDE_BY_ID.get(device.id)
+            if side is None:
+                reduced.append(device)
+                continue
+
+            snapshot = snapshots.get(side)
+            try:
+                aim_valid = snapshot is not None and bool(snapshot.aim_pose.is_valid)
+                grip_valid = snapshot is not None and bool(snapshot.grip_pose.is_valid)
+            except Exception as error:  # noqa: BLE001 - native status reads are isolated
+                self._controller_untracked_since_ns[side] = None
+                reduced.append(
+                    replace(
+                        device,
+                        status=DeviceState.UNKNOWN,
+                        reason=StatusReason.OPENXR_POLL_ERROR,
+                        error=str(error),
+                    )
+                )
+                continue
+
+            if aim_valid and grip_valid:
+                self._controller_untracked_since_ns[side] = None
+                reduced.append(
+                    replace(
+                        device,
+                        status=DeviceState.CONNECTED,
+                        reason=StatusReason.NONE,
+                        error="",
+                    )
+                )
+                continue
+
+            untracked_since_ns = self._controller_untracked_since_ns[side]
+            if untracked_since_ns is None:
+                self._controller_untracked_since_ns[side] = now_ns
+                reduced.append(device)
+                continue
+            if now_ns - untracked_since_ns < OPENXR_CONTROLLER_TRACKING_LOSS_DELAY_NS:
+                reduced.append(device)
+                continue
+
+            if snapshot is None:
+                status = DeviceState.DISCONNECTED
+                detail = "controller is inactive"
+            else:
+                status = DeviceState.DEGRADED
+                missing = [
+                    pose
+                    for pose, valid in (("aim", aim_valid), ("grip", grip_valid))
+                    if not valid
+                ]
+                detail = f"{' and '.join(missing)} pose tracking unavailable"
+            reduced.append(
+                replace(
+                    device,
+                    status=status,
+                    reason=StatusReason.NO_CURRENT_DATA,
+                    error=f"{detail} for at least 1 second",
+                )
+            )
+        return tuple(reduced)
+
     def refresh_plugin_processes(self, *, now_ns: int | None = None) -> None:
         """Refresh process-owned states without polling device reports."""
         with self._update_lock:

src/python/isaaccapture/teleop_session_manager/teleop_session.py

@@ -233,6 +233,7 @@ def __init__(self, config: TeleopSessionConfig):
         self._in_context: bool = False
         # Discover sources and external leaves from pipeline
         self._discover_sources()
+        self._controller_status_trackers = self._find_controller_status_trackers()
         # Vendor selection is a live-only concern; reject it up front in replay.
         self._reject_vendor_selection_in_replay()
 
@@ -580,7 +581,11 @@ def _execute_step_request(
         except BaseException as error:
             self._status_monitor.mark_runtime_failed(error)
             raise
-        self._status_monitor.refresh(self.deviceio_session, self._oxr_session)
+        self._status_monitor.refresh(
+            self.deviceio_session,
+            self._oxr_session,
+            openxr_controllers=self._controller_status_snapshots(),
+        )
         pipeline_inputs = self._collect_tracker_data()
 
         if request.external_inputs:
@@ -1007,6 +1012,36 @@ def _collect_tracker_data(self) -> dict[str, Any]:
             for source in self._sources
         }
 
+    def _find_controller_status_trackers(self) -> tuple[Any, ...]:
+        """Controller trackers belonging to configured input sources."""
+        trackers = []
+        seen_ids = set()
+        for source in self._sources:
+            tracker = source.get_tracker()
+            if (
+                id(tracker) in seen_ids
+                or not callable(getattr(tracker, "get_left_controller", None))
+                or not callable(getattr(tracker, "get_right_controller", None))
+            ):
+                continue
+            seen_ids.add(id(tracker))
+            trackers.append(tracker)
+        return tuple(trackers)
+
+    def _controller_status_snapshots(self) -> dict[str, Any] | None:
+        """Read the latest cached per-side samples without another DeviceIO update."""
+        if not self._controller_status_trackers:
+            return None
+        snapshots = {"left": None, "right": None}
+        for tracker in self._controller_status_trackers:
+            for side in snapshots:
+                sample = getattr(tracker, f"get_{side}_controller")(
+                    self.deviceio_session
+                )
+                if snapshots[side] is None and sample is not None:
+                    snapshots[side] = sample
+        return snapshots
+
     def _check_plugin_health(self):
         """Check health of all running plugins."""
         for plugin_context in self.plugin_contexts:
@@ -1095,6 +1130,8 @@ def _enter_resources(self, stack: ExitStack) -> None:
             (),
             include_openxr=live_mode,
             external_openxr=live_mode and external_openxr,
+            include_openxr_controllers=live_mode
+            and bool(self._controller_status_trackers),
         )
         if live_mode:
             self._resolve_monitored_plugins()
@@ -1106,6 +1143,7 @@ def _enter_resources(self, stack: ExitStack) -> None:
                 ),
                 include_openxr=True,
                 external_openxr=external_openxr,
+                include_openxr_controllers=bool(self._controller_status_trackers),
             )
 
         # Auto-populate mcap_config from pipeline sources if recording or replaying.