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 34942b2bfe..22d12a9f67 100644 --- a/src/plugins/sharpa_avatar/core/avatar_hand_tracking_plugin.cpp +++ b/src/plugins/sharpa_avatar/core/avatar_hand_tracking_plugin.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -35,7 +36,7 @@ namespace { constexpr size_t kJointFlatbufferSize = 4096; -constexpr auto kAvatarDataTimeout = std::chrono::seconds(10); +constexpr auto kAvatarDataTimeout = std::chrono::seconds(3); constexpr auto kGloveRetryInterval = std::chrono::seconds(2); constexpr auto kGloveWaitLogInterval = std::chrono::seconds(10); constexpr auto kHapticCommandTimeout = std::chrono::milliseconds(200); @@ -256,6 +257,7 @@ void GloveState::reset() noexcept robot_frame = {}; device.reset(); last_successful_fetch.reset(); + last_sample_stamp = {}; } AvatarTracker::AvatarTracker(AvatarPluginConfig config) : m_config(std::move(config)), m_sdk(m_config.sdk_config_path) @@ -428,15 +430,26 @@ void AvatarTracker::refresh_data() } const bool expects_data = 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 (std::tie(stamp.sec, stamp.nanosec) > + std::tie(state.last_sample_stamp.sec, state.last_sample_stamp.nanosec)) + { + state.last_sample_stamp = stamp; + fetched_any = true; + } + }; if (m_config.human) { ::avatar::AvatarDataFrame frame; const bool fetched = state.device->fetch_data(frame, ::avatar::DeviceDataCategory::HUMAN) == ::avatar::ErrorCode::SUCCESS; - fetched_any = fetched_any || fetched; if (fetched) { + note_stamp(frame.skeleton.header.stamp); state.landmarks = std::move(frame.skeleton.landmark); } else @@ -452,10 +465,10 @@ void AvatarTracker::refresh_data() } ::avatar::AvatarDataFrame frame; const bool fetched = state.device->fetch_data(frame, category) == ::avatar::ErrorCode::SUCCESS; - fetched_any = fetched_any || fetched; ::avatar::AvatarDataFrame& cached = cached_joint_frame(state, category); if (fetched) { + note_stamp(hand_payload(frame, category).header.stamp); cached = std::move(frame); } else 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 5dbd2657c0..1bf0bf97d4 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 @@ -100,6 +100,8 @@ struct GloveState ::avatar::AvatarDataFrame robot_frame; // Empty until start() or a successful fetch; drives the data-timeout window. std::optional last_successful_fetch; + // Newest sample stamp seen; a fetch only counts as data when it advances this. + ::avatar::Stamp last_sample_stamp; }; class __attribute__((visibility("default"))) AvatarTracker