diff --git a/docs/source/references/device_provider_monitoring.rst b/docs/source/references/device_provider_monitoring.rst index 10ab72b68c..bfadd754be 100644 --- a/docs/source/references/device_provider_monitoring.rst +++ b/docs/source/references/device_provider_monitoring.rst @@ -45,7 +45,8 @@ Identity and inventory - A monitored plugin device ID is ````. - 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. diff --git a/src/python/isaaccapture/teleop_session_manager/status_monitor.py b/src/python/isaaccapture/teleop_session_manager/status_monitor.py index 973ec9264b..566f558f99 100644 --- a/src/python/isaaccapture/teleop_session_manager/status_monitor.py +++ b/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,6 +619,7 @@ 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.""" @@ -580,6 +627,7 @@ def refresh( 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: diff --git a/src/python/isaaccapture/teleop_session_manager/teleop_session.py b/src/python/isaaccapture/teleop_session_manager/teleop_session.py index d4bf267b68..f9c019ac7d 100644 --- a/src/python/isaaccapture/teleop_session_manager/teleop_session.py +++ b/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. diff --git a/tests/python/core/teleop_session_manager/test_status_monitor.py b/tests/python/core/teleop_session_manager/test_status_monitor.py index 3cc7e84126..fe041e8daf 100644 --- a/tests/python/core/teleop_session_manager/test_status_monitor.py +++ b/tests/python/core/teleop_session_manager/test_status_monitor.py @@ -14,8 +14,11 @@ StatusReason, ) from isaaccapture.teleop_session_manager.status_monitor import ( + OPENXR_CONTROLLER_TRACKING_LOSS_DELAY_NS, OPENXR_DEVICE_ID, + OPENXR_LEFT_CONTROLLER_DEVICE_ID, OPENXR_PROVIDER_ID, + OPENXR_RIGHT_CONTROLLER_DEVICE_ID, PluginProviderSpec, StatusMonitor, ) @@ -83,6 +86,13 @@ def openxr_snapshot( ) +def controller_snapshot(*, aim_valid=True, grip_valid=True): + return SimpleNamespace( + aim_pose=SimpleNamespace(is_valid=aim_valid), + grip_pose=SimpleNamespace(is_valid=grip_valid), + ) + + def report(now_ns, entries, *, version=1, report_time_ns=None): return SimpleNamespace( schema_version=version, @@ -360,6 +370,198 @@ def test_openxr_owned_and_external_states(external, device_state, reason): assert monitor.get_device_status(OPENXR_DEVICE_ID).reason == reason +def test_openxr_controller_inventory_is_opt_in_and_waits_for_first_sample(): + without_controllers = StatusMonitor() + without_controllers.rebuild( + (), + include_openxr=True, + external_openxr=False, + now_ns=1, + ) + + with_controllers = StatusMonitor() + with_controllers.rebuild( + (), + include_openxr=True, + external_openxr=False, + include_openxr_controllers=True, + now_ns=1, + ) + with_controllers.mark_available(now_ns=2) + + assert [device.id for device in without_controllers.get_status().devices] == [ + OPENXR_DEVICE_ID + ] + assert [device.id for device in with_controllers.get_status().devices] == [ + OPENXR_DEVICE_ID, + OPENXR_LEFT_CONTROLLER_DEVICE_ID, + OPENXR_RIGHT_CONTROLLER_DEVICE_ID, + ] + for device_id in ( + OPENXR_LEFT_CONTROLLER_DEVICE_ID, + OPENXR_RIGHT_CONTROLLER_DEVICE_ID, + ): + device = with_controllers.get_device_status(device_id) + assert device.status == DeviceState.UNKNOWN + assert device.reason == StatusReason.NO_CURRENT_DATA + + +def test_openxr_controllers_track_each_side_after_one_second_grace_and_recover(): + monitor = StatusMonitor() + monitor.rebuild( + (), + include_openxr=True, + external_openxr=False, + include_openxr_controllers=True, + now_ns=1, + ) + monitor.mark_available(now_ns=2) + session = FakeOpenXRSession(openxr_snapshot()) + both_connected = { + "left": controller_snapshot(), + "right": controller_snapshot(), + } + monitor.refresh( + object(), + session, + openxr_controllers=both_connected, + now_ns=10, + ) + + loss_started_ns = 20 + missing_tracking = { + "left": controller_snapshot(aim_valid=False), + "right": None, + } + monitor.refresh( + object(), + session, + openxr_controllers=missing_tracking, + now_ns=loss_started_ns, + ) + monitor.refresh( + object(), + session, + openxr_controllers=missing_tracking, + now_ns=loss_started_ns + OPENXR_CONTROLLER_TRACKING_LOSS_DELAY_NS - 1, + ) + assert ( + monitor.get_device_status(OPENXR_LEFT_CONTROLLER_DEVICE_ID).status + == DeviceState.CONNECTED + ) + assert ( + monitor.get_device_status(OPENXR_RIGHT_CONTROLLER_DEVICE_ID).status + == DeviceState.CONNECTED + ) + + monitor.refresh( + object(), + session, + openxr_controllers=missing_tracking, + now_ns=loss_started_ns + OPENXR_CONTROLLER_TRACKING_LOSS_DELAY_NS, + ) + left = monitor.get_device_status(OPENXR_LEFT_CONTROLLER_DEVICE_ID) + right = monitor.get_device_status(OPENXR_RIGHT_CONTROLLER_DEVICE_ID) + assert (left.status, left.reason) == ( + DeviceState.DEGRADED, + StatusReason.NO_CURRENT_DATA, + ) + assert "aim pose" in left.error + assert (right.status, right.reason) == ( + DeviceState.DISCONNECTED, + StatusReason.NO_CURRENT_DATA, + ) + assert "inactive" in right.error + + monitor.refresh( + object(), + session, + openxr_controllers=both_connected, + now_ns=loss_started_ns + OPENXR_CONTROLLER_TRACKING_LOSS_DELAY_NS + 1, + ) + for device_id in ( + OPENXR_LEFT_CONTROLLER_DEVICE_ID, + OPENXR_RIGHT_CONTROLLER_DEVICE_ID, + ): + device = monitor.get_device_status(device_id) + assert (device.status, device.reason, device.error) == ( + DeviceState.CONNECTED, + StatusReason.NONE, + "", + ) + + +def test_openxr_controller_observation_errors_are_isolated_per_side(): + monitor = StatusMonitor() + monitor.rebuild( + (), + include_openxr=True, + external_openxr=True, + include_openxr_controllers=True, + now_ns=1, + ) + monitor.mark_available(now_ns=2) + + monitor.refresh( + object(), + openxr_controllers={ + "left": SimpleNamespace(), + "right": controller_snapshot(), + }, + now_ns=3, + ) + + left = monitor.get_device_status(OPENXR_LEFT_CONTROLLER_DEVICE_ID) + right = monitor.get_device_status(OPENXR_RIGHT_CONTROLLER_DEVICE_ID) + assert (left.status, left.reason) == ( + DeviceState.UNKNOWN, + StatusReason.OPENXR_POLL_ERROR, + ) + assert (right.status, right.reason) == ( + DeviceState.CONNECTED, + StatusReason.NONE, + ) + assert ( + monitor.get_device_status(OPENXR_DEVICE_ID).reason + == StatusReason.EXTERNAL_OPENXR_HEALTH_UNKNOWN + ) + + +def test_openxr_provider_failure_makes_controller_inventory_unavailable(): + monitor = StatusMonitor() + monitor.rebuild( + (), + include_openxr=True, + external_openxr=False, + include_openxr_controllers=True, + now_ns=1, + ) + monitor.mark_available(now_ns=2) + + monitor.refresh( + object(), + FakeOpenXRSession( + openxr_snapshot( + state="FAILED", + reason="SESSION_LOST", + error="session lost", + ) + ), + openxr_controllers={ + "left": controller_snapshot(), + "right": controller_snapshot(), + }, + now_ns=3, + ) + + assert {device.status for device in monitor.get_status().devices} == { + DeviceState.UNAVAILABLE + } + assert {device.reason for device in monitor.get_status().devices} == { + StatusReason.OPENXR_SESSION_LOST + } + + @pytest.mark.parametrize( ( "native_snapshot", diff --git a/tests/python/core/teleop_session_manager/test_teleop_session.py b/tests/python/core/teleop_session_manager/test_teleop_session.py index 919e79de11..7c37700560 100644 --- a/tests/python/core/teleop_session_manager/test_teleop_session.py +++ b/tests/python/core/teleop_session_manager/test_teleop_session.py @@ -1563,6 +1563,38 @@ def test_plugin_args_default_empty(self, tmp_path): class TestStatusMonitoringIntegration: """Test monitored-plugin inventory, refresh, and provider failure integration.""" + def test_controller_sources_add_live_per_side_status(self): + class ControllerSample(float): + def __new__(cls, value, *, aim_valid=True, grip_valid=True): + sample = super().__new__(cls, value) + sample.aim_pose = SimpleNamespace(is_valid=aim_valid) + sample.grip_pose = SimpleNamespace(is_valid=grip_valid) + return sample + + controllers = MockControllersSource() + controllers._tracker._left_controller = ControllerSample(3.0) + controllers._tracker._right_controller = ControllerSample(4.0) + config = make_config(MockPipeline(leaf_nodes=[controllers])) + + with mock_session_dependencies(): + with TeleopSession(config) as session: + assert {device.id for device in session.get_status().devices} == { + "openxr/headset", + "openxr/controller/left", + "openxr/controller/right", + } + + session.step() + + assert ( + session.get_device_status("openxr/controller/left").status + == teleop_session_manager.DeviceState.CONNECTED + ) + assert ( + session.get_device_status("openxr/controller/right").status + == teleop_session_manager.DeviceState.CONNECTED + ) + @pytest.mark.parametrize("plugin", ["spacemouse", "gamepad"]) def test_device_plugin_is_monitored(self, tmp_path, plugin): manager = MockPluginManager(plugin_names=[plugin])