From 1b964c036c028d92a7a57ef6a0c36bfe36acee84 Mon Sep 17 00:00:00 2001 From: zihan Date: Wed, 28 Oct 2026 17:35:19 +0800 Subject: [PATCH 1/5] fix(sharpa-avatar): backport lifecycle and health fixes Signed-off-by: zihan --- .../device/hand_tracking/sharpa_avatar.rst | 26 +- examples/sharpa_avatar/README.md | 3 +- .../sharpa_avatar/__main__.py | 5 +- .../sharpa_avatar/live_view.py | 33 +- .../inc/live_trackers/schema_tracker_base.hpp | 2 + .../cpp/live_hand_tracker_impl.cpp | 153 ++-- .../cpp/live_hand_tracker_impl.hpp | 16 +- .../live_trackers/cpp/schema_tracker_base.cpp | 23 +- src/plugins/sharpa_avatar/AGENTS.md | 17 + src/plugins/sharpa_avatar/README.md | 28 +- src/plugins/sharpa_avatar/app/main.cpp | 2 +- src/plugins/sharpa_avatar/core/CMakeLists.txt | 2 + .../core/avatar_hand_tracking_plugin.cpp | 777 ++++++++++++++---- .../sharpa_avatar/core/avatar_health.cpp | 224 +++++ .../avatar/avatar_hand_tracking_plugin.hpp | 69 +- .../core/inc/avatar/avatar_health.hpp | 63 ++ src/plugins/sharpa_avatar/plugin.yaml.in | 9 + .../teleop_session_manager/teleop_session.py | 7 +- tests/cpp/plugins/CMakeLists.txt | 4 + .../cpp/plugins/sharpa_avatar/CMakeLists.txt | 14 + .../sharpa_avatar/test_avatar_health.cpp | 282 +++++++ .../test_teleop_session.py | 34 +- 22 files changed, 1557 insertions(+), 236 deletions(-) create mode 100644 src/plugins/sharpa_avatar/core/avatar_health.cpp create mode 100644 src/plugins/sharpa_avatar/core/inc/avatar/avatar_health.hpp create mode 100644 tests/cpp/plugins/sharpa_avatar/CMakeLists.txt create mode 100644 tests/cpp/plugins/sharpa_avatar/test_avatar_health.cpp diff --git a/docs/source/device/hand_tracking/sharpa_avatar.rst b/docs/source/device/hand_tracking/sharpa_avatar.rst index a7cde9d791..b8b609c32d 100644 --- a/docs/source/device/hand_tracking/sharpa_avatar.rst +++ b/docs/source/device/hand_tracking/sharpa_avatar.rst @@ -5,9 +5,9 @@ Sharpa AvatarExo Glove ====================== A Linux-only plugin for integrating Sharpa AvatarExo (AE) Gloves into the -Isaac Capture framework. The gloves stream IMU and encoder data through the -official Avatar SDK over USB. The plugin maps that stream into Isaac Capture's -usual surfaces: OpenXR hand tracking for the HUMAN skeleton, +Isaac Capture framework. The gloves stream IMU and encoder data wirelessly +through a USB dongle and the official Avatar SDK. The plugin maps that stream +into Isaac Capture's usual surfaces: OpenXR hand tracking for the HUMAN skeleton, ``JointStateOutput`` tensors for the RAW and ROBOT 22-DoF joint sets, and inbound per-finger vibration on a haptic collection. @@ -33,7 +33,7 @@ named joint-state collections: .. code-block:: text - Sharpa AvatarExo Gloves (USB) + Sharpa AvatarExo Gloves (wireless via USB dongle) │ ▼ avatar_hand_plugin (Avatar SDK, production package) @@ -56,9 +56,7 @@ Prerequisites - **Pinned production Avatar SDK** (``avatar-sdk`` 1.7.3-17) from Sharpa's signed APT repository. ``install_avatar_sdk.sh`` retrieves it; do not substitute ``avatar-sdk-dev`` or ``avatar-sdk-beta``. -- **Sharpa AvatarExo Gloves** powered and connected over USB. Transport settings - are selected in ``/share/sdk_config.json`` (default root - ``/opt/avatar-sdk``). +- **Sharpa AvatarExo Gloves** powered and paired with the USB wireless dongle. - **CloudXR runtime** on the plugin host, unless a ``TeleopSession`` example launches it for you. - A built Isaac Capture checkout. CMake 3.24 or newer is required. @@ -175,6 +173,16 @@ The plugin reads ``HapticCommand`` on ``avatar_glove_haptic`` and drives five finger vibration motors. The visualization example uses the same per-finger pinch path as the generic glove haptic sample. +Health reporting +~~~~~~~~~~~~~~~~ + +The plugin reports the USB dongle, each physical glove, and each dataset as +separate devices. Physical-glove health combines SDK inventory, live ingress, +firmware diagnostics, and haptic errors. HUMAN, RAW, and ROBOT then report their +own freshness and fetch errors, so one healthy stream cannot hide another +stalled stream. Outputs disappear after ingress stops and are recreated when +the glove resumes; existing DeviceIO consumers bind to the new generation. + Wrist pose ---------- @@ -208,9 +216,9 @@ Troubleshooting - Install the pinned production package, or point ``AVATAR_SDK_ROOT`` at a complete SDK tree. A ``-dev`` / ``-beta`` package at ``/opt/avatar-sdk`` is not the pin. - * - USB glove is not detected + * - USB dongle is not detected - Run ``install_udev_rules.sh`` on the host, then unplug and reconnect - the AE Gloves. + the USB dongle. * - Plugin binary is not found - Build and install through the top-level CMake workflow. The example looks under ``install/plugins/sharpa_avatar/``. diff --git a/examples/sharpa_avatar/README.md b/examples/sharpa_avatar/README.md index 4b4a2acce4..45be8354de 100644 --- a/examples/sharpa_avatar/README.md +++ b/examples/sharpa_avatar/README.md @@ -26,7 +26,7 @@ cmake -B build \ cmake --build build --parallel cmake --install build uv pip install -e ./examples/sharpa_avatar -python -m isaaccapture_examples.sharpa_avatar +python -m isaaccapture_examples.sharpa_avatar --print-health ``` Open the URL it prints (default ). The left hand is cyan @@ -43,6 +43,7 @@ above first. `uv pip install -e` pulls `viser`; CloudXR comes from the ```text --no-haptic disable pinch feedback --no-viz terminal + haptic only (no browser view) +--print-health print Sharpa device health initially and on changes --host / --port viser bind (default 127.0.0.1:8080) --no-launch-plugin connect to an already running plugin --no-launch-cloudxr-runtime connect to an already running CloudXR runtime diff --git a/examples/sharpa_avatar/python/isaaccapture_examples/sharpa_avatar/__main__.py b/examples/sharpa_avatar/python/isaaccapture_examples/sharpa_avatar/__main__.py index 375c7b4155..b8dc23189b 100644 --- a/examples/sharpa_avatar/python/isaaccapture_examples/sharpa_avatar/__main__.py +++ b/examples/sharpa_avatar/python/isaaccapture_examples/sharpa_avatar/__main__.py @@ -6,4 +6,7 @@ from .live_view import main -sys.exit(main()) +try: + sys.exit(main()) +except KeyboardInterrupt: + pass diff --git a/examples/sharpa_avatar/python/isaaccapture_examples/sharpa_avatar/live_view.py b/examples/sharpa_avatar/python/isaaccapture_examples/sharpa_avatar/live_view.py index e30ae82ab6..f11342ab28 100644 --- a/examples/sharpa_avatar/python/isaaccapture_examples/sharpa_avatar/live_view.py +++ b/examples/sharpa_avatar/python/isaaccapture_examples/sharpa_avatar/live_view.py @@ -157,8 +157,6 @@ def _finger_chain(root: int, *joints: int) -> list[tuple[int, int]]: HandJointIndex.WRIST, HandJointIndex.LITTLE_METACARPAL, HandJointIndex.LITTLE_PROXIMAL, - HandJointIndex.LITTLE_INTERMEDIATE, - HandJointIndex.LITTLE_DISTAL, HandJointIndex.LITTLE_TIP, ) ) @@ -388,6 +386,21 @@ def _fmt_optional(group, *, kind: str, hand_input_index) -> str: return f"error:{type(exc).__name__}" +def _print_health_changes(snapshot, previous: dict[str, tuple]) -> None: + for device in snapshot.devices: + if not device.id.startswith(f"{PLUGIN_ROOT_ID}/"): + continue + current = (device.status, device.reason, device.error) + if previous.get(device.id) == current: + continue + previous[device.id] = current + detail = f": {device.error}" if device.error else "" + print( + f"[HEALTH] {device.id}: {device.status.value} " + f"({device.reason.value}){detail}" + ) + + def _build_haptic_pipeline(hands, mapping, sinks, hands_source_cls) -> None: from isaaccapture.haptic_devices.glove import haptic_glove_device from isaaccapture.retargeters.tactile_retargeters import ( @@ -489,6 +502,11 @@ def main() -> int: default=DEFAULT_PRINT_HZ, help="Terminal status rate. 0 silences prints.", ) + parser.add_argument( + "--print-health", + action="store_true", + help="Print Sharpa device health initially and whenever it changes.", + ) parser.add_argument( "--haptic", action=argparse.BooleanOptionalAction, @@ -621,6 +639,7 @@ def main() -> int: print_period = (1.0 / args.print_hz) if args.print_hz > 0 else None next_print = time.monotonic() last_offline_hint = 0.0 + previous_health: dict[str, tuple] = {} upright = not args.world_frame print("=" * 72) @@ -644,13 +663,21 @@ def main() -> int: print(f" viser: http://{args.host}:{args.port} (Ctrl+C to exit)") else: print(" viser: off (--no-viz); terminal + haptic only. Ctrl+C to exit.") + print(f" health: {'changes' if args.print_health else 'off'}") print("=" * 72) with CloudXRLauncher.launch_context(args), TeleopSession(config) as session: if args.viz: _server, viz_left, viz_right = _start_viser(args) while True: - result = session.step() + try: + result = session.step() + except Exception: + if args.print_health: + _print_health_changes(session.get_status(), previous_health) + raise + if args.print_health: + _print_health_changes(session.get_status(), previous_health) n_l = n_r = 0 if viz_left is not None and viz_right is not None: pts, mask, n_l = _hand_points( diff --git a/src/core/live_trackers/cpp/inc/live_trackers/schema_tracker_base.hpp b/src/core/live_trackers/cpp/inc/live_trackers/schema_tracker_base.hpp index 119cfe3ba5..410d982192 100644 --- a/src/core/live_trackers/cpp/inc/live_trackers/schema_tracker_base.hpp +++ b/src/core/live_trackers/cpp/inc/live_trackers/schema_tracker_base.hpp @@ -118,6 +118,7 @@ class SchemaTrackerBase private: void initialize_tensor_data_functions(); void create_tensor_list(); + void clear_collection() noexcept; bool ensure_collection(); bool read_next_sample(SampleResult& out); @@ -137,6 +138,7 @@ class SchemaTrackerBase PFN_xrDestroyTensorListNV m_destroy_list_fn{ nullptr }; std::optional m_target_collection_index; + std::optional m_target_collection_id; uint32_t m_sample_batch_stride{ 0 }; uint32_t m_sample_size{ 0 }; diff --git a/src/core/live_trackers/cpp/live_hand_tracker_impl.cpp b/src/core/live_trackers/cpp/live_hand_tracker_impl.cpp index 7544f5a6f3..828b1e55d4 100644 --- a/src/core/live_trackers/cpp/live_hand_tracker_impl.cpp +++ b/src/core/live_trackers/cpp/live_hand_tracker_impl.cpp @@ -21,6 +21,8 @@ namespace core namespace { +constexpr int64_t kXDevRefreshIntervalNs = 1'000'000'000; + template std::string bounded_string(const char (&value)[N]) { @@ -45,9 +47,6 @@ bool is_display_device_xdev(const XrXDevPropertiesMNDX& properties) const std::string name = bounded_string(properties.name); const std::string serial = bounded_string(properties.serial); - auto logger = isaaccapture::Logger::get("isaaccapture.core.LiveHandTrackerImpl"); - logger->debug("candidate XDev name: {}, serial: {}", name, serial); - return contains_case_insensitive(name, "displaydevice") || contains_case_insensitive(name, "display device") || contains_case_insensitive(serial, "displaydevice") || contains_case_insensitive(serial, "display device") || contains_case_insensitive(name, "head device") || contains_case_insensitive(serial, "head device"); @@ -75,6 +74,7 @@ std::unique_ptr LiveHandTrackerImpl::create_mcap_channels(mcap LiveHandTrackerImpl::LiveHandTrackerImpl(const OpenXRSessionHandles& handles, std::unique_ptr mcap_channels) : time_converter_(handles), + session_(handles.session), base_space_(handles.space), xdev_list_(XR_NULL_HANDLE), pfn_create_hand_tracker_(nullptr), @@ -128,47 +128,42 @@ LiveHandTrackerImpl::LiveHandTrackerImpl(const OpenXRSessionHandles& handles, { initialize_xdev_hand_trackers(handles); - // Keep the plain session hand trackers wired as the final fallback candidate. - // - // Current deployments are expected to use the XDev-backed candidates above because - // XR_MNDX_xdev_space is now a required extension for live hand tracking. In that - // normal path these default trackers should not be the data source selected by - // update_hand(); they sit after every XDev tracker in the priority list and only win - // if all XDev candidates fail to create or return inactive data. - // - // We intentionally keep this path alive instead of deleting it because future runtime - // modes may use the default/display hand tracker as a real fallback source once the - // source-priority policy is expanded beyond today's XDev-first behavior. - try_create_default_hand_tracker(handles.session, XR_HAND_LEFT_EXT, left_hand_trackers_); - try_create_default_hand_tracker(handles.session, XR_HAND_RIGHT_EXT, right_hand_trackers_); - - if (left_hand_trackers_.empty()) + // Default trackers stay alive across XDev generations and remain the final fallback. + try_create_default_hand_tracker(handles.session, XR_HAND_LEFT_EXT, left_default_hand_trackers_); + try_create_default_hand_tracker(handles.session, XR_HAND_RIGHT_EXT, right_default_hand_trackers_); + + if (left_xdev_hand_trackers_.empty() && left_default_hand_trackers_.empty()) { throw std::runtime_error("Failed to create any left hand tracker"); } - if (right_hand_trackers_.empty()) + if (right_xdev_hand_trackers_.empty() && right_default_hand_trackers_.empty()) { throw std::runtime_error("Failed to create any right hand tracker"); } } catch (...) { - destroy_hand_trackers(left_hand_trackers_); - destroy_hand_trackers(right_hand_trackers_); + destroy_hand_trackers(left_xdev_hand_trackers_); + destroy_hand_trackers(right_xdev_hand_trackers_); + destroy_hand_trackers(left_default_hand_trackers_); + destroy_hand_trackers(right_default_hand_trackers_); destroy_xdev_list(); throw; } - logger_->info("HandTracker initialized (left candidates: {}, right candidates: {})", left_hand_trackers_.size(), - right_hand_trackers_.size()); + logger_->info("HandTracker initialized (left candidates: {}, right candidates: {})", + left_xdev_hand_trackers_.size() + left_default_hand_trackers_.size(), + right_xdev_hand_trackers_.size() + right_default_hand_trackers_.size()); } LiveHandTrackerImpl::~LiveHandTrackerImpl() { assert(pfn_destroy_hand_tracker_ != nullptr && "pfn_destroy_hand_tracker must not be null"); - destroy_hand_trackers(left_hand_trackers_); - destroy_hand_trackers(right_hand_trackers_); + destroy_hand_trackers(left_xdev_hand_trackers_); + destroy_hand_trackers(right_xdev_hand_trackers_); + destroy_hand_trackers(left_default_hand_trackers_); + destroy_hand_trackers(right_default_hand_trackers_); destroy_xdev_list(); } @@ -179,11 +174,16 @@ void LiveHandTrackerImpl::update(int64_t monotonic_time_ns) left_tracked_.reset(); right_tracked_.reset(); last_update_time_ = monotonic_time_ns; + if (last_xdev_refresh_time_ == 0 || monotonic_time_ns - last_xdev_refresh_time_ >= kXDevRefreshIntervalNs) + { + refresh_xdev_hand_trackers(); + last_xdev_refresh_time_ = monotonic_time_ns; + } const XrTime xr_time = time_converter_.convert_monotonic_ns_to_xrtime(monotonic_time_ns); std::optional left_native; std::optional right_native; - update_hand(left_hand_trackers_, xr_time, left_native); - update_hand(right_hand_trackers_, xr_time, right_native); + update_hand(left_xdev_hand_trackers_, left_default_hand_trackers_, xr_time, left_native); + update_hand(right_xdev_hand_trackers_, right_default_hand_trackers_, xr_time, right_native); const DeviceDataTimestamp timestamp(last_update_time_, last_update_time_, xr_time); left_tracked_ = publish_and_record(mcap_channels_.get(), 0, timestamp, value_ptr(left_native)); @@ -228,20 +228,31 @@ void LiveHandTrackerImpl::initialize_xdev_hand_trackers(const OpenXRSessionHandl return; } - uint32_t xdev_count = 0; - result = pfn_enumerate_xdevs_(xdev_list_, 0, &xdev_count, nullptr); - if (XR_FAILED(result) || xdev_count == 0) + refresh_xdev_hand_trackers(); +} + +bool LiveHandTrackerImpl::enumerate_xdev_ids(std::vector& ordered_ids) +{ + if (xdev_list_ == XR_NULL_HANDLE) { - destroy_xdev_list(); - return; + return false; } - std::vector xdev_ids(xdev_count); - result = pfn_enumerate_xdevs_(xdev_list_, xdev_count, &xdev_count, xdev_ids.data()); + uint32_t xdev_count = 0; + XrResult result = pfn_enumerate_xdevs_(xdev_list_, 0, &xdev_count, nullptr); if (XR_FAILED(result)) { - destroy_xdev_list(); - return; + return false; + } + std::vector xdev_ids(xdev_count); + if (xdev_count != 0) + { + result = pfn_enumerate_xdevs_(xdev_list_, xdev_count, &xdev_count, xdev_ids.data()); + if (XR_FAILED(result)) + { + return false; + } + xdev_ids.resize(xdev_count); } std::vector preferred_xdev_ids; @@ -255,7 +266,7 @@ void LiveHandTrackerImpl::initialize_xdev_hand_trackers(const OpenXRSessionHandl result = pfn_get_xdev_properties_(xdev_list_, &get_info, &properties); if (XR_FAILED(result)) { - continue; + return false; } if (is_display_device_xdev(properties)) @@ -268,22 +279,42 @@ void LiveHandTrackerImpl::initialize_xdev_hand_trackers(const OpenXRSessionHandl } } - auto add_xdev_candidates = [this, &handles](const std::vector& xdev_ids_to_add) - { - for (const XrXDevIdMNDX xdev_id : xdev_ids_to_add) - { - try_create_xdev_hand_tracker(handles.session, xdev_id, XR_HAND_LEFT_EXT, left_hand_trackers_); - try_create_xdev_hand_tracker(handles.session, xdev_id, XR_HAND_RIGHT_EXT, right_hand_trackers_); - } - }; + ordered_ids.reserve(preferred_xdev_ids.size() + display_xdev_ids.size()); + ordered_ids.insert(ordered_ids.end(), preferred_xdev_ids.begin(), preferred_xdev_ids.end()); + ordered_ids.insert(ordered_ids.end(), display_xdev_ids.begin(), display_xdev_ids.end()); + return true; +} - add_xdev_candidates(preferred_xdev_ids); - add_xdev_candidates(display_xdev_ids); +bool LiveHandTrackerImpl::refresh_xdev_hand_trackers() +{ + std::vector next_xdev_ids; + if (!enumerate_xdev_ids(next_xdev_ids)) + { + return false; + } + if (next_xdev_ids == xdev_ids_) + { + return true; + } - if (left_hand_trackers_.empty() && right_hand_trackers_.empty()) + std::vector next_left_trackers; + std::vector next_right_trackers; + next_left_trackers.reserve(next_xdev_ids.size()); + next_right_trackers.reserve(next_xdev_ids.size()); + for (const XrXDevIdMNDX xdev_id : next_xdev_ids) { - destroy_xdev_list(); + try_create_xdev_hand_tracker(session_, xdev_id, XR_HAND_LEFT_EXT, next_left_trackers); + try_create_xdev_hand_tracker(session_, xdev_id, XR_HAND_RIGHT_EXT, next_right_trackers); } + + left_xdev_hand_trackers_.swap(next_left_trackers); + right_xdev_hand_trackers_.swap(next_right_trackers); + destroy_hand_trackers(next_left_trackers); + destroy_hand_trackers(next_right_trackers); + xdev_ids_ = std::move(next_xdev_ids); + logger_->info("Refreshed XDev hand trackers (devices: {}, left: {}, right: {})", xdev_ids_.size(), + left_xdev_hand_trackers_.size(), right_xdev_hand_trackers_.size()); + return true; } bool LiveHandTrackerImpl::try_create_xdev_hand_tracker(XrSession session, @@ -363,21 +394,29 @@ void LiveHandTrackerImpl::destroy_xdev_list() } } -void LiveHandTrackerImpl::update_hand(const std::vector& trackers, +void LiveHandTrackerImpl::update_hand(const std::vector& xdev_trackers, + const std::vector& default_trackers, XrTime time, std::optional& tracked) { - for (XrHandTrackerEXT tracker : trackers) + const auto try_candidates = [this, time, &tracked](const std::vector& trackers) { - std::optional candidate; - if (try_update_hand(tracker, time, candidate)) + for (XrHandTrackerEXT tracker : trackers) { - tracked = std::move(candidate); - return; + std::optional candidate; + if (try_update_hand(tracker, time, candidate)) + { + tracked = std::move(candidate); + return true; + } } - } + return false; + }; - tracked.reset(); + if (!try_candidates(xdev_trackers) && !try_candidates(default_trackers)) + { + tracked.reset(); + } } bool LiveHandTrackerImpl::try_update_hand(XrHandTrackerEXT tracker, XrTime time, std::optional& tracked) diff --git a/src/core/live_trackers/cpp/live_hand_tracker_impl.hpp b/src/core/live_trackers/cpp/live_hand_tracker_impl.hpp index 55b17d5608..8d0c59dacf 100644 --- a/src/core/live_trackers/cpp/live_hand_tracker_impl.hpp +++ b/src/core/live_trackers/cpp/live_hand_tracker_impl.hpp @@ -44,6 +44,8 @@ class LiveHandTrackerImpl : public IHandTrackerImpl private: void initialize_xdev_hand_trackers(const OpenXRSessionHandles& handles); + bool enumerate_xdev_ids(std::vector& xdev_ids); + bool refresh_xdev_hand_trackers(); bool try_create_xdev_hand_tracker(XrSession session, XrXDevIdMNDX xdev_id, XrHandEXT hand, @@ -51,16 +53,24 @@ class LiveHandTrackerImpl : public IHandTrackerImpl bool try_create_default_hand_tracker(XrSession session, XrHandEXT hand, std::vector& trackers); void destroy_hand_trackers(std::vector& trackers); void destroy_xdev_list(); - void update_hand(const std::vector& trackers, XrTime time, std::optional& tracked); + void update_hand(const std::vector& xdev_trackers, + const std::vector& default_trackers, + XrTime time, + std::optional& tracked); bool try_update_hand(XrHandTrackerEXT tracker, XrTime time, std::optional& tracked); XrTimeConverter time_converter_; + XrSession session_; XrSpace base_space_; std::shared_ptr logger_ = isaaccapture::Logger::get("isaaccapture.core.LiveHandTrackerImpl"); - std::vector left_hand_trackers_; - std::vector right_hand_trackers_; + std::vector left_xdev_hand_trackers_; + std::vector right_xdev_hand_trackers_; + std::vector left_default_hand_trackers_; + std::vector right_default_hand_trackers_; + std::vector xdev_ids_; XrXDevListMNDX xdev_list_; + int64_t last_xdev_refresh_time_ = 0; // The snapshots published each frame, encoded from locals in update(). Serialized left_tracked_; diff --git a/src/core/live_trackers/cpp/schema_tracker_base.cpp b/src/core/live_trackers/cpp/schema_tracker_base.cpp index d99cdb39b1..4ffa5cfa9c 100644 --- a/src/core/live_trackers/cpp/schema_tracker_base.cpp +++ b/src/core/live_trackers/cpp/schema_tracker_base.cpp @@ -75,7 +75,7 @@ bool SchemaTrackerBase::ensure_collection() { if (is_known_non_fatal_tensor_unavailable(result)) { - m_target_collection_index.reset(); + clear_collection(); return false; } throw std::runtime_error("Failed to get latest generation, result=" + std::to_string(result)); @@ -93,7 +93,7 @@ bool SchemaTrackerBase::ensure_collection() { if (is_known_non_fatal_tensor_unavailable(result)) { - m_target_collection_index.reset(); + clear_collection(); return false; } throw std::runtime_error("Failed to update tensor list, result=" + std::to_string(result)); @@ -107,7 +107,7 @@ bool SchemaTrackerBase::ensure_collection() { if (is_known_non_fatal_tensor_unavailable(result)) { - m_target_collection_index.reset(); + clear_collection(); return false; } throw std::runtime_error("Failed to get list properties, result=" + std::to_string(result)); @@ -121,7 +121,7 @@ bool SchemaTrackerBase::ensure_collection() { if (is_known_non_fatal_tensor_unavailable(result)) { - m_target_collection_index.reset(); + clear_collection(); return false; } throw std::runtime_error("Failed to get collection properties, result=" + std::to_string(result)); @@ -143,19 +143,26 @@ bool SchemaTrackerBase::ensure_collection() m_sample_batch_stride = coll_props.sampleBatchStride; m_sample_size = coll_props.data.totalSampleSize; - if (i != m_target_collection_index) + if (i != m_target_collection_index || coll_props.tensorCollectionId != m_target_collection_id) { m_last_sample_index.reset(); m_logger->info("Found target collection at index {}", i); } m_target_collection_index = i; + m_target_collection_id = coll_props.tensorCollectionId; return true; } - m_target_collection_index.reset(); + clear_collection(); return false; } +void SchemaTrackerBase::clear_collection() noexcept +{ + m_target_collection_index.reset(); + m_target_collection_id.reset(); +} + bool SchemaTrackerBase::read_all_samples(std::vector& samples) { if (!ensure_collection()) @@ -252,10 +259,10 @@ bool SchemaTrackerBase::read_next_sample(SampleResult& out) } else { - m_target_collection_index = std::nullopt; + clear_collection(); throw std::runtime_error("[SchemaTracker] xrGetTensorDataNV failed, result=" + std::to_string(result)); } - m_target_collection_index = std::nullopt; + clear_collection(); return false; } diff --git a/src/plugins/sharpa_avatar/AGENTS.md b/src/plugins/sharpa_avatar/AGENTS.md index 560e94301d..d2fad8d191 100644 --- a/src/plugins/sharpa_avatar/AGENTS.md +++ b/src/plugins/sharpa_avatar/AGENTS.md @@ -48,6 +48,23 @@ SPDX-License-Identifier: Apache-2.0 - OpenXR setup is required for publication; initialization errors must abort construction instead of leaving an Avatar-only idle plugin. - Keep initialization non-blocking; `update()` owns retries for absent gloves. +- In pinned SDK 1.7.3, `get_device()` returns an initialized handle; do not call + `init()` again. Use its lightweight ingress callback for liveness and keep SDK + work off the callback thread. Scope callback signals to one device generation; + callbacks must not retain a `GloveState` address across reset or destruction. +- USB transport health combines physical enumeration with + `AvatarSDK::get_usb_connection_state()`: the production SDK can retain + `CONNECTED` while its serial listener is reconnecting, so neither signal is + sufficient alone. Before stopping a glove handle, best-effort clear manual + haptics. Isolate dynamic output setup failures to that side and retry without + terminating the plugin. +- Treat a successful `list_device_info()` result as the handle-lifecycle + authority: retain handles while their row exists (including `online=false`), + and release them only when a successful snapshot drops the row. A failed + refresh makes inventory health unknown and must not reconcile cached rows. +- Keep transport presence, physical-glove ingress/faults, and HUMAN/RAW/ROBOT + output freshness/errors as separate health layers. One advancing dataset must + not keep another dataset healthy. - Keep the static HUMAN-to-OpenXR index table synchronized with the pinned production SDK's landmark order. Do not read joint names from `sdk_config.json` at runtime or fill unsupported OpenXR slots with neighbouring diff --git a/src/plugins/sharpa_avatar/README.md b/src/plugins/sharpa_avatar/README.md index afc8da4b0e..035ce9624b 100644 --- a/src/plugins/sharpa_avatar/README.md +++ b/src/plugins/sharpa_avatar/README.md @@ -22,7 +22,7 @@ The visualization example is separate: - Linux x86_64 (Ubuntu 22.04) - A built Isaac Capture checkout -- Sharpa Avatar gloves connected through the USB dongle or wired Ethernet +- Sharpa AvatarExo Gloves connected wirelessly through the USB dongle `install_avatar_sdk.sh` retrieves a pinned production `avatar-sdk` version from Sharpa's signed production APT repository, without selecting the `avatar-sdk-dev` or @@ -72,8 +72,10 @@ or dongle: udev does not run inside containers, so this command must run on the host. The plugin follows the Sharpa host application layout and uses the SDK, configuration, and runtime assets directly from the selected SDK root -(`AVATAR_SDK_ROOT`, default `/opt/avatar-sdk`). Transport selection, including -wired Ethernet, is controlled by `/share/sdk_config.json`. +(`AVATAR_SDK_ROOT`, default `/opt/avatar-sdk`). The supported workflow uses the +USB dongle for a wireless glove connection. This plugin does not accept a +`--transport` option and must not run concurrently with `avatar-backend` or +Avatar Desktop. ## Run @@ -97,6 +99,24 @@ The gloves do not provide a world-space wrist pose. Downstream consumers that need a display frame (the example) place both skeletons locally; a headset wrist source is used when one is available. +## Health reporting + +When launched by `TeleopSession`, the plugin reports three independent layers: + +- `/transport/dongle`: physical USB enumeration combined with the Avatar SDK + transport connection state. +- `/glove/{left,right}`: SDK inventory, current ingress, firmware diagnostics, + and haptic command errors for each physical glove. +- The HUMAN, RAW, and ROBOT manifest paths: per-dataset sample freshness and + `fetch_data()` errors. + +A failed SDK inventory refresh makes glove and dataset health unknown; cached +rows are not republished as newly healthy. A successfully refreshed inventory +row remains the same physical glove while `online=false`, allowing the SDK's +short reconnect path to recover it. Outputs are withdrawn after three seconds +without ingress and recreated on the next fresh sample. The timeout uses +`std::chrono::steady_clock` and is unaffected by wall-clock adjustments. + ## Troubleshooting | Symptom | Resolution | @@ -104,7 +124,7 @@ wrist source is used when one is available. | Avatar SDK installation fails | Check access to Sharpa's production APT endpoint and rerun `install_avatar_sdk.sh` | | CMake skipped the Sharpa Avatar plugin | The SDK is missing; run `install_avatar_sdk.sh` and reconfigure with `-DBUILD_PLUGIN_SHARPA_AVATAR=ON` | | CMake rejected the Avatar SDK | Install the pinned production package with `install_avatar_sdk.sh`, or point `AVATAR_SDK_ROOT` at a complete SDK tree | -| USB glove is not detected | Run `install_udev_rules.sh` on the host, then unplug and reconnect the glove or dongle | +| USB dongle is not detected | Run `install_udev_rules.sh` on the host, then unplug and reconnect the dongle | | CMake 3.24 or newer is required | Install a newer CMake in the Isaac Capture environment and rerun the configure/build/install commands above | | Plugin binary is not found | Build and install the plugin through the top-level CMake workflow above | | Hand and joint streams stay offline | Power on the gloves and stop any other process using the Avatar SDK | diff --git a/src/plugins/sharpa_avatar/app/main.cpp b/src/plugins/sharpa_avatar/app/main.cpp index cfd50cb117..6c813ee4c8 100644 --- a/src/plugins/sharpa_avatar/app/main.cpp +++ b/src/plugins/sharpa_avatar/app/main.cpp @@ -57,7 +57,7 @@ AvatarPluginConfig parse_args(int argc, char** argv) } else if (starts_with(arg, "--plugin-root-id=")) { - // Injected by the PluginManager; unused by this plugin. + config.monitoring_plugin_root_id = arg.substr(std::string("--plugin-root-id=").size()); } else if (!starts_with(arg, "--")) { diff --git a/src/plugins/sharpa_avatar/core/CMakeLists.txt b/src/plugins/sharpa_avatar/core/CMakeLists.txt index 8a47428c7e..8dbe5e0137 100644 --- a/src/plugins/sharpa_avatar/core/CMakeLists.txt +++ b/src/plugins/sharpa_avatar/core/CMakeLists.txt @@ -3,7 +3,9 @@ add_library(avatar_plugin_core STATIC avatar_glove_collection.hpp + avatar_health.cpp avatar_hand_tracking_plugin.cpp + inc/avatar/avatar_health.hpp inc/avatar/avatar_hand_tracking_plugin.hpp ) diff --git a/src/plugins/sharpa_avatar/core/avatar_hand_tracking_plugin.cpp b/src/plugins/sharpa_avatar/core/avatar_hand_tracking_plugin.cpp index 9a7df1454d..950cbf40a6 100644 --- a/src/plugins/sharpa_avatar/core/avatar_hand_tracking_plugin.cpp +++ b/src/plugins/sharpa_avatar/core/avatar_hand_tracking_plugin.cpp @@ -18,12 +18,14 @@ #include #include #include +#include +#include #include #include +#include #include #include #include -#include #include #include @@ -36,9 +38,35 @@ namespace { constexpr size_t kJointFlatbufferSize = 4096; -constexpr auto kAvatarDataTimeout = std::chrono::seconds(3); constexpr auto kGloveRetryInterval = std::chrono::seconds(2); +constexpr auto kOutputSetupRetryInterval = std::chrono::seconds(2); constexpr auto kHapticCommandTimeout = std::chrono::milliseconds(200); +constexpr auto kHapticFailureRetryInterval = std::chrono::seconds(1); +constexpr auto kSdkHealthRefreshInterval = std::chrono::milliseconds(100); +constexpr auto kDonglePresenceInterval = std::chrono::seconds(1); +constexpr double kIngressCallbackMaxHz = 10.0; +constexpr uint16_t kAvatarDongleVendorId = 0x1915; +constexpr uint16_t kAvatarDongleProductId = 0x520F; +constexpr std::string_view kUsbDevicesRoot = "/sys/bus/usb/devices"; + +constexpr std::array kHandDevicePaths{ "/hand/left", "/hand/right" }; +constexpr std::array kPhysicalGlovePaths{ "/glove/left", "/glove/right" }; +constexpr std::array kRawDevicePaths{ "/glove/avatar_raw_left", "/glove/avatar_raw_right" }; +constexpr std::array kRobotDevicePaths{ "/glove/avatar_robot_left", "/glove/avatar_robot_right" }; + +constexpr std::array kRawJointNames = { + "thumb_CMC_FE", "thumb_CMC_AA", "thumb_MCP_FE", "thumb_MCP_AA", "thumb_IP", "index_MCP_AA", + "index_MCP_FE", "index_PIP", "index_DIP", "middle_MCP_AA", "middle_MCP_FE", "middle_PIP", + "middle_DIP", "ring_MCP_AA", "ring_MCP_FE", "ring_PIP", "ring_DIP", "pinky_MCP_AA", + "pinky_MCP_FE", "pinky_DIP1", "pinky_DIP2", "pinky_DIP3", +}; + +constexpr std::array kRobotJointNames = { + "thumb_CMC_FE", "thumb_CMC_AA", "thumb_MCP_FE", "thumb_MCP_AA", "thumb_IP", "index_MCP_FE", + "index_MCP_AA", "index_PIP", "index_DIP", "middle_MCP_FE", "middle_MCP_AA", "middle_PIP", + "middle_DIP", "ring_MCP_FE", "ring_MCP_AA", "ring_PIP", "ring_DIP", "pinky_CMC", + "pinky_MCP_FE", "pinky_MCP_AA", "pinky_PIP", "pinky_DIP", +}; const std::shared_ptr& logger() { @@ -84,6 +112,16 @@ size_t joint_category_index(::avatar::DeviceDataCategory category) throw std::invalid_argument("HUMAN data is not a joint stream"); } +size_t data_category_index(::avatar::DeviceDataCategory category) +{ + const auto it = std::find(kDataCategories.begin(), kDataCategories.end(), category); + if (it == kDataCategories.end()) + { + throw std::invalid_argument("Unsupported Avatar data category"); + } + return static_cast(std::distance(kDataCategories.begin(), it)); +} + const char* collection_id(::avatar::DeviceSide side, ::avatar::DeviceDataCategory category) { static constexpr std::array, 2> kCollectionIds = { { @@ -149,6 +187,20 @@ const ::avatar::Hand& hand_payload(const ::avatar::AvatarDataFrame& frame, ::ava throw std::invalid_argument("HUMAN data has no joint payload"); } +const std::array& joint_names(::avatar::DeviceDataCategory category) +{ + switch (category) + { + case ::avatar::DeviceDataCategory::RAW: + return kRawJointNames; + case ::avatar::DeviceDataCategory::ROBOT: + return kRobotJointNames; + case ::avatar::DeviceDataCategory::HUMAN: + break; + } + throw std::invalid_argument("HUMAN data has no joint names"); +} + constexpr std::string_view error_name(::avatar::ErrorCode error) { using ErrorCode = ::avatar::ErrorCode; @@ -196,6 +248,33 @@ constexpr std::string_view error_name(::avatar::ErrorCode error) return "UNRECOGNIZED_ERROR"; } +std::optional physical_device_error(const ::avatar::DeviceInfo& info, const GloveState& glove) +{ + std::ostringstream message; + bool has_error = false; + for (const auto& [code, unused] : info.diagnostics) + { + (void)unused; + if (code == static_cast(::avatar::FaultCode::NORMAL)) + { + continue; + } + message << (has_error ? ", " : "Avatar SDK fault: ") << "0x" << std::hex << std::uppercase << code; + has_error = true; + } + if (!has_error && info.error_code != 0) + { + message << "Avatar SDK fault: 0x" << std::hex << std::uppercase << info.error_code; + has_error = true; + } + if (glove.haptic_error) + { + message << (has_error ? "; " : "") << *glove.haptic_error; + has_error = true; + } + return has_error ? std::optional(message.str()) : std::nullopt; +} + // OpenXR slot -> HUMAN landmark index for avatar-sdk 1.7.3-17. Unsupported // palm and little-finger intermediate/distal slots remain invalid. constexpr std::array, XR_HAND_JOINT_COUNT_EXT> kOpenXRSlotSourceIndices = { @@ -238,6 +317,30 @@ ::avatar::AvatarSDK& AvatarSdkSession::get() return ::avatar::AvatarSDK::get_instance(); } +void GloveIngressSignal::record() noexcept +{ + const auto now_ns = + std::chrono::duration_cast(std::chrono::steady_clock::now().time_since_epoch()).count(); + m_last_event_ns.store(now_ns, std::memory_order_relaxed); + m_generation.fetch_add(1, std::memory_order_release); +} + +uint64_t GloveIngressSignal::generation() const noexcept +{ + return m_generation.load(std::memory_order_acquire); +} + +std::optional GloveIngressSignal::last_event() const noexcept +{ + const int64_t event_ns = m_last_event_ns.load(std::memory_order_relaxed); + if (event_ns < 0) + { + return std::nullopt; + } + return std::chrono::steady_clock::time_point( + std::chrono::duration_cast(std::chrono::nanoseconds(event_ns))); +} + GloveState::~GloveState() noexcept { reset(); @@ -245,25 +348,48 @@ GloveState::~GloveState() noexcept void GloveState::reset() noexcept { - try + ingress.reset(); + if (device) { - if (device) + try + { + device->set_raw_human_frame_callback({}); + } + catch (...) + { + // Continue teardown even if the vendor callback unregister fails. + } + try { device->stop(); } + catch (...) + { + // Device release must not escape a destructor. + } } - catch (...) - { - // Reset remains safe when called from GloveState's destructor. - } - // AvatarSDK owns and destroys its cached devices with the SDK session. + clear_current_data(); + device.reset(); + observed_ingress_generation = 0; + last_ingress.reset(); + haptic_error.reset(); + output_setup_error.reset(); + last_output_setup_attempt.reset(); + started = false; + ready = false; +} + +void GloveState::clear_current_data() noexcept +{ landmarks.clear(); raw_frame = {}; robot_frame = {}; - device.reset(); - last_successful_fetch.reset(); - last_sample_stamp.reset(); - streaming = false; + for (auto& dataset : datasets) + { + dataset.last_stamp.reset(); + dataset.last_fresh_data.reset(); + dataset.fetch_error.reset(); + } } AvatarTracker::AvatarTracker(AvatarPluginConfig config) : m_config(std::move(config)), m_sdk(m_config.sdk_config_path) @@ -271,12 +397,24 @@ AvatarTracker::AvatarTracker(AvatarPluginConfig config) : m_config(std::move(con logger()->info("Avatar SDK initialized."); logger()->info("datasets: human={} raw={} robot={} haptic={}", m_config.human ? "on" : "off", m_config.raw ? "on" : "off", m_config.robot ? "on" : "off", m_config.haptic ? "on" : "off"); - try_connect_missing_gloves(); initialize_openxr(); } +AvatarTracker::~AvatarTracker() noexcept +{ + for (const ::avatar::DeviceSide side : kDeviceSides) + { + reset_glove(side); + } +} + void AvatarTracker::initialize_openxr() { + if (m_config.monitoring_plugin_root_id && m_config.monitoring_plugin_root_id->empty()) + { + throw std::invalid_argument("AvatarTracker monitoring_plugin_root_id must not be empty"); + } + plugin_utils::WristSourceConfig wrist_config; wrist_config.mode = plugin_utils::WristSourceMode::Auto; wrist_config.left_aim_to_wrist = kLeftHandOffset; @@ -310,6 +448,13 @@ void AvatarTracker::initialize_openxr() { add_extension(XR_NVX1_DEVICE_INTERFACE_BASE_EXTENSION_NAME); } + if (m_config.monitoring_plugin_root_id) + { + for (const auto& extension : plugin_utils::PluginDeviceStatusPublisher::get_required_extensions()) + { + add_extension(extension); + } + } for (const char* extension : wrist_requirements.extensions) { add_extension(extension); @@ -319,38 +464,13 @@ void AvatarTracker::initialize_openxr() m_session = std::make_shared(m_config.app_name, extensions, wait_for_openxr_system); m_handles = m_session->get_handles(); - m_time_converter.emplace(m_handles); - - if (m_config.human) + if (m_config.monitoring_plugin_root_id) { - for (const ::avatar::DeviceSide side : kDeviceSides) - { - m_injectors[side_index(side)] = std::make_unique( - m_handles.instance, m_handles.session, xr_hand(side), m_handles.space); - } + m_status_publisher = + std::make_unique(m_handles, *m_config.monitoring_plugin_root_id); } - auto make_joint_pusher = [this](::avatar::DeviceSide side, ::avatar::DeviceDataCategory category) - { - const std::string localized_name = - "Avatar " + std::string(to_string(category)) + " " + std::string(to_string(side)); - return std::make_unique( - m_handles, core::SchemaPusherConfig{ .collection_id = collection_id(side, category), - .max_flatbuffer_size = kJointFlatbufferSize, - .tensor_identifier = "joint_state", - .localized_name = localized_name }); - }; - for (const ::avatar::DeviceDataCategory category : kJointDataCategories) - { - if (!dataset_enabled(category)) - { - continue; - } - for (const ::avatar::DeviceSide side : kDeviceSides) - { - m_joint_pushers[side_index(side)][joint_category_index(category)] = make_joint_pusher(side, category); - } - } + m_time_converter.emplace(m_handles); m_deviceio_session = core::DeviceIOSession::run(trackers, m_handles); m_wrist_source = std::make_unique( @@ -370,15 +490,12 @@ void AvatarTracker::start_glove_if_present(GloveState& glove, ::avatar::DeviceSi return; } - const auto init_error = glove.device->init("{}"); - if (init_error != ::avatar::ErrorCode::SUCCESS) - { - logger()->warn("{} glove initialization failed: {} ({})", to_string(side), error_name(init_error), - static_cast(init_error)); - glove.reset(); - return; - } - glove.device->set_human_frame_build_enabled(m_config.human); + glove.device->set_human_frame_build_enabled(false); + glove.device->set_raw_human_callback_max_hz(kIngressCallbackMaxHz); + glove.ingress = std::make_shared(); + const std::shared_ptr ingress = glove.ingress; + glove.device->set_raw_human_frame_callback( + [ingress](const ::avatar::AvatarDataFrame&, const ::avatar::AvatarDataFrame&) { ingress->record(); }); const auto start_error = glove.device->start(); if (start_error != ::avatar::ErrorCode::SUCCESS) @@ -388,17 +505,114 @@ void AvatarTracker::start_glove_if_present(GloveState& glove, ::avatar::DeviceSi glove.reset(); return; } - glove.last_successful_fetch = std::chrono::steady_clock::now(); + glove.started = true; } -void AvatarTracker::try_connect_missing_gloves() +void AvatarTracker::refresh_transport_state() noexcept { - const auto has_device = [](const GloveState& state) { return state.device != nullptr; }; - if (std::all_of(m_gloves.begin(), m_gloves.end(), has_device)) + try + { + m_transport_state = m_sdk.get().get_usb_connection_state(); + } + catch (...) + { + m_transport_state = ::avatar::UsbConnectionState::ERROR; + } + + if (!m_status_publisher) { return; } + const auto now = std::chrono::steady_clock::now(); + if (!m_last_dongle_check || now - *m_last_dongle_check >= kDonglePresenceInterval) + { + m_dongle_presence = detect_usb_device_presence(kUsbDevicesRoot, kAvatarDongleVendorId, kAvatarDongleProductId); + m_last_dongle_check = now; + } +} +void AvatarTracker::refresh_sdk_inventory() +{ + const auto now = std::chrono::steady_clock::now(); + if (m_last_inventory_refresh && now - *m_last_inventory_refresh < kSdkHealthRefreshInterval) + { + return; + } + m_last_inventory_refresh = now; + + try + { + std::array, 2> next_inventory; + for (const auto& info : m_sdk.get().list_device_info(::avatar::DeviceType::GLOVE)) + { + next_inventory[side_index(info.device_side)] = info; + } + m_inventory = std::move(next_inventory); + m_inventory_available = true; + for (size_t index = 0; index < m_inventory.size(); ++index) + { + m_ever_observed[index] = m_ever_observed[index] || m_inventory[index].has_value(); + } + reconcile_glove_handles(); + m_inventory_error_logged = false; + } + catch (const std::exception& error) + { + m_inventory_available = false; + if (!m_inventory_error_logged) + { + logger()->warn("Avatar SDK inventory refresh failed; tracking handles are retained: {}", error.what()); + m_inventory_error_logged = true; + } + } + catch (...) + { + m_inventory_available = false; + if (!m_inventory_error_logged) + { + logger()->warn("Avatar SDK inventory refresh failed; tracking handles are retained."); + m_inventory_error_logged = true; + } + } +} + +void AvatarTracker::reconcile_glove_handles() +{ + if (!m_inventory_available) + { + return; + } + for (const ::avatar::DeviceSide side : kDeviceSides) + { + const size_t index = side_index(side); + GloveState& state = glove(side); + if (m_inventory[index] || !state.device) + { + continue; + } + logger()->info("{} glove disappeared from the Avatar SDK inventory.", to_string(side)); + reset_glove(side); + } +} + +void AvatarTracker::reset_glove(::avatar::DeviceSide side) noexcept +{ + const size_t index = side_index(side); + stop_haptics(side); + destroy_output_resources(side); + glove(side).reset(); + m_haptic_stopped[index] = true; + m_last_haptic_sample_times[index].reset(); + m_haptic_retry_after[index].reset(); + m_haptic_error_logged[index] = false; +} + +void AvatarTracker::try_connect_missing_gloves() +{ + if (!m_inventory_available) + { + return; + } const auto now = std::chrono::steady_clock::now(); if (m_last_glove_retry && now - *m_last_glove_retry < kGloveRetryInterval) { @@ -408,9 +622,15 @@ void AvatarTracker::try_connect_missing_gloves() for (const ::avatar::DeviceSide side : kDeviceSides) { - start_glove_if_present(glove(side), side); + const size_t index = side_index(side); + GloveState& state = glove(side); + if (!state.device && m_inventory[index] && m_inventory[index]->online) + { + start_glove_if_present(state, side); + } } - // Log once per stretch with no glove; a glove connecting re-arms the message. + + const auto has_device = [](const GloveState& state) { return state.device != nullptr; }; const bool waiting = std::none_of(m_gloves.begin(), m_gloves.end(), has_device); if (waiting && !m_glove_wait_logged) { @@ -419,87 +639,254 @@ void AvatarTracker::try_connect_missing_gloves() m_glove_wait_logged = waiting; } -void AvatarTracker::refresh_data() +void AvatarTracker::refresh_glove_activity() { + const auto now = std::chrono::steady_clock::now(); for (const ::avatar::DeviceSide side : kDeviceSides) { GloveState& state = glove(side); - if (!state.device) + if (!state.started) { continue; } - // Liveness comes from fetched data, not get_device_info().online: the SDK - // leaves that flag false on a glove first discovered offline and powered on later. - // A haptic-only glove still fetches RAW so the data timeout below can drop a dead handle. - const bool probe_raw = !m_config.human && !m_config.raw && !m_config.robot; - // fetch_data() keeps returning a silent glove's last sample, so only a newer - // sample stamp counts as data for the timeout below. - bool fetched_any = false; - const auto note_stamp = [&state, &fetched_any](const ::avatar::Stamp& stamp) + if (!state.ingress) + { + continue; + } + const uint64_t generation = state.ingress->generation(); + if (generation != state.observed_ingress_generation) { - if (!state.last_sample_stamp) + state.observed_ingress_generation = generation; + state.last_ingress = state.ingress->last_event(); + state.ever_received_ingress = state.ever_received_ingress || state.last_ingress.has_value(); + } + const bool ingress_fresh = state.last_ingress && now - *state.last_ingress < kAvatarDataTimeout; + if (ingress_fresh && !state.ready) + { + if (state.last_output_setup_attempt && now - *state.last_output_setup_attempt < kOutputSetupRetryInterval) { - state.last_sample_stamp = stamp; + continue; } - else if (std::tie(stamp.sec, stamp.nanosec) > - std::tie(state.last_sample_stamp->sec, state.last_sample_stamp->nanosec)) + state.last_output_setup_attempt = now; + if (!try_create_output_resources(side)) { - state.last_sample_stamp = stamp; - fetched_any = true; + continue; + } + state.ready = true; + if (m_config.haptic) + { + stop_haptics(side); + } + logger()->info("{} glove connected and streaming.", to_string(side)); + } + else if (!ingress_fresh && state.ready) + { + logger()->warn("{} glove stream is stale; withdrawing its outputs.", to_string(side)); + if (m_config.haptic) + { + stop_haptics(side); + } + destroy_output_resources(side); + state.ready = false; + state.clear_current_data(); + } + } +} + +bool AvatarTracker::try_create_output_resources(::avatar::DeviceSide side) +{ + const size_t index = side_index(side); + GloveState& state = glove(side); + try + { + std::unique_ptr injector; + std::array, 2> joint_pushers; + + if (m_config.human) + { + injector = std::make_unique( + m_handles.instance, m_handles.session, xr_hand(side), m_handles.space); + } + for (const ::avatar::DeviceDataCategory category : kJointDataCategories) + { + if (!dataset_enabled(category)) + { + continue; + } + const std::string localized_name = + "Avatar " + std::string(to_string(category)) + " " + std::string(to_string(side)); + joint_pushers[joint_category_index(category)] = std::make_unique( + m_handles, core::SchemaPusherConfig{ .collection_id = collection_id(side, category), + .max_flatbuffer_size = kJointFlatbufferSize, + .tensor_identifier = "joint_state", + .localized_name = localized_name }); + } + + m_injectors[index] = std::move(injector); + m_joint_pushers[index] = std::move(joint_pushers); + state.output_setup_error.reset(); + return true; + } + catch (const std::exception& error) + { + state.output_setup_error = "output setup failed: " + std::string(error.what()); + logger()->warn("{} glove output setup failed; this side will retry: {}", to_string(side), error.what()); + } + catch (...) + { + state.output_setup_error = "output setup failed with an unknown error"; + logger()->warn("{} glove output setup failed; this side will retry.", to_string(side)); + } + return false; +} + +void AvatarTracker::destroy_output_resources(::avatar::DeviceSide side) noexcept +{ + const size_t index = side_index(side); + m_injectors[index].reset(); + for (auto& pusher : m_joint_pushers[index]) + { + pusher.reset(); + } +} + +void AvatarTracker::refresh_data() +{ + for (const ::avatar::DeviceSide side : kDeviceSides) + { + GloveState& state = glove(side); + if (!state.ready) + { + continue; + } + const auto now = std::chrono::steady_clock::now(); + const auto note_result = [&state, now](::avatar::DeviceDataCategory category, ::avatar::ErrorCode error, + const ::avatar::Stamp* stamp) + { + const size_t category_idx = data_category_index(category); + auto& dataset = state.datasets[category_idx]; + if (error != ::avatar::ErrorCode::SUCCESS) + { + dataset.fetch_error = error; + return; + } + dataset.fetch_error.reset(); + if (stamp != nullptr && (!dataset.last_stamp || stamp->sec != dataset.last_stamp->sec || + stamp->nanosec != dataset.last_stamp->nanosec)) + { + dataset.last_stamp = *stamp; + dataset.last_fresh_data = now; + dataset.ever_received_data = true; } }; if (m_config.human) { ::avatar::AvatarDataFrame frame; - const bool fetched = - state.device->fetch_data(frame, ::avatar::DeviceDataCategory::HUMAN) == ::avatar::ErrorCode::SUCCESS; - if (fetched) + const auto error = state.device->fetch_data(frame, ::avatar::DeviceDataCategory::HUMAN); + if (error == ::avatar::ErrorCode::SUCCESS) { - note_stamp(frame.skeleton.header.stamp); + note_result(::avatar::DeviceDataCategory::HUMAN, error, &frame.skeleton.header.stamp); state.landmarks = std::move(frame.skeleton.landmark); } else { + note_result(::avatar::DeviceDataCategory::HUMAN, error, nullptr); state.landmarks.clear(); } } for (const ::avatar::DeviceDataCategory category : kJointDataCategories) { - if (!dataset_enabled(category) && !(probe_raw && category == ::avatar::DeviceDataCategory::RAW)) + if (!dataset_enabled(category)) { continue; } ::avatar::AvatarDataFrame frame; - const bool fetched = state.device->fetch_data(frame, category) == ::avatar::ErrorCode::SUCCESS; + const auto error = state.device->fetch_data(frame, category); ::avatar::AvatarDataFrame& cached = cached_joint_frame(state, category); - if (fetched) + if (error == ::avatar::ErrorCode::SUCCESS) { - note_stamp(hand_payload(frame, category).header.stamp); + note_result(category, error, &hand_payload(frame, category).header.stamp); cached = std::move(frame); } else { + note_result(category, error, nullptr); cached = {}; } } - const auto now = std::chrono::steady_clock::now(); - if (fetched_any) + } +} + +void AvatarTracker::publish_device_status() +{ + if (!m_status_publisher) + { + return; + } + + const auto now = std::chrono::steady_clock::now(); + std::vector entries; + entries.reserve(9); + entries.push_back(make_dongle_status(m_dongle_presence, m_transport_state)); + + for (const ::avatar::DeviceSide side : kDeviceSides) + { + const size_t index = side_index(side); + const GloveState& state = glove(side); + const bool inventory_present = m_inventory[index].has_value(); + const bool ingress_fresh = state.last_ingress && now - *state.last_ingress < kAvatarDataTimeout; + entries.push_back(make_physical_glove_status( + kPhysicalGlovePaths[index], + PhysicalGloveHealthInput{ + .inventory_available = m_inventory_available, + .inventory_present = inventory_present, + .ever_observed = m_ever_observed[index], + .sdk_online = inventory_present && m_inventory[index]->online, + .ingress_fresh = ingress_fresh, + .ever_received_ingress = state.ever_received_ingress, + .device_error = inventory_present ? physical_device_error(*m_inventory[index], state) : std::nullopt, + })); + + const auto make_input = [this, &state, index, ingress_fresh, inventory_present, now]( + ::avatar::DeviceDataCategory category, bool enabled) { - if (!state.streaming) + const size_t category_idx = data_category_index(category); + const auto& dataset = state.datasets[category_idx]; + const bool has_no_current_data = dataset.fetch_error == ::avatar::ErrorCode::NO_VALID_DATA_RETURNED; + std::optional fetch_error; + if (state.output_setup_error) { - state.streaming = true; - isaaccapture::Logger::get("isaaccapture.plugins.sharpa_avatar.AvatarTracker") - ->info("{} glove connected and streaming.", to_string(side)); + fetch_error = state.output_setup_error; } - state.last_successful_fetch = now; - } - else if (!state.last_successful_fetch || now - *state.last_successful_fetch >= kAvatarDataTimeout) - { - logger()->warn("{} glove disconnected: no data for {} s.", to_string(side), kAvatarDataTimeout.count()); - state.reset(); - } + else if (dataset.fetch_error && !has_no_current_data) + { + fetch_error = "Avatar SDK fetch_data failed: " + std::string(error_name(*dataset.fetch_error)) + " (" + + std::to_string(static_cast(*dataset.fetch_error)) + ")"; + } + return DatasetHealthInput{ + .enabled = enabled, + .inventory_available = m_inventory_available, + .inventory_present = inventory_present, + .ever_observed = m_ever_observed[index], + .ingress_fresh = ingress_fresh, + .ever_received_ingress = state.ever_received_ingress, + .data_fresh = dataset.last_fresh_data && now - *dataset.last_fresh_data < kAvatarDataTimeout, + .ever_received_data = dataset.ever_received_data, + .fetch_has_no_current_data = has_no_current_data, + .fetch_error = std::move(fetch_error), + }; + }; + + entries.push_back(make_dataset_status( + kHandDevicePaths[index], make_input(::avatar::DeviceDataCategory::HUMAN, m_config.human))); + entries.push_back( + make_dataset_status(kRawDevicePaths[index], make_input(::avatar::DeviceDataCategory::RAW, m_config.raw))); + entries.push_back(make_dataset_status( + kRobotDevicePaths[index], make_input(::avatar::DeviceDataCategory::ROBOT, m_config.robot))); } + + m_status_publisher->publish_if_changed(entries, core::os_monotonic_now_ns()); } const GloveState& AvatarTracker::glove(::avatar::DeviceSide side) const @@ -514,39 +901,32 @@ GloveState& AvatarTracker::glove(::avatar::DeviceSide side) void AvatarTracker::update() { + refresh_transport_state(); + refresh_sdk_inventory(); try_connect_missing_gloves(); + refresh_glove_activity(); m_deviceio_session->update(); refresh_data(); - if (m_haptic_reader) + process_haptics(); + try { - const auto now = std::chrono::steady_clock::now(); - for (const ::avatar::DeviceSide side : kDeviceSides) + publish_device_status(); + m_status_publish_error_logged = false; + } + catch (const std::exception& error) + { + if (!m_status_publish_error_logged) { - const size_t index = side_index(side); - const auto& tracked = m_haptic_reader->get_data(*m_deviceio_session, to_string(side)); - const core::HapticCommand* command = tracked.get(); - if (command != nullptr && command != m_last_haptic_commands[index].get()) - { - m_last_haptic_commands[index] = tracked; - if (command->values() != nullptr && command->values()->size() == kAvatarFingerCount) - { - std::array powers{}; - for (size_t i = 0; i < kAvatarFingerCount; ++i) - { - powers[i] = command->values()->Get(i); - } - apply_haptic_command(side, powers); - m_last_haptic_sample_times[index] = now; - m_haptic_stopped[index] = std::all_of( - powers.begin(), powers.end(), [](float power) { return !std::isfinite(power) || power <= 0.0f; }); - } - } - if (!m_haptic_stopped[index] && m_last_haptic_sample_times[index] && - now - *m_last_haptic_sample_times[index] >= kHapticCommandTimeout) - { - apply_haptic_command(side, {}); - m_haptic_stopped[index] = true; - } + logger()->warn("Device status publishing failed; tracking will continue: {}", error.what()); + m_status_publish_error_logged = true; + } + } + catch (...) + { + if (!m_status_publish_error_logged) + { + logger()->warn("Device status publishing failed; tracking will continue."); + m_status_publish_error_logged = true; } } if (m_config.raw || m_config.robot) @@ -559,6 +939,47 @@ void AvatarTracker::update() } } +void AvatarTracker::process_haptics() +{ + if (!m_haptic_reader) + { + return; + } + + const auto now = std::chrono::steady_clock::now(); + for (const ::avatar::DeviceSide side : kDeviceSides) + { + const size_t index = side_index(side); + const auto& tracked = m_haptic_reader->get_data(*m_deviceio_session, to_string(side)); + const core::HapticCommand* command = tracked.get(); + if (command != nullptr && command != m_last_haptic_commands[index].get()) + { + m_last_haptic_commands[index] = tracked; + const bool retry_ready = !m_haptic_retry_after[index] || now >= *m_haptic_retry_after[index]; + if (retry_ready && command->values() != nullptr && command->values()->size() == kAvatarFingerCount) + { + std::array powers{}; + for (size_t i = 0; i < kAvatarFingerCount; ++i) + { + powers[i] = command->values()->Get(i); + } + if (apply_haptic_command(side, powers)) + { + m_last_haptic_sample_times[index] = now; + m_haptic_stopped[index] = std::all_of( + powers.begin(), powers.end(), [](float power) { return !std::isfinite(power) || power <= 0.0f; }); + } + } + } + if (!m_haptic_stopped[index] && m_last_haptic_sample_times[index] && + now - *m_last_haptic_sample_times[index] >= kHapticCommandTimeout && + (!m_haptic_retry_after[index] || now >= *m_haptic_retry_after[index])) + { + stop_haptics(side); + } + } +} + bool AvatarTracker::dataset_enabled(::avatar::DeviceDataCategory category) const { switch (category) @@ -610,11 +1031,12 @@ void AvatarTracker::push_joint_frame(const GloveState& glove, output.has_effort = false; output.ee_pose_valid = false; output.joints.reserve(hand.joint.position.size()); + const auto& names = joint_names(category); for (size_t i = 0; i < hand.joint.position.size(); ++i) { auto joint = std::make_shared(); - joint->name = i < hand.joint.name.size() ? hand.joint.name[i] : std::string{}; + joint->name = i < names.size() ? std::string(names[i]) : std::string{}; joint->position = hand.joint.position[i]; joint->valid = std::isfinite(joint->position); output.joints.push_back(std::move(joint)); @@ -627,38 +1049,115 @@ void AvatarTracker::push_joint_frame(const GloveState& glove, pusher.push_buffer(builder.GetBufferPointer(), builder.GetSize(), sample_time_ns, sample_time_ns); } -void AvatarTracker::apply_haptic_command(::avatar::DeviceSide side, const std::array& powers) +bool AvatarTracker::apply_haptic_command(::avatar::DeviceSide side, const std::array& powers) +{ + if (!glove(side).ready) + { + return false; + } + return send_haptic_command(side, powers); +} + +bool AvatarTracker::send_haptic_command(::avatar::DeviceSide side, const std::array& powers) { GloveState& state = glove(side); if (!state.device) { - return; + return false; } - std::string request = R"({"key":"set_vibration","intensity":[)"; - for (size_t i = 0; i < kAvatarFingerCount; ++i) + try { - const float normalized = std::isfinite(powers[i]) ? std::clamp(powers[i], 0.0f, 1.0f) : 0.0f; - if (i != 0) + std::string request = R"({"key":"set_vibration","intensity":[)"; + for (size_t i = 0; i < kAvatarFingerCount; ++i) { - request += ','; + const float normalized = std::isfinite(powers[i]) ? std::clamp(powers[i], 0.0f, 1.0f) : 0.0f; + if (i != 0) + { + request += ','; + } + request += std::to_string(static_cast(std::lround(normalized * 255.0f))); } - request += std::to_string(static_cast(std::lround(normalized * 255.0f))); - } - request += "]}"; + request += "]}"; - const auto [error, unused] = state.device->set_task(request); - (void)unused; - if (error != ::avatar::ErrorCode::SUCCESS) + const auto [error, unused] = state.device->set_task(request); + (void)unused; + if (error != ::avatar::ErrorCode::SUCCESS) + { + state.haptic_error = "set_vibration failed: " + std::string(error_name(error)) + " (" + + std::to_string(static_cast(error)) + ")"; + const size_t index = side_index(side); + m_haptic_retry_after[index] = std::chrono::steady_clock::now() + kHapticFailureRetryInterval; + if (!m_haptic_error_logged[index]) + { + m_haptic_error_logged[index] = true; + logger()->warn( + "set_vibration failed for {} glove: {} ({}); further errors for this side will be silenced.", + to_string(side), error_name(error), static_cast(error)); + } + return false; + } + state.haptic_error.reset(); + const size_t index = side_index(side); + m_haptic_retry_after[index].reset(); + m_haptic_error_logged[index] = false; + return true; + } + catch (const std::exception& error) { + state.haptic_error = "set_vibration threw: " + std::string(error.what()); const size_t index = side_index(side); + m_haptic_retry_after[index] = std::chrono::steady_clock::now() + kHapticFailureRetryInterval; if (!m_haptic_error_logged[index]) { m_haptic_error_logged[index] = true; - logger()->warn("set_vibration failed for {} glove: {} ({}); further errors for this side will be silenced.", - to_string(side), error_name(error), static_cast(error)); + logger()->warn("set_vibration threw for {} glove: {}; further errors for this side will be silenced.", + to_string(side), error.what()); } } + catch (...) + { + state.haptic_error = "set_vibration threw an unknown error"; + const size_t index = side_index(side); + m_haptic_retry_after[index] = std::chrono::steady_clock::now() + kHapticFailureRetryInterval; + if (!m_haptic_error_logged[index]) + { + m_haptic_error_logged[index] = true; + logger()->warn( + "set_vibration threw for {} glove; further errors for this side will be silenced.", to_string(side)); + } + } + return false; +} + +void AvatarTracker::stop_haptics(::avatar::DeviceSide side) noexcept +{ + const size_t index = side_index(side); + if (!m_config.haptic || !glove(side).device) + { + m_haptic_stopped[index] = true; + m_last_haptic_sample_times[index].reset(); + m_haptic_retry_after[index].reset(); + return; + } + + try + { + m_haptic_stopped[index] = send_haptic_command(side, {}); + } + catch (...) + { + m_haptic_stopped[index] = false; + m_haptic_retry_after[index] = std::chrono::steady_clock::now() + kHapticFailureRetryInterval; + } + if (m_haptic_stopped[index]) + { + m_last_haptic_sample_times[index].reset(); + } + else + { + m_last_haptic_sample_times[index] = std::chrono::steady_clock::now(); + } } void AvatarTracker::map_landmarks_to_openxr(const std::vector<::avatar::Pose>& landmarks, diff --git a/src/plugins/sharpa_avatar/core/avatar_health.cpp b/src/plugins/sharpa_avatar/core/avatar_health.cpp new file mode 100644 index 0000000000..9902a8a2e3 --- /dev/null +++ b/src/plugins/sharpa_avatar/core/avatar_health.cpp @@ -0,0 +1,224 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 Avatar SDK contributors. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/avatar/avatar_health.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace avatar +{ + +namespace +{ + +std::optional read_trimmed(const std::filesystem::path& path) +{ + std::ifstream stream(path); + std::string value; + if (!stream || !std::getline(stream, value)) + { + return std::nullopt; + } + value.erase( + std::remove_if(value.begin(), value.end(), [](unsigned char c) { return std::isspace(c) != 0; }), value.end()); + std::transform(value.begin(), value.end(), value.begin(), [](unsigned char c) { return std::tolower(c); }); + return value; +} + +std::string hex_id(uint16_t value) +{ + std::ostringstream stream; + stream << std::hex << std::setfill('0') << std::setw(4) << value; + return stream.str(); +} + +} // anonymous namespace + +plugin_utils::PluginDeviceStatusEntry make_physical_glove_status(std::string path, const PhysicalGloveHealthInput& input) +{ + plugin_utils::PluginDeviceStatusEntry entry{ .path = std::move(path) }; + if (!input.inventory_available) + { + entry.state = core::PluginDeviceState_UNKNOWN; + entry.reason = core::PluginDeviceReason_DEVICE_ERROR; + entry.error = "Avatar SDK inventory is unavailable"; + } + else if (!input.inventory_present) + { + entry.state = input.ever_observed ? core::PluginDeviceState_DISCONNECTED : core::PluginDeviceState_UNKNOWN; + entry.reason = core::PluginDeviceReason_NO_HARDWARE_SIGNAL; + entry.error = input.ever_observed ? "glove no longer appears in the Avatar SDK inventory" : + "waiting for the Avatar SDK to discover the glove"; + } + else if (input.device_error) + { + entry.state = core::PluginDeviceState_DEGRADED; + entry.reason = core::PluginDeviceReason_DEVICE_ERROR; + entry.error = *input.device_error; + } + else if (!input.ingress_fresh) + { + entry.state = input.ever_received_ingress ? core::PluginDeviceState_DEGRADED : core::PluginDeviceState_UNKNOWN; + entry.reason = core::PluginDeviceReason_NO_CURRENT_DATA; + entry.error = input.sdk_online ? "glove ingress is not advancing" : "Avatar SDK reports no recent glove ingress"; + } + else + { + entry.state = core::PluginDeviceState_CONNECTED; + entry.reason = core::PluginDeviceReason_NONE; + } + return entry; +} + +plugin_utils::PluginDeviceStatusEntry make_dataset_status(std::string path, const DatasetHealthInput& input) +{ + plugin_utils::PluginDeviceStatusEntry entry{ .path = std::move(path) }; + if (!input.enabled) + { + entry.state = core::PluginDeviceState_DISABLED; + entry.reason = core::PluginDeviceReason_DISABLED_BY_CONFIGURATION; + } + else if (!input.inventory_available) + { + entry.state = core::PluginDeviceState_UNKNOWN; + entry.reason = core::PluginDeviceReason_DEVICE_ERROR; + entry.error = "Avatar SDK inventory is unavailable"; + } + else if (!input.inventory_present) + { + entry.state = input.ever_observed ? core::PluginDeviceState_DISCONNECTED : core::PluginDeviceState_UNKNOWN; + entry.reason = core::PluginDeviceReason_NO_HARDWARE_SIGNAL; + entry.error = input.ever_observed ? "glove no longer appears in the Avatar SDK inventory" : + "waiting for the Avatar SDK to discover the glove"; + } + else if (!input.ingress_fresh) + { + entry.state = input.ever_received_ingress ? core::PluginDeviceState_DEGRADED : core::PluginDeviceState_UNKNOWN; + entry.reason = core::PluginDeviceReason_NO_CURRENT_DATA; + entry.error = "waiting for current glove ingress"; + } + else if (input.fetch_error) + { + entry.state = core::PluginDeviceState_DEGRADED; + entry.reason = core::PluginDeviceReason_DEVICE_ERROR; + entry.error = *input.fetch_error; + } + else if (input.fetch_has_no_current_data || !input.data_fresh) + { + entry.state = input.ever_received_data ? core::PluginDeviceState_DEGRADED : core::PluginDeviceState_UNKNOWN; + entry.reason = core::PluginDeviceReason_NO_CURRENT_DATA; + entry.error = input.ever_received_data ? "dataset is not advancing" : "waiting for the first dataset sample"; + } + else + { + entry.state = core::PluginDeviceState_CONNECTED; + entry.reason = core::PluginDeviceReason_NONE; + } + return entry; +} + +plugin_utils::PluginDeviceStatusEntry make_dongle_status(UsbDevicePresence presence, ::avatar::UsbConnectionState state) +{ + plugin_utils::PluginDeviceStatusEntry entry{ .path = "/transport/dongle" }; + if (presence == UsbDevicePresence::UNKNOWN) + { + entry.state = core::PluginDeviceState_UNKNOWN; + entry.reason = core::PluginDeviceReason_NO_HARDWARE_SIGNAL; + entry.error = "USB device enumeration is unavailable"; + return entry; + } + if (presence == UsbDevicePresence::ABSENT) + { + entry.state = core::PluginDeviceState_DISCONNECTED; + entry.reason = core::PluginDeviceReason_NO_HARDWARE_SIGNAL; + entry.error = "Avatar USB dongle 1915:520f is not enumerated"; + return entry; + } + + switch (state) + { + case ::avatar::UsbConnectionState::CONNECTED: + entry.state = core::PluginDeviceState_CONNECTED; + entry.reason = core::PluginDeviceReason_NONE; + break; + case ::avatar::UsbConnectionState::DISCONNECTED: + entry.state = core::PluginDeviceState_DISCONNECTED; + entry.reason = core::PluginDeviceReason_NO_HARDWARE_SIGNAL; + entry.error = "Avatar SDK USB transport is disconnected"; + break; + case ::avatar::UsbConnectionState::CONNECTING: + entry.state = core::PluginDeviceState_UNKNOWN; + entry.reason = core::PluginDeviceReason_NO_CURRENT_DATA; + entry.error = "Avatar SDK USB transport is connecting"; + break; + case ::avatar::UsbConnectionState::RECONNECTING: + entry.state = core::PluginDeviceState_DEGRADED; + entry.reason = core::PluginDeviceReason_NO_CURRENT_DATA; + entry.error = "Avatar SDK USB transport is reconnecting"; + break; + case ::avatar::UsbConnectionState::ERROR: + entry.state = core::PluginDeviceState_FAILED; + entry.reason = core::PluginDeviceReason_DEVICE_ERROR; + entry.error = "Avatar SDK USB transport failed"; + break; + } + return entry; +} + +UsbDevicePresence detect_usb_device_presence(const std::filesystem::path& devices_root, + uint16_t vendor_id, + uint16_t product_id) +{ + std::error_code error; + std::filesystem::directory_iterator devices(devices_root, error); + if (error) + { + return UsbDevicePresence::UNKNOWN; + } + + const std::string expected_vendor = hex_id(vendor_id); + const std::string expected_product = hex_id(product_id); + bool incomplete_scan = false; + const std::filesystem::directory_iterator end; + for (; devices != end; devices.increment(error)) + { + if (error) + { + return UsbDevicePresence::UNKNOWN; + } + const auto& device = *devices; + // USB interface rows such as 1-2:1.0 do not own VID/PID files. + if (device.path().filename().string().find(':') != std::string::npos) + { + continue; + } + const auto vendor = read_trimmed(device.path() / "idVendor"); + const auto product = read_trimmed(device.path() / "idProduct"); + if (!vendor || !product) + { + incomplete_scan = true; + continue; + } + if (*vendor == expected_vendor && *product == expected_product) + { + return UsbDevicePresence::PRESENT; + } + } + if (error) + { + return UsbDevicePresence::UNKNOWN; + } + return incomplete_scan ? UsbDevicePresence::UNKNOWN : UsbDevicePresence::ABSENT; +} + +} // namespace avatar +} // namespace plugins diff --git a/src/plugins/sharpa_avatar/core/inc/avatar/avatar_hand_tracking_plugin.hpp b/src/plugins/sharpa_avatar/core/inc/avatar/avatar_hand_tracking_plugin.hpp index 0fa977ab95..861619630d 100644 --- a/src/plugins/sharpa_avatar/core/inc/avatar/avatar_hand_tracking_plugin.hpp +++ b/src/plugins/sharpa_avatar/core/inc/avatar/avatar_hand_tracking_plugin.hpp @@ -3,6 +3,8 @@ #pragma once +#include "avatar_health.hpp" + #include #include #include @@ -10,12 +12,15 @@ #include #include #include +#include #include #include #include +#include #include #include +#include #include #include #include @@ -31,6 +36,8 @@ using DeviceSide = ::avatar::DeviceSide; using DeviceDataCategory = ::avatar::DeviceDataCategory; inline constexpr std::array kDeviceSides{ DeviceSide::LEFT, DeviceSide::RIGHT }; +inline constexpr std::array kDataCategories{ DeviceDataCategory::HUMAN, DeviceDataCategory::RAW, + DeviceDataCategory::ROBOT }; inline constexpr std::array kJointDataCategories{ DeviceDataCategory::RAW, DeviceDataCategory::ROBOT }; @@ -65,6 +72,7 @@ inline constexpr uint32_t kAvatarHumanLandmarkCount = 25; struct AvatarPluginConfig { std::string app_name = "AvatarHandPlugin"; + std::optional monitoring_plugin_root_id; std::string sdk_config_path; bool human = true; bool raw = true; @@ -84,6 +92,18 @@ class AvatarSdkSession ::avatar::AvatarSDK& get(); }; +class GloveIngressSignal +{ +public: + void record() noexcept; + uint64_t generation() const noexcept; + std::optional last_event() const noexcept; + +private: + std::atomic m_generation{ 0 }; + std::atomic m_last_event_ns{ -1 }; +}; + struct GloveState { ~GloveState() noexcept; @@ -93,22 +113,37 @@ struct GloveState GloveState& operator=(const GloveState&) = delete; void reset() noexcept; + void clear_current_data() noexcept; + + struct DatasetState + { + std::optional<::avatar::Stamp> last_stamp; + std::optional last_fresh_data; + std::optional<::avatar::ErrorCode> fetch_error; + bool ever_received_data = false; + }; ::avatar::DevicePtr device; std::vector<::avatar::Pose> landmarks; ::avatar::AvatarDataFrame raw_frame; ::avatar::AvatarDataFrame robot_frame; - // Empty until start() or a successful fetch; drives the data-timeout window. - std::optional last_successful_fetch; - // The first stamp is a baseline; only a later stamp proves the glove is streaming. - std::optional<::avatar::Stamp> last_sample_stamp; - bool streaming = false; + std::array datasets; + std::shared_ptr ingress; + uint64_t observed_ingress_generation = 0; + std::optional last_ingress; + std::optional haptic_error; + std::optional output_setup_error; + std::optional last_output_setup_attempt; + bool ever_received_ingress = false; + bool started = false; + bool ready = false; }; class __attribute__((visibility("default"))) AvatarTracker { public: explicit AvatarTracker(AvatarPluginConfig config = {}); + ~AvatarTracker() noexcept; void update(); @@ -116,9 +151,18 @@ class __attribute__((visibility("default"))) AvatarTracker static constexpr std::size_t kAvatarFingerCount = 5; void initialize_openxr(); + void refresh_transport_state() noexcept; + void refresh_sdk_inventory(); + void reconcile_glove_handles(); + void reset_glove(::avatar::DeviceSide side) noexcept; void try_connect_missing_gloves(); void start_glove_if_present(GloveState& glove, ::avatar::DeviceSide side); + void refresh_glove_activity(); + bool try_create_output_resources(::avatar::DeviceSide side); + void destroy_output_resources(::avatar::DeviceSide side) noexcept; void refresh_data(); + void publish_device_status(); + void process_haptics(); void inject_hand_data(); void push_joint_frames(); void push_joint_frame(const GloveState& glove, @@ -126,7 +170,9 @@ class __attribute__((visibility("default"))) AvatarTracker ::avatar::DeviceDataCategory category, core::SchemaPusher& pusher); bool dataset_enabled(::avatar::DeviceDataCategory category) const; - void apply_haptic_command(::avatar::DeviceSide side, const std::array& powers); + bool apply_haptic_command(::avatar::DeviceSide side, const std::array& powers); + bool send_haptic_command(::avatar::DeviceSide side, const std::array& powers); + void stop_haptics(::avatar::DeviceSide side) noexcept; const GloveState& glove(::avatar::DeviceSide side) const; GloveState& glove(::avatar::DeviceSide side); void map_landmarks_to_openxr(const std::vector<::avatar::Pose>& landmarks, @@ -141,6 +187,7 @@ class __attribute__((visibility("default"))) AvatarTracker std::shared_ptr m_session; core::OpenXRSessionHandles m_handles; std::array, 2> m_injectors; + std::unique_ptr m_status_publisher; std::shared_ptr m_haptic_reader; std::unique_ptr m_deviceio_session; std::optional m_time_converter; @@ -149,10 +196,20 @@ class __attribute__((visibility("default"))) AvatarTracker std::array, 2> m_last_haptic_commands; std::array, 2> m_last_haptic_sample_times; + std::array, 2> m_haptic_retry_after; std::array m_haptic_stopped{ { true, true } }; std::array m_haptic_error_logged{ { false, false } }; std::optional m_last_glove_retry; + std::array, 2> m_inventory; + std::array m_ever_observed{}; + bool m_inventory_available = false; + std::optional m_last_inventory_refresh; + std::optional m_last_dongle_check; + UsbDevicePresence m_dongle_presence = UsbDevicePresence::UNKNOWN; + ::avatar::UsbConnectionState m_transport_state = ::avatar::UsbConnectionState::DISCONNECTED; bool m_glove_wait_logged = false; + bool m_inventory_error_logged = false; + bool m_status_publish_error_logged = false; }; } // namespace avatar diff --git a/src/plugins/sharpa_avatar/core/inc/avatar/avatar_health.hpp b/src/plugins/sharpa_avatar/core/inc/avatar/avatar_health.hpp new file mode 100644 index 0000000000..3e9ea735e4 --- /dev/null +++ b/src/plugins/sharpa_avatar/core/inc/avatar/avatar_health.hpp @@ -0,0 +1,63 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 Avatar SDK contributors. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include + +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace avatar +{ + +inline constexpr auto kAvatarDataTimeout = std::chrono::seconds(3); + +enum class UsbDevicePresence +{ + UNKNOWN, + ABSENT, + PRESENT, +}; + +struct PhysicalGloveHealthInput +{ + bool inventory_available = false; + bool inventory_present = false; + bool ever_observed = false; + bool sdk_online = false; + bool ingress_fresh = false; + bool ever_received_ingress = false; + std::optional device_error; +}; + +struct DatasetHealthInput +{ + bool enabled = true; + bool inventory_available = false; + bool inventory_present = false; + bool ever_observed = false; + bool ingress_fresh = false; + bool ever_received_ingress = false; + bool data_fresh = false; + bool ever_received_data = false; + bool fetch_has_no_current_data = false; + std::optional fetch_error; +}; + +plugin_utils::PluginDeviceStatusEntry make_physical_glove_status(std::string path, const PhysicalGloveHealthInput& input); +plugin_utils::PluginDeviceStatusEntry make_dataset_status(std::string path, const DatasetHealthInput& input); +plugin_utils::PluginDeviceStatusEntry make_dongle_status(UsbDevicePresence presence, ::avatar::UsbConnectionState state); + +UsbDevicePresence detect_usb_device_presence(const std::filesystem::path& devices_root, + uint16_t vendor_id, + uint16_t product_id); + +} // namespace avatar +} // namespace plugins diff --git a/src/plugins/sharpa_avatar/plugin.yaml.in b/src/plugins/sharpa_avatar/plugin.yaml.in index 9ab77b55a1..e81fccce9f 100644 --- a/src/plugins/sharpa_avatar/plugin.yaml.in +++ b/src/plugins/sharpa_avatar/plugin.yaml.in @@ -11,6 +11,15 @@ description: > command: "./avatar_hand_plugin" version: "1.1.0" devices: + - path: "/transport/dongle" + type: "transport" + description: "Sharpa AvatarExo wireless USB dongle" + - path: "/glove/left" + type: "glove" + description: "Left physical Sharpa AvatarExo glove" + - path: "/glove/right" + type: "glove" + description: "Right physical Sharpa AvatarExo glove" - path: "/hand/left" type: "hand_tracker" description: "Left Avatar glove — 26 OpenXR joints (HUMAN FK → OpenXR)" diff --git a/src/python/isaaccapture/teleop_session_manager/teleop_session.py b/src/python/isaaccapture/teleop_session_manager/teleop_session.py index 32bd405355..85ca46a640 100644 --- a/src/python/isaaccapture/teleop_session_manager/teleop_session.py +++ b/src/python/isaaccapture/teleop_session_manager/teleop_session.py @@ -61,7 +61,8 @@ logger = logging.getLogger(__name__) -_MONITORED_PLUGIN_NAME = "manus_hand_plugin" +# Plugins that publish device status (see PluginDeviceStatusPublisher); others launch unmonitored. +_MONITORED_PLUGIN_NAMES = frozenset({"manus_hand_plugin", "avatar_hand_plugin"}) def _resolve_sink(entry: GraphExecutable) -> tuple[GraphExecutable, IDeviceIOSink]: @@ -1261,11 +1262,11 @@ def _destroy_twin(self) -> None: self._twin_teardown_clean = stopped and joined def _resolve_monitored_plugins(self) -> None: - """Resolve the initially supported Manus inventory and status reader.""" + """Resolve inventories and status readers for monitored plugins.""" for plugin_config in self.config.plugins: if ( not plugin_config.enabled - or plugin_config.plugin_name != _MONITORED_PLUGIN_NAME + or plugin_config.plugin_name not in _MONITORED_PLUGIN_NAMES ): continue diff --git a/tests/cpp/plugins/CMakeLists.txt b/tests/cpp/plugins/CMakeLists.txt index 6ad14ea6e3..f57373702d 100644 --- a/tests/cpp/plugins/CMakeLists.txt +++ b/tests/cpp/plugins/CMakeLists.txt @@ -3,6 +3,10 @@ add_subdirectory(plugin_utils) +if(TARGET avatar_plugin_core) + add_subdirectory(sharpa_avatar) +endif() + if(BUILD_PLUGIN_OGLO) add_subdirectory(oglo_tactile) endif() diff --git a/tests/cpp/plugins/sharpa_avatar/CMakeLists.txt b/tests/cpp/plugins/sharpa_avatar/CMakeLists.txt new file mode 100644 index 0000000000..cdef58dbc5 --- /dev/null +++ b/tests/cpp/plugins/sharpa_avatar/CMakeLists.txt @@ -0,0 +1,14 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +add_executable(sharpa_avatar_plugin_tests + test_avatar_health.cpp +) + +target_link_libraries(sharpa_avatar_plugin_tests + PRIVATE + Catch2::Catch2WithMain + isaac::avatar_plugin +) + +catch_discover_tests(sharpa_avatar_plugin_tests) diff --git a/tests/cpp/plugins/sharpa_avatar/test_avatar_health.cpp b/tests/cpp/plugins/sharpa_avatar/test_avatar_health.cpp new file mode 100644 index 0000000000..d7dc9aad57 --- /dev/null +++ b/tests/cpp/plugins/sharpa_avatar/test_avatar_health.cpp @@ -0,0 +1,282 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include +#include + +#include +#include +#include +#include +#include + +using plugins::avatar::DatasetHealthInput; +using plugins::avatar::GloveIngressSignal; +using plugins::avatar::GloveState; +using plugins::avatar::PhysicalGloveHealthInput; +using plugins::avatar::UsbDevicePresence; + +namespace +{ + +class TemporaryDirectory +{ +public: + TemporaryDirectory() + { + const auto nonce = std::chrono::steady_clock::now().time_since_epoch().count(); + path = std::filesystem::temp_directory_path() / ("sharpa-avatar-health-" + std::to_string(nonce)); + std::filesystem::create_directories(path); + } + + ~TemporaryDirectory() + { + std::error_code error; + std::filesystem::remove_all(path, error); + } + + std::filesystem::path path; +}; + +void write_text(const std::filesystem::path& path, const std::string& value) +{ + std::ofstream stream(path); + REQUIRE(stream); + stream << value; + REQUIRE(stream.good()); +} + +} // anonymous namespace + +TEST_CASE("Physical glove health follows inventory, ingress, and device errors", "[sharpa_avatar][health]") +{ + PhysicalGloveHealthInput input; + + SECTION("inventory failure is unknown rather than cached healthy") + { + input.inventory_present = true; + input.ingress_fresh = true; + const auto status = plugins::avatar::make_physical_glove_status("/glove/left", input); + CHECK(status.state == core::PluginDeviceState_UNKNOWN); + CHECK(status.reason == core::PluginDeviceReason_DEVICE_ERROR); + } + + SECTION("startup before discovery") + { + input.inventory_available = true; + const auto status = plugins::avatar::make_physical_glove_status("/glove/left", input); + CHECK(status.state == core::PluginDeviceState_UNKNOWN); + CHECK(status.reason == core::PluginDeviceReason_NO_HARDWARE_SIGNAL); + } + + SECTION("previously observed glove disappears") + { + input.inventory_available = true; + input.ever_observed = true; + const auto status = plugins::avatar::make_physical_glove_status("/glove/left", input); + CHECK(status.state == core::PluginDeviceState_DISCONNECTED); + CHECK(status.reason == core::PluginDeviceReason_NO_HARDWARE_SIGNAL); + } + + SECTION("online inventory row still waits for ingress") + { + input.inventory_available = true; + input.inventory_present = true; + input.sdk_online = true; + const auto status = plugins::avatar::make_physical_glove_status("/glove/left", input); + CHECK(status.state == core::PluginDeviceState_UNKNOWN); + CHECK(status.reason == core::PluginDeviceReason_NO_CURRENT_DATA); + } + + SECTION("fresh ingress is authoritative even if coarse SDK online lags") + { + input.inventory_available = true; + input.inventory_present = true; + input.ingress_fresh = true; + input.ever_received_ingress = true; + const auto status = plugins::avatar::make_physical_glove_status("/glove/left", input); + CHECK(status.state == core::PluginDeviceState_CONNECTED); + CHECK(status.reason == core::PluginDeviceReason_NONE); + } + + SECTION("haptic or firmware errors degrade the physical glove") + { + input.inventory_available = true; + input.inventory_present = true; + input.ingress_fresh = true; + input.ever_received_ingress = true; + input.device_error = "set_vibration failed: CONNECTION_FAILED"; + const auto status = plugins::avatar::make_physical_glove_status("/glove/left", input); + CHECK(status.state == core::PluginDeviceState_DEGRADED); + CHECK(status.reason == core::PluginDeviceReason_DEVICE_ERROR); + CHECK(status.error.find("set_vibration") != std::string::npos); + } +} + +TEST_CASE("Dataset health is independent per output", "[sharpa_avatar][health]") +{ + DatasetHealthInput input; + + SECTION("disabled output") + { + input.enabled = false; + const auto status = plugins::avatar::make_dataset_status("/hand/left", input); + CHECK(status.state == core::PluginDeviceState_DISABLED); + CHECK(status.reason == core::PluginDeviceReason_DISABLED_BY_CONFIGURATION); + } + + SECTION("inventory failure overrides cached data") + { + input.inventory_present = true; + input.ingress_fresh = true; + input.data_fresh = true; + const auto status = plugins::avatar::make_dataset_status("/hand/left", input); + CHECK(status.state == core::PluginDeviceState_UNKNOWN); + CHECK(status.reason == core::PluginDeviceReason_DEVICE_ERROR); + } + + SECTION("missing physical glove") + { + input.inventory_available = true; + input.ever_observed = true; + const auto status = plugins::avatar::make_dataset_status("/hand/left", input); + CHECK(status.state == core::PluginDeviceState_DISCONNECTED); + CHECK(status.reason == core::PluginDeviceReason_NO_HARDWARE_SIGNAL); + } + + SECTION("fresh ingress without a first dataset sample remains startup") + { + input.inventory_available = true; + input.inventory_present = true; + input.ingress_fresh = true; + input.ever_received_ingress = true; + const auto status = plugins::avatar::make_dataset_status("/hand/left", input); + CHECK(status.state == core::PluginDeviceState_UNKNOWN); + CHECK(status.reason == core::PluginDeviceReason_NO_CURRENT_DATA); + } + + SECTION("NO_VALID_DATA_RETURNED is no current data") + { + input.inventory_available = true; + input.inventory_present = true; + input.ingress_fresh = true; + input.ever_received_ingress = true; + input.fetch_has_no_current_data = true; + const auto status = plugins::avatar::make_dataset_status("/hand/left", input); + CHECK(status.state == core::PluginDeviceState_UNKNOWN); + CHECK(status.reason == core::PluginDeviceReason_NO_CURRENT_DATA); + } + + SECTION("other fetch failures surface as device errors") + { + input.inventory_available = true; + input.inventory_present = true; + input.ingress_fresh = true; + input.ever_received_ingress = true; + input.fetch_error = "Avatar SDK fetch_data failed: CONNECTION_FAILED"; + const auto status = plugins::avatar::make_dataset_status("/hand/left", input); + CHECK(status.state == core::PluginDeviceState_DEGRADED); + CHECK(status.reason == core::PluginDeviceReason_DEVICE_ERROR); + } + + SECTION("each output keeps its own freshness") + { + input.inventory_available = true; + input.inventory_present = true; + input.ingress_fresh = true; + input.ever_received_ingress = true; + input.ever_received_data = true; + + input.data_fresh = true; + const auto human = plugins::avatar::make_dataset_status("/hand/left", input); + input.data_fresh = false; + const auto raw = plugins::avatar::make_dataset_status("/glove/avatar_raw_left", input); + + CHECK(human.state == core::PluginDeviceState_CONNECTED); + CHECK(raw.state == core::PluginDeviceState_DEGRADED); + CHECK(raw.reason == core::PluginDeviceReason_NO_CURRENT_DATA); + } +} + +TEST_CASE("Ingress signals keep callbacks scoped to one device generation", "[sharpa_avatar][lifecycle]") +{ + GloveState state; + auto retired = std::make_shared(); + state.ingress = retired; + retired->record(); + CHECK(retired->generation() == 1); + CHECK(retired->last_event().has_value()); + + state.reset(); + CHECK_FALSE(state.ingress); + + auto current = std::make_shared(); + state.ingress = current; + retired->record(); + CHECK(retired->generation() == 2); + CHECK(current->generation() == 0); + CHECK_FALSE(current->last_event().has_value()); +} + +TEST_CASE("Dongle detection handles complete and incomplete USB enumeration", "[sharpa_avatar][health]") +{ + TemporaryDirectory sysfs; + + CHECK(plugins::avatar::detect_usb_device_presence(sysfs.path, 0x1915, 0x520F) == UsbDevicePresence::ABSENT); + + std::filesystem::create_directories(sysfs.path / "1-2:1.0"); + CHECK(plugins::avatar::detect_usb_device_presence(sysfs.path, 0x1915, 0x520F) == UsbDevicePresence::ABSENT); + + const auto incomplete_device = sysfs.path / "1-3"; + std::filesystem::create_directories(incomplete_device); + write_text(incomplete_device / "idVendor", "1915\n"); + CHECK(plugins::avatar::detect_usb_device_presence(sysfs.path, 0x1915, 0x520F) == UsbDevicePresence::UNKNOWN); + + const auto dongle = sysfs.path / "1-2"; + std::filesystem::create_directories(dongle); + write_text(dongle / "idVendor", "1915\n"); + write_text(dongle / "idProduct", "520F\n"); + CHECK(plugins::avatar::detect_usb_device_presence(sysfs.path, 0x1915, 0x520F) == UsbDevicePresence::PRESENT); + + CHECK(plugins::avatar::detect_usb_device_presence(sysfs.path / "missing", 0x1915, 0x520F) == + UsbDevicePresence::UNKNOWN); +} + +TEST_CASE("Dongle health combines physical presence with the SDK link state", "[sharpa_avatar][health]") +{ + const auto connected = + plugins::avatar::make_dongle_status(UsbDevicePresence::PRESENT, ::avatar::UsbConnectionState::CONNECTED); + CHECK(connected.state == core::PluginDeviceState_CONNECTED); + CHECK(connected.reason == core::PluginDeviceReason_NONE); + + const auto absent_with_stale_sdk_state = + plugins::avatar::make_dongle_status(UsbDevicePresence::ABSENT, ::avatar::UsbConnectionState::CONNECTED); + CHECK(absent_with_stale_sdk_state.state == core::PluginDeviceState_DISCONNECTED); + CHECK(absent_with_stale_sdk_state.reason == core::PluginDeviceReason_NO_HARDWARE_SIGNAL); + + const auto unknown_presence = + plugins::avatar::make_dongle_status(UsbDevicePresence::UNKNOWN, ::avatar::UsbConnectionState::CONNECTED); + CHECK(unknown_presence.state == core::PluginDeviceState_UNKNOWN); + CHECK(unknown_presence.reason == core::PluginDeviceReason_NO_HARDWARE_SIGNAL); + + const auto disconnected = + plugins::avatar::make_dongle_status(UsbDevicePresence::PRESENT, ::avatar::UsbConnectionState::DISCONNECTED); + CHECK(disconnected.state == core::PluginDeviceState_DISCONNECTED); + CHECK(disconnected.reason == core::PluginDeviceReason_NO_HARDWARE_SIGNAL); + + const auto connecting = + plugins::avatar::make_dongle_status(UsbDevicePresence::PRESENT, ::avatar::UsbConnectionState::CONNECTING); + CHECK(connecting.state == core::PluginDeviceState_UNKNOWN); + CHECK(connecting.reason == core::PluginDeviceReason_NO_CURRENT_DATA); + + const auto reconnecting = + plugins::avatar::make_dongle_status(UsbDevicePresence::PRESENT, ::avatar::UsbConnectionState::RECONNECTING); + CHECK(reconnecting.state == core::PluginDeviceState_DEGRADED); + CHECK(reconnecting.reason == core::PluginDeviceReason_NO_CURRENT_DATA); + + const auto failed = + plugins::avatar::make_dongle_status(UsbDevicePresence::PRESENT, ::avatar::UsbConnectionState::ERROR); + CHECK(failed.state == core::PluginDeviceState_FAILED); + CHECK(failed.reason == core::PluginDeviceReason_DEVICE_ERROR); +} 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 f1c47caa10..e8a32d5155 100644 --- a/tests/python/core/teleop_session_manager/test_teleop_session.py +++ b/tests/python/core/teleop_session_manager/test_teleop_session.py @@ -1560,7 +1560,39 @@ def test_plugin_args_default_empty(self, tmp_path): class TestStatusMonitoringIntegration: - """Test Manus-only inventory, refresh, and provider failure integration.""" + """Test monitored-plugin inventory, refresh, and provider failure integration.""" + + @pytest.mark.parametrize("plugin", ["avatar_hand_plugin"]) + def test_device_plugin_is_monitored(self, tmp_path, plugin): + manager = MockPluginManager(plugin_names=[plugin]) + manager.get_plugin_info = MagicMock( + return_value=SimpleNamespace( + name=plugin, + devices=( + SimpleNamespace(path=f"/{plugin}", type=plugin, description=plugin), + ), + ) + ) + config = make_config( + MockPipeline(leaf_nodes=[]), + plugins=[ + PluginConfig( + plugin_name=plugin, + plugin_root_id=plugin, + search_paths=[tmp_path], + ) + ], + ) + + with ( + patch( + "isaaccapture.deviceio_trackers.PluginDeviceStatusTracker" + ) as tracker_cls, + mock_session_dependencies(mock_pm=manager, collected_trackers=[]), + ): + with TeleopSession(config) as session: + tracker_cls.assert_called_once_with(f"{plugin}/device_status") + assert session.get_device_status(f"{plugin}/{plugin}") is not None def test_only_manus_is_monitored_and_plugin_launch_order_is_preserved( self, tmp_path From c9bc9fa08ddbf00b066c28c978d861f52ddac5f1 Mon Sep 17 00:00:00 2001 From: zihan Date: Thu, 29 Oct 2026 10:15:03 +0800 Subject: [PATCH 2/5] fix: preserve health reports and retry XDev binding Signed-off-by: zihan --- src/core/live_trackers/AGENTS.md | 2 + .../cpp/live_hand_tracker_impl.cpp | 14 +- src/plugins/sharpa_avatar/AGENTS.md | 2 + .../teleop_session_manager/status_monitor.py | 1 + tests/cpp/core/live_trackers/CMakeLists.txt | 1 + .../test_hand_tracker_recovery.cpp | 205 ++++++++++++++++++ .../test_status_monitor.py | 48 ++++ 7 files changed, 271 insertions(+), 2 deletions(-) create mode 100644 tests/cpp/core/live_trackers/test_hand_tracker_recovery.cpp diff --git a/src/core/live_trackers/AGENTS.md b/src/core/live_trackers/AGENTS.md index 8e6b1f646a..83b97412a2 100644 --- a/src/core/live_trackers/AGENTS.md +++ b/src/core/live_trackers/AGENTS.md @@ -9,6 +9,8 @@ SPDX-License-Identifier: Apache-2.0 ## Time and OpenXR +- Cache an XDev topology only after every tracker for that topology is created; a partial bind must remain retryable + even when the enumerated IDs do not change. - Store **`last_update_time_` as `int64_t`** (monotonic ns), not **`XrTime`**. - **Once per `update` call:** `const XrTime xr_time = time_converter_.convert_monotonic_ns_to_xrtime(monotonic_time_ns);` then use **`xr_time`** for every **`xrLocate*`** / hand / body call **and** for MCAP (see below). **Do not** call **`convert_monotonic_ns_to_xrtime`** again in the MCAP block. - **Full-body limp mode:** if the body tracker handle is null and you **return early**, **do not** compute **`xr_time`** first—only convert after you know you will call OpenXR. diff --git a/src/core/live_trackers/cpp/live_hand_tracker_impl.cpp b/src/core/live_trackers/cpp/live_hand_tracker_impl.cpp index 828b1e55d4..b841754762 100644 --- a/src/core/live_trackers/cpp/live_hand_tracker_impl.cpp +++ b/src/core/live_trackers/cpp/live_hand_tracker_impl.cpp @@ -301,10 +301,20 @@ bool LiveHandTrackerImpl::refresh_xdev_hand_trackers() std::vector next_right_trackers; next_left_trackers.reserve(next_xdev_ids.size()); next_right_trackers.reserve(next_xdev_ids.size()); + bool all_trackers_created = true; for (const XrXDevIdMNDX xdev_id : next_xdev_ids) { - try_create_xdev_hand_tracker(session_, xdev_id, XR_HAND_LEFT_EXT, next_left_trackers); - try_create_xdev_hand_tracker(session_, xdev_id, XR_HAND_RIGHT_EXT, next_right_trackers); + const bool left_created = try_create_xdev_hand_tracker(session_, xdev_id, XR_HAND_LEFT_EXT, next_left_trackers); + const bool right_created = + try_create_xdev_hand_tracker(session_, xdev_id, XR_HAND_RIGHT_EXT, next_right_trackers); + all_trackers_created = left_created && right_created && all_trackers_created; + } + + if (!all_trackers_created) + { + destroy_hand_trackers(next_left_trackers); + destroy_hand_trackers(next_right_trackers); + return false; } left_xdev_hand_trackers_.swap(next_left_trackers); diff --git a/src/plugins/sharpa_avatar/AGENTS.md b/src/plugins/sharpa_avatar/AGENTS.md index d2fad8d191..13dd22eae7 100644 --- a/src/plugins/sharpa_avatar/AGENTS.md +++ b/src/plugins/sharpa_avatar/AGENTS.md @@ -65,6 +65,8 @@ SPDX-License-Identifier: Apache-2.0 - Keep transport presence, physical-glove ingress/faults, and HUMAN/RAW/ROBOT output freshness/errors as separate health layers. One advancing dataset must not keep another dataset healthy. +- Validate every emitted health state/reason pair through the Python status reducer; producer-only tests do not + protect the cross-language monitoring contract. - Keep the static HUMAN-to-OpenXR index table synchronized with the pinned production SDK's landmark order. Do not read joint names from `sdk_config.json` at runtime or fill unsupported OpenXR slots with neighbouring diff --git a/src/python/isaaccapture/teleop_session_manager/status_monitor.py b/src/python/isaaccapture/teleop_session_manager/status_monitor.py index 973ec9264b..9633c2fb97 100644 --- a/src/python/isaaccapture/teleop_session_manager/status_monitor.py +++ b/src/python/isaaccapture/teleop_session_manager/status_monitor.py @@ -39,6 +39,7 @@ _VALID_PLUGIN_DEVICE_STATE_REASON_PAIRS = { ("UNKNOWN", "NO_HARDWARE_SIGNAL"), ("UNKNOWN", "NO_CURRENT_DATA"), + ("UNKNOWN", "DEVICE_ERROR"), ("CONNECTED", "NONE"), ("DISCONNECTED", "NO_HARDWARE_SIGNAL"), ("DEGRADED", "CALIBRATION_FAILED"), diff --git a/tests/cpp/core/live_trackers/CMakeLists.txt b/tests/cpp/core/live_trackers/CMakeLists.txt index e9094c08e9..5f9f23c693 100644 --- a/tests/cpp/core/live_trackers/CMakeLists.txt +++ b/tests/cpp/core/live_trackers/CMakeLists.txt @@ -6,6 +6,7 @@ cmake_minimum_required(VERSION 3.20) add_executable(live_trackers_tests test_vendor_validation.cpp test_keyboard_session.cpp + test_hand_tracker_recovery.cpp ) target_link_libraries(live_trackers_tests PRIVATE diff --git a/tests/cpp/core/live_trackers/test_hand_tracker_recovery.cpp b/tests/cpp/core/live_trackers/test_hand_tracker_recovery.cpp new file mode 100644 index 0000000000..8988513c5c --- /dev/null +++ b/tests/cpp/core/live_trackers/test_hand_tracker_recovery.cpp @@ -0,0 +1,205 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace +{ + +class FakeHandRuntime +{ +public: + FakeHandRuntime() + { + if (current_ != nullptr) + { + throw std::logic_error("FakeHandRuntime: one instance at a time"); + } + current_ = this; + } + + ~FakeHandRuntime() + { + current_ = nullptr; + } + + FakeHandRuntime(const FakeHandRuntime&) = delete; + FakeHandRuntime& operator=(const FakeHandRuntime&) = delete; + + core::OpenXRSessionHandles handles() const + { + core::OpenXRSessionHandles handles; + handles.instance = reinterpret_cast(1); + handles.session = reinterpret_cast(1); + handles.space = reinterpret_cast(1); + handles.xrGetInstanceProcAddr = &get_instance_proc_addr; + return handles; + } + + int left_xdev_create_attempts() const + { + return left_xdev_create_attempts_; + } + +private: + static XrResult XRAPI_CALL get_system(XrInstance, const XrSystemGetInfo*, XrSystemId* system_id) + { + *system_id = 1; + return XR_SUCCESS; + } + + static XrResult XRAPI_CALL get_system_properties(XrInstance, XrSystemId, XrSystemProperties* properties) + { + auto* hand_tracking = static_cast(properties->next); + hand_tracking->supportsHandTracking = XR_TRUE; + return XR_SUCCESS; + } + + static XrResult XRAPI_CALL create_reference_space(XrSession, const XrReferenceSpaceCreateInfo*, XrSpace* space) + { + *space = reinterpret_cast(2); + return XR_SUCCESS; + } + + static XrResult XRAPI_CALL destroy_space(XrSpace) + { + return XR_SUCCESS; + } + + static XrResult XRAPI_CALL locate_space(XrSpace, XrSpace, XrTime, XrSpaceLocation* location) + { + location->locationFlags = 0; + return XR_SUCCESS; + } + + static XrResult XRAPI_CALL create_xdev_list(XrSession, const XrCreateXDevListInfoMNDX*, XrXDevListMNDX* list) + { + *list = reinterpret_cast(1); + return XR_SUCCESS; + } + + static XrResult XRAPI_CALL destroy_xdev_list(XrXDevListMNDX) + { + return XR_SUCCESS; + } + + static XrResult XRAPI_CALL enumerate_xdevs(XrXDevListMNDX, uint32_t capacity, uint32_t* count, XrXDevIdMNDX* xdevs) + { + *count = 1; + if (capacity != 0) + { + xdevs[0] = 42; + } + return XR_SUCCESS; + } + + static XrResult XRAPI_CALL get_xdev_properties(XrXDevListMNDX, + const XrGetXDevInfoMNDX*, + XrXDevPropertiesMNDX* properties) + { + std::strncpy(properties->name, "Avatar glove", sizeof(properties->name) - 1); + properties->canCreateSpace = XR_TRUE; + return XR_SUCCESS; + } + + static XrResult XRAPI_CALL create_hand_tracker(XrSession, + const XrHandTrackerCreateInfoEXT* create_info, + XrHandTrackerEXT* tracker) + { + const auto* xdev_info = static_cast(create_info->next); + if (xdev_info != nullptr && create_info->hand == XR_HAND_LEFT_EXT) + { + ++current_->left_xdev_create_attempts_; + if (current_->fail_first_left_xdev_create_) + { + current_->fail_first_left_xdev_create_ = false; + return XR_ERROR_RUNTIME_FAILURE; + } + } + + *tracker = reinterpret_cast(current_->next_tracker_handle_++); + return XR_SUCCESS; + } + + static XrResult XRAPI_CALL destroy_hand_tracker(XrHandTrackerEXT) + { + return XR_SUCCESS; + } + + static XrResult XRAPI_CALL locate_hand_joints(XrHandTrackerEXT, + const XrHandJointsLocateInfoEXT*, + XrHandJointLocationsEXT* locations) + { + locations->isActive = XR_FALSE; + return XR_SUCCESS; + } + + static XrResult XRAPI_CALL timespec_to_time(XrInstance, const struct timespec* timespec, XrTime* time) + { + *time = static_cast(timespec->tv_sec) * 1'000'000'000 + timespec->tv_nsec; + return XR_SUCCESS; + } + + static XrResult XRAPI_CALL time_to_timespec(XrInstance, XrTime time, struct timespec* timespec) + { + timespec->tv_sec = static_cast(time / 1'000'000'000); + timespec->tv_nsec = static_cast(time % 1'000'000'000); + return XR_SUCCESS; + } + + static XrResult XRAPI_CALL get_instance_proc_addr(XrInstance, const char* name, PFN_xrVoidFunction* function) + { + static const std::map functions = { + { "xrGetSystem", reinterpret_cast(&get_system) }, + { "xrGetSystemProperties", reinterpret_cast(&get_system_properties) }, + { "xrCreateReferenceSpace", reinterpret_cast(&create_reference_space) }, + { "xrDestroySpace", reinterpret_cast(&destroy_space) }, + { "xrLocateSpace", reinterpret_cast(&locate_space) }, + { "xrCreateXDevListMNDX", reinterpret_cast(&create_xdev_list) }, + { "xrDestroyXDevListMNDX", reinterpret_cast(&destroy_xdev_list) }, + { "xrEnumerateXDevsMNDX", reinterpret_cast(&enumerate_xdevs) }, + { "xrGetXDevPropertiesMNDX", reinterpret_cast(&get_xdev_properties) }, + { "xrCreateHandTrackerEXT", reinterpret_cast(&create_hand_tracker) }, + { "xrDestroyHandTrackerEXT", reinterpret_cast(&destroy_hand_tracker) }, + { "xrLocateHandJointsEXT", reinterpret_cast(&locate_hand_joints) }, + { "xrConvertTimespecTimeToTimeKHR", reinterpret_cast(×pec_to_time) }, + { "xrConvertTimeToTimespecTimeKHR", reinterpret_cast(&time_to_timespec) }, + }; + const auto it = functions.find(name); + *function = it == functions.end() ? nullptr : it->second; + return it == functions.end() ? XR_ERROR_FUNCTION_UNSUPPORTED : XR_SUCCESS; + } + + inline static FakeHandRuntime* current_ = nullptr; + + uintptr_t next_tracker_handle_ = 1; + int left_xdev_create_attempts_ = 0; + bool fail_first_left_xdev_create_ = true; +}; + +} // anonymous namespace + +TEST_CASE("HandTracker retries an incomplete XDev binding without a topology change", "[unit][hand][xdev]") +{ + FakeHandRuntime runtime; + auto hand = std::make_shared(); + auto session = core::DeviceIOSession::run({ hand }, runtime.handles()); + + REQUIRE(runtime.left_xdev_create_attempts() == 1); + + session->update(); + + CHECK(runtime.left_xdev_create_attempts() == 2); +} 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..647f516ddf 100644 --- a/tests/python/core/teleop_session_manager/test_status_monitor.py +++ b/tests/python/core/teleop_session_manager/test_status_monitor.py @@ -8,6 +8,12 @@ from types import SimpleNamespace import pytest +from isaaccapture.schema import ( + PluginDeviceReason, + PluginDeviceState, + PluginDeviceStatus, + PluginDeviceStatusSnapshot, +) from isaaccapture.teleop_session_manager import ( DeviceState, ProviderState, @@ -139,6 +145,12 @@ def make_monitor(*, entries=None, context=None, now_ns=10_000_000_000): DeviceState.UNKNOWN, StatusReason.NO_CURRENT_DATA, ), + ( + "UNKNOWN", + "DEVICE_ERROR", + DeviceState.UNKNOWN, + StatusReason.DEVICE_ERROR, + ), ("CONNECTED", "NONE", DeviceState.CONNECTED, StatusReason.NONE), ( "DISCONNECTED", @@ -196,6 +208,42 @@ def test_all_schema_device_states_are_mapped( assert device.reason == expected_reason +def test_inventory_failure_report_preserves_independent_device_states(): + now_ns = 10_000_000_000 + snapshot = PluginDeviceStatusSnapshot( + 1, + now_ns, + [ + PluginDeviceStatus( + "/left", + PluginDeviceState.UNKNOWN, + PluginDeviceReason.DEVICE_ERROR, + "Avatar SDK inventory is unavailable", + ), + PluginDeviceStatus( + "/right", + PluginDeviceState.DISABLED, + PluginDeviceReason.DISABLED_BY_CONFIGURATION, + ), + ], + ) + monitor, tracker = make_monitor(now_ns=now_ns) + tracker.report = snapshot + + monitor.refresh(object(), now_ns=now_ns) + + left = monitor.get_device_status("/provider/left") + right = monitor.get_device_status("/provider/right") + assert (left.status, left.reason) == ( + DeviceState.UNKNOWN, + StatusReason.DEVICE_ERROR, + ) + assert (right.status, right.reason) == ( + DeviceState.DISABLED, + StatusReason.DISABLED_BY_CONFIGURATION, + ) + + @pytest.mark.parametrize( ("process", "provider_state", "provider_reason"), [ From 8b3f42102e397480f40afb3a7924c41b4efe4bea Mon Sep 17 00:00:00 2001 From: zihan Date: Thu, 29 Oct 2026 10:23:03 +0800 Subject: [PATCH 3/5] fix: settle unsupported XDev bindings Signed-off-by: zihan --- src/core/live_trackers/AGENTS.md | 5 ++-- .../cpp/live_hand_tracker_impl.cpp | 28 +++++++++-------- .../cpp/live_hand_tracker_impl.hpp | 15 +++++++--- .../test_hand_tracker_recovery.cpp | 30 ++++++++++++++++++- 4 files changed, 59 insertions(+), 19 deletions(-) diff --git a/src/core/live_trackers/AGENTS.md b/src/core/live_trackers/AGENTS.md index 83b97412a2..ee0a2b6bf4 100644 --- a/src/core/live_trackers/AGENTS.md +++ b/src/core/live_trackers/AGENTS.md @@ -9,8 +9,9 @@ SPDX-License-Identifier: Apache-2.0 ## Time and OpenXR -- Cache an XDev topology only after every tracker for that topology is created; a partial bind must remain retryable - even when the enumerated IDs do not change. +- Cache an XDev topology only after every ID/hand pair reaches a settled outcome. Treat + `XR_ERROR_FEATURE_UNSUPPORTED` as settled, but leave transient creation failures retryable even when the IDs do not + change. - Store **`last_update_time_` as `int64_t`** (monotonic ns), not **`XrTime`**. - **Once per `update` call:** `const XrTime xr_time = time_converter_.convert_monotonic_ns_to_xrtime(monotonic_time_ns);` then use **`xr_time`** for every **`xrLocate*`** / hand / body call **and** for MCAP (see below). **Do not** call **`convert_monotonic_ns_to_xrtime`** again in the MCAP block. - **Full-body limp mode:** if the body tracker handle is null and you **return early**, **do not** compute **`xr_time`** first—only convert after you know you will call OpenXR. diff --git a/src/core/live_trackers/cpp/live_hand_tracker_impl.cpp b/src/core/live_trackers/cpp/live_hand_tracker_impl.cpp index b841754762..ef3f9f2cc6 100644 --- a/src/core/live_trackers/cpp/live_hand_tracker_impl.cpp +++ b/src/core/live_trackers/cpp/live_hand_tracker_impl.cpp @@ -301,16 +301,18 @@ bool LiveHandTrackerImpl::refresh_xdev_hand_trackers() std::vector next_right_trackers; next_left_trackers.reserve(next_xdev_ids.size()); next_right_trackers.reserve(next_xdev_ids.size()); - bool all_trackers_created = true; + bool retry_needed = false; for (const XrXDevIdMNDX xdev_id : next_xdev_ids) { - const bool left_created = try_create_xdev_hand_tracker(session_, xdev_id, XR_HAND_LEFT_EXT, next_left_trackers); - const bool right_created = + const XDevTrackerCreation left_result = + try_create_xdev_hand_tracker(session_, xdev_id, XR_HAND_LEFT_EXT, next_left_trackers); + const XDevTrackerCreation right_result = try_create_xdev_hand_tracker(session_, xdev_id, XR_HAND_RIGHT_EXT, next_right_trackers); - all_trackers_created = left_created && right_created && all_trackers_created; + retry_needed = + left_result == XDevTrackerCreation::RETRY || right_result == XDevTrackerCreation::RETRY || retry_needed; } - if (!all_trackers_created) + if (retry_needed) { destroy_hand_trackers(next_left_trackers); destroy_hand_trackers(next_right_trackers); @@ -327,14 +329,12 @@ bool LiveHandTrackerImpl::refresh_xdev_hand_trackers() return true; } -bool LiveHandTrackerImpl::try_create_xdev_hand_tracker(XrSession session, - XrXDevIdMNDX xdev_id, - XrHandEXT hand, - std::vector& trackers) +LiveHandTrackerImpl::XDevTrackerCreation LiveHandTrackerImpl::try_create_xdev_hand_tracker( + XrSession session, XrXDevIdMNDX xdev_id, XrHandEXT hand, std::vector& trackers) { if (xdev_list_ == XR_NULL_HANDLE || xdev_id == 0) { - return false; + return XDevTrackerCreation::RETRY; } XrCreateHandTrackerXDevMNDX xdev_create_info{ XR_TYPE_CREATE_HAND_TRACKER_XDEV_MNDX }; @@ -351,13 +351,17 @@ bool LiveHandTrackerImpl::try_create_xdev_hand_tracker(XrSession session, XrHandTrackerEXT tracker = XR_NULL_HANDLE; const XrResult result = pfn_create_hand_tracker_(session, &create_info, &tracker); + if (result == XR_ERROR_FEATURE_UNSUPPORTED) + { + return XDevTrackerCreation::UNSUPPORTED; + } if (XR_FAILED(result)) { - return false; + return XDevTrackerCreation::RETRY; } trackers.push_back(tracker); - return true; + return XDevTrackerCreation::CREATED; } bool LiveHandTrackerImpl::try_create_default_hand_tracker(XrSession session, diff --git a/src/core/live_trackers/cpp/live_hand_tracker_impl.hpp b/src/core/live_trackers/cpp/live_hand_tracker_impl.hpp index 8d0c59dacf..e31ebe4399 100644 --- a/src/core/live_trackers/cpp/live_hand_tracker_impl.hpp +++ b/src/core/live_trackers/cpp/live_hand_tracker_impl.hpp @@ -43,13 +43,20 @@ class LiveHandTrackerImpl : public IHandTrackerImpl const Serialized& get_right_hand() const override; private: + enum class XDevTrackerCreation + { + CREATED, + UNSUPPORTED, + RETRY, + }; + void initialize_xdev_hand_trackers(const OpenXRSessionHandles& handles); bool enumerate_xdev_ids(std::vector& xdev_ids); bool refresh_xdev_hand_trackers(); - bool try_create_xdev_hand_tracker(XrSession session, - XrXDevIdMNDX xdev_id, - XrHandEXT hand, - std::vector& trackers); + XDevTrackerCreation try_create_xdev_hand_tracker(XrSession session, + XrXDevIdMNDX xdev_id, + XrHandEXT hand, + std::vector& trackers); bool try_create_default_hand_tracker(XrSession session, XrHandEXT hand, std::vector& trackers); void destroy_hand_trackers(std::vector& trackers); void destroy_xdev_list(); diff --git a/tests/cpp/core/live_trackers/test_hand_tracker_recovery.cpp b/tests/cpp/core/live_trackers/test_hand_tracker_recovery.cpp index 8988513c5c..712b53402d 100644 --- a/tests/cpp/core/live_trackers/test_hand_tracker_recovery.cpp +++ b/tests/cpp/core/live_trackers/test_hand_tracker_recovery.cpp @@ -21,7 +21,13 @@ namespace class FakeHandRuntime { public: - FakeHandRuntime() + enum class XDevBehavior + { + FAIL_FIRST_LEFT, + UNSUPPORTED, + }; + + explicit FakeHandRuntime(XDevBehavior xdev_behavior = XDevBehavior::FAIL_FIRST_LEFT) : xdev_behavior_(xdev_behavior) { if (current_ != nullptr) { @@ -122,12 +128,20 @@ class FakeHandRuntime if (xdev_info != nullptr && create_info->hand == XR_HAND_LEFT_EXT) { ++current_->left_xdev_create_attempts_; + if (current_->xdev_behavior_ == XDevBehavior::UNSUPPORTED) + { + return XR_ERROR_FEATURE_UNSUPPORTED; + } if (current_->fail_first_left_xdev_create_) { current_->fail_first_left_xdev_create_ = false; return XR_ERROR_RUNTIME_FAILURE; } } + if (xdev_info != nullptr && current_->xdev_behavior_ == XDevBehavior::UNSUPPORTED) + { + return XR_ERROR_FEATURE_UNSUPPORTED; + } *tracker = reinterpret_cast(current_->next_tracker_handle_++); return XR_SUCCESS; @@ -186,6 +200,7 @@ class FakeHandRuntime uintptr_t next_tracker_handle_ = 1; int left_xdev_create_attempts_ = 0; + XDevBehavior xdev_behavior_; bool fail_first_left_xdev_create_ = true; }; @@ -203,3 +218,16 @@ TEST_CASE("HandTracker retries an incomplete XDev binding without a topology cha CHECK(runtime.left_xdev_create_attempts() == 2); } + +TEST_CASE("HandTracker caches XDevs that do not support hand tracking", "[unit][hand][xdev]") +{ + FakeHandRuntime runtime(FakeHandRuntime::XDevBehavior::UNSUPPORTED); + auto hand = std::make_shared(); + auto session = core::DeviceIOSession::run({ hand }, runtime.handles()); + + REQUIRE(runtime.left_xdev_create_attempts() == 1); + + session->update(); + + CHECK(runtime.left_xdev_create_attempts() == 1); +} From fa3aa8215b62fa19b362aba1fd5497ee2b6ebdd8 Mon Sep 17 00:00:00 2001 From: zihan Date: Fri, 30 Oct 2026 10:42:49 +0800 Subject: [PATCH 4/5] fix: preserve dynamic XDev and haptic retries Signed-off-by: zihan --- .../cpp/live_hand_tracker_impl.cpp | 72 +++++++++++++------ .../cpp/live_hand_tracker_impl.hpp | 5 +- .../core/avatar_hand_tracking_plugin.cpp | 33 ++++++--- .../avatar/avatar_hand_tracking_plugin.hpp | 34 ++++++++- .../test_hand_tracker_recovery.cpp | 64 ++++++++++++++++- .../sharpa_avatar/test_avatar_health.cpp | 41 +++++++++++ 6 files changed, 211 insertions(+), 38 deletions(-) diff --git a/src/core/live_trackers/cpp/live_hand_tracker_impl.cpp b/src/core/live_trackers/cpp/live_hand_tracker_impl.cpp index ef3f9f2cc6..d59117164d 100644 --- a/src/core/live_trackers/cpp/live_hand_tracker_impl.cpp +++ b/src/core/live_trackers/cpp/live_hand_tracker_impl.cpp @@ -82,6 +82,7 @@ LiveHandTrackerImpl::LiveHandTrackerImpl(const OpenXRSessionHandles& handles, pfn_locate_hand_joints_(nullptr), pfn_create_xdev_list_(nullptr), pfn_destroy_xdev_list_(nullptr), + pfn_get_xdev_list_generation_(nullptr), pfn_enumerate_xdevs_(nullptr), pfn_get_xdev_properties_(nullptr), mcap_channels_(std::move(mcap_channels)) @@ -210,36 +211,31 @@ void LiveHandTrackerImpl::initialize_xdev_hand_trackers(const OpenXRSessionHandl if (!load_optional_func("xrCreateXDevListMNDX", reinterpret_cast(&pfn_create_xdev_list_)) || !load_optional_func("xrDestroyXDevListMNDX", reinterpret_cast(&pfn_destroy_xdev_list_)) || + !load_optional_func("xrGetXDevListGenerationNumberMNDX", + reinterpret_cast(&pfn_get_xdev_list_generation_)) || !load_optional_func("xrEnumerateXDevsMNDX", reinterpret_cast(&pfn_enumerate_xdevs_)) || !load_optional_func("xrGetXDevPropertiesMNDX", reinterpret_cast(&pfn_get_xdev_properties_))) { pfn_create_xdev_list_ = nullptr; pfn_destroy_xdev_list_ = nullptr; + pfn_get_xdev_list_generation_ = nullptr; pfn_enumerate_xdevs_ = nullptr; pfn_get_xdev_properties_ = nullptr; return; } - XrCreateXDevListInfoMNDX create_info{ XR_TYPE_CREATE_XDEV_LIST_INFO_MNDX }; - XrResult result = pfn_create_xdev_list_(handles.session, &create_info, &xdev_list_); - if (XR_FAILED(result)) - { - xdev_list_ = XR_NULL_HANDLE; - return; - } - refresh_xdev_hand_trackers(); } -bool LiveHandTrackerImpl::enumerate_xdev_ids(std::vector& ordered_ids) +bool LiveHandTrackerImpl::enumerate_xdev_ids(XrXDevListMNDX xdev_list, std::vector& ordered_ids) { - if (xdev_list_ == XR_NULL_HANDLE) + if (xdev_list == XR_NULL_HANDLE) { return false; } uint32_t xdev_count = 0; - XrResult result = pfn_enumerate_xdevs_(xdev_list_, 0, &xdev_count, nullptr); + XrResult result = pfn_enumerate_xdevs_(xdev_list, 0, &xdev_count, nullptr); if (XR_FAILED(result)) { return false; @@ -247,7 +243,7 @@ bool LiveHandTrackerImpl::enumerate_xdev_ids(std::vector& ordered_ std::vector xdev_ids(xdev_count); if (xdev_count != 0) { - result = pfn_enumerate_xdevs_(xdev_list_, xdev_count, &xdev_count, xdev_ids.data()); + result = pfn_enumerate_xdevs_(xdev_list, xdev_count, &xdev_count, xdev_ids.data()); if (XR_FAILED(result)) { return false; @@ -263,7 +259,7 @@ bool LiveHandTrackerImpl::enumerate_xdev_ids(std::vector& ordered_ get_info.id = xdev_id; XrXDevPropertiesMNDX properties{ XR_TYPE_XDEV_PROPERTIES_MNDX }; - result = pfn_get_xdev_properties_(xdev_list_, &get_info, &properties); + result = pfn_get_xdev_properties_(xdev_list, &get_info, &properties); if (XR_FAILED(result)) { return false; @@ -287,16 +283,37 @@ bool LiveHandTrackerImpl::enumerate_xdev_ids(std::vector& ordered_ bool LiveHandTrackerImpl::refresh_xdev_hand_trackers() { - std::vector next_xdev_ids; - if (!enumerate_xdev_ids(next_xdev_ids)) + if (pfn_create_xdev_list_ == nullptr || pfn_destroy_xdev_list_ == nullptr || pfn_get_xdev_list_generation_ == nullptr) + { + return false; + } + + XrXDevListMNDX next_xdev_list = XR_NULL_HANDLE; + XrCreateXDevListInfoMNDX create_info{ XR_TYPE_CREATE_XDEV_LIST_INFO_MNDX }; + if (XR_FAILED(pfn_create_xdev_list_(session_, &create_info, &next_xdev_list))) + { + return false; + } + + uint64_t next_generation = 0; + if (XR_FAILED(pfn_get_xdev_list_generation_(next_xdev_list, &next_generation))) { + pfn_destroy_xdev_list_(next_xdev_list); return false; } - if (next_xdev_ids == xdev_ids_) + if (xdev_generation_ && next_generation == *xdev_generation_) { + pfn_destroy_xdev_list_(next_xdev_list); return true; } + std::vector next_xdev_ids; + if (!enumerate_xdev_ids(next_xdev_list, next_xdev_ids)) + { + pfn_destroy_xdev_list_(next_xdev_list); + return false; + } + std::vector next_left_trackers; std::vector next_right_trackers; next_left_trackers.reserve(next_xdev_ids.size()); @@ -305,9 +322,9 @@ bool LiveHandTrackerImpl::refresh_xdev_hand_trackers() for (const XrXDevIdMNDX xdev_id : next_xdev_ids) { const XDevTrackerCreation left_result = - try_create_xdev_hand_tracker(session_, xdev_id, XR_HAND_LEFT_EXT, next_left_trackers); + try_create_xdev_hand_tracker(session_, next_xdev_list, xdev_id, XR_HAND_LEFT_EXT, next_left_trackers); const XDevTrackerCreation right_result = - try_create_xdev_hand_tracker(session_, xdev_id, XR_HAND_RIGHT_EXT, next_right_trackers); + try_create_xdev_hand_tracker(session_, next_xdev_list, xdev_id, XR_HAND_RIGHT_EXT, next_right_trackers); retry_needed = left_result == XDevTrackerCreation::RETRY || right_result == XDevTrackerCreation::RETRY || retry_needed; } @@ -316,29 +333,37 @@ bool LiveHandTrackerImpl::refresh_xdev_hand_trackers() { destroy_hand_trackers(next_left_trackers); destroy_hand_trackers(next_right_trackers); + pfn_destroy_xdev_list_(next_xdev_list); return false; } + const XrXDevListMNDX previous_xdev_list = xdev_list_; + xdev_list_ = next_xdev_list; left_xdev_hand_trackers_.swap(next_left_trackers); right_xdev_hand_trackers_.swap(next_right_trackers); destroy_hand_trackers(next_left_trackers); destroy_hand_trackers(next_right_trackers); + if (previous_xdev_list != XR_NULL_HANDLE) + { + pfn_destroy_xdev_list_(previous_xdev_list); + } xdev_ids_ = std::move(next_xdev_ids); - logger_->info("Refreshed XDev hand trackers (devices: {}, left: {}, right: {})", xdev_ids_.size(), - left_xdev_hand_trackers_.size(), right_xdev_hand_trackers_.size()); + xdev_generation_ = next_generation; + logger_->info("Refreshed XDev hand trackers (generation: {}, devices: {}, left: {}, right: {})", next_generation, + xdev_ids_.size(), left_xdev_hand_trackers_.size(), right_xdev_hand_trackers_.size()); return true; } LiveHandTrackerImpl::XDevTrackerCreation LiveHandTrackerImpl::try_create_xdev_hand_tracker( - XrSession session, XrXDevIdMNDX xdev_id, XrHandEXT hand, std::vector& trackers) + XrSession session, XrXDevListMNDX xdev_list, XrXDevIdMNDX xdev_id, XrHandEXT hand, std::vector& trackers) { - if (xdev_list_ == XR_NULL_HANDLE || xdev_id == 0) + if (xdev_list == XR_NULL_HANDLE || xdev_id == 0) { return XDevTrackerCreation::RETRY; } XrCreateHandTrackerXDevMNDX xdev_create_info{ XR_TYPE_CREATE_HAND_TRACKER_XDEV_MNDX }; - xdev_create_info.xdevList = xdev_list_; + xdev_create_info.xdevList = xdev_list; xdev_create_info.id = xdev_id; XrHandTrackerCreateInfoEXT create_info{ XR_TYPE_HAND_TRACKER_CREATE_INFO_EXT }; @@ -405,6 +430,7 @@ void LiveHandTrackerImpl::destroy_xdev_list() { pfn_destroy_xdev_list_(xdev_list_); xdev_list_ = XR_NULL_HANDLE; + xdev_generation_.reset(); } } diff --git a/src/core/live_trackers/cpp/live_hand_tracker_impl.hpp b/src/core/live_trackers/cpp/live_hand_tracker_impl.hpp index e31ebe4399..d52be7f328 100644 --- a/src/core/live_trackers/cpp/live_hand_tracker_impl.hpp +++ b/src/core/live_trackers/cpp/live_hand_tracker_impl.hpp @@ -51,9 +51,10 @@ class LiveHandTrackerImpl : public IHandTrackerImpl }; void initialize_xdev_hand_trackers(const OpenXRSessionHandles& handles); - bool enumerate_xdev_ids(std::vector& xdev_ids); + bool enumerate_xdev_ids(XrXDevListMNDX xdev_list, std::vector& xdev_ids); bool refresh_xdev_hand_trackers(); XDevTrackerCreation try_create_xdev_hand_tracker(XrSession session, + XrXDevListMNDX xdev_list, XrXDevIdMNDX xdev_id, XrHandEXT hand, std::vector& trackers); @@ -77,6 +78,7 @@ class LiveHandTrackerImpl : public IHandTrackerImpl std::vector right_default_hand_trackers_; std::vector xdev_ids_; XrXDevListMNDX xdev_list_; + std::optional xdev_generation_; int64_t last_xdev_refresh_time_ = 0; // The snapshots published each frame, encoded from locals in update(). @@ -89,6 +91,7 @@ class LiveHandTrackerImpl : public IHandTrackerImpl PFN_xrLocateHandJointsEXT pfn_locate_hand_joints_; PFN_xrCreateXDevListMNDX pfn_create_xdev_list_; PFN_xrDestroyXDevListMNDX pfn_destroy_xdev_list_; + PFN_xrGetXDevListGenerationNumberMNDX pfn_get_xdev_list_generation_; PFN_xrEnumerateXDevsMNDX pfn_enumerate_xdevs_; PFN_xrGetXDevPropertiesMNDX pfn_get_xdev_properties_; diff --git a/src/plugins/sharpa_avatar/core/avatar_hand_tracking_plugin.cpp b/src/plugins/sharpa_avatar/core/avatar_hand_tracking_plugin.cpp index 950cbf40a6..c88faebd16 100644 --- a/src/plugins/sharpa_avatar/core/avatar_hand_tracking_plugin.cpp +++ b/src/plugins/sharpa_avatar/core/avatar_hand_tracking_plugin.cpp @@ -601,6 +601,7 @@ void AvatarTracker::reset_glove(::avatar::DeviceSide side) noexcept stop_haptics(side); destroy_output_resources(side); glove(side).reset(); + m_haptic_commands[index].discard(); m_haptic_stopped[index] = true; m_last_haptic_sample_times[index].reset(); m_haptic_retry_after[index].reset(); @@ -951,23 +952,33 @@ void AvatarTracker::process_haptics() { const size_t index = side_index(side); const auto& tracked = m_haptic_reader->get_data(*m_deviceio_session, to_string(side)); - const core::HapticCommand* command = tracked.get(); - if (command != nullptr && command != m_last_haptic_commands[index].get()) + HapticCommandDeliveryState& delivery = m_haptic_commands[index]; + delivery.observe(tracked); + const core::HapticCommand* command = delivery.pending(); + if (command != nullptr) { - m_last_haptic_commands[index] = tracked; const bool retry_ready = !m_haptic_retry_after[index] || now >= *m_haptic_retry_after[index]; - if (retry_ready && command->values() != nullptr && command->values()->size() == kAvatarFingerCount) + if (retry_ready) { - std::array powers{}; - for (size_t i = 0; i < kAvatarFingerCount; ++i) + if (command->values() == nullptr || command->values()->size() != kAvatarFingerCount) { - powers[i] = command->values()->Get(i); + delivery.discard(); } - if (apply_haptic_command(side, powers)) + else { - m_last_haptic_sample_times[index] = now; - m_haptic_stopped[index] = std::all_of( - powers.begin(), powers.end(), [](float power) { return !std::isfinite(power) || power <= 0.0f; }); + std::array powers{}; + for (size_t i = 0; i < kAvatarFingerCount; ++i) + { + powers[i] = command->values()->Get(i); + } + if (apply_haptic_command(side, powers)) + { + delivery.acknowledge(); + m_last_haptic_sample_times[index] = now; + m_haptic_stopped[index] = + std::all_of(powers.begin(), powers.end(), + [](float power) { return !std::isfinite(power) || power <= 0.0f; }); + } } } } diff --git a/src/plugins/sharpa_avatar/core/inc/avatar/avatar_hand_tracking_plugin.hpp b/src/plugins/sharpa_avatar/core/inc/avatar/avatar_hand_tracking_plugin.hpp index 861619630d..7200ca6516 100644 --- a/src/plugins/sharpa_avatar/core/inc/avatar/avatar_hand_tracking_plugin.hpp +++ b/src/plugins/sharpa_avatar/core/inc/avatar/avatar_hand_tracking_plugin.hpp @@ -139,6 +139,38 @@ struct GloveState bool ready = false; }; +class HapticCommandDeliveryState +{ +public: + void observe(const core::Serialized& command) + { + if (command.get() != m_latest.get()) + { + m_latest = command; + m_pending = command.get() != nullptr; + } + } + + const core::HapticCommand* pending() const noexcept + { + return m_pending ? m_latest.get() : nullptr; + } + + void acknowledge() noexcept + { + m_pending = false; + } + + void discard() noexcept + { + m_pending = false; + } + +private: + core::Serialized m_latest; + bool m_pending = false; +}; + class __attribute__((visibility("default"))) AvatarTracker { public: @@ -194,7 +226,7 @@ class __attribute__((visibility("default"))) AvatarTracker std::array, 2>, 2> m_joint_pushers; std::unique_ptr m_wrist_source; - std::array, 2> m_last_haptic_commands; + std::array m_haptic_commands; std::array, 2> m_last_haptic_sample_times; std::array, 2> m_haptic_retry_after; std::array m_haptic_stopped{ { true, true } }; diff --git a/tests/cpp/core/live_trackers/test_hand_tracker_recovery.cpp b/tests/cpp/core/live_trackers/test_hand_tracker_recovery.cpp index 712b53402d..2c9762e3b0 100644 --- a/tests/cpp/core/live_trackers/test_hand_tracker_recovery.cpp +++ b/tests/cpp/core/live_trackers/test_hand_tracker_recovery.cpp @@ -23,6 +23,7 @@ class FakeHandRuntime public: enum class XDevBehavior { + FAIL_FIRST_LIST, FAIL_FIRST_LEFT, UNSUPPORTED, }; @@ -59,6 +60,16 @@ class FakeHandRuntime return left_xdev_create_attempts_; } + int xdev_list_create_attempts() const + { + return xdev_list_create_attempts_; + } + + void advance_xdev_generation() + { + ++xdev_generation_; + } + private: static XrResult XRAPI_CALL get_system(XrInstance, const XrSystemGetInfo*, XrSystemId* system_id) { @@ -92,12 +103,26 @@ class FakeHandRuntime static XrResult XRAPI_CALL create_xdev_list(XrSession, const XrCreateXDevListInfoMNDX*, XrXDevListMNDX* list) { - *list = reinterpret_cast(1); + ++current_->xdev_list_create_attempts_; + if (current_->xdev_behavior_ == XDevBehavior::FAIL_FIRST_LIST && current_->fail_first_xdev_list_create_) + { + current_->fail_first_xdev_list_create_ = false; + return XR_ERROR_RUNTIME_FAILURE; + } + *list = reinterpret_cast(current_->next_xdev_list_handle_++); + current_->xdev_list_generations_[*list] = current_->xdev_generation_; return XR_SUCCESS; } - static XrResult XRAPI_CALL destroy_xdev_list(XrXDevListMNDX) + static XrResult XRAPI_CALL destroy_xdev_list(XrXDevListMNDX list) { + current_->xdev_list_generations_.erase(list); + return XR_SUCCESS; + } + + static XrResult XRAPI_CALL get_xdev_list_generation(XrXDevListMNDX list, uint64_t* generation) + { + *generation = current_->xdev_list_generations_.at(list); return XR_SUCCESS; } @@ -183,6 +208,7 @@ class FakeHandRuntime { "xrLocateSpace", reinterpret_cast(&locate_space) }, { "xrCreateXDevListMNDX", reinterpret_cast(&create_xdev_list) }, { "xrDestroyXDevListMNDX", reinterpret_cast(&destroy_xdev_list) }, + { "xrGetXDevListGenerationNumberMNDX", reinterpret_cast(&get_xdev_list_generation) }, { "xrEnumerateXDevsMNDX", reinterpret_cast(&enumerate_xdevs) }, { "xrGetXDevPropertiesMNDX", reinterpret_cast(&get_xdev_properties) }, { "xrCreateHandTrackerEXT", reinterpret_cast(&create_hand_tracker) }, @@ -199,8 +225,13 @@ class FakeHandRuntime inline static FakeHandRuntime* current_ = nullptr; uintptr_t next_tracker_handle_ = 1; + uintptr_t next_xdev_list_handle_ = 1; int left_xdev_create_attempts_ = 0; + int xdev_list_create_attempts_ = 0; + uint64_t xdev_generation_ = 1; + std::map xdev_list_generations_; XDevBehavior xdev_behavior_; + bool fail_first_xdev_list_create_ = true; bool fail_first_left_xdev_create_ = true; }; @@ -231,3 +262,32 @@ TEST_CASE("HandTracker caches XDevs that do not support hand tracking", "[unit][ CHECK(runtime.left_xdev_create_attempts() == 1); } + +TEST_CASE("HandTracker recovers when the initial XDev list creation fails", "[unit][hand][xdev]") +{ + FakeHandRuntime runtime(FakeHandRuntime::XDevBehavior::FAIL_FIRST_LIST); + auto hand = std::make_shared(); + auto session = core::DeviceIOSession::run({ hand }, runtime.handles()); + + REQUIRE(runtime.xdev_list_create_attempts() == 1); + REQUIRE(runtime.left_xdev_create_attempts() == 0); + + session->update(); + + CHECK(runtime.xdev_list_create_attempts() == 2); + CHECK(runtime.left_xdev_create_attempts() == 1); +} + +TEST_CASE("HandTracker rebinds unchanged XDev IDs from a new generation", "[unit][hand][xdev]") +{ + FakeHandRuntime runtime(FakeHandRuntime::XDevBehavior::UNSUPPORTED); + auto hand = std::make_shared(); + auto session = core::DeviceIOSession::run({ hand }, runtime.handles()); + + REQUIRE(runtime.left_xdev_create_attempts() == 1); + runtime.advance_xdev_generation(); + + session->update(); + + CHECK(runtime.left_xdev_create_attempts() == 2); +} diff --git a/tests/cpp/plugins/sharpa_avatar/test_avatar_health.cpp b/tests/cpp/plugins/sharpa_avatar/test_avatar_health.cpp index d7dc9aad57..65a5580d4c 100644 --- a/tests/cpp/plugins/sharpa_avatar/test_avatar_health.cpp +++ b/tests/cpp/plugins/sharpa_avatar/test_avatar_health.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include @@ -14,6 +15,7 @@ using plugins::avatar::DatasetHealthInput; using plugins::avatar::GloveIngressSignal; using plugins::avatar::GloveState; +using plugins::avatar::HapticCommandDeliveryState; using plugins::avatar::PhysicalGloveHealthInput; using plugins::avatar::UsbDevicePresence; @@ -219,6 +221,45 @@ TEST_CASE("Ingress signals keep callbacks scoped to one device generation", "[sh CHECK_FALSE(current->last_event().has_value()); } +TEST_CASE("Haptic delivery keeps failed commands pending until success", "[sharpa_avatar][haptic]") +{ + core::HapticCommandT first_native; + first_native.values = { 0.5f, 0.0f, 0.0f, 0.0f, 0.0f }; + const auto first = core::pack(first_native); + + HapticCommandDeliveryState delivery; + delivery.observe(first); + REQUIRE(delivery.pending() == first.get()); + + delivery.observe(first); + CHECK(delivery.pending() == first.get()); + + delivery.acknowledge(); + CHECK(delivery.pending() == nullptr); + delivery.observe(first); + CHECK(delivery.pending() == nullptr); +} + +TEST_CASE("Haptic delivery retains the newest command during retry backoff", "[sharpa_avatar][haptic]") +{ + core::HapticCommandT first_native; + first_native.values = { 0.5f, 0.0f, 0.0f, 0.0f, 0.0f }; + const auto first = core::pack(first_native); + core::HapticCommandT second_native; + second_native.values = { 0.0f, 0.5f, 0.0f, 0.0f, 0.0f }; + const auto second = core::pack(second_native); + + HapticCommandDeliveryState delivery; + delivery.observe(first); + delivery.observe(second); + CHECK(delivery.pending() == second.get()); + + delivery.discard(); + CHECK(delivery.pending() == nullptr); + delivery.observe(second); + CHECK(delivery.pending() == nullptr); +} + TEST_CASE("Dongle detection handles complete and incomplete USB enumeration", "[sharpa_avatar][health]") { TemporaryDirectory sysfs; From 48b76d604f3c050db0921ff1a032ae7dcabd56e4 Mon Sep 17 00:00:00 2001 From: zihan Date: Fri, 30 Oct 2026 12:03:15 +0800 Subject: [PATCH 5/5] fix: expire stale haptic commands Signed-off-by: zihan --- src/plugins/sharpa_avatar/AGENTS.md | 3 ++ .../core/avatar_hand_tracking_plugin.cpp | 8 ++- .../avatar/avatar_hand_tracking_plugin.hpp | 22 +++++--- .../sharpa_avatar/test_avatar_health.cpp | 50 ++++++++++++++----- 4 files changed, 61 insertions(+), 22 deletions(-) diff --git a/src/plugins/sharpa_avatar/AGENTS.md b/src/plugins/sharpa_avatar/AGENTS.md index 13dd22eae7..604f8a1771 100644 --- a/src/plugins/sharpa_avatar/AGENTS.md +++ b/src/plugins/sharpa_avatar/AGENTS.md @@ -58,6 +58,9 @@ SPDX-License-Identifier: Apache-2.0 sufficient alone. Before stopping a glove handle, best-effort clear manual haptics. Isolate dynamic output setup failures to that side and retry without terminating the plugin. +- Bound pending haptic commands by source observation freshness and discard them + while a glove is unready. Delivery retries may retain only commands the source + keeps refreshing; a cached command must never replay after reconnection. - Treat a successful `list_device_info()` result as the handle-lifecycle authority: retain handles while their row exists (including `online=false`), and release them only when a successful snapshot drops the row. A failed diff --git a/src/plugins/sharpa_avatar/core/avatar_hand_tracking_plugin.cpp b/src/plugins/sharpa_avatar/core/avatar_hand_tracking_plugin.cpp index c88faebd16..41b11e6a97 100644 --- a/src/plugins/sharpa_avatar/core/avatar_hand_tracking_plugin.cpp +++ b/src/plugins/sharpa_avatar/core/avatar_hand_tracking_plugin.cpp @@ -953,8 +953,12 @@ void AvatarTracker::process_haptics() const size_t index = side_index(side); const auto& tracked = m_haptic_reader->get_data(*m_deviceio_session, to_string(side)); HapticCommandDeliveryState& delivery = m_haptic_commands[index]; - delivery.observe(tracked); - const core::HapticCommand* command = delivery.pending(); + delivery.observe(tracked, now); + if (!glove(side).ready) + { + delivery.discard(); + } + const core::HapticCommand* command = delivery.pending(now, kHapticCommandTimeout); if (command != nullptr) { const bool retry_ready = !m_haptic_retry_after[index] || now >= *m_haptic_retry_after[index]; diff --git a/src/plugins/sharpa_avatar/core/inc/avatar/avatar_hand_tracking_plugin.hpp b/src/plugins/sharpa_avatar/core/inc/avatar/avatar_hand_tracking_plugin.hpp index 7200ca6516..cea5351da7 100644 --- a/src/plugins/sharpa_avatar/core/inc/avatar/avatar_hand_tracking_plugin.hpp +++ b/src/plugins/sharpa_avatar/core/inc/avatar/avatar_hand_tracking_plugin.hpp @@ -142,33 +142,41 @@ struct GloveState class HapticCommandDeliveryState { public: - void observe(const core::Serialized& command) + using Clock = std::chrono::steady_clock; + + void observe(const core::Serialized& command, Clock::time_point observed_at) { if (command.get() != m_latest.get()) { m_latest = command; - m_pending = command.get() != nullptr; + m_pending_since = command.get() != nullptr ? std::make_optional(observed_at) : std::nullopt; } } - const core::HapticCommand* pending() const noexcept + const core::HapticCommand* pending(Clock::time_point now, Clock::duration freshness_timeout) noexcept { - return m_pending ? m_latest.get() : nullptr; + if (!m_pending_since || now - *m_pending_since >= freshness_timeout) + { + m_pending_since.reset(); + return nullptr; + } + return m_latest.get(); } void acknowledge() noexcept { - m_pending = false; + m_pending_since.reset(); } void discard() noexcept { - m_pending = false; + // Retain the identity so a cached tracker value cannot become a new command. + m_pending_since.reset(); } private: core::Serialized m_latest; - bool m_pending = false; + std::optional m_pending_since; }; class __attribute__((visibility("default"))) AvatarTracker diff --git a/tests/cpp/plugins/sharpa_avatar/test_avatar_health.cpp b/tests/cpp/plugins/sharpa_avatar/test_avatar_health.cpp index 65a5580d4c..b69af3ee36 100644 --- a/tests/cpp/plugins/sharpa_avatar/test_avatar_health.cpp +++ b/tests/cpp/plugins/sharpa_avatar/test_avatar_health.cpp @@ -223,41 +223,65 @@ TEST_CASE("Ingress signals keep callbacks scoped to one device generation", "[sh TEST_CASE("Haptic delivery keeps failed commands pending until success", "[sharpa_avatar][haptic]") { + using namespace std::chrono_literals; + core::HapticCommandT first_native; first_native.values = { 0.5f, 0.0f, 0.0f, 0.0f, 0.0f }; const auto first = core::pack(first_native); + const auto observed_at = HapticCommandDeliveryState::Clock::time_point{}; HapticCommandDeliveryState delivery; - delivery.observe(first); - REQUIRE(delivery.pending() == first.get()); + delivery.observe(first, observed_at); + REQUIRE(delivery.pending(observed_at, 200ms) == first.get()); - delivery.observe(first); - CHECK(delivery.pending() == first.get()); + delivery.observe(first, observed_at + 100ms); + CHECK(delivery.pending(observed_at + 100ms, 200ms) == first.get()); delivery.acknowledge(); - CHECK(delivery.pending() == nullptr); - delivery.observe(first); - CHECK(delivery.pending() == nullptr); + CHECK(delivery.pending(observed_at + 100ms, 200ms) == nullptr); + delivery.observe(first, observed_at + 100ms); + CHECK(delivery.pending(observed_at + 100ms, 200ms) == nullptr); } TEST_CASE("Haptic delivery retains the newest command during retry backoff", "[sharpa_avatar][haptic]") { + using namespace std::chrono_literals; + core::HapticCommandT first_native; first_native.values = { 0.5f, 0.0f, 0.0f, 0.0f, 0.0f }; const auto first = core::pack(first_native); core::HapticCommandT second_native; second_native.values = { 0.0f, 0.5f, 0.0f, 0.0f, 0.0f }; const auto second = core::pack(second_native); + const auto observed_at = HapticCommandDeliveryState::Clock::time_point{}; HapticCommandDeliveryState delivery; - delivery.observe(first); - delivery.observe(second); - CHECK(delivery.pending() == second.get()); + delivery.observe(first, observed_at); + delivery.observe(second, observed_at + 150ms); + CHECK(delivery.pending(observed_at + 250ms, 200ms) == second.get()); delivery.discard(); - CHECK(delivery.pending() == nullptr); - delivery.observe(second); - CHECK(delivery.pending() == nullptr); + CHECK(delivery.pending(observed_at + 250ms, 200ms) == nullptr); + delivery.observe(second, observed_at + 250ms); + CHECK(delivery.pending(observed_at + 250ms, 200ms) == nullptr); +} + +TEST_CASE("Haptic delivery expires commands that are no longer observed", "[sharpa_avatar][haptic]") +{ + using namespace std::chrono_literals; + + core::HapticCommandT command_native; + command_native.values = { 0.5f, 0.0f, 0.0f, 0.0f, 0.0f }; + const auto command = core::pack(command_native); + const auto observed_at = HapticCommandDeliveryState::Clock::time_point{}; + + HapticCommandDeliveryState delivery; + delivery.observe(command, observed_at); + CHECK(delivery.pending(observed_at + 199ms, 200ms) == command.get()); + CHECK(delivery.pending(observed_at + 200ms, 200ms) == nullptr); + + delivery.observe(command, observed_at + 201ms); + CHECK(delivery.pending(observed_at + 201ms, 200ms) == nullptr); } TEST_CASE("Dongle detection handles complete and incomplete USB enumeration", "[sharpa_avatar][health]")