Fix schema tracker to have the proper timestamps passed through

nvddr · nvddr · commit 2716d9bf7bc6 · 2026-02-20T09:31:10.000-08:00
diff --git a/src/core/deviceio/cpp/frame_metadata_tracker_oak.cpp b/src/core/deviceio/cpp/frame_metadata_tracker_oak.cpp
@@ -24,18 +24,30 @@ class FrameMetadataTrackerOak::Impl : public ITrackerImpl
     {
     }
 
-    bool update(XrTime time) override
+    bool update(XrTime /* time */) override
     {
-        if (m_schema_reader.read_buffer(m_buffer))
+        m_pending_records.clear();
+
+        std::vector<SchemaTracker::SampleResult> raw_samples;
+        m_schema_reader.read_all_samples(raw_samples);
+
+        for (auto& sample : raw_samples)
         {
-            auto fb = flatbuffers::GetRoot<FrameMetadata>(m_buffer.data());
+            auto fb = flatbuffers::GetRoot<FrameMetadata>(sample.buffer.data());
             if (fb)
             {
-                fb->UnPackTo(&m_data);
-                m_last_timestamp = DeviceDataTimestamp(time, time, 0);
-                return true;
+                FrameMetadataT parsed;
+                fb->UnPackTo(&parsed);
+                m_pending_records.push_back({ std::move(parsed), sample.timestamp });
             }
         }
+
+        if (!m_pending_records.empty())
+        {
+            m_data = m_pending_records.back().data;
+            m_last_timestamp = m_pending_records.back().timestamp;
+        }
+
         return true;
     }
 
@@ -51,16 +63,38 @@ class FrameMetadataTrackerOak::Impl : public ITrackerImpl
         return m_last_timestamp;
     }
 
+    void serialize_all(size_t /* channel_index */, const RecordCallback& callback) const override
+    {
+        for (const auto& record : m_pending_records)
+        {
+            flatbuffers::FlatBufferBuilder builder(256);
+            auto data_offset = FrameMetadata::Pack(builder, &record.data);
+
+            FrameMetadataRecordBuilder record_builder(builder);
+            record_builder.add_data(data_offset);
+            record_builder.add_timestamp(&record.timestamp);
+            builder.Finish(record_builder.Finish());
+
+            callback(record.timestamp, builder.GetBufferPointer(), builder.GetSize());
+        }
+    }
+
     const FrameMetadataT& get_data() const
     {
         return m_data;
     }
 
 private:
+    struct PendingRecord
+    {
+        FrameMetadataT data;
+        DeviceDataTimestamp timestamp;
+    };
+
     SchemaTracker m_schema_reader;
-    std::vector<uint8_t> m_buffer;
     FrameMetadataT m_data;
     DeviceDataTimestamp m_last_timestamp{};
+    std::vector<PendingRecord> m_pending_records;
 };
 
 // ============================================================================
diff --git a/src/core/deviceio/cpp/generic_3axis_pedal_tracker.cpp b/src/core/deviceio/cpp/generic_3axis_pedal_tracker.cpp
@@ -23,19 +23,34 @@ class Generic3AxisPedalTracker::Impl : public ITrackerImpl
     {
     }
 
-    bool update(XrTime time) override
+    bool update(XrTime /* time */) override
     {
-        if (m_schema_reader.read_buffer(m_buffer))
+        m_pending_records.clear();
+
+        std::vector<SchemaTracker::SampleResult> raw_samples;
+        m_schema_reader.read_all_samples(raw_samples);
+
+        for (auto& sample : raw_samples)
         {
-            auto fb = flatbuffers::GetRoot<Generic3AxisPedalOutput>(m_buffer.data());
+            auto fb = flatbuffers::GetRoot<Generic3AxisPedalOutput>(sample.buffer.data());
             if (fb)
             {
-                fb->UnPackTo(&m_data);
-                m_last_timestamp = DeviceDataTimestamp(time, time, 0);
-                return true;
+                Generic3AxisPedalOutputT parsed;
+                fb->UnPackTo(&parsed);
+                m_pending_records.push_back({ std::move(parsed), sample.timestamp });
             }
         }
-        m_data.is_valid = false;
+
+        if (!m_pending_records.empty())
+        {
+            m_data = m_pending_records.back().data;
+            m_last_timestamp = m_pending_records.back().timestamp;
+        }
+        else
+        {
+            m_data.is_valid = false;
+        }
+
         return true;
     }
 
@@ -51,16 +66,38 @@ class Generic3AxisPedalTracker::Impl : public ITrackerImpl
         return m_last_timestamp;
     }
 
+    void serialize_all(size_t /* channel_index */, const RecordCallback& callback) const override
+    {
+        for (const auto& record : m_pending_records)
+        {
+            flatbuffers::FlatBufferBuilder builder(256);
+            auto data_offset = Generic3AxisPedalOutput::Pack(builder, &record.data);
+
+            Generic3AxisPedalOutputRecordBuilder record_builder(builder);
+            record_builder.add_data(data_offset);
+            record_builder.add_timestamp(&record.timestamp);
+            builder.Finish(record_builder.Finish());
+
+            callback(record.timestamp, builder.GetBufferPointer(), builder.GetSize());
+        }
+    }
+
     const Generic3AxisPedalOutputT& get_data() const
     {
         return m_data;
     }
 
 private:
+    struct PendingRecord
+    {
+        Generic3AxisPedalOutputT data;
+        DeviceDataTimestamp timestamp;
+    };
+
     SchemaTracker m_schema_reader;
-    std::vector<uint8_t> m_buffer;
     Generic3AxisPedalOutputT m_data;
     DeviceDataTimestamp m_last_timestamp{};
+    std::vector<PendingRecord> m_pending_records;
 };
 
 // ============================================================================
diff --git a/src/core/deviceio/cpp/inc/deviceio/schema_tracker.hpp b/src/core/deviceio/cpp/inc/deviceio/schema_tracker.hpp
@@ -4,6 +4,7 @@
 #pragma once
 
 #include <oxr_utils/oxr_session_handles.hpp>
+#include <schema/timestamp_generated.h>
 
 #include <XR_NVX1_tensor_data.h>
 #include <cstddef>
@@ -87,32 +88,51 @@ struct SchemaTrackerConfig
  *         Impl(const OpenXRSessionHandles& handles, SchemaTrackerConfig config)
  *             : m_schema_reader(handles, std::move(config)) {}
  *
- *         bool update(XrTime time) override {
- *             if (m_schema_reader.read_buffer(buffer_)) {
- *                 auto fb = GetLocomotionCommand(buffer_.data());
+ *         bool update(XrTime) override {
+ *             m_pending.clear();
+ *             std::vector<SchemaTracker::SampleResult> raw;
+ *             m_schema_reader.read_all_samples(raw);
+ *             for (auto& s : raw) {
+ *                 auto fb = flatbuffers::GetRoot<LocomotionCommand>(s.buffer.data());
  *                 if (fb) {
- *                     fb->UnPackTo(&data_);
- *                     return true;
+ *                     LocomotionCommandT parsed;
+ *                     fb->UnPackTo(&parsed);
+ *                     m_pending.push_back({std::move(parsed), s.timestamp});
  *                 }
  *             }
- *             return false;
+ *             if (!m_pending.empty()) data_ = m_pending.back().data;
+ *             return true;
  *         }
  *
  *         DeviceDataTimestamp serialize(flatbuffers::FlatBufferBuilder& builder, size_t) const override {
- *             auto data_offset = LocomotionCommand::Pack(builder, &data_);
+ *             auto offset = LocomotionCommand::Pack(builder, &data_);
+ *             auto& ts = m_pending.empty() ? DeviceDataTimestamp{} : m_pending.back().timestamp;
  *             LocomotionCommandRecordBuilder rb(builder);
- *             rb.add_data(data_offset);
- *             rb.add_timestamp(&m_last_timestamp);
+ *             rb.add_data(offset);
+ *             rb.add_timestamp(&ts);
  *             builder.Finish(rb.Finish());
- *             return m_last_timestamp;
+ *             return ts;
+ *         }
+ *
+ *         void serialize_all(size_t, const RecordCallback& cb) const override {
+ *             for (const auto& r : m_pending) {
+ *                 flatbuffers::FlatBufferBuilder b(256);
+ *                 auto offset = LocomotionCommand::Pack(b, &r.data);
+ *                 LocomotionCommandRecordBuilder rb(b);
+ *                 rb.add_data(offset);
+ *                 rb.add_timestamp(&r.timestamp);
+ *                 b.Finish(rb.Finish());
+ *                 cb(r.timestamp, b.GetBufferPointer(), b.GetSize());
+ *             }
  *         }
  *
  *         const LocomotionCommandT& get_data() const { return data_; }
  *
  *     private:
+ *         struct Pending { LocomotionCommandT data; DeviceDataTimestamp timestamp; };
  *         SchemaTracker m_schema_reader;
- *         std::vector<uint8_t> buffer_;
  *         LocomotionCommandT data_;
+ *         std::vector<Pending> m_pending;
  *     };
  * };
  * @endcode
@@ -145,16 +165,26 @@ class SchemaTracker
      */
     static std::vector<std::string> get_required_extensions();
 
+    //! A single tensor sample with its data buffer and real timestamps.
+    struct SampleResult
+    {
+        std::vector<uint8_t> buffer;
+        DeviceDataTimestamp timestamp;
+    };
+
     /*!
-     * @brief Read the next available raw sample buffer.
+     * @brief Read ALL pending samples from the tensor collection.
      *
-     * This method polls for tensor list updates, discovers the target collection
-     * if not already connected, and retrieves the next available sample.
+     * Drains every available sample since the last read, appending each to the
+     * output vector with real timestamps from XrTensorSampleMetadataNV:
+     *   - sample_time_device_clock  = rawDeviceTimestamp
+     *   - sample_time_common_clock  = timestamp  (OpenXR time domain)
+     *   - available_time_common_clock = arrivalTimestamp
      *
-     * @param buffer Output vector that will be resized and filled with sample data.
-     * @return true if data was read, false if no new data available.
+     * @param samples Output vector; new samples are appended (not cleared).
+     * @return Number of samples read in this call.
      */
-    bool read_buffer(std::vector<uint8_t>& buffer);
+    size_t read_all_samples(std::vector<SampleResult>& samples);
 
     /*!
      * @brief Access the configuration.
@@ -164,9 +194,10 @@ class SchemaTracker
 private:
     void initialize_tensor_data_functions();
     void create_tensor_list();
+    bool ensure_collection();
     void poll_for_updates();
     std::optional<uint32_t> find_target_collection();
-    bool read_next_sample(std::vector<uint8_t>& buffer);
+    bool read_next_sample(SampleResult& out);
 
     OpenXRSessionHandles m_handles;
     SchemaTrackerConfig m_config;
diff --git a/src/core/deviceio/cpp/inc/deviceio/tracker.hpp b/src/core/deviceio/cpp/inc/deviceio/tracker.hpp
@@ -7,6 +7,7 @@
 #include <openxr/openxr.h>
 #include <schema/timestamp_generated.h>
 
+#include <functional>
 #include <memory>
 #include <string>
 #include <string_view>
@@ -41,6 +42,29 @@ class ITrackerImpl
      * @return DeviceDataTimestamp for MCAP log time.
      */
     virtual DeviceDataTimestamp serialize(flatbuffers::FlatBufferBuilder& builder, size_t channel_index) const = 0;
+
+    /**
+     * @brief Callback type for serialize_all: receives timestamp, raw buffer pointer, and size.
+     */
+    using RecordCallback = std::function<void(const DeviceDataTimestamp&, const uint8_t*, size_t)>;
+
+    /**
+     * @brief Serialize all pending records for a channel, invoking the callback for each.
+     *
+     * The default implementation calls serialize() once, which is correct for
+     * direct OpenXR trackers that always have exactly one state per update.
+     * SchemaTracker-based trackers override this to emit every queued tensor
+     * sample so that no data is lost in MCAP recording.
+     *
+     * @param channel_index Which record channel to serialize.
+     * @param callback Invoked once per record with (timestamp, data_ptr, data_size).
+     */
+    virtual void serialize_all(size_t channel_index, const RecordCallback& callback) const
+    {
+        flatbuffers::FlatBufferBuilder builder(256);
+        DeviceDataTimestamp ts = serialize(builder, channel_index);
+        callback(ts, builder.GetBufferPointer(), builder.GetSize());
+    }
 };
 
 // Base interface for all trackers
diff --git a/src/core/deviceio/cpp/schema_tracker.cpp b/src/core/deviceio/cpp/schema_tracker.cpp
diff --git a/src/core/mcap/cpp/mcap_recorder.cpp b/src/core/mcap/cpp/mcap_recorder.cpp
