Add tracked data and device output timestamp

Author: nvddr

examples/oxr/cpp/oxr_session_sharing.cpp

@@ -95,11 +95,11 @@ try
         if (i % 3 == 0)
         {
             std::cout << "Frame " << i << ": "
-                      << "Hands=" << (left.is_active ? "ACTIVE" : "INACTIVE") << " | "
-                      << "Head=" << (head.is_valid ? "VALID" : "INVALID");
-            if (head.is_valid && head.pose)
+                      << "Hands=" << (left.data->is_active ? "ACTIVE" : "INACTIVE") << " | "
+                      << "Head=" << (head.data->is_valid ? "VALID" : "INVALID");
+            if (head.data->is_valid && head.data->pose)
             {
-                const auto& pos = head.pose->position();
+                const auto& pos = head.data->pose->position();
                 std::cout << " [" << pos.x() << ", " << pos.y() << ", " << pos.z() << "]";
             }
             std::cout << std::endl;

examples/oxr/cpp/oxr_simple_api_demo.cpp

@@ -87,13 +87,13 @@ try
         const auto& head = head_tracker->get_head(*session);
 
         std::cout << "Frame " << i << ":" << std::endl;
-        std::cout << "  Left hand:  " << (left.is_active ? "ACTIVE" : "INACTIVE") << std::endl;
-        std::cout << "  Right hand: " << (right.is_active ? "ACTIVE" : "INACTIVE") << std::endl;
-        std::cout << "  Head pose:  " << (head.is_valid ? "VALID" : "INVALID") << std::endl;
+        std::cout << "  Left hand:  " << (left.data->is_active ? "ACTIVE" : "INACTIVE") << std::endl;
+        std::cout << "  Right hand: " << (right.data->is_active ? "ACTIVE" : "INACTIVE") << std::endl;
+        std::cout << "  Head pose:  " << (head.data->is_valid ? "VALID" : "INVALID") << std::endl;
 
-        if (head.is_valid && head.pose)
+        if (head.data->is_valid && head.data->pose)
         {
-            const auto& pos = head.pose->position();
+            const auto& pos = head.data->pose->position();
             std::cout << "    Position: [" << pos.x() << ", " << pos.y() << ", " << pos.z() << "]" << std::endl;
         }
         std::cout << std::endl;

examples/schemaio/frame_metadata_printer.cpp

@@ -114,7 +114,8 @@ try
         }
 
         // Print when we have new data (sequence_number changed)
-        const auto& data = tracker->get_data(*session);
+        const auto& tracked = tracker->get_data(*session);
+        const auto& data = *tracked.data;
         if (data.sequence_number != last_printed_sequence)
         {
             print_frame_metadata(data, ++received_count);

examples/schemaio/pedal_printer.cpp

@@ -76,7 +76,8 @@ try
         }
 
         // Print current data if available
-        const auto& data = tracker->get_data(*session);
+        const auto& tracked = tracker->get_data(*session);
+        const auto& data = *tracked.data;
         if (data.is_valid)
         {
             print_pedal_data(data, received_count++);

src/core/deviceio/cpp/controller_tracker.cpp

@@ -6,6 +6,7 @@
 #include "inc/deviceio/deviceio_session.hpp"
 
 #include <cassert>
+#include <chrono>
 #include <cmath>
 #include <cstring>
 #include <iostream>
@@ -259,6 +260,11 @@ ControllerTracker::Impl::Impl(const OpenXRSessionHandles& handles)
         throw std::runtime_error("Failed to attach action sets: " + std::to_string(result));
     }
 
+    left_tracked_.data = std::make_shared<ControllerSnapshot>();
+    left_tracked_.timestamp = std::make_shared<DeviceOutputTimestamp>();
+    right_tracked_.data = std::make_shared<ControllerSnapshot>();
+    right_tracked_.timestamp = std::make_shared<DeviceOutputTimestamp>();
+
     std::cout << "ControllerTracker initialized (left + right)" << std::endl;
 }
 
@@ -335,39 +341,38 @@ bool ControllerTracker::Impl::update(XrTime time)
         snapshot_out = ControllerSnapshot(grip_pose, aim_pose, inputs, is_active);
     };
 
-    update_controller(left_hand_path_, left_grip_space_, left_aim_space_, left_controller_);
-    update_controller(right_hand_path_, right_grip_space_, right_aim_space_, right_controller_);
-
-    last_timestamp_ = DeviceDataTimestamp(time, time, 0);
+    update_controller(left_hand_path_, left_grip_space_, left_aim_space_, *left_tracked_.data);
+    update_controller(right_hand_path_, right_grip_space_, right_aim_space_, *right_tracked_.data);
 
-    return left_controller_.is_active() || right_controller_.is_active();
-}
+    auto now_ns =
+        std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::steady_clock::now().time_since_epoch()).count();
+    *left_tracked_.timestamp = DeviceOutputTimestamp(now_ns, time, 0);
+    *right_tracked_.timestamp = DeviceOutputTimestamp(now_ns, time, 0);
+    last_record_timestamp_ = DeviceDataTimestamp(time, time, 0);
 
-const ControllerSnapshot& ControllerTracker::Impl::get_left_controller() const
-{
-    return left_controller_;
+    return left_tracked_.data->is_active() || right_tracked_.data->is_active();
 }
 
-const ControllerSnapshot& ControllerTracker::Impl::get_right_controller() const
+const ControllerSnapshotTrackedT& ControllerTracker::Impl::get_left_controller() const
 {
-    return right_controller_;
+    return left_tracked_;
 }
 
-XrTime ControllerTracker::Impl::get_last_update_time() const
+const ControllerSnapshotTrackedT& ControllerTracker::Impl::get_right_controller() const
 {
-    return last_timestamp_.sample_time_device_clock();
+    return right_tracked_;
 }
 
 DeviceDataTimestamp ControllerTracker::Impl::serialize(flatbuffers::FlatBufferBuilder& builder, size_t channel_index) const
 {
-    const ControllerSnapshot& snapshot = (channel_index == 0) ? left_controller_ : right_controller_;
+    const ControllerSnapshot& snapshot = (channel_index == 0) ? *left_tracked_.data : *right_tracked_.data;
 
     ControllerSnapshotRecordBuilder record_builder(builder);
     record_builder.add_data(&snapshot);
-    record_builder.add_timestamp(&last_timestamp_);
+    record_builder.add_timestamp(&last_record_timestamp_);
     builder.Finish(record_builder.Finish());
 
-    return last_timestamp_;
+    return last_record_timestamp_;
 }
 
 // ============================================================================
@@ -380,21 +385,16 @@ std::vector<std::string> ControllerTracker::get_required_extensions() const
     return {};
 }
 
-const ControllerSnapshot& ControllerTracker::get_left_controller(const DeviceIOSession& session) const
+const ControllerSnapshotTrackedT& ControllerTracker::get_left_controller(const DeviceIOSession& session) const
 {
     return static_cast<const Impl&>(session.get_tracker_impl(*this)).get_left_controller();
 }
 
-const ControllerSnapshot& ControllerTracker::get_right_controller(const DeviceIOSession& session) const
+const ControllerSnapshotTrackedT& ControllerTracker::get_right_controller(const DeviceIOSession& session) const
 {
     return static_cast<const Impl&>(session.get_tracker_impl(*this)).get_right_controller();
 }
 
-XrTime ControllerTracker::get_last_update_time(const DeviceIOSession& session) const
-{
-    return static_cast<const Impl&>(session.get_tracker_impl(*this)).get_last_update_time();
-}
-
 std::shared_ptr<ITrackerImpl> ControllerTracker::create_tracker(const OpenXRSessionHandles& handles) const
 {
     return std::make_shared<Impl>(handles);

src/core/deviceio/cpp/frame_metadata_tracker_oak.cpp

@@ -8,6 +8,7 @@
 #include <flatbuffers/flatbuffers.h>
 #include <schema/oak_bfbs_generated.h>
 
+#include <chrono>
 #include <vector>
 
 namespace core
@@ -22,6 +23,8 @@ class FrameMetadataTrackerOak::Impl : public ITrackerImpl
 public:
     Impl(const OpenXRSessionHandles& handles, SchemaTrackerConfig config) : m_schema_reader(handles, std::move(config))
     {
+        m_tracked.data = std::make_shared<FrameMetadataT>();
+        m_tracked.timestamp = std::make_shared<DeviceOutputTimestamp>();
     }
 
     bool update(XrTime /* time */) override
@@ -44,23 +47,27 @@ class FrameMetadataTrackerOak::Impl : public ITrackerImpl
 
         if (!m_pending_records.empty())
         {
-            m_data = m_pending_records.back().data;
-            m_last_timestamp = m_pending_records.back().timestamp;
+            *m_tracked.data = m_pending_records.back().data;
+            auto now_ns =
+                std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::steady_clock::now().time_since_epoch())
+                    .count();
+            *m_tracked.timestamp = DeviceOutputTimestamp(now_ns, 0, 0);
+            m_last_record_timestamp = m_pending_records.back().timestamp;
         }
 
         return true;
     }
 
     DeviceDataTimestamp serialize(flatbuffers::FlatBufferBuilder& builder, size_t /* channel_index */) const override
     {
-        auto data_offset = FrameMetadata::Pack(builder, &m_data);
+        auto data_offset = FrameMetadata::Pack(builder, m_tracked.data.get());
 
         FrameMetadataRecordBuilder record_builder(builder);
         record_builder.add_data(data_offset);
-        record_builder.add_timestamp(&m_last_timestamp);
+        record_builder.add_timestamp(&m_last_record_timestamp);
         builder.Finish(record_builder.Finish());
 
-        return m_last_timestamp;
+        return m_last_record_timestamp;
     }
 
     void serialize_all(size_t /* channel_index */, const RecordCallback& callback) const override
@@ -79,9 +86,9 @@ class FrameMetadataTrackerOak::Impl : public ITrackerImpl
         }
     }
 
-    const FrameMetadataT& get_data() const
+    const FrameMetadataTrackedT& get_data() const
     {
-        return m_data;
+        return m_tracked;
     }
 
 private:
@@ -92,8 +99,8 @@ class FrameMetadataTrackerOak::Impl : public ITrackerImpl
     };
 
     SchemaTracker m_schema_reader;
-    FrameMetadataT m_data;
-    DeviceDataTimestamp m_last_timestamp{};
+    FrameMetadataTrackedT m_tracked;
+    DeviceDataTimestamp m_last_record_timestamp{};
     std::vector<PendingRecord> m_pending_records;
 };
 
@@ -140,7 +147,7 @@ const SchemaTrackerConfig& FrameMetadataTrackerOak::get_config() const
     return m_config;
 }
 
-const FrameMetadataT& FrameMetadataTrackerOak::get_data(const DeviceIOSession& session) const
+const FrameMetadataTrackedT& FrameMetadataTrackerOak::get_data(const DeviceIOSession& session) const
 {
     return static_cast<const Impl&>(session.get_tracker_impl(*this)).get_data();
 }

src/core/deviceio/cpp/full_body_tracker_pico.cpp

@@ -6,6 +6,7 @@
 #include "inc/deviceio/deviceio_session.hpp"
 
 #include <cassert>
+#include <chrono>
 #include <cstring>
 #include <iostream>
 
@@ -26,13 +27,13 @@ class FullBodyTrackerPicoImpl : public ITrackerImpl
     bool update(XrTime time) override;
     DeviceDataTimestamp serialize(flatbuffers::FlatBufferBuilder& builder, size_t channel_index) const override;
 
-    const FullBodyPosePicoT& get_body_pose() const;
+    const FullBodyPosePicoTrackedT& get_body_pose() const;
 
 private:
     XrSpace base_space_;
     XrBodyTrackerBD body_tracker_;
-    FullBodyPosePicoT body_pose_;
-    DeviceDataTimestamp last_timestamp_{};
+    FullBodyPosePicoTrackedT tracked_;
+    DeviceDataTimestamp last_record_timestamp_{};
 
     // Extension function pointers
     PFN_xrCreateBodyTrackerBD pfn_create_body_tracker_;
@@ -100,7 +101,10 @@ FullBodyTrackerPicoImpl::FullBodyTrackerPicoImpl(const OpenXRSessionHandles& han
         throw std::runtime_error("Failed to create body tracker: " + std::to_string(result));
     }
 
-    body_pose_.is_active = false;
+    tracked_.data = std::make_shared<FullBodyPosePicoT>();
+    tracked_.timestamp = std::make_shared<DeviceOutputTimestamp>();
+
+    tracked_.data->is_active = false;
 
     std::cout << "FullBodyTrackerPico initialized (24 joints)" << std::endl;
 }
@@ -134,19 +138,22 @@ bool FullBodyTrackerPicoImpl::update(XrTime time)
     XrResult result = pfn_locate_body_joints_(body_tracker_, &locate_info, &locations);
     if (XR_FAILED(result))
     {
-        body_pose_.is_active = false;
+        tracked_.data->is_active = false;
         return false;
     }
 
     // allJointPosesTracked indicates if all joint poses are valid
-    body_pose_.is_active = locations.allJointPosesTracked;
+    tracked_.data->is_active = locations.allJointPosesTracked;
 
-    last_timestamp_ = DeviceDataTimestamp(time, time, 0);
+    auto now_ns =
+        std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::steady_clock::now().time_since_epoch()).count();
+    *tracked_.timestamp = DeviceOutputTimestamp(now_ns, time, 0);
+    last_record_timestamp_ = DeviceDataTimestamp(time, time, 0);
 
     // Ensure joints struct is allocated
-    if (!body_pose_.joints)
+    if (!tracked_.data->joints)
     {
-        body_pose_.joints = std::make_unique<BodyJointsPico>();
+        tracked_.data->joints = std::make_unique<BodyJointsPico>();
     }
 
     for (uint32_t i = 0; i < XR_BODY_JOINT_COUNT_BD; ++i)
@@ -164,28 +171,28 @@ bool FullBodyTrackerPicoImpl::update(XrTime time)
 
         // Create BodyJointPose and set it in the array
         BodyJointPose joint_pose(pose, is_valid);
-        body_pose_.joints->mutable_joints()->Mutate(i, joint_pose);
+        tracked_.data->joints->mutable_joints()->Mutate(i, joint_pose);
     }
 
     return true;
 }
 
-const FullBodyPosePicoT& FullBodyTrackerPicoImpl::get_body_pose() const
+const FullBodyPosePicoTrackedT& FullBodyTrackerPicoImpl::get_body_pose() const
 {
-    return body_pose_;
+    return tracked_;
 }
 
 DeviceDataTimestamp FullBodyTrackerPicoImpl::serialize(flatbuffers::FlatBufferBuilder& builder,
                                                        size_t /* channel_index */) const
 {
-    auto data_offset = FullBodyPosePico::Pack(builder, &body_pose_);
+    auto data_offset = FullBodyPosePico::Pack(builder, tracked_.data.get());
 
     FullBodyPosePicoRecordBuilder record_builder(builder);
     record_builder.add_data(data_offset);
-    record_builder.add_timestamp(&last_timestamp_);
+    record_builder.add_timestamp(&last_record_timestamp_);
     builder.Finish(record_builder.Finish());
 
-    return last_timestamp_;
+    return last_record_timestamp_;
 }
 
 // ============================================================================
@@ -197,7 +204,7 @@ std::vector<std::string> FullBodyTrackerPico::get_required_extensions() const
     return { XR_BD_BODY_TRACKING_EXTENSION_NAME };
 }
 
-const FullBodyPosePicoT& FullBodyTrackerPico::get_body_pose(const DeviceIOSession& session) const
+const FullBodyPosePicoTrackedT& FullBodyTrackerPico::get_body_pose(const DeviceIOSession& session) const
 {
     return static_cast<const FullBodyTrackerPicoImpl&>(session.get_tracker_impl(*this)).get_body_pose();
 }