diff --git a/examples/oxr/python/test_controller_tracker.py b/examples/oxr/python/test_controller_tracker.py index 1991b466b..90ae80661 100644 --- a/examples/oxr/python/test_controller_tracker.py +++ b/examples/oxr/python/test_controller_tracker.py @@ -57,9 +57,8 @@ # Test 5: Check initial controller state print("[Test 5] Checking controller state...") - controller_data = controller_tracker.get_controller_data(session) - left_snap = controller_data.left_controller - right_snap = controller_data.right_controller + left_snap = controller_tracker.get_left_controller(session) + right_snap = controller_tracker.get_right_controller(session) print( f" Left controller: {'ACTIVE' if left_snap and left_snap.is_active else 'INACTIVE'}" ) @@ -101,9 +100,8 @@ current_time = time.time() if current_time - last_status_print >= 0.5: # Print every 0.5 seconds elapsed = current_time - start_time - controller_data = controller_tracker.get_controller_data(session) - left_snap = controller_data.left_controller - right_snap = controller_data.right_controller + left_snap = controller_tracker.get_left_controller(session) + right_snap = controller_tracker.get_right_controller(session) # Show current state left_trigger = left_snap.inputs.trigger_value if left_snap else 0.0 @@ -212,10 +210,11 @@ def print_controller_summary(hand_name, snapshot): else: print(" Status: INACTIVE") - controller_data = controller_tracker.get_controller_data(session) - print_controller_summary("Left", controller_data.left_controller) + left_snap = controller_tracker.get_left_controller(session) + right_snap = controller_tracker.get_right_controller(session) + print_controller_summary("Left", left_snap) print() - print_controller_summary("Right", controller_data.right_controller) + print_controller_summary("Right", right_snap) print() # Cleanup diff --git a/examples/retargeting/python/sources_example.py b/examples/retargeting/python/sources_example.py index a422627c8..9f6974c44 100755 --- a/examples/retargeting/python/sources_example.py +++ b/examples/retargeting/python/sources_example.py @@ -132,7 +132,10 @@ def main(): hand_left_raw = hand_tracker.get_left_hand(session) hand_right_raw = hand_tracker.get_right_hand(session) head_raw = head_tracker.get_head(session) - controller_data_raw = controller_tracker.get_controller_data( + left_controller_raw = controller_tracker.get_left_controller( + session + ) + right_controller_raw = controller_tracker.get_right_controller( session ) @@ -163,9 +166,9 @@ def main(): for input_name, group_type in controllers_input_spec.items(): tg = TensorGroup(group_type) if "left" in input_name.lower(): - tg[0] = controller_data_raw.left_controller + tg[0] = left_controller_raw elif "right" in input_name.lower(): - tg[0] = controller_data_raw.right_controller + tg[0] = right_controller_raw controllers_inputs[input_name] = tg # ==================================================== diff --git a/examples/schemaio/frame_metadata_printer.cpp b/examples/schemaio/frame_metadata_printer.cpp index d8f091ceb..f90e728ce 100644 --- a/examples/schemaio/frame_metadata_printer.cpp +++ b/examples/schemaio/frame_metadata_printer.cpp @@ -33,17 +33,7 @@ static constexpr size_t MAX_FLATBUFFER_SIZE = 128; void print_frame_metadata(const core::FrameMetadataT& data, size_t sample_count) { - std::cout << "Sample " << sample_count; - - std::cout << " [seq=" << data.sequence_number; - if (data.timestamp) - { - std::cout << ", device_time=" << data.timestamp->device_time() - << ", common_time=" << data.timestamp->common_time(); - } - std::cout << "]"; - - std::cout << std::endl; + std::cout << "Sample " << sample_count << " [seq=" << data.sequence_number << "]" << std::endl; } void print_usage(const char* program_name) @@ -123,9 +113,9 @@ try break; } - // Print when we have new data (timestamp indicates real data; sequence_number changed) + // Print when we have new data (sequence_number changed) const auto& data = tracker->get_data(*session); - if (data.timestamp && data.sequence_number != last_printed_sequence) + if (data.sequence_number != last_printed_sequence) { print_frame_metadata(data, ++received_count); last_printed_sequence = data.sequence_number; diff --git a/examples/schemaio/pedal_pusher.cpp b/examples/schemaio/pedal_pusher.cpp index 3692e99bd..b3724c955 100644 --- a/examples/schemaio/pedal_pusher.cpp +++ b/examples/schemaio/pedal_pusher.cpp @@ -51,13 +51,13 @@ class Generic3AxisPedalPusher * @param data The Generic3AxisPedalOutputT native object to serialize and push. * @throws std::runtime_error if the push fails. */ - void push(const core::Generic3AxisPedalOutputT& data) + void push(const core::Generic3AxisPedalOutputT& data, int64_t device_time_ns, int64_t common_time_ns) { flatbuffers::FlatBufferBuilder builder(m_pusher.config().max_flatbuffer_size); auto offset = core::Generic3AxisPedalOutput::Pack(builder, &data); builder.Finish(offset); - m_pusher.push_buffer(builder.GetBufferPointer(), builder.GetSize()); + m_pusher.push_buffer(builder.GetBufferPointer(), builder.GetSize(), device_time_ns, common_time_ns); } private: @@ -105,9 +105,8 @@ try auto now = std::chrono::steady_clock::now(); auto ns = std::chrono::duration_cast(now.time_since_epoch()).count(); - pedal_output.timestamp = std::make_shared(ns, ns); - pusher->push(pedal_output); + pusher->push(pedal_output, ns, ns); std::cout << "Pushed sample " << i << std::fixed << std::setprecision(3) << " [left=" << left_pedal << ", right=" << right_pedal << ", rudder=" << rudder << "]" << std::endl; diff --git a/src/core/CMakeLists.txt b/src/core/CMakeLists.txt index b2a0ac6cb..9719214c0 100644 --- a/src/core/CMakeLists.txt +++ b/src/core/CMakeLists.txt @@ -36,6 +36,9 @@ add_subdirectory(retargeting_engine_ui) # Build TeleopSessionManager (pure Python module) add_subdirectory(teleop_session_manager) +# Build Synchronization utilities (clock sync, etc.) +add_subdirectory(synchronization) + # Python wheel packaging (combines both modules) if(BUILD_PYTHON_BINDINGS) add_subdirectory(python) diff --git a/src/core/deviceio/cpp/controller_tracker.cpp b/src/core/deviceio/cpp/controller_tracker.cpp index f70731fb8..dbf86adb2 100644 --- a/src/core/deviceio/cpp/controller_tracker.cpp +++ b/src/core/deviceio/cpp/controller_tracker.cpp @@ -278,16 +278,13 @@ bool ControllerTracker::Impl::update(XrTime time) return false; } - // Helper to update a single controller - creates a new immutable struct snapshot auto update_controller = [&](XrPath hand_path, const XrSpacePtr& grip_space, const XrSpacePtr& aim_space, - std::shared_ptr& snapshot_ptr) + ControllerSnapshot& snapshot_out) { - // Initialize with default values ControllerPose grip_pose{}; ControllerPose aim_pose{}; ControllerInputState inputs{}; bool is_active = false; - Timestamp timestamp{}; // Update grip pose XrSpaceLocation grip_location{ XR_TYPE_SPACE_LOCATION }; @@ -321,9 +318,6 @@ bool ControllerTracker::Impl::update(XrTime time) is_active = grip_pose.is_valid() || aim_pose.is_valid(); - // Update timestamp - timestamp = Timestamp(time, time); - // Update all input values bool primary_click = get_boolean_action_state(session_, core_funcs_, primary_click_action_, hand_path); bool secondary_click = get_boolean_action_state(session_, core_funcs_, secondary_click_action_, hand_path); @@ -338,37 +332,42 @@ bool ControllerTracker::Impl::update(XrTime time) inputs = ControllerInputState( primary_click, secondary_click, thumbstick_click, thumbstick_x, thumbstick_y, squeeze_value, trigger_value); - // Create new snapshot struct - snapshot_ptr = std::make_shared(grip_pose, aim_pose, inputs, is_active, timestamp); + snapshot_out = ControllerSnapshot(grip_pose, aim_pose, inputs, is_active); }; - // Update both controllers - update_controller(left_hand_path_, left_grip_space_, left_aim_space_, controller_data_.left_controller); - update_controller(right_hand_path_, right_grip_space_, right_aim_space_, controller_data_.right_controller); + 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); - return controller_data_.left_controller->is_active() || controller_data_.right_controller->is_active(); + return left_controller_.is_active() || right_controller_.is_active(); } -const ControllerDataT& ControllerTracker::Impl::get_controller_data() const +const ControllerSnapshot& ControllerTracker::Impl::get_left_controller() const { - return controller_data_; + return left_controller_; } -Timestamp ControllerTracker::Impl::serialize(flatbuffers::FlatBufferBuilder& builder) const +const ControllerSnapshot& ControllerTracker::Impl::get_right_controller() const { - auto offset = ControllerData::Pack(builder, &controller_data_); - builder.Finish(offset); + return right_controller_; +} - // Use left controller timestamp (or right if left is inactive) - if (controller_data_.left_controller && controller_data_.left_controller->is_active()) - { - return controller_data_.left_controller->timestamp(); - } - else if (controller_data_.right_controller && controller_data_.right_controller->is_active()) - { - return controller_data_.right_controller->timestamp(); - } - return Timestamp{}; +XrTime ControllerTracker::Impl::get_last_update_time() const +{ + return last_timestamp_.sample_time_device_clock(); +} + +DeviceDataTimestamp ControllerTracker::Impl::serialize(flatbuffers::FlatBufferBuilder& builder, size_t channel_index) const +{ + const ControllerSnapshot& snapshot = (channel_index == 0) ? left_controller_ : right_controller_; + + ControllerSnapshotRecordBuilder record_builder(builder); + record_builder.add_data(&snapshot); + record_builder.add_timestamp(&last_timestamp_); + builder.Finish(record_builder.Finish()); + + return last_timestamp_; } // ============================================================================ @@ -381,9 +380,19 @@ std::vector ControllerTracker::get_required_extensions() const return {}; } -const ControllerDataT& ControllerTracker::get_controller_data(const DeviceIOSession& session) const +const ControllerSnapshot& ControllerTracker::get_left_controller(const DeviceIOSession& session) const +{ + return static_cast(session.get_tracker_impl(*this)).get_left_controller(); +} + +const ControllerSnapshot& ControllerTracker::get_right_controller(const DeviceIOSession& session) const +{ + return static_cast(session.get_tracker_impl(*this)).get_right_controller(); +} + +XrTime ControllerTracker::get_last_update_time(const DeviceIOSession& session) const { - return static_cast(session.get_tracker_impl(*this)).get_controller_data(); + return static_cast(session.get_tracker_impl(*this)).get_last_update_time(); } std::shared_ptr ControllerTracker::create_tracker(const OpenXRSessionHandles& handles) const diff --git a/src/core/deviceio/cpp/frame_metadata_tracker_oak.cpp b/src/core/deviceio/cpp/frame_metadata_tracker_oak.cpp index d3933317b..c18b5edac 100644 --- a/src/core/deviceio/cpp/frame_metadata_tracker_oak.cpp +++ b/src/core/deviceio/cpp/frame_metadata_tracker_oak.cpp @@ -26,25 +26,57 @@ class FrameMetadataTrackerOak::Impl : public ITrackerImpl bool update(XrTime /* time */) override { - // Try to read new data from tensor stream - if (m_schema_reader.read_buffer(m_buffer)) + m_pending_records.clear(); + + std::vector raw_samples; + m_schema_reader.read_all_samples(raw_samples); + + for (auto& sample : raw_samples) { - auto fb = GetFrameMetadata(m_buffer.data()); + auto fb = flatbuffers::GetRoot(sample.buffer.data()); if (fb) { - fb->UnPackTo(&m_data); - return true; + FrameMetadataT parsed; + fb->UnPackTo(&parsed); + m_pending_records.push_back({ std::move(parsed), sample.timestamp }); } } - // Return true even if no new data - we're still running + + if (!m_pending_records.empty()) + { + m_data = m_pending_records.back().data; + m_last_timestamp = m_pending_records.back().timestamp; + } + return true; } - Timestamp serialize(flatbuffers::FlatBufferBuilder& builder) const override + DeviceDataTimestamp serialize(flatbuffers::FlatBufferBuilder& builder, size_t /* channel_index */) const override + { + auto data_offset = FrameMetadata::Pack(builder, &m_data); + + FrameMetadataRecordBuilder record_builder(builder); + record_builder.add_data(data_offset); + record_builder.add_timestamp(&m_last_timestamp); + builder.Finish(record_builder.Finish()); + + return m_last_timestamp; + } + + void serialize_all(size_t /* channel_index */, const RecordCallback& callback) const override { - auto offset = FrameMetadata::Pack(builder, &m_data); - builder.Finish(offset); - return m_data.timestamp ? *m_data.timestamp : Timestamp{}; + 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 @@ -53,9 +85,16 @@ class FrameMetadataTrackerOak::Impl : public ITrackerImpl } private: + struct PendingRecord + { + FrameMetadataT data; + DeviceDataTimestamp timestamp; + }; + SchemaTracker m_schema_reader; - std::vector m_buffer; FrameMetadataT m_data; + DeviceDataTimestamp m_last_timestamp{}; + std::vector m_pending_records; }; // ============================================================================ @@ -82,13 +121,18 @@ std::string_view FrameMetadataTrackerOak::get_name() const std::string_view FrameMetadataTrackerOak::get_schema_name() const { - return "core.FrameMetadata"; + return "core.FrameMetadataRecord"; } std::string_view FrameMetadataTrackerOak::get_schema_text() const { - return std::string_view( - reinterpret_cast(FrameMetadataBinarySchema::data()), FrameMetadataBinarySchema::size()); + return std::string_view(reinterpret_cast(FrameMetadataRecordBinarySchema::data()), + FrameMetadataRecordBinarySchema::size()); +} + +std::vector FrameMetadataTrackerOak::get_record_channels() const +{ + return { "frame_metadata" }; } const SchemaTrackerConfig& FrameMetadataTrackerOak::get_config() const diff --git a/src/core/deviceio/cpp/full_body_tracker_pico.cpp b/src/core/deviceio/cpp/full_body_tracker_pico.cpp index 783f2c43f..12072fb17 100644 --- a/src/core/deviceio/cpp/full_body_tracker_pico.cpp +++ b/src/core/deviceio/cpp/full_body_tracker_pico.cpp @@ -19,21 +19,20 @@ namespace core class FullBodyTrackerPicoImpl : public ITrackerImpl { public: - // Constructor - throws std::runtime_error on failure explicit FullBodyTrackerPicoImpl(const OpenXRSessionHandles& handles); ~FullBodyTrackerPicoImpl(); // Override from ITrackerImpl bool update(XrTime time) override; - Timestamp serialize(flatbuffers::FlatBufferBuilder& builder) const override; + DeviceDataTimestamp serialize(flatbuffers::FlatBufferBuilder& builder, size_t channel_index) const override; - // Get body pose data const FullBodyPosePicoT& get_body_pose() const; private: XrSpace base_space_; XrBodyTrackerBD body_tracker_; FullBodyPosePicoT body_pose_; + DeviceDataTimestamp last_timestamp_{}; // Extension function pointers PFN_xrCreateBodyTrackerBD pfn_create_body_tracker_; @@ -142,8 +141,7 @@ bool FullBodyTrackerPicoImpl::update(XrTime time) // allJointPosesTracked indicates if all joint poses are valid body_pose_.is_active = locations.allJointPosesTracked; - // Update timestamp (device time and common time) - body_pose_.timestamp = std::make_shared(time, time); + last_timestamp_ = DeviceDataTimestamp(time, time, 0); // Ensure joints struct is allocated if (!body_pose_.joints) @@ -177,16 +175,17 @@ const FullBodyPosePicoT& FullBodyTrackerPicoImpl::get_body_pose() const return body_pose_; } -Timestamp FullBodyTrackerPicoImpl::serialize(flatbuffers::FlatBufferBuilder& builder) const +DeviceDataTimestamp FullBodyTrackerPicoImpl::serialize(flatbuffers::FlatBufferBuilder& builder, + size_t /* channel_index */) const { - auto offset = FullBodyPosePico::Pack(builder, &body_pose_); - builder.Finish(offset); + auto data_offset = FullBodyPosePico::Pack(builder, &body_pose_); - if (body_pose_.timestamp) - { - return *body_pose_.timestamp; - } - return Timestamp{}; + FullBodyPosePicoRecordBuilder record_builder(builder); + record_builder.add_data(data_offset); + record_builder.add_timestamp(&last_timestamp_); + builder.Finish(record_builder.Finish()); + + return last_timestamp_; } // ============================================================================ diff --git a/src/core/deviceio/cpp/generic_3axis_pedal_tracker.cpp b/src/core/deviceio/cpp/generic_3axis_pedal_tracker.cpp index 214330ab8..fb8aed351 100644 --- a/src/core/deviceio/cpp/generic_3axis_pedal_tracker.cpp +++ b/src/core/deviceio/cpp/generic_3axis_pedal_tracker.cpp @@ -25,26 +25,61 @@ class Generic3AxisPedalTracker::Impl : public ITrackerImpl bool update(XrTime /* time */) override { - // Try to read new data from tensor stream - if (m_schema_reader.read_buffer(m_buffer)) + m_pending_records.clear(); + + std::vector raw_samples; + m_schema_reader.read_all_samples(raw_samples); + + for (auto& sample : raw_samples) { - auto fb = GetGeneric3AxisPedalOutput(m_buffer.data()); + auto fb = flatbuffers::GetRoot(sample.buffer.data()); if (fb) { - fb->UnPackTo(&m_data); - return true; + Generic3AxisPedalOutputT parsed; + fb->UnPackTo(&parsed); + m_pending_records.push_back({ std::move(parsed), sample.timestamp }); } } - // Return true even if no new data - we're still running, but invalid data. - 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; } - Timestamp serialize(flatbuffers::FlatBufferBuilder& builder) const override + DeviceDataTimestamp serialize(flatbuffers::FlatBufferBuilder& builder, size_t /* channel_index */) const override + { + auto data_offset = Generic3AxisPedalOutput::Pack(builder, &m_data); + + Generic3AxisPedalOutputRecordBuilder record_builder(builder); + record_builder.add_data(data_offset); + record_builder.add_timestamp(&m_last_timestamp); + builder.Finish(record_builder.Finish()); + + return m_last_timestamp; + } + + void serialize_all(size_t /* channel_index */, const RecordCallback& callback) const override { - auto offset = Generic3AxisPedalOutput::Pack(builder, &m_data); - builder.Finish(offset); - return m_data.timestamp ? *m_data.timestamp : Timestamp{}; + 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 @@ -53,9 +88,16 @@ class Generic3AxisPedalTracker::Impl : public ITrackerImpl } private: + struct PendingRecord + { + Generic3AxisPedalOutputT data; + DeviceDataTimestamp timestamp; + }; + SchemaTracker m_schema_reader; - std::vector m_buffer; Generic3AxisPedalOutputT m_data; + DeviceDataTimestamp m_last_timestamp{}; + std::vector m_pending_records; }; // ============================================================================ @@ -82,13 +124,18 @@ std::string_view Generic3AxisPedalTracker::get_name() const std::string_view Generic3AxisPedalTracker::get_schema_name() const { - return "core.Generic3AxisPedalOutput"; + return "core.Generic3AxisPedalOutputRecord"; } std::string_view Generic3AxisPedalTracker::get_schema_text() const { - return std::string_view(reinterpret_cast(Generic3AxisPedalOutputBinarySchema::data()), - Generic3AxisPedalOutputBinarySchema::size()); + return std::string_view(reinterpret_cast(Generic3AxisPedalOutputRecordBinarySchema::data()), + Generic3AxisPedalOutputRecordBinarySchema::size()); +} + +std::vector Generic3AxisPedalTracker::get_record_channels() const +{ + return { "generic_3axis_pedal" }; } const SchemaTrackerConfig& Generic3AxisPedalTracker::get_config() const diff --git a/src/core/deviceio/cpp/hand_tracker.cpp b/src/core/deviceio/cpp/hand_tracker.cpp index 08142532e..13e24d4c3 100644 --- a/src/core/deviceio/cpp/hand_tracker.cpp +++ b/src/core/deviceio/cpp/hand_tracker.cpp @@ -5,8 +5,6 @@ #include "inc/deviceio/deviceio_session.hpp" -#include - #include #include #include @@ -88,23 +86,18 @@ HandTracker::Impl::Impl(const OpenXRSessionHandles& handles) std::cout << "HandTracker initialized (left + right)" << std::endl; } -Timestamp HandTracker::Impl::serialize(flatbuffers::FlatBufferBuilder& builder) const +DeviceDataTimestamp HandTracker::Impl::serialize(flatbuffers::FlatBufferBuilder& builder, size_t channel_index) const { - // Serialize both hands into a combined HandsPose message - auto left_offset = HandPose::Pack(builder, &left_hand_); - auto right_offset = HandPose::Pack(builder, &right_hand_); + const HandPoseT& hand = (channel_index == 0) ? left_hand_ : right_hand_; - HandsPoseBuilder hands_builder(builder); - hands_builder.add_left_hand(left_offset); - hands_builder.add_right_hand(right_offset); - builder.Finish(hands_builder.Finish()); + auto data_offset = HandPose::Pack(builder, &hand); - // For hand tracker, we use left hand's timestamp (both hands are updated at the same time) - if (left_hand_.timestamp) - { - return *left_hand_.timestamp; - } - return Timestamp{}; + HandPoseRecordBuilder record_builder(builder); + record_builder.add_data(data_offset); + record_builder.add_timestamp(&last_timestamp_); + builder.Finish(record_builder.Finish()); + + return last_timestamp_; } HandTracker::Impl::~Impl() @@ -130,6 +123,8 @@ bool HandTracker::Impl::update(XrTime time) bool left_ok = update_hand(left_hand_tracker_, time, left_hand_); bool right_ok = update_hand(right_hand_tracker_, time, right_hand_); + last_timestamp_ = DeviceDataTimestamp(time, time, 0); + // Return true if at least one hand updated successfully return left_ok || right_ok; } @@ -166,13 +161,6 @@ bool HandTracker::Impl::update_hand(XrHandTrackerEXT tracker, XrTime time, HandP out_data.is_active = locations.isActive; - // Update timestamp (device time and common time) - if (!out_data.timestamp) - { - out_data.timestamp = std::make_shared(); - } - out_data.timestamp = std::make_shared(time, time); - // Ensure joints struct is allocated if (!out_data.joints) { diff --git a/src/core/deviceio/cpp/head_tracker.cpp b/src/core/deviceio/cpp/head_tracker.cpp index 0705af15a..821d9c0e5 100644 --- a/src/core/deviceio/cpp/head_tracker.cpp +++ b/src/core/deviceio/cpp/head_tracker.cpp @@ -48,8 +48,7 @@ bool HeadTracker::Impl::update(XrTime time) head_.is_valid = position_valid && orientation_valid; - // Update timestamp (device time and common time) - head_.timestamp = std::make_shared(time, time); + last_timestamp_ = DeviceDataTimestamp(time, time, 0); if (head_.is_valid) { @@ -73,16 +72,16 @@ const HeadPoseT& HeadTracker::Impl::get_head() const return head_; } -Timestamp HeadTracker::Impl::serialize(flatbuffers::FlatBufferBuilder& builder) const +DeviceDataTimestamp HeadTracker::Impl::serialize(flatbuffers::FlatBufferBuilder& builder, size_t /* channel_index */) const { - auto offset = HeadPose::Pack(builder, &head_); - builder.Finish(offset); + auto data_offset = HeadPose::Pack(builder, &head_); - if (head_.timestamp) - { - return *head_.timestamp; - } - return Timestamp{}; + HeadPoseRecordBuilder record_builder(builder); + record_builder.add_data(data_offset); + record_builder.add_timestamp(&last_timestamp_); + builder.Finish(record_builder.Finish()); + + return last_timestamp_; } // ============================================================================ diff --git a/src/core/deviceio/cpp/inc/deviceio/controller_tracker.hpp b/src/core/deviceio/cpp/inc/deviceio/controller_tracker.hpp index 2d5c52fd4..5b50dd71f 100644 --- a/src/core/deviceio/cpp/inc/deviceio/controller_tracker.hpp +++ b/src/core/deviceio/cpp/inc/deviceio/controller_tracker.hpp @@ -1,4 +1,4 @@ -// SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. // SPDX-License-Identifier: Apache-2.0 #pragma once @@ -34,16 +34,25 @@ class ControllerTracker : public ITracker std::string_view get_schema_text() const override { - return std::string_view( - reinterpret_cast(ControllerDataBinarySchema::data()), ControllerDataBinarySchema::size()); + return std::string_view(reinterpret_cast(ControllerSnapshotRecordBinarySchema::data()), + ControllerSnapshotRecordBinarySchema::size()); } - // Get complete controller data (both left and right controllers) - const ControllerDataT& get_controller_data(const DeviceIOSession& session) const; + std::vector get_record_channels() const override + { + return { "left_controller", "right_controller" }; + } + + // Query methods - public API for getting individual controller data + const ControllerSnapshot& get_left_controller(const DeviceIOSession& session) const; + const ControllerSnapshot& get_right_controller(const DeviceIOSession& session) const; + + // Get the XrTime from the last update (useful for plugins that need timing) + XrTime get_last_update_time(const DeviceIOSession& session) const; private: static constexpr const char* TRACKER_NAME = "ControllerTracker"; - static constexpr const char* SCHEMA_NAME = "core.ControllerData"; + static constexpr const char* SCHEMA_NAME = "core.ControllerSnapshotRecord"; std::shared_ptr create_tracker(const OpenXRSessionHandles& handles) const override; @@ -55,9 +64,11 @@ class ControllerTracker : public ITracker // Override from ITrackerImpl bool update(XrTime time) override; - Timestamp serialize(flatbuffers::FlatBufferBuilder& builder) const override; + DeviceDataTimestamp serialize(flatbuffers::FlatBufferBuilder& builder, size_t channel_index) const override; - const ControllerDataT& get_controller_data() const; + const ControllerSnapshot& get_left_controller() const; + const ControllerSnapshot& get_right_controller() const; + XrTime get_last_update_time() const; private: const OpenXRCoreFunctions core_funcs_; @@ -69,7 +80,7 @@ class ControllerTracker : public ITracker XrPath left_hand_path_; XrPath right_hand_path_; - // Actions - simplified to only the inputs we care about + // Actions XrActionSetPtr action_set_; XrAction grip_pose_action_; XrAction aim_pose_action_; @@ -86,8 +97,12 @@ class ControllerTracker : public ITracker XrSpacePtr left_aim_space_; XrSpacePtr right_aim_space_; - // Controller data for both hands (table wrapper with struct snapshots) - ControllerDataT controller_data_; + // Controller snapshots stored separately + ControllerSnapshot left_controller_{}; + ControllerSnapshot right_controller_{}; + + // Timestamp from last update + DeviceDataTimestamp last_timestamp_{}; }; }; diff --git a/src/core/deviceio/cpp/inc/deviceio/frame_metadata_tracker_oak.hpp b/src/core/deviceio/cpp/inc/deviceio/frame_metadata_tracker_oak.hpp index 835997ab8..2e2c3b111 100644 --- a/src/core/deviceio/cpp/inc/deviceio/frame_metadata_tracker_oak.hpp +++ b/src/core/deviceio/cpp/inc/deviceio/frame_metadata_tracker_oak.hpp @@ -48,6 +48,7 @@ class FrameMetadataTrackerOak : public ITracker std::string_view get_name() const override; std::string_view get_schema_name() const override; std::string_view get_schema_text() const override; + std::vector get_record_channels() const override; /*! * @brief Get the current frame metadata. diff --git a/src/core/deviceio/cpp/inc/deviceio/full_body_tracker_pico.hpp b/src/core/deviceio/cpp/inc/deviceio/full_body_tracker_pico.hpp index 624dc81fa..e6167b0d4 100644 --- a/src/core/deviceio/cpp/inc/deviceio/full_body_tracker_pico.hpp +++ b/src/core/deviceio/cpp/inc/deviceio/full_body_tracker_pico.hpp @@ -32,13 +32,18 @@ class FullBodyTrackerPico : public ITracker std::string_view get_schema_name() const override { - return "core.FullBodyPosePico"; + return "core.FullBodyPosePicoRecord"; } std::string_view get_schema_text() const override { - return std::string_view( - reinterpret_cast(FullBodyPosePicoBinarySchema::data()), FullBodyPosePicoBinarySchema::size()); + return std::string_view(reinterpret_cast(FullBodyPosePicoRecordBinarySchema::data()), + FullBodyPosePicoRecordBinarySchema::size()); + } + + std::vector get_record_channels() const override + { + return { "full_body" }; } // Query method - public API for getting body pose data diff --git a/src/core/deviceio/cpp/inc/deviceio/generic_3axis_pedal_tracker.hpp b/src/core/deviceio/cpp/inc/deviceio/generic_3axis_pedal_tracker.hpp index 66637fec7..3182da13c 100644 --- a/src/core/deviceio/cpp/inc/deviceio/generic_3axis_pedal_tracker.hpp +++ b/src/core/deviceio/cpp/inc/deviceio/generic_3axis_pedal_tracker.hpp @@ -48,6 +48,7 @@ class Generic3AxisPedalTracker : public ITracker std::string_view get_name() const override; std::string_view get_schema_name() const override; std::string_view get_schema_text() const override; + std::vector get_record_channels() const override; /*! * @brief Get the current foot pedal data. diff --git a/src/core/deviceio/cpp/inc/deviceio/hand_tracker.hpp b/src/core/deviceio/cpp/inc/deviceio/hand_tracker.hpp index 6518d9c6b..561f2f055 100644 --- a/src/core/deviceio/cpp/inc/deviceio/hand_tracker.hpp +++ b/src/core/deviceio/cpp/inc/deviceio/hand_tracker.hpp @@ -1,13 +1,12 @@ -// SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. // SPDX-License-Identifier: Apache-2.0 #pragma once #include "tracker.hpp" +#include #include -#include -#include #include @@ -34,7 +33,12 @@ class HandTracker : public ITracker std::string_view get_schema_text() const override { return std::string_view( - reinterpret_cast(HandsPoseBinarySchema::data()), HandsPoseBinarySchema::size()); + reinterpret_cast(HandPoseRecordBinarySchema::data()), HandPoseRecordBinarySchema::size()); + } + + std::vector get_record_channels() const override + { + return { "left_hand", "right_hand" }; } // Query methods - public API for getting hand data @@ -46,7 +50,7 @@ class HandTracker : public ITracker private: static constexpr const char* TRACKER_NAME = "HandTracker"; - static constexpr const char* SCHEMA_NAME = "core.HandsPose"; + static constexpr const char* SCHEMA_NAME = "core.HandPoseRecord"; std::shared_ptr create_tracker(const OpenXRSessionHandles& handles) const override; @@ -61,13 +65,12 @@ class HandTracker : public ITracker // Override from ITrackerImpl bool update(XrTime time) override; - Timestamp serialize(flatbuffers::FlatBufferBuilder& builder) const override; + DeviceDataTimestamp serialize(flatbuffers::FlatBufferBuilder& builder, size_t channel_index) const override; const HandPoseT& get_left_hand() const; const HandPoseT& get_right_hand() const; private: - // Helper functions bool update_hand(XrHandTrackerEXT tracker, XrTime time, HandPoseT& out_data); XrSpace base_space_; @@ -80,6 +83,9 @@ class HandTracker : public ITracker HandPoseT left_hand_; HandPoseT right_hand_; + // Timestamp from last update + DeviceDataTimestamp last_timestamp_{}; + // Extension function pointers PFN_xrCreateHandTrackerEXT pfn_create_hand_tracker_; PFN_xrDestroyHandTrackerEXT pfn_destroy_hand_tracker_; diff --git a/src/core/deviceio/cpp/inc/deviceio/head_tracker.hpp b/src/core/deviceio/cpp/inc/deviceio/head_tracker.hpp index fe042a513..833276e57 100644 --- a/src/core/deviceio/cpp/inc/deviceio/head_tracker.hpp +++ b/src/core/deviceio/cpp/inc/deviceio/head_tracker.hpp @@ -1,4 +1,4 @@ -// SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. // SPDX-License-Identifier: Apache-2.0 #pragma once @@ -34,7 +34,12 @@ class HeadTracker : public ITracker std::string_view get_schema_text() const override { return std::string_view( - reinterpret_cast(HeadPoseBinarySchema::data()), HeadPoseBinarySchema::size()); + reinterpret_cast(HeadPoseRecordBinarySchema::data()), HeadPoseRecordBinarySchema::size()); + } + + std::vector get_record_channels() const override + { + return { "head" }; } // Query methods - public API for getting head data @@ -42,7 +47,7 @@ class HeadTracker : public ITracker private: static constexpr const char* TRACKER_NAME = "HeadTracker"; - static constexpr const char* SCHEMA_NAME = "core.HeadPose"; + static constexpr const char* SCHEMA_NAME = "core.HeadPoseRecord"; std::shared_ptr create_tracker(const OpenXRSessionHandles& handles) const override; @@ -54,7 +59,7 @@ class HeadTracker : public ITracker // Override from ITrackerImpl bool update(XrTime time) override; - Timestamp serialize(flatbuffers::FlatBufferBuilder& builder) const override; + DeviceDataTimestamp serialize(flatbuffers::FlatBufferBuilder& builder, size_t channel_index) const override; const HeadPoseT& get_head() const; @@ -63,6 +68,7 @@ class HeadTracker : public ITracker XrSpace base_space_; XrSpacePtr view_space_; HeadPoseT head_; + DeviceDataTimestamp last_timestamp_{}; }; }; diff --git a/src/core/deviceio/cpp/inc/deviceio/schema_tracker.hpp b/src/core/deviceio/cpp/inc/deviceio/schema_tracker.hpp index be5a53e73..4cb591ad7 100644 --- 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 +#include #include #include @@ -87,29 +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 raw; + * m_schema_reader.read_all_samples(raw); + * for (auto& s : raw) { + * auto fb = flatbuffers::GetRoot(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; * } * - * Timestamp serialize(flatbuffers::FlatBufferBuilder& builder) const override { + * DeviceDataTimestamp serialize(flatbuffers::FlatBufferBuilder& builder, size_t) const override { * auto offset = LocomotionCommand::Pack(builder, &data_); - * builder.Finish(offset); - * return data_.timestamp ? *data_.timestamp : Timestamp{}; + * auto& ts = m_pending.empty() ? DeviceDataTimestamp{} : m_pending.back().timestamp; + * LocomotionCommandRecordBuilder rb(builder); + * rb.add_data(offset); + * rb.add_timestamp(&ts); + * builder.Finish(rb.Finish()); + * 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 buffer_; * LocomotionCommandT data_; + * std::vector m_pending; * }; * }; * @endcode @@ -142,16 +165,26 @@ class SchemaTracker */ static std::vector get_required_extensions(); + //! A single tensor sample with its data buffer and real timestamps. + struct SampleResult + { + std::vector 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& buffer); + size_t read_all_samples(std::vector& samples); /*! * @brief Access the configuration. @@ -161,9 +194,10 @@ class SchemaTracker private: void initialize_tensor_data_functions(); void create_tensor_list(); + bool ensure_collection(); void poll_for_updates(); std::optional find_target_collection(); - bool read_next_sample(std::vector& 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 index 8670d6308..820659a42 100644 --- a/src/core/deviceio/cpp/inc/deviceio/tracker.hpp +++ b/src/core/deviceio/cpp/inc/deviceio/tracker.hpp @@ -1,4 +1,4 @@ -// SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. // SPDX-License-Identifier: Apache-2.0 #pragma once @@ -7,6 +7,7 @@ #include #include +#include #include #include #include @@ -30,12 +31,40 @@ class ITrackerImpl virtual bool update(XrTime time) = 0; /** - * @brief Serialize the tracker data to a FlatBuffer. + * @brief Serialize a single record channel to a FlatBuffer. + * + * Each call serializes the XXRecord type (data + DeviceDataTimestamp) for + * the given channel. Multi-channel trackers (e.g., left/right hand) are + * called once per channel. * * @param builder Output FlatBufferBuilder to write serialized data into. - * @return Timestamp for MCAP recording (device_time and common_time). + * @param channel_index Which record channel to serialize (0-based). + * @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. */ - virtual Timestamp serialize(flatbuffers::FlatBufferBuilder& builder) const = 0; + using RecordCallback = std::function; + + /** + * @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 @@ -53,16 +82,26 @@ class ITracker /** * @brief Get the FlatBuffer schema name (root type) for MCAP recording. * - * This should return the fully qualified FlatBuffer type name (e.g., "core.HandPose") - * which matches the root_type defined in the .fbs schema file. + * Returns the fully qualified FlatBuffer record type name (e.g., + * "core.HandPoseRecord") matching the root_type in the .fbs schema. */ virtual std::string_view get_schema_name() const = 0; /** - * @brief Get the binary FlatBuffer schema text for MCAP recording. + * @brief Get the binary FlatBuffer schema for MCAP recording. */ virtual std::string_view get_schema_text() const = 0; + /** + * @brief Get the MCAP channel names this tracker produces. + * + * Single-channel trackers return one name (e.g., {"head"}). + * Multi-channel trackers return multiple (e.g., {"left_hand", "right_hand"}). + * The indices correspond to the channel_index parameter in + * ITrackerImpl::serialize(). + */ + virtual std::vector get_record_channels() const = 0; + protected: // Internal lifecycle methods - only accessible to friend classes // External users should NOT call these directly diff --git a/src/core/deviceio/cpp/schema_tracker.cpp b/src/core/deviceio/cpp/schema_tracker.cpp index 43bb12401..a112bca75 100644 --- a/src/core/deviceio/cpp/schema_tracker.cpp +++ b/src/core/deviceio/cpp/schema_tracker.cpp @@ -53,24 +53,39 @@ std::vector SchemaTracker::get_required_extensions() return { "XR_NVX1_tensor_data" }; } -bool SchemaTracker::read_buffer(std::vector& buffer) +bool SchemaTracker::ensure_collection() { - // Try to discover target collection if not found yet (or if it was lost) + if (m_target_collection_index) + { + return true; + } + + poll_for_updates(); + + m_target_collection_index = find_target_collection(); if (!m_target_collection_index) { - // Poll for tensor list updates only when we need to discover - poll_for_updates(); + return false; + } + std::cout << "Found target collection at index " << *m_target_collection_index << std::endl; + return true; +} - m_target_collection_index = find_target_collection(); - if (!m_target_collection_index) - { - return false; // Collection not available yet - } - std::cout << "Found target collection at index " << *m_target_collection_index << std::endl; +size_t SchemaTracker::read_all_samples(std::vector& samples) +{ + if (!ensure_collection()) + { + return 0; } - // Try to read next sample - return read_next_sample(buffer); + size_t count = 0; + SampleResult result; + while (read_next_sample(result)) + { + samples.push_back(std::move(result)); + ++count; + } + return count; } const SchemaTrackerConfig& SchemaTracker::config() const @@ -170,20 +185,18 @@ std::optional SchemaTracker::find_target_collection() return std::nullopt; } -bool SchemaTracker::read_next_sample(std::vector& buffer) +bool SchemaTracker::read_next_sample(SampleResult& out) { if (!m_target_collection_index.has_value()) { return false; } - // Prepare retrieval info XrTensorDataRetrievalInfoNV retrievalInfo{ XR_TYPE_TENSOR_DATA_RETRIEVAL_INFO_NV }; retrievalInfo.next = nullptr; retrievalInfo.tensorCollectionIndex = m_target_collection_index.value(); retrievalInfo.startSampleIndex = m_last_sample_index.has_value() ? m_last_sample_index.value() + 1 : 0; - // Prepare output buffers (read one sample at a time for simplicity) XrTensorSampleMetadataNV metadata{}; std::vector dataBuffer(m_sample_batch_stride); @@ -195,16 +208,9 @@ bool SchemaTracker::read_next_sample(std::vector& buffer) tensorData.bufferCapacity = static_cast(dataBuffer.size()); tensorData.writtenSampleCount = 0; - // Retrieve samples XrResult result = m_get_data_fn(m_tensor_list, &retrievalInfo, &tensorData); if (result != XR_SUCCESS) { - // TODO: Check against XR_ERROR_TENSOR_LOST_NV when it's reported by the runtime. - // if (result == XR_ERROR_TENSOR_LOST_NV) - // { - // m_target_collection_index = std::nullopt; - // return false; - // } std::cerr << "Failed to get tensor data, result=" << result << std::endl; m_target_collection_index = std::nullopt; return false; @@ -212,18 +218,20 @@ bool SchemaTracker::read_next_sample(std::vector& buffer) if (tensorData.writtenSampleCount == 0) { - return false; // No new samples + return false; } - // Update last sample index if (!m_last_sample_index.has_value() || metadata.sampleIndex > m_last_sample_index.value()) { m_last_sample_index = metadata.sampleIndex; } - // Copy data to output buffer (trim to sample size, not batch stride) - buffer.resize(m_sample_size); - std::memcpy(buffer.data(), dataBuffer.data(), m_sample_size); + out.buffer.resize(m_sample_size); + std::memcpy(out.buffer.data(), dataBuffer.data(), m_sample_size); + + out.timestamp = + DeviceDataTimestamp(static_cast(metadata.rawDeviceTimestamp), static_cast(metadata.timestamp), + static_cast(metadata.arrivalTimestamp)); return true; } diff --git a/src/core/deviceio/python/deviceio_bindings.cpp b/src/core/deviceio/python/deviceio_bindings.cpp index 8685b8e7d..998374b23 100644 --- a/src/core/deviceio/python/deviceio_bindings.cpp +++ b/src/core/deviceio/python/deviceio_bindings.cpp @@ -46,11 +46,20 @@ PYBIND11_MODULE(_deviceio, m) py::class_>(m, "ControllerTracker") .def(py::init<>()) .def( - "get_controller_data", - [](core::ControllerTracker& self, PyDeviceIOSession& session) -> const core::ControllerDataT& - { return self.get_controller_data(session.native()); }, - py::arg("session"), py::return_value_policy::reference_internal, - "Get complete controller data for both left and right controllers"); + "get_left_controller", + [](core::ControllerTracker& self, PyDeviceIOSession& session) -> const core::ControllerSnapshot& + { return self.get_left_controller(session.native()); }, + py::arg("session"), py::return_value_policy::reference_internal, "Get the left controller snapshot") + .def( + "get_right_controller", + [](core::ControllerTracker& self, PyDeviceIOSession& session) -> const core::ControllerSnapshot& + { return self.get_right_controller(session.native()); }, + py::arg("session"), py::return_value_policy::reference_internal, "Get the right controller snapshot") + .def( + "get_last_update_time", + [](core::ControllerTracker& self, PyDeviceIOSession& session) -> int64_t + { return self.get_last_update_time(session.native()); }, + py::arg("session"), "Get the XrTime from the last update"); // FrameMetadataTrackerOak class py::class_>( diff --git a/src/core/deviceio/python/deviceio_init.py b/src/core/deviceio/python/deviceio_init.py index 32903e7af..585ee7f42 100644 --- a/src/core/deviceio/python/deviceio_init.py +++ b/src/core/deviceio/python/deviceio_init.py @@ -7,7 +7,7 @@ Note: HeadTracker.get_head(session) returns HeadPoseT from isaacteleop.schema. HandTracker.get_left_hand(session) / get_right_hand(session) return HandPoseT from isaacteleop.schema. - ControllerTracker.get_controller_data(session) returns ControllerSnapshot from isaacteleop.schema. + ControllerTracker.get_left_controller(session) / get_right_controller(session) return ControllerSnapshot from isaacteleop.schema. FrameMetadataTrackerOak.get_data(session) returns FrameMetadata from isaacteleop.schema. Import these types from isaacteleop.schema if you need to work with pose types. """ @@ -35,16 +35,16 @@ ControllerInputState, ControllerPose, ControllerSnapshot, + DeviceDataTimestamp, FrameMetadata, - Timestamp, ) __all__ = [ "ControllerInputState", "ControllerPose", "ControllerSnapshot", + "DeviceDataTimestamp", "FrameMetadata", - "Timestamp", "ITracker", "HandTracker", "HeadTracker", diff --git a/src/core/mcap/cpp/inc/mcap/recorder.hpp b/src/core/mcap/cpp/inc/mcap/recorder.hpp index beefc9b2b..64a373cc5 100644 --- a/src/core/mcap/cpp/inc/mcap/recorder.hpp +++ b/src/core/mcap/cpp/inc/mcap/recorder.hpp @@ -1,4 +1,4 @@ -// SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. // SPDX-License-Identifier: Apache-2.0 #pragma once @@ -7,7 +7,6 @@ #include #include -#include #include namespace core @@ -19,14 +18,11 @@ class DeviceIOSession; /** * @brief MCAP Recorder for recording tracking data to MCAP files. * - * This class provides a simple interface to record tracker data - * to MCAP format files, which can be visualized with tools like Foxglove. + * Records XXRecord FlatBuffer types (data + DeviceDataTimestamp) to MCAP. + * Each tracker defines its own record channels via get_record_channels(). * * Usage: - * auto recorder = McapRecorder::create("output.mcap", { - * {hand_tracker, "hands"}, - * {head_tracker, "head"}, - * }); + * auto recorder = McapRecorder::create("output.mcap", {hand_tracker, head_tracker}); * // In your loop: * recorder->record(session); * // When done, let the recorder go out of scope or reset it @@ -34,26 +30,20 @@ class DeviceIOSession; class McapRecorder { public: - /// Tracker configuration: pair of (tracker, channel_name) - using TrackerChannelPair = std::pair, std::string>; - /** * @brief Create a recorder for the specified MCAP file and trackers. * - * This is the main factory method. Opens the file, registers schemas/channels, - * and returns a recorder ready for use. + * Each tracker's get_record_channels() defines which MCAP channels are + * created (e.g., HandTracker creates "left_hand" and "right_hand"). * * @param filename Path to the output MCAP file. - * @param trackers List of (tracker, channel_name) pairs to record. + * @param trackers List of trackers to record. * @return A unique_ptr to the McapRecorder. * @throws std::runtime_error if the recorder cannot be created. */ static std::unique_ptr create(const std::string& filename, - const std::vector& trackers); + const std::vector>& trackers); - /** - * @brief Destructor - closes the MCAP file. - */ ~McapRecorder(); /** @@ -66,8 +56,7 @@ class McapRecorder void record(const DeviceIOSession& session); private: - // Private constructor - use create() factory method - McapRecorder(const std::string& filename, const std::vector& trackers); + McapRecorder(const std::string& filename, const std::vector>& trackers); class Impl; std::unique_ptr impl_; diff --git a/src/core/mcap/cpp/mcap_recorder.cpp b/src/core/mcap/cpp/mcap_recorder.cpp index 7e2365ec2..b3c96d96d 100644 --- a/src/core/mcap/cpp/mcap_recorder.cpp +++ b/src/core/mcap/cpp/mcap_recorder.cpp @@ -1,4 +1,4 @@ -// SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. // SPDX-License-Identifier: Apache-2.0 #define MCAP_IMPLEMENTATION @@ -15,20 +15,27 @@ namespace core { +// Per-channel recording state binding a tracker, its channel index, and MCAP channel ID +struct ChannelBinding +{ + std::shared_ptr tracker; + size_t channel_index; + mcap::ChannelId mcap_channel_id; +}; + class McapRecorder::Impl { public: - explicit Impl(const std::string& filename, const std::vector& trackers) - : filename_(filename), tracker_configs_(trackers) + explicit Impl(const std::string& filename, const std::vector>& trackers) + : filename_(filename) { - if (tracker_configs_.empty()) + if (trackers.empty()) { throw std::runtime_error("McapRecorder: No trackers provided"); } - // Configure MCAP writer options mcap::McapWriterOptions options("teleop"); - options.compression = mcap::Compression::None; // No compression to avoid deps + options.compression = mcap::Compression::None; auto status = writer_.open(filename_, options); if (!status.ok()) @@ -36,10 +43,9 @@ class McapRecorder::Impl throw std::runtime_error("McapRecorder: Failed to open file " + filename_ + ": " + status.message); } - // Register all trackers immediately - for (const auto& config : tracker_configs_) + for (const auto& tracker : trackers) { - register_tracker(config.first.get(), config.second); + register_tracker(tracker); } std::cout << "McapRecorder: Started recording to " << filename_ << std::endl; @@ -53,27 +59,27 @@ class McapRecorder::Impl void record(const DeviceIOSession& session) { - for (const auto& config : tracker_configs_) + for (const auto& binding : channel_bindings_) { try { - const auto& tracker_impl = session.get_tracker_impl(*config.first); - if (!record_tracker(config.first.get(), tracker_impl)) + const auto& tracker_impl = session.get_tracker_impl(*binding.tracker); + if (!record_channel(binding, tracker_impl)) { - std::cerr << "McapRecorder: Failed to record tracker " << config.second << std::endl; + std::cerr << "McapRecorder: Failed to record channel " << binding.mcap_channel_id << std::endl; } } catch (const std::exception& e) { - std::cerr << "McapRecorder: Failed to record tracker " << config.second << ": " << e.what() << std::endl; + std::cerr << "McapRecorder: Failed to record tracker " << binding.tracker->get_name() << ": " + << e.what() << std::endl; } } } private: - void register_tracker(const ITracker* tracker, const std::string& channel_name) + void register_tracker(const std::shared_ptr& tracker) { - // Get schema info from the tracker std::string schema_name(tracker->get_schema_name()); if (schema_ids_.find(schema_name) == schema_ids_.end()) @@ -83,76 +89,74 @@ class McapRecorder::Impl schema_ids_[schema_name] = schema.id; } - mcap::Channel channel(channel_name, "flatbuffer", schema_ids_[schema_name]); - writer_.addChannel(channel); + auto channels = tracker->get_record_channels(); + for (size_t i = 0; i < channels.size(); ++i) + { + mcap::Channel channel(channels[i], "flatbuffer", schema_ids_[schema_name]); + writer_.addChannel(channel); - // Store the mapping from tracker to its assigned channel ID - tracker_channel_ids_[tracker] = channel.id; + channel_bindings_.push_back({ tracker, i, channel.id }); + } } - bool record_tracker(const ITracker* tracker, const ITrackerImpl& tracker_impl) + bool record_channel(const ChannelBinding& binding, const ITrackerImpl& tracker_impl) { - auto it = tracker_channel_ids_.find(tracker); - if (it == tracker_channel_ids_.end()) - { - std::cerr << "McapRecorder: Tracker not registered" << std::endl; - return false; - } + bool success = true; - flatbuffers::FlatBufferBuilder builder(256); - Timestamp timestamp = tracker_impl.serialize(builder); + tracker_impl.serialize_all( + binding.channel_index, + [&](const DeviceDataTimestamp& timestamp, const uint8_t* data, size_t size) + { + mcap::Timestamp log_time; + if (timestamp.sample_time_common_clock() != 0) + { + log_time = static_cast(timestamp.sample_time_common_clock()); + } + else + { + log_time = static_cast(std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()) + .count()); + } - // Use tracker timestamp for log time, fall back to system time if 0 - mcap::Timestamp log_time; - if (timestamp.device_time() != 0) - { - // XrTime is in nanoseconds, same as MCAP Timestamp - log_time = static_cast(timestamp.device_time()); - } - else - { - log_time = static_cast( - std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()) - .count()); - } + mcap::Message msg; + msg.channelId = binding.mcap_channel_id; + msg.logTime = log_time; + msg.publishTime = log_time; + msg.sequence = static_cast(message_count_); + msg.data = reinterpret_cast(data); + msg.dataSize = size; - mcap::Message msg; - msg.channelId = it->second; - msg.logTime = log_time; - msg.publishTime = log_time; - msg.sequence = static_cast(message_count_); - msg.data = reinterpret_cast(builder.GetBufferPointer()); - msg.dataSize = builder.GetSize(); + auto status = writer_.write(msg); + if (!status.ok()) + { + std::cerr << "McapRecorder: Failed to write message: " << status.message << std::endl; + success = false; + } - auto status = writer_.write(msg); - if (!status.ok()) - { - std::cerr << "McapRecorder: Failed to write message: " << status.message << std::endl; - return false; - } + ++message_count_; + }); - ++message_count_; - return true; + return success; } std::string filename_; - std::vector tracker_configs_; mcap::McapWriter writer_; uint64_t message_count_ = 0; std::unordered_map schema_ids_; - std::unordered_map tracker_channel_ids_; + std::vector channel_bindings_; }; // McapRecorder public interface implementation std::unique_ptr McapRecorder::create(const std::string& filename, - const std::vector& trackers) + const std::vector>& trackers) { return std::unique_ptr(new McapRecorder(filename, trackers)); } -McapRecorder::McapRecorder(const std::string& filename, const std::vector& trackers) +McapRecorder::McapRecorder(const std::string& filename, const std::vector>& trackers) : impl_(std::make_unique(filename, trackers)) { } diff --git a/src/core/mcap/python/mcap_bindings.cpp b/src/core/mcap/python/mcap_bindings.cpp index aceea8afb..f4d1148e0 100644 --- a/src/core/mcap/python/mcap_bindings.cpp +++ b/src/core/mcap/python/mcap_bindings.cpp @@ -52,7 +52,7 @@ PYBIND11_MODULE(_mcap, m) py::class_(m, "McapRecorder") .def_static( "create", - [](const std::string& filename, const std::vector& trackers) + [](const std::string& filename, const std::vector>& trackers) { return std::make_unique(core::McapRecorder::create(filename, trackers)); }, py::arg("filename"), py::arg("trackers"), "Create a recorder for an MCAP file with the specified trackers. " diff --git a/src/core/mcap_tests/cpp/test_mcap_recorder.cpp b/src/core/mcap_tests/cpp/test_mcap_recorder.cpp index df94df41b..d6af8961a 100644 --- a/src/core/mcap_tests/cpp/test_mcap_recorder.cpp +++ b/src/core/mcap_tests/cpp/test_mcap_recorder.cpp @@ -5,6 +5,7 @@ #include #include +#include #include #include #include @@ -13,7 +14,9 @@ #include #include #include +#include #include +#include namespace fs = std::filesystem; @@ -37,7 +40,7 @@ class MockTrackerImpl : public core::ITrackerImpl return true; } - core::Timestamp serialize(flatbuffers::FlatBufferBuilder& builder) const override + core::DeviceDataTimestamp serialize(flatbuffers::FlatBufferBuilder& builder, size_t /*channel_index*/) const override { // Create minimal valid FlatBuffer data (just some bytes for testing) // In a real scenario, this would be actual FlatBuffer serialization @@ -45,7 +48,7 @@ class MockTrackerImpl : public core::ITrackerImpl auto vec = builder.CreateVector(data); builder.Finish(vec); serialize_count_++; - return core::Timestamp(timestamp_, timestamp_); + return core::DeviceDataTimestamp(timestamp_, timestamp_, 0); } // Test helpers @@ -101,6 +104,11 @@ class MockTracker : public core::ITracker return SCHEMA_TEXT; } + std::vector get_record_channels() const override + { + return { "test_channel" }; + } + std::shared_ptr get_impl() const { return impl_; @@ -160,7 +168,7 @@ TEST_CASE("McapRecorder create static factory", "[mcap_recorder]") SECTION("create creates file and returns recorder") { - auto recorder = core::McapRecorder::create(path, { { tracker, "test_channel" } }); + auto recorder = core::McapRecorder::create(path, { tracker }); REQUIRE(recorder != nullptr); recorder.reset(); // Close via destructor @@ -184,8 +192,7 @@ TEST_CASE("McapRecorder with multiple trackers", "[mcap_recorder]") auto tracker2 = std::make_shared(); auto tracker3 = std::make_shared(); - auto recorder = core::McapRecorder::create( - path, { { tracker1, "channel1" }, { tracker2, "channel2" }, { tracker3, "channel3" } }); + auto recorder = core::McapRecorder::create(path, { tracker1, tracker2, tracker3 }); REQUIRE(recorder != nullptr); recorder.reset(); // Close via destructor @@ -200,7 +207,7 @@ TEST_CASE("McapRecorder destructor closes file", "[mcap_recorder]") auto tracker = std::make_shared(); { - auto recorder = core::McapRecorder::create(path, { { tracker, "test_channel" } }); + auto recorder = core::McapRecorder::create(path, { tracker }); REQUIRE(recorder != nullptr); // Destructor closes the file } @@ -221,7 +228,7 @@ TEST_CASE("McapRecorder creates valid MCAP file", "[mcap_recorder][file]") auto tracker = std::make_shared(); { - auto recorder = core::McapRecorder::create(path, { { tracker, "test_channel" } }); + auto recorder = core::McapRecorder::create(path, { tracker }); REQUIRE(recorder != nullptr); // Note: We can't test record() without a real DeviceIOSession, diff --git a/src/core/oxr_utils/cpp/inc/oxr_utils/oxr_time.hpp b/src/core/oxr_utils/cpp/inc/oxr_utils/oxr_time.hpp index 1c6c1440f..db7e8594b 100644 --- a/src/core/oxr_utils/cpp/inc/oxr_utils/oxr_time.hpp +++ b/src/core/oxr_utils/cpp/inc/oxr_utils/oxr_time.hpp @@ -112,6 +112,50 @@ class XrTimeConverter return time; } + /*! + * @brief Converts a system monotonic time (in nanoseconds) to XrTime. + * @param monotonic_ns Time in nanoseconds from the system monotonic clock + * (CLOCK_MONOTONIC on Linux). + * @return The equivalent XrTime value. + * @throws std::runtime_error if time conversion failed. + */ + XrTime convert_monotonic_ns_to_xrtime(int64_t monotonic_ns) const + { + XrTime time; +#if defined(XR_USE_PLATFORM_WIN32) + LARGE_INTEGER frequency; + QueryPerformanceFrequency(&frequency); + + // Convert nanoseconds back to QPC ticks, splitting to avoid overflow: + // ticks = seconds * freq + (remainder_ns * freq) / 1e9 + int64_t seconds = monotonic_ns / 1000000000LL; + int64_t remainder_ns = monotonic_ns % 1000000000LL; + + LARGE_INTEGER counter; + counter.QuadPart = seconds * frequency.QuadPart + remainder_ns * frequency.QuadPart / 1000000000LL; + + XrResult result = pfn_convert_win32_(handles_.instance, &counter, &time); + if (result != XR_SUCCESS) + { + throw std::runtime_error("xrConvertWin32PerformanceCounterToTimeKHR failed with code " + + std::to_string(result)); + } +#elif defined(XR_USE_TIMESPEC) + struct timespec ts; + ts.tv_sec = static_cast(monotonic_ns / 1000000000LL); + ts.tv_nsec = static_cast(monotonic_ns % 1000000000LL); + + XrResult result = pfn_convert_timespec_(handles_.instance, &ts, &time); + if (result != XR_SUCCESS) + { + throw std::runtime_error("xrConvertTimespecTimeToTimeKHR failed with code " + std::to_string(result)); + } +#else + static_assert(false, "OpenXR time conversion not implemented on this platform."); +#endif + return time; + } + private: OpenXRSessionHandles handles_; diff --git a/src/core/pusherio/cpp/inc/pusherio/schema_pusher.hpp b/src/core/pusherio/cpp/inc/pusherio/schema_pusher.hpp index 711a963c2..1be608f42 100644 --- a/src/core/pusherio/cpp/inc/pusherio/schema_pusher.hpp +++ b/src/core/pusherio/cpp/inc/pusherio/schema_pusher.hpp @@ -67,11 +67,12 @@ struct SchemaPusherConfig * .localized_name = "HeadPose Data" * }) {} * - * void push(const HeadPoseT& data) { + * void push(const HeadPoseT& data, int64_t device_time_ns, int64_t common_time_ns) { * flatbuffers::FlatBufferBuilder builder(m_pusher.config().max_flatbuffer_size); * auto offset = HeadPose::Pack(builder, &data); * builder.Finish(offset); - * m_pusher.push_buffer(builder.GetBufferPointer(), builder.GetSize()); + * m_pusher.push_buffer(builder.GetBufferPointer(), builder.GetSize(), + * device_time_ns, common_time_ns); * } * * private: @@ -120,12 +121,24 @@ class SchemaPusher * @brief Push raw serialized FlatBuffer data. * * The buffer will be padded to max_flatbuffer_size if smaller. + * The available_time_common_clock is calculated internally when this method + * is called (system monotonic time at push time). + * + * The tensor API's timestamp is set to sample_time_common_clock_ns converted + * to XrTime, and rawDeviceTimestamp is set to sample_time_device_clock_ns as-is. * * @param buffer Pointer to serialized FlatBuffer data. * @param size Size of the serialized data in bytes. + * @param sample_time_device_clock_ns Sample timestamp from the device's own clock + * (nanoseconds, device-specific clock domain). + * @param sample_time_common_clock_ns Sample timestamp in the common clock domain + * (system monotonic time, nanoseconds). * @throws std::runtime_error if the push fails. */ - void push_buffer(const uint8_t* buffer, size_t size); + void push_buffer(const uint8_t* buffer, + size_t size, + int64_t sample_time_device_clock_ns, + int64_t sample_time_common_clock_ns); /*! * @brief Access the configuration. diff --git a/src/core/pusherio/cpp/schema_pusher.cpp b/src/core/pusherio/cpp/schema_pusher.cpp index 1a58147c2..03d3d543b 100644 --- a/src/core/pusherio/cpp/schema_pusher.cpp +++ b/src/core/pusherio/cpp/schema_pusher.cpp @@ -5,6 +5,7 @@ #include #include +#include #include #include #include @@ -47,7 +48,10 @@ SchemaPusher::~SchemaPusher() } } -void SchemaPusher::push_buffer(const uint8_t* buffer, size_t size) +void SchemaPusher::push_buffer(const uint8_t* buffer, + size_t size, + int64_t sample_time_device_clock_ns, + int64_t sample_time_common_clock_ns) { // Validate that the serialized size fits within our declared buffer if (size > m_config.max_flatbuffer_size) @@ -62,15 +66,23 @@ void SchemaPusher::push_buffer(const uint8_t* buffer, size_t size) std::vector padded_buffer(m_config.max_flatbuffer_size, 0); std::memcpy(padded_buffer.data(), buffer, size); - // Get current time for timestamps - XrTime xr_time = m_time_converter.get_current_time(); + // Calculate available_time_common_clock (system monotonic time at push time) + // TODO: available_time_common_clock is computed here but dropped because the + // tensor API (XrPushTensorCollectionDataNV) only has timestamp and + // rawDeviceTimestamp fields. When the tensor API supports a third timestamp + // field, plumb available_time through. + [[maybe_unused]] auto available_time_common_clock_ns = + std::chrono::duration_cast(std::chrono::steady_clock::now().time_since_epoch()).count(); + + // Convert sample common clock time to XrTime for the tensor header + XrTime xr_time = m_time_converter.convert_monotonic_ns_to_xrtime(sample_time_common_clock_ns); // Prepare push data structure XrPushTensorCollectionDataNV tensorData{}; tensorData.type = XR_TYPE_PUSH_TENSOR_COLLECTION_DATA_NV; tensorData.next = nullptr; tensorData.timestamp = xr_time; - tensorData.rawDeviceTimestamp = static_cast(xr_time); + tensorData.rawDeviceTimestamp = static_cast(sample_time_device_clock_ns); tensorData.buffer = padded_buffer.data(); tensorData.bufferSize = static_cast(m_config.max_flatbuffer_size); diff --git a/src/core/retargeting_engine/python/deviceio_source_nodes/controllers_source.py b/src/core/retargeting_engine/python/deviceio_source_nodes/controllers_source.py index 798f35adc..2722a3295 100644 --- a/src/core/retargeting_engine/python/deviceio_source_nodes/controllers_source.py +++ b/src/core/retargeting_engine/python/deviceio_source_nodes/controllers_source.py @@ -39,10 +39,11 @@ class ControllersSource(IDeviceIOSource): Usage: # In TeleopSession, manually poll tracker and pass data - controller_data = controller_tracker.get_controller_data(session) + left_snapshot = controller_tracker.get_left_controller(session) + right_snapshot = controller_tracker.get_right_controller(session) result = controllers_source_node({ - "deviceio_controller_left": controller_data.left_controller, - "deviceio_controller_right": controller_data.right_controller + "deviceio_controller_left": left_snapshot, + "deviceio_controller_right": right_snapshot }) """ @@ -80,15 +81,16 @@ def poll_tracker(self, deviceio_session: Any) -> RetargeterIO: Dict with "deviceio_controller_left" and "deviceio_controller_right" TensorGroups containing raw ControllerSnapshot data. """ - controller_data = self._controller_tracker.get_controller_data(deviceio_session) + left_snapshot = self._controller_tracker.get_left_controller(deviceio_session) + right_snapshot = self._controller_tracker.get_right_controller(deviceio_session) source_inputs = self.input_spec() result = {} for input_name, group_type in source_inputs.items(): tg = TensorGroup(group_type) if "left" in input_name: - tg[0] = controller_data.left_controller + tg[0] = left_snapshot elif "right" in input_name: - tg[0] = controller_data.right_controller + tg[0] = right_snapshot result[input_name] = tg return result diff --git a/src/core/retargeting_engine/python/deviceio_source_nodes/hands_source.py b/src/core/retargeting_engine/python/deviceio_source_nodes/hands_source.py index 323422608..b21f31a58 100644 --- a/src/core/retargeting_engine/python/deviceio_source_nodes/hands_source.py +++ b/src/core/retargeting_engine/python/deviceio_source_nodes/hands_source.py @@ -155,7 +155,6 @@ def _update_hand_data(self, group: TensorGroup, hand_data: "HandPoseT") -> None: group[2] = radii group[3] = valid group[4] = hand_data.is_active - group[5] = int(hand_data.timestamp.device_time) def transformed(self, transform_input: OutputSelector) -> RetargeterSubgraph: """ diff --git a/src/core/retargeting_engine/python/deviceio_source_nodes/head_source.py b/src/core/retargeting_engine/python/deviceio_source_nodes/head_source.py index 465134420..d3b2a168d 100644 --- a/src/core/retargeting_engine/python/deviceio_source_nodes/head_source.py +++ b/src/core/retargeting_engine/python/deviceio_source_nodes/head_source.py @@ -33,7 +33,7 @@ class HeadSource(IDeviceIOSource): - "deviceio_head": Raw HeadPoseT flatbuffer object from DeviceIO Outputs: - - "head": Standard HeadPose tensor (position, orientation, is_valid, timestamp) + - "head": Standard HeadPose tensor (position, orientation, is_valid) Usage: # In TeleopSession, manually poll tracker and pass data @@ -124,7 +124,6 @@ def compute(self, inputs: RetargeterIO, outputs: RetargeterIO) -> None: output[0] = position output[1] = orientation output[2] = head_pose.is_valid - output[3] = int(head_pose.timestamp.device_time) def transformed(self, transform_input: OutputSelector) -> RetargeterSubgraph: """ diff --git a/src/core/retargeting_engine/python/tensor_types/standard_types.py b/src/core/retargeting_engine/python/tensor_types/standard_types.py index 680d83fd6..86f1da9f3 100644 --- a/src/core/retargeting_engine/python/tensor_types/standard_types.py +++ b/src/core/retargeting_engine/python/tensor_types/standard_types.py @@ -37,7 +37,6 @@ def HandInput() -> TensorGroupType: - joint_radii: (26,) float32 array - Radius of each joint - joint_valid: (26,) bool array - Validity flag for each joint - is_active: bool - Whether hand tracking is active - - timestamp: int - Timestamp in XrTime format (int64) Returns: TensorGroupType for hand tracking data @@ -80,7 +79,6 @@ def input_spec(self) -> RetargeterIO: dtype_bits=8, # bool represented as uint8 ), BoolType("hand_is_active"), - IntType("hand_timestamp"), ], ) @@ -100,7 +98,6 @@ def HeadPose() -> TensorGroupType: - head_position: (3,) float32 array - XYZ position - head_orientation: (4,) float32 array - XYZW quaternion - head_is_valid: bool - Whether head tracking data is valid - - head_timestamp: int - Timestamp in XrTime format (int64) Returns: TensorGroupType for head tracking data @@ -117,7 +114,6 @@ def HeadPose() -> TensorGroupType: "head_orientation", shape=(4,), dtype=DLDataType.FLOAT, dtype_bits=32 ), BoolType("head_is_valid"), - IntType("head_timestamp"), ], ) diff --git a/src/core/retargeting_engine/python/utilities/hand_transform.py b/src/core/retargeting_engine/python/utilities/hand_transform.py index ba2bac685..d96f36c89 100644 --- a/src/core/retargeting_engine/python/utilities/hand_transform.py +++ b/src/core/retargeting_engine/python/utilities/hand_transform.py @@ -5,8 +5,7 @@ Hand Transform Node - Applies a 4x4 transform to hand tracking data. Transforms all hand joint positions and orientations using a homogeneous -transformation matrix while preserving joint radii, validity, active state, -and timestamp fields. +transformation matrix while preserving joint radii, validity, and active state. The transform matrix is received as a tensor input from the graph, typically provided by a TransformSource node. @@ -41,7 +40,7 @@ class HandTransform(BaseRetargeter): Transforms all 26 joint positions (R @ p + t) and orientations (R_quat * q) for both left and right hands while passing through joint radii, validity - flags, active state, and timestamp unchanged. + flags, and active state unchanged. The transform matrix is provided as a tensor input, allowing it to be sourced from a TransformSource node in the graph. @@ -99,7 +98,7 @@ def compute(self, inputs: RetargeterIO, outputs: RetargeterIO) -> None: Position is transformed as: p' = R @ p + t (batch over 26 joints) Orientation is transformed as: q' = R_quat * q (batch over 26 joints) - All other fields (radii, validity, active, timestamp) are copied unchanged. + All other fields (radii, validity, active) are copied unchanged. Args: inputs: Dict with "hand_left", "hand_right", and "transform" TensorGroups. diff --git a/src/core/retargeting_engine/python/utilities/head_transform.py b/src/core/retargeting_engine/python/utilities/head_transform.py index a2145ee32..61c9fa0aa 100644 --- a/src/core/retargeting_engine/python/utilities/head_transform.py +++ b/src/core/retargeting_engine/python/utilities/head_transform.py @@ -5,7 +5,7 @@ Head Transform Node - Applies a 4x4 transform to head pose data. Transforms head position and orientation using a homogeneous transformation -matrix while preserving validity and timestamp fields. +matrix while preserving validity field. The transform matrix is received as a tensor input from the graph, typically provided by a TransformSource node. @@ -34,13 +34,13 @@ class HeadTransform(BaseRetargeter): Applies a 4x4 homogeneous transform to head pose data. Transforms the head position (R @ p + t) and orientation (R_quat * q) - while passing through is_valid and timestamp unchanged. + while passing through is_valid unchanged. The transform matrix is provided as a tensor input, allowing it to be sourced from a TransformSource node in the graph. Inputs: - - "head": HeadPose tensor (position, orientation, is_valid, timestamp) + - "head": HeadPose tensor (position, orientation, is_valid) - "transform": TransformMatrix tensor containing the (4, 4) matrix Outputs: @@ -60,7 +60,6 @@ class HeadTransform(BaseRetargeter): _POSITION = 0 _ORIENTATION = 1 _IS_VALID = 2 - _TIMESTAMP = 3 def __init__(self, name: str) -> None: """ @@ -88,7 +87,7 @@ def compute(self, inputs: RetargeterIO, outputs: RetargeterIO) -> None: Position is transformed as: p' = R @ p + t Orientation is transformed as: q' = R_quat * q - All other fields (is_valid, timestamp) are copied unchanged. + All other fields (is_valid) are copied unchanged. Args: inputs: Dict with "head" and "transform" TensorGroups. diff --git a/src/core/retargeting_engine_tests/python/test_sources.py b/src/core/retargeting_engine_tests/python/test_sources.py index 848e18b93..d14f32625 100644 --- a/src/core/retargeting_engine_tests/python/test_sources.py +++ b/src/core/retargeting_engine_tests/python/test_sources.py @@ -26,7 +26,6 @@ ControllerPose, ControllerInputState, ControllerSnapshot, - Timestamp, ) @@ -60,13 +59,8 @@ def create_controller_snapshot(grip_pos, aim_pos, trigger_val): trigger_value=trigger_val, ) - # Create timestamp - timestamp = Timestamp(123, 456) - # Create snapshot - return ControllerSnapshot( - grip_controller_pose, aim_controller_pose, inputs, True, timestamp - ) + return ControllerSnapshot(grip_controller_pose, aim_controller_pose, inputs, True) # ============================================================================ diff --git a/src/core/retargeting_engine_tests/python/test_transforms.py b/src/core/retargeting_engine_tests/python/test_transforms.py index f8d37b3dc..0b5838707 100644 --- a/src/core/retargeting_engine_tests/python/test_transforms.py +++ b/src/core/retargeting_engine_tests/python/test_transforms.py @@ -288,12 +288,11 @@ def test_preserves_shape_and_dtype(self): class TestHeadTransform: - def _make_head_input(self, position, orientation, is_valid=True, timestamp=100): + def _make_head_input(self, position, orientation, is_valid=True): tg = TensorGroup(HeadPose()) tg[0] = np.array(position, dtype=np.float32) tg[1] = np.array(orientation, dtype=np.float32) tg[2] = is_valid - tg[3] = timestamp return tg def test_identity_transform(self): @@ -305,7 +304,6 @@ def test_identity_transform(self): npt.assert_array_almost_equal(np.from_dlpack(out[0]), [1, 2, 3], decimal=5) npt.assert_array_almost_equal(np.from_dlpack(out[1]), [0, 0, 0, 1], decimal=5) assert out[2] is True - assert out[3] == 100 def test_translation_transform(self): node = HeadTransform("head_xform") @@ -327,14 +325,11 @@ def test_rotation_transform(self): def test_passthrough_fields_preserved(self): node = HeadTransform("head_xform") - head_in = self._make_head_input( - [0, 0, 0], [0, 0, 0, 1], is_valid=False, timestamp=42 - ) + head_in = self._make_head_input([0, 0, 0], [0, 0, 0, 1], is_valid=False) xform_in = _make_transform_input(_rotation_z_90_with_translation()) result = _run_retargeter(node, {"head": head_in, "transform": xform_in}) out = result["head"] assert out[2] is False - assert out[3] == 42 # ============================================================================ @@ -480,7 +475,7 @@ def test_rotation_transforms_grip_and_aim(self): class TestHandTransform: - def _make_hand_input(self, joint_offset=0.0, is_active=True, timestamp=200): + def _make_hand_input(self, joint_offset=0.0, is_active=True): tg = TensorGroup(HandInput()) positions = np.zeros((NUM_HAND_JOINTS, 3), dtype=np.float32) positions[:, 0] = np.arange(NUM_HAND_JOINTS, dtype=np.float32) + joint_offset @@ -494,7 +489,6 @@ def _make_hand_input(self, joint_offset=0.0, is_active=True, timestamp=200): tg[HandInputIndex.JOINT_RADII] = radii tg[HandInputIndex.JOINT_VALID] = valid tg[HandInputIndex.IS_ACTIVE] = is_active - tg[HandInputIndex.TIMESTAMP] = timestamp return tg def test_identity_transform(self): @@ -541,8 +535,8 @@ def test_translation_transforms_all_joints(self): def test_passthrough_fields_preserved(self): node = HandTransform("hand_xform") - left = self._make_hand_input(is_active=False, timestamp=42) - right = self._make_hand_input(is_active=True, timestamp=99) + left = self._make_hand_input(is_active=False) + right = self._make_hand_input(is_active=True) xform = _make_transform_input(_rotation_z_90_with_translation()) result = _run_retargeter( @@ -557,9 +551,7 @@ def test_passthrough_fields_preserved(self): out_l = result["hand_left"] out_r = result["hand_right"] assert out_l[HandInputIndex.IS_ACTIVE] is False - assert out_l[HandInputIndex.TIMESTAMP] == 42 assert out_r[HandInputIndex.IS_ACTIVE] is True - assert out_r[HandInputIndex.TIMESTAMP] == 99 # Radii and validity should be unchanged npt.assert_array_almost_equal( @@ -614,7 +606,6 @@ def test_mutating_output_position_does_not_affect_input(self): head_in[0] = np.array([1.0, 2.0, 3.0], dtype=np.float32) head_in[1] = np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float32) head_in[2] = True - head_in[3] = 100 xform_in = _make_transform_input(_identity_4x4()) # Save a copy of the original input values @@ -703,7 +694,6 @@ def test_mutating_output_does_not_affect_input(self): left[HandInputIndex.JOINT_RADII] = radii left[HandInputIndex.JOINT_VALID] = valid left[HandInputIndex.IS_ACTIVE] = True - left[HandInputIndex.TIMESTAMP] = 200 right = TensorGroup(HandInput()) right[HandInputIndex.JOINT_POSITIONS] = positions.copy() @@ -711,7 +701,6 @@ def test_mutating_output_does_not_affect_input(self): right[HandInputIndex.JOINT_RADII] = radii.copy() right[HandInputIndex.JOINT_VALID] = valid.copy() right[HandInputIndex.IS_ACTIVE] = True - right[HandInputIndex.TIMESTAMP] = 200 xform = _make_transform_input(_identity_4x4()) diff --git a/src/core/schema/fbs/controller.fbs b/src/core/schema/fbs/controller.fbs index a8ed31f5d..24ceaa4b0 100644 --- a/src/core/schema/fbs/controller.fbs +++ b/src/core/schema/fbs/controller.fbs @@ -42,18 +42,12 @@ struct ControllerSnapshot { // Whether the controller is active (connected and tracked) is_active: bool; - - // Dual timestamp with device and runtime clock domains - timestamp: Timestamp; } -// Controller data for both hands (table type for top-level serialization) -table ControllerData { - // Left controller state - left_controller: ControllerSnapshot (id: 0); - - // Right controller state - right_controller: ControllerSnapshot (id: 1); +// MCAP recording wrapper: pairs a ControllerSnapshot with a DeviceDataTimestamp. +table ControllerSnapshotRecord { + data: ControllerSnapshot (id: 0); + timestamp: DeviceDataTimestamp (id: 1); } -root_type ControllerData; +root_type ControllerSnapshotRecord; diff --git a/src/core/schema/fbs/full_body.fbs b/src/core/schema/fbs/full_body.fbs index edaa92a3f..82f7c0491 100644 --- a/src/core/schema/fbs/full_body.fbs +++ b/src/core/schema/fbs/full_body.fbs @@ -57,12 +57,15 @@ table FullBodyPosePico { // Whether the body tracking is active is_active: bool (id: 0); - // Dual timestamp with device and runtime clock domains - timestamp: Timestamp (id: 1); - // Vector of BodyJointPose. // For XR_BD_body_tracking, this is 24 joints. - joints: BodyJointsPico (id: 2); + joints: BodyJointsPico (id: 1); +} + +// MCAP recording wrapper: pairs a FullBodyPosePico with a DeviceDataTimestamp. +table FullBodyPosePicoRecord { + data: FullBodyPosePico (id: 0); + timestamp: DeviceDataTimestamp (id: 1); } -root_type FullBodyPosePico; +root_type FullBodyPosePicoRecord; diff --git a/src/core/schema/fbs/hand.fbs b/src/core/schema/fbs/hand.fbs index 11487b475..16c5eaa3f 100644 --- a/src/core/schema/fbs/hand.fbs +++ b/src/core/schema/fbs/hand.fbs @@ -31,9 +31,12 @@ table HandPose { // Whether the hand pose data is active is_active: bool (id: 1); +} - // Dual timestamp with device and runtime clock domains - timestamp: Timestamp (id: 2); +// MCAP recording wrapper: pairs a HandPose with a DeviceDataTimestamp. +table HandPoseRecord { + data: HandPose (id: 0); + timestamp: DeviceDataTimestamp (id: 1); } -root_type HandPose; +root_type HandPoseRecord; diff --git a/src/core/schema/fbs/hands.fbs b/src/core/schema/fbs/hands.fbs deleted file mode 100644 index 54106e13a..000000000 --- a/src/core/schema/fbs/hands.fbs +++ /dev/null @@ -1,18 +0,0 @@ -// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// SPDX-License-Identifier: Apache-2.0 - -include "hand.fbs"; - -namespace core; - -// Combined left and right hand poses. -// Used by HandTracker to serialize both hands in a single message. -table HandsPose { - // Left hand pose data - left_hand: HandPose (id: 0); - - // Right hand pose data - right_hand: HandPose (id: 1); -} - -root_type HandsPose; diff --git a/src/core/schema/fbs/head.fbs b/src/core/schema/fbs/head.fbs index 0e9bf7765..2efa3e631 100644 --- a/src/core/schema/fbs/head.fbs +++ b/src/core/schema/fbs/head.fbs @@ -13,9 +13,12 @@ table HeadPose { // Whether the head pose data is valid is_valid: bool (id: 1); +} - // Dual timestamp with device and runtime clock domains - timestamp: Timestamp (id: 2); +// MCAP recording wrapper: pairs a HeadPose with a DeviceDataTimestamp. +table HeadPoseRecord { + data: HeadPose (id: 0); + timestamp: DeviceDataTimestamp (id: 1); } -root_type HeadPose; +root_type HeadPoseRecord; diff --git a/src/core/schema/fbs/locomotion.fbs b/src/core/schema/fbs/locomotion.fbs index 0a0501a3e..9e905cfc3 100644 --- a/src/core/schema/fbs/locomotion.fbs +++ b/src/core/schema/fbs/locomotion.fbs @@ -9,14 +9,17 @@ namespace core; // Locomotion command output from input devices (foot pedal, gamepad, etc.) table LocomotionCommand { - // Dual timestamp with device and runtime clock domains. - timestamp: Timestamp (id: 0); - // Computed velocity command. - velocity: Twist (id: 1); + velocity: Twist (id: 0); // Computed pose command (e.g., for squat/vertical mode). - pose: Pose (id: 2); + pose: Pose (id: 1); +} + +// MCAP recording wrapper: pairs a LocomotionCommand with a DeviceDataTimestamp. +table LocomotionCommandRecord { + data: LocomotionCommand (id: 0); + timestamp: DeviceDataTimestamp (id: 1); } -root_type LocomotionCommand; +root_type LocomotionCommandRecord; diff --git a/src/core/schema/fbs/oak.fbs b/src/core/schema/fbs/oak.fbs index 93b40e686..14fd7add2 100644 --- a/src/core/schema/fbs/oak.fbs +++ b/src/core/schema/fbs/oak.fbs @@ -7,10 +7,13 @@ namespace core; // OAK frame metadata pushed by the OAK camera plugin. table FrameMetadata { - // Dual timestamp with device and runtime clock domains. - timestamp: Timestamp (id: 0); + sequence_number: int (id: 0); +} - sequence_number: int (id: 1); +// MCAP recording wrapper: pairs a FrameMetadata with a DeviceDataTimestamp. +table FrameMetadataRecord { + data: FrameMetadata (id: 0); + timestamp: DeviceDataTimestamp (id: 1); } -root_type FrameMetadata; +root_type FrameMetadataRecord; diff --git a/src/core/schema/fbs/pedals.fbs b/src/core/schema/fbs/pedals.fbs index a1e516e6b..9e79c8410 100644 --- a/src/core/schema/fbs/pedals.fbs +++ b/src/core/schema/fbs/pedals.fbs @@ -14,14 +14,17 @@ table Generic3AxisPedalOutput { // Whether the output data is valid. is_valid: bool (id: 0); - // Dual timestamp with device and runtime clock domains. - timestamp: Timestamp (id: 1); + left_pedal: float (id: 1); - left_pedal: float (id: 2); + right_pedal: float (id: 2); - right_pedal: float (id: 3); + rudder: float (id: 3); +} - rudder: float (id: 4); +// MCAP recording wrapper: pairs a Generic3AxisPedalOutput with a DeviceDataTimestamp. +table Generic3AxisPedalOutputRecord { + data: Generic3AxisPedalOutput (id: 0); + timestamp: DeviceDataTimestamp (id: 1); } -root_type Generic3AxisPedalOutput; +root_type Generic3AxisPedalOutputRecord; diff --git a/src/core/schema/fbs/timestamp.fbs b/src/core/schema/fbs/timestamp.fbs index 3227d0036..68ceaf69b 100644 --- a/src/core/schema/fbs/timestamp.fbs +++ b/src/core/schema/fbs/timestamp.fbs @@ -3,19 +3,24 @@ namespace core; -// Dual timestamp representation with device and runtime clock domains. -// Device timestamps come from the measurement source and provide low-jitter -// timing, but may be in different clock domains for different data sources. -// Runtime timestamps are assigned by CloudXR runtime in a common clock domain -// to enable cross-source synchronization. -struct Timestamp { - // Low-jitter timestamp from the measurement source (e.g., device clock). - // This is in nanoseconds, but may be in different clock domains for different data - // sources so it may drift apart from other timestamps. - device_time: int64; +// Timestamp for recording and synchronization across data sources. +// +// All timestamps are in nanoseconds. The "common clock" is the system monotonic +// clock (CLOCK_MONOTONIC on Linux, QueryPerformanceCounter on Windows). +struct DeviceDataTimestamp { + // Timestamp from the measurement source's own clock (e.g., device clock). + // This is in nanoseconds, but may be in a different clock domain for each + // data source, so values from different devices are not directly comparable. + sample_time_device_clock: int64; - // Timestamp assigned by the Isaac Teleop in a common clock domain in nanoseconds. - // This enables synchronization across different data sources. - // These may be noisy, but will not drift apart arbitrary across different devices. - common_time: int64; + // Timestamp of when the sample was taken, expressed in the common clock domain + // (system monotonic time, nanoseconds). Enables synchronization across + // different data sources. May be noisier than device clock timestamps, but + // values from different devices will not drift apart arbitrarily. + sample_time_common_clock: int64; + + // Timestamp of when the sample became available to the recording system, + // expressed in the common clock domain (system monotonic time, nanoseconds). + // Useful for measuring pipeline latency (available - sample). + available_time_common_clock: int64; } diff --git a/src/core/schema/python/controller_bindings.h b/src/core/schema/python/controller_bindings.h index 51c31122c..f6201f5ec 100644 --- a/src/core/schema/python/controller_bindings.h +++ b/src/core/schema/python/controller_bindings.h @@ -1,9 +1,8 @@ -// SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. // SPDX-License-Identifier: Apache-2.0 // Python bindings for the Controller FlatBuffer schema. -// ControllerInputState, ControllerPose, ControllerSnapshot, Timestamp are structs. -// ControllerDataT is a table (native type) containing struct snapshots. +// ControllerInputState, ControllerPose, ControllerSnapshot, DeviceDataTimestamp are structs. #pragma once @@ -20,19 +19,23 @@ namespace core inline void bind_controller(py::module& m) { - // Bind Timestamp struct (if not already bound) - if (!py::hasattr(m, "Timestamp")) + // Bind DeviceDataTimestamp struct (if not already bound) + if (!py::hasattr(m, "DeviceDataTimestamp")) { - py::class_(m, "Timestamp") + py::class_(m, "DeviceDataTimestamp") .def(py::init<>()) - .def(py::init(), py::arg("device_time"), py::arg("common_time")) - .def_property_readonly("device_time", &Timestamp::device_time) - .def_property_readonly("common_time", &Timestamp::common_time) + .def(py::init(), py::arg("sample_time_device_clock"), + py::arg("sample_time_common_clock"), py::arg("available_time_common_clock") = 0) + .def_property_readonly("sample_time_device_clock", &DeviceDataTimestamp::sample_time_device_clock) + .def_property_readonly("sample_time_common_clock", &DeviceDataTimestamp::sample_time_common_clock) + .def_property_readonly("available_time_common_clock", &DeviceDataTimestamp::available_time_common_clock) .def("__repr__", - [](const Timestamp& self) + [](const DeviceDataTimestamp& self) { - return "Timestamp(device_time=" + std::to_string(self.device_time()) + - ", common_time=" + std::to_string(self.common_time()) + ")"; + return "DeviceDataTimestamp(sample_time_device_clock=" + + std::to_string(self.sample_time_device_clock()) + + ", sample_time_common_clock=" + std::to_string(self.sample_time_common_clock()) + + ", available_time_common_clock=" + std::to_string(self.available_time_common_clock()) + ")"; }); } @@ -79,16 +82,15 @@ inline void bind_controller(py::module& m) return "ControllerPose(pose=" + pose_str + ", is_valid=" + (self.is_valid() ? "True" : "False") + ")"; }); - // Bind ControllerSnapshot struct + // Bind ControllerSnapshot struct (timestamp no longer embedded) py::class_(m, "ControllerSnapshot") .def(py::init<>()) - .def(py::init(), - py::arg("grip_pose"), py::arg("aim_pose"), py::arg("inputs"), py::arg("is_active"), py::arg("timestamp")) + .def(py::init(), + py::arg("grip_pose"), py::arg("aim_pose"), py::arg("inputs"), py::arg("is_active")) .def_property_readonly("grip_pose", &ControllerSnapshot::grip_pose, py::return_value_policy::reference_internal) .def_property_readonly("aim_pose", &ControllerSnapshot::aim_pose, py::return_value_policy::reference_internal) .def_property_readonly("inputs", &ControllerSnapshot::inputs, py::return_value_policy::reference_internal) .def_property_readonly("is_active", &ControllerSnapshot::is_active) - .def_property_readonly("timestamp", &ControllerSnapshot::timestamp, py::return_value_policy::reference_internal) .def("__repr__", [](const ControllerSnapshot& self) { @@ -99,27 +101,6 @@ inline void bind_controller(py::module& m) return "ControllerSnapshot(grip_pose=" + grip_str + ", aim_pose=" + aim_str + ", is_active=" + (self.is_active() ? "True" : "False") + ")"; }); - - // Bind ControllerDataT class (table native type - root object, read-only from Python) - py::class_>(m, "ControllerData") - .def(py::init<>()) - .def_property_readonly( - "left_controller", - [](const ControllerDataT& self) -> const ControllerSnapshot* { return self.left_controller.get(); }, - py::return_value_policy::reference_internal) - .def_property_readonly( - "right_controller", - [](const ControllerDataT& self) -> const ControllerSnapshot* { return self.right_controller.get(); }, - py::return_value_policy::reference_internal) - .def("__repr__", - [](const ControllerDataT& self) - { - std::string left_str = - self.left_controller ? (self.left_controller->is_active() ? "active" : "inactive") : "None"; - std::string right_str = - self.right_controller ? (self.right_controller->is_active() ? "active" : "inactive") : "None"; - return "ControllerData(left=" + left_str + ", right=" + right_str + ")"; - }); } } // namespace core diff --git a/src/core/schema/python/full_body_bindings.h b/src/core/schema/python/full_body_bindings.h index 2a3423060..39a44d1c4 100644 --- a/src/core/schema/python/full_body_bindings.h +++ b/src/core/schema/python/full_body_bindings.h @@ -106,9 +106,6 @@ inline void bind_full_body(py::module& m) "joints", [](const FullBodyPosePicoT& self) -> const BodyJointsPico* { return self.joints.get(); }, py::return_value_policy::reference_internal) .def_readonly("is_active", &FullBodyPosePicoT::is_active) - .def_property_readonly( - "timestamp", [](const FullBodyPosePicoT& self) -> const Timestamp* { return self.timestamp.get(); }, - py::return_value_policy::reference_internal) .def("__repr__", [](const FullBodyPosePicoT& self) { @@ -117,14 +114,8 @@ inline void bind_full_body(py::module& m) { joints_str = "BodyJointsPico(joints=[...24 entries...])"; } - std::string timestamp_str = "None"; - if (self.timestamp) - { - timestamp_str = "Timestamp(device=" + std::to_string(self.timestamp->device_time()) + - ", common=" + std::to_string(self.timestamp->common_time()) + ")"; - } return "FullBodyPosePicoT(joints=" + joints_str + - ", is_active=" + (self.is_active ? "True" : "False") + ", timestamp=" + timestamp_str + ")"; + ", is_active=" + (self.is_active ? "True" : "False") + ")"; }); } diff --git a/src/core/schema/python/hand_bindings.h b/src/core/schema/python/hand_bindings.h index 02a97b503..895bbc1c3 100644 --- a/src/core/schema/python/hand_bindings.h +++ b/src/core/schema/python/hand_bindings.h @@ -79,9 +79,6 @@ inline void bind_hand(py::module& m) "joints", [](const HandPoseT& self) -> const HandJoints* { return self.joints.get(); }, py::return_value_policy::reference_internal) .def_readonly("is_active", &HandPoseT::is_active) - .def_property_readonly( - "timestamp", [](const HandPoseT& self) -> const Timestamp* { return self.timestamp.get(); }, - py::return_value_policy::reference_internal) .def("__repr__", [](const HandPoseT& self) { @@ -90,14 +87,7 @@ inline void bind_hand(py::module& m) { joints_str = "HandJoints(poses=[...26 entries...])"; } - std::string timestamp_str = "None"; - if (self.timestamp) - { - timestamp_str = "Timestamp(device=" + std::to_string(self.timestamp->device_time()) + - ", common=" + std::to_string(self.timestamp->common_time()) + ")"; - } - return "HandPoseT(joints=" + joints_str + ", is_active=" + (self.is_active ? "True" : "False") + - ", timestamp=" + timestamp_str + ")"; + return "HandPoseT(joints=" + joints_str + ", is_active=" + (self.is_active ? "True" : "False") + ")"; }); } diff --git a/src/core/schema/python/head_bindings.h b/src/core/schema/python/head_bindings.h index f44c0028d..e01f4c2e2 100644 --- a/src/core/schema/python/head_bindings.h +++ b/src/core/schema/python/head_bindings.h @@ -25,9 +25,6 @@ inline void bind_head(py::module& m) "pose", [](const HeadPoseT& self) -> const Pose* { return self.pose.get(); }, py::return_value_policy::reference_internal) .def_readonly("is_valid", &HeadPoseT::is_valid) - .def_property_readonly( - "timestamp", [](const HeadPoseT& self) -> const Timestamp* { return self.timestamp.get(); }, - py::return_value_policy::reference_internal) .def("__repr__", [](const HeadPoseT& self) { @@ -42,14 +39,7 @@ inline void bind_head(py::module& m) ", z=" + std::to_string(self.pose->orientation().z()) + ", w=" + std::to_string(self.pose->orientation().w()) + "))"; } - std::string timestamp_str = "None"; - if (self.timestamp) - { - timestamp_str = "Timestamp(device=" + std::to_string(self.timestamp->device_time()) + - ", common=" + std::to_string(self.timestamp->common_time()) + ")"; - } - return "HeadPoseT(pose=" + pose_str + ", is_valid=" + (self.is_valid ? "True" : "False") + - ", timestamp=" + timestamp_str + ")"; + return "HeadPoseT(pose=" + pose_str + ", is_valid=" + (self.is_valid ? "True" : "False") + ")"; }); } diff --git a/src/core/schema/python/locomotion_bindings.h b/src/core/schema/python/locomotion_bindings.h index 74ca35cf4..5accb6812 100644 --- a/src/core/schema/python/locomotion_bindings.h +++ b/src/core/schema/python/locomotion_bindings.h @@ -36,9 +36,6 @@ inline void bind_locomotion(py::module& m) // Bind LocomotionCommand table using the native type (LocomotionCommandT). py::class_(m, "LocomotionCommand") .def(py::init<>()) - .def_property( - "timestamp", [](const LocomotionCommandT& self) -> const Timestamp* { return self.timestamp.get(); }, - [](LocomotionCommandT& self, const Timestamp& ts) { self.timestamp = std::make_unique(ts); }) .def_property( "velocity", [](const LocomotionCommandT& self) -> const Twist* { return self.velocity.get(); }, [](LocomotionCommandT& self, const Twist& vel) { self.velocity = std::make_unique(vel); }) @@ -48,17 +45,7 @@ inline void bind_locomotion(py::module& m) .def("__repr__", [](const LocomotionCommandT& cmd) { - std::string result = "LocomotionCommand("; - if (cmd.timestamp) - { - result += "timestamp=Timestamp(device_time=" + std::to_string(cmd.timestamp->device_time()) + - ", common_time=" + std::to_string(cmd.timestamp->common_time()) + ")"; - } - else - { - result += "timestamp=None"; - } - result += ", velocity="; + std::string result = "LocomotionCommand(velocity="; if (cmd.velocity) { result += "Twist(...)"; diff --git a/src/core/schema/python/oak_bindings.h b/src/core/schema/python/oak_bindings.h index 0c517c322..ef149f569 100644 --- a/src/core/schema/python/oak_bindings.h +++ b/src/core/schema/python/oak_bindings.h @@ -19,33 +19,13 @@ namespace core inline void bind_oak(py::module& m) { // Bind FrameMetadata table using the native type (FrameMetadataT). - // This is the base metadata type for all camera frames. py::class_(m, "FrameMetadata") .def(py::init<>()) - .def_property( - "timestamp", [](const FrameMetadataT& self) -> const Timestamp* { return self.timestamp.get(); }, - [](FrameMetadataT& self, const Timestamp& ts) { self.timestamp = std::make_unique(ts); }, - "Get or set the dual timestamp (device and common time)") .def_property( "sequence_number", [](const FrameMetadataT& self) { return self.sequence_number; }, [](FrameMetadataT& self, int val) { self.sequence_number = val; }, "Get or set the frame sequence number") - .def("__repr__", - [](const FrameMetadataT& metadata) - { - std::string result = "FrameMetadata("; - if (metadata.timestamp) - { - result += "timestamp=Timestamp(device_time=" + std::to_string(metadata.timestamp->device_time()) + - ", common_time=" + std::to_string(metadata.timestamp->common_time()) + ")"; - } - else - { - result += "timestamp=None"; - } - result += ", sequence_number=" + std::to_string(metadata.sequence_number); - result += ")"; - return result; - }); + .def("__repr__", [](const FrameMetadataT& metadata) + { return "FrameMetadata(sequence_number=" + std::to_string(metadata.sequence_number) + ")"; }); } } // namespace core diff --git a/src/core/schema/python/pedals_bindings.h b/src/core/schema/python/pedals_bindings.h index 221d41df9..b268e316c 100644 --- a/src/core/schema/python/pedals_bindings.h +++ b/src/core/schema/python/pedals_bindings.h @@ -22,9 +22,6 @@ inline void bind_pedals(py::module& m) py::class_(m, "Generic3AxisPedalOutput") .def(py::init<>()) .def_readwrite("is_valid", &Generic3AxisPedalOutputT::is_valid) - .def_property( - "timestamp", [](const Generic3AxisPedalOutputT& self) -> const Timestamp* { return self.timestamp.get(); }, - [](Generic3AxisPedalOutputT& self, const Timestamp& ts) { self.timestamp = std::make_unique(ts); }) .def_property( "left_pedal", [](const Generic3AxisPedalOutputT& self) { return self.left_pedal; }, [](Generic3AxisPedalOutputT& self, float val) { self.left_pedal = val; }) @@ -39,15 +36,6 @@ inline void bind_pedals(py::module& m) { std::string result = "Generic3AxisPedalOutput(is_valid=" + std::string(output.is_valid ? "True" : "False"); - if (output.timestamp) - { - result += ", timestamp=Timestamp(device_time=" + std::to_string(output.timestamp->device_time()) + - ", common_time=" + std::to_string(output.timestamp->common_time()) + ")"; - } - else - { - result += ", timestamp=None"; - } result += ", left_pedal=" + std::to_string(output.left_pedal); result += ", right_pedal=" + std::to_string(output.right_pedal); result += ", rudder=" + std::to_string(output.rudder); diff --git a/src/core/schema/python/schema_init.py b/src/core/schema/python/schema_init.py index 63301a7ba..a7ac8a0a3 100644 --- a/src/core/schema/python/schema_init.py +++ b/src/core/schema/python/schema_init.py @@ -12,6 +12,8 @@ Point, Quaternion, Pose, + # Timestamp type. + DeviceDataTimestamp, # Head-related types. HeadPoseT, # Hand-related types. @@ -21,9 +23,7 @@ # Controller-related types. ControllerInputState, ControllerPose, - Timestamp, ControllerSnapshot, - ControllerData, # Locomotion-related types. Twist, LocomotionCommand, @@ -44,6 +44,8 @@ "Point", "Quaternion", "Pose", + # Timestamp type. + "DeviceDataTimestamp", # Head types. "HeadPoseT", # Hand types. @@ -53,9 +55,7 @@ # Controller types. "ControllerInputState", "ControllerPose", - "Timestamp", "ControllerSnapshot", - "ControllerData", # Locomotion types. "Twist", "LocomotionCommand", diff --git a/src/core/schema_tests/cpp/test_controller.cpp b/src/core/schema_tests/cpp/test_controller.cpp index 91604e449..026b71310 100644 --- a/src/core/schema_tests/cpp/test_controller.cpp +++ b/src/core/schema_tests/cpp/test_controller.cpp @@ -12,24 +12,13 @@ #include -// ============================================================================= -// Compile-time verification of FlatBuffer field IDs for ControllerData table. -// These ensure schema field IDs remain stable across changes. -// VT values are computed as: (field_id + 2) * 2. -// ============================================================================= -#define VT(field) (field + 2) * 2 - -// ControllerData field IDs (ControllerData is a table) -static_assert(core::ControllerData::VT_LEFT_CONTROLLER == VT(0)); -static_assert(core::ControllerData::VT_RIGHT_CONTROLLER == VT(1)); - // ============================================================================= // Compile-time verification that controller types are structs (not tables) // ============================================================================= static_assert(std::is_trivially_copyable_v); static_assert(std::is_trivially_copyable_v); static_assert(std::is_trivially_copyable_v); -static_assert(std::is_trivially_copyable_v); +static_assert(std::is_trivially_copyable_v); // ============================================================================= // ControllerInputState Tests (struct) @@ -91,22 +80,24 @@ TEST_CASE("ControllerPose can store pose data", "[controller][struct]") } // ============================================================================= -// Timestamp Tests (struct) +// DeviceDataTimestamp Tests (struct) // ============================================================================= -TEST_CASE("Timestamp default construction", "[controller][struct]") +TEST_CASE("DeviceDataTimestamp default construction", "[controller][struct]") { - core::Timestamp timestamp{}; + core::DeviceDataTimestamp timestamp{}; - CHECK(timestamp.device_time() == 0); - CHECK(timestamp.common_time() == 0); + CHECK(timestamp.sample_time_device_clock() == 0); + CHECK(timestamp.sample_time_common_clock() == 0); + CHECK(timestamp.available_time_common_clock() == 0); } -TEST_CASE("Timestamp can store timestamp values", "[controller][struct]") +TEST_CASE("DeviceDataTimestamp can store timestamp values", "[controller][struct]") { - core::Timestamp timestamp(1000000000, 2000000000); + core::DeviceDataTimestamp timestamp(1000000000, 2000000000, 3000000000); - CHECK(timestamp.device_time() == 1000000000); - CHECK(timestamp.common_time() == 2000000000); + CHECK(timestamp.sample_time_device_clock() == 1000000000); + CHECK(timestamp.sample_time_common_clock() == 2000000000); + CHECK(timestamp.available_time_common_clock() == 3000000000); } // ============================================================================= @@ -136,105 +127,72 @@ TEST_CASE("ControllerSnapshot can store complete controller state", "[controller // Create inputs core::ControllerInputState inputs(true, false, true, 0.5f, -0.5f, 0.8f, 1.0f); - // Create timestamp - core::Timestamp timestamp(1000000000, 2000000000); - // Create snapshot - core::ControllerSnapshot snapshot(grip_pose, aim_pose, inputs, true, timestamp); + core::ControllerSnapshot snapshot(grip_pose, aim_pose, inputs, true); CHECK(snapshot.is_active() == true); CHECK(snapshot.grip_pose().is_valid() == true); CHECK(snapshot.aim_pose().is_valid() == true); CHECK(snapshot.inputs().primary_click() == true); CHECK(snapshot.inputs().trigger_value() == Catch::Approx(1.0f)); - CHECK(snapshot.timestamp().device_time() == 1000000000); - CHECK(snapshot.timestamp().common_time() == 2000000000); } // ============================================================================= -// ControllerDataT Tests (table native type) +// ControllerSnapshotRecord Serialization Tests // ============================================================================= -TEST_CASE("ControllerDataT default construction", "[controller][native]") -{ - core::ControllerDataT controller_data; - - // Controllers should be null by default - CHECK(controller_data.left_controller == nullptr); - CHECK(controller_data.right_controller == nullptr); -} - -TEST_CASE("ControllerDataT can store both controllers", "[controller][native]") -{ - core::ControllerDataT controller_data; - - // Create left controller - auto left = std::make_unique(); - controller_data.left_controller = std::move(left); - - // Create right controller - auto right = std::make_unique(); - controller_data.right_controller = std::move(right); - - CHECK(controller_data.left_controller != nullptr); - CHECK(controller_data.right_controller != nullptr); -} - -// ============================================================================= -// ControllerDataT Serialization Tests -// ============================================================================= -TEST_CASE("ControllerDataT serialization and deserialization", "[controller][serialize]") +TEST_CASE("ControllerSnapshotRecord serialization and deserialization", "[controller][serialize]") { flatbuffers::FlatBufferBuilder builder; - // Create controller data - core::ControllerDataT controller_data; - - // Create left controller snapshot + // Create controller snapshot core::Point left_pos(1.0f, 2.0f, 3.0f); core::Quaternion left_orient(0.0f, 0.0f, 0.0f, 1.0f); core::Pose left_p(left_pos, left_orient); core::ControllerPose left_grip(left_p, true); core::ControllerInputState left_inputs(true, false, false, 0.5f, 0.0f, 0.5f, 0.5f); - core::Timestamp left_timestamp(1000, 2000); - controller_data.left_controller = - std::make_unique(left_grip, left_grip, left_inputs, true, left_timestamp); + core::ControllerSnapshot snapshot(left_grip, left_grip, left_inputs, true); + + // Create record with data and timestamp + core::ControllerSnapshotRecordT record; + record.data = std::make_unique(snapshot); + record.timestamp = std::make_unique(1000, 2000, 0); // Serialize - auto offset = core::ControllerData::Pack(builder, &controller_data); + auto offset = core::ControllerSnapshotRecord::Pack(builder, &record); builder.Finish(offset); // Deserialize - auto* deserialized = flatbuffers::GetRoot(builder.GetBufferPointer()); + auto* deserialized = flatbuffers::GetRoot(builder.GetBufferPointer()); // Verify - CHECK(deserialized->left_controller() != nullptr); - CHECK(deserialized->left_controller()->is_active() == true); - CHECK(deserialized->left_controller()->inputs().primary_click() == true); + REQUIRE(deserialized->data() != nullptr); + CHECK(deserialized->data()->is_active() == true); + CHECK(deserialized->data()->inputs().primary_click() == true); + REQUIRE(deserialized->timestamp() != nullptr); + CHECK(deserialized->timestamp()->sample_time_device_clock() == 1000); } -// ============================================================================= -// ControllerSnapshot Serialization/Unpacking Tests -// ============================================================================= -TEST_CASE("ControllerSnapshot can be unpacked from buffer", "[controller][serialize]") +TEST_CASE("ControllerSnapshotRecord can be unpacked from buffer", "[controller][serialize]") { flatbuffers::FlatBufferBuilder builder; // Create native object - core::ControllerDataT controller_data; + core::ControllerSnapshotRecordT record; core::ControllerSnapshot snapshot; - controller_data.left_controller = std::make_unique(snapshot); + record.data = std::make_unique(snapshot); + record.timestamp = std::make_unique(0, 0, 0); // Serialize - auto offset = core::ControllerData::Pack(builder, &controller_data); + auto offset = core::ControllerSnapshotRecord::Pack(builder, &record); builder.Finish(offset); // Deserialize to table - auto* table = flatbuffers::GetRoot(builder.GetBufferPointer()); + auto* table = flatbuffers::GetRoot(builder.GetBufferPointer()); // Unpack to native - auto unpacked = std::make_unique(); + auto unpacked = std::make_unique(); table->UnPackTo(unpacked.get()); // Verify - CHECK(unpacked->left_controller != nullptr); + CHECK(unpacked->data != nullptr); } diff --git a/src/core/schema_tests/cpp/test_full_body.cpp b/src/core/schema_tests/cpp/test_full_body.cpp index eed2328d1..9acdebc62 100644 --- a/src/core/schema_tests/cpp/test_full_body.cpp +++ b/src/core/schema_tests/cpp/test_full_body.cpp @@ -20,8 +20,7 @@ // ============================================================================= #define VT(field) (field + 2) * 2 static_assert(core::FullBodyPosePico::VT_IS_ACTIVE == VT(0)); -static_assert(core::FullBodyPosePico::VT_TIMESTAMP == VT(1)); -static_assert(core::FullBodyPosePico::VT_JOINTS == VT(2)); +static_assert(core::FullBodyPosePico::VT_JOINTS == VT(1)); // ============================================================================= // Compile-time verification of FlatBuffer field types. @@ -30,7 +29,6 @@ static_assert(core::FullBodyPosePico::VT_JOINTS == VT(2)); #define TYPE(field) decltype(std::declval().field()) static_assert(std::is_same_v); static_assert(std::is_same_v); -static_assert(std::is_same_v); // ============================================================================= // Compile-time verification of BodyJointPose struct. @@ -131,7 +129,6 @@ TEST_CASE("FullBodyPosePicoT default construction", "[full_body][native]") // Default values. CHECK(body_pose->joints == nullptr); CHECK(body_pose->is_active == false); - CHECK(body_pose->timestamp == nullptr); } TEST_CASE("FullBodyPosePicoT can store joints data", "[full_body][native]") @@ -146,20 +143,6 @@ TEST_CASE("FullBodyPosePicoT can store joints data", "[full_body][native]") CHECK(body_pose->joints->joints()->size() == static_cast(core::BodyJointPico_NUM_JOINTS)); } -TEST_CASE("FullBodyPosePicoT can store timestamp", "[full_body][native]") -{ - auto body_pose = std::make_unique(); - - // Set timestamp (XrTime is int64_t). - int64_t test_device_time = 1234567890123456789LL; - int64_t test_common_time = 9876543210LL; - body_pose->timestamp = std::make_shared(test_device_time, test_common_time); - - REQUIRE(body_pose->timestamp != nullptr); - CHECK(body_pose->timestamp->device_time() == test_device_time); - CHECK(body_pose->timestamp->common_time() == test_common_time); -} - TEST_CASE("FullBodyPosePicoT joints can be mutated via flatbuffers Array", "[full_body][native]") { auto body_pose = std::make_unique(); @@ -199,30 +182,32 @@ TEST_CASE("FullBodyPosePicoT serialization and deserialization", "[full_body][fl body_pose->joints->mutable_joints()->Mutate(0, joint_pose); body_pose->is_active = true; - body_pose->timestamp = std::make_shared(9876543210LL, 1234567890LL); + + // Create FullBodyPosePicoRecord for serialization (root type) + core::FullBodyPosePicoRecordT record; + record.data = std::move(body_pose); + record.timestamp = std::make_unique(9876543210LL, 1234567890LL, 0); // Serialize. - auto offset = core::FullBodyPosePico::Pack(builder, body_pose.get()); + auto offset = core::FullBodyPosePicoRecord::Pack(builder, &record); builder.Finish(offset); // Deserialize. auto buffer = builder.GetBufferPointer(); - auto deserialized = core::GetFullBodyPosePico(buffer); + auto deserialized = flatbuffers::GetRoot(buffer); - // Verify. - REQUIRE(deserialized->joints() != nullptr); - CHECK(deserialized->joints()->joints()->size() == static_cast(core::BodyJointPico_NUM_JOINTS)); + // Verify (access data via record). + REQUIRE(deserialized->data() != nullptr); + REQUIRE(deserialized->data()->joints() != nullptr); + CHECK(deserialized->data()->joints()->joints()->size() == static_cast(core::BodyJointPico_NUM_JOINTS)); - const auto* first_joint = (*deserialized->joints()->joints())[0]; + const auto* first_joint = (*deserialized->data()->joints()->joints())[0]; CHECK(first_joint->pose().position().x() == Catch::Approx(1.5f)); CHECK(first_joint->pose().position().y() == Catch::Approx(2.5f)); CHECK(first_joint->pose().position().z() == Catch::Approx(3.5f)); CHECK(first_joint->is_valid() == true); - CHECK(deserialized->is_active() == true); - REQUIRE(deserialized->timestamp() != nullptr); - CHECK(deserialized->timestamp()->device_time() == 9876543210LL); - CHECK(deserialized->timestamp()->common_time() == 1234567890LL); + CHECK(deserialized->data()->is_active() == true); } TEST_CASE("FullBodyPosePicoT can be unpacked from buffer", "[full_body][flatbuffers]") @@ -244,16 +229,21 @@ TEST_CASE("FullBodyPosePicoT can be unpacked from buffer", "[full_body][flatbuff } original->is_active = true; - original->timestamp = std::make_shared(1111111111LL, 2222222222LL); - auto offset = core::FullBodyPosePico::Pack(builder, original.get()); + // Create FullBodyPosePicoRecord for serialization (root type) + core::FullBodyPosePicoRecordT record; + record.data = std::move(original); + record.timestamp = std::make_unique(1111111111LL, 2222222222LL, 0); + + auto offset = core::FullBodyPosePicoRecord::Pack(builder, &record); builder.Finish(offset); - // Unpack to FullBodyPosePicoT. + // Unpack to FullBodyPosePicoRecordT, then extract data. auto buffer = builder.GetBufferPointer(); - auto body_pose_fb = core::GetFullBodyPosePico(buffer); + auto record_fb = flatbuffers::GetRoot(buffer); auto unpacked = std::make_unique(); - body_pose_fb->UnPackTo(unpacked.get()); + REQUIRE(record_fb->data() != nullptr); + record_fb->data()->UnPackTo(unpacked.get()); // Verify unpacked data. REQUIRE(unpacked->joints != nullptr); @@ -270,9 +260,6 @@ TEST_CASE("FullBodyPosePicoT can be unpacked from buffer", "[full_body][flatbuff CHECK(joint_23->pose().position().z() == Catch::Approx(69.0f)); CHECK(unpacked->is_active == true); - REQUIRE(unpacked->timestamp != nullptr); - CHECK(unpacked->timestamp->device_time() == 1111111111LL); - CHECK(unpacked->timestamp->common_time() == 2222222222LL); } TEST_CASE("FullBodyPosePicoT all 24 joints can be set and verified", "[full_body][native]") @@ -333,15 +320,6 @@ TEST_CASE("FullBodyPosePicoT joint indices correspond to body parts", "[full_bod // ============================================================================= // Edge Cases // ============================================================================= -TEST_CASE("FullBodyPosePicoT with large timestamp values", "[full_body][edge]") -{ - auto body_pose = std::make_unique(); - int64_t max_int64 = 9223372036854775807LL; - body_pose->timestamp = std::make_shared(max_int64, max_int64 - 1000); - - CHECK(body_pose->timestamp->device_time() == max_int64); - CHECK(body_pose->timestamp->common_time() == max_int64 - 1000); -} TEST_CASE("FullBodyPosePicoT buffer size is reasonable", "[full_body][serialize]") { @@ -349,9 +327,12 @@ TEST_CASE("FullBodyPosePicoT buffer size is reasonable", "[full_body][serialize] auto body_pose = std::make_unique(); body_pose->joints = std::make_unique(); - body_pose->timestamp = std::make_shared(0, 0); - auto offset = core::FullBodyPosePico::Pack(builder, body_pose.get()); + core::FullBodyPosePicoRecordT record; + record.data = std::move(body_pose); + record.timestamp = std::make_unique(0, 0, 0); + + auto offset = core::FullBodyPosePicoRecord::Pack(builder, &record); builder.Finish(offset); // Buffer should be reasonably sized for 24 joints. diff --git a/src/core/schema_tests/cpp/test_hand.cpp b/src/core/schema_tests/cpp/test_hand.cpp index 4cafea300..c6a94c073 100644 --- a/src/core/schema_tests/cpp/test_hand.cpp +++ b/src/core/schema_tests/cpp/test_hand.cpp @@ -21,7 +21,6 @@ #define VT(field) (field + 2) * 2 static_assert(core::HandPose::VT_JOINTS == VT(0)); static_assert(core::HandPose::VT_IS_ACTIVE == VT(1)); -static_assert(core::HandPose::VT_TIMESTAMP == VT(2)); // ============================================================================= // Compile-time verification of FlatBuffer field types. @@ -30,7 +29,6 @@ static_assert(core::HandPose::VT_TIMESTAMP == VT(2)); #define TYPE(field) decltype(std::declval().field()) static_assert(std::is_same_v); static_assert(std::is_same_v); -static_assert(std::is_same_v); // ============================================================================= // Compile-time verification of HandJointPose struct. @@ -130,7 +128,6 @@ TEST_CASE("HandPoseT default construction", "[hand][native]") // Default values. CHECK(hand_pose->joints == nullptr); CHECK(hand_pose->is_active == false); - CHECK(hand_pose->timestamp == nullptr); } TEST_CASE("HandPoseT can store joints data", "[hand][native]") @@ -145,20 +142,6 @@ TEST_CASE("HandPoseT can store joints data", "[hand][native]") CHECK(hand_pose->joints->poses()->size() == 26); } -TEST_CASE("HandPoseT can store timestamp", "[hand][native]") -{ - auto hand_pose = std::make_unique(); - - // Set timestamp (XrTime is int64_t). - int64_t test_device_time = 1234567890123456789LL; - int64_t test_common_time = 9876543210LL; - hand_pose->timestamp = std::make_shared(test_device_time, test_common_time); - - REQUIRE(hand_pose->timestamp != nullptr); - CHECK(hand_pose->timestamp->device_time() == test_device_time); - CHECK(hand_pose->timestamp->common_time() == test_common_time); -} - TEST_CASE("HandPoseT joints can be mutated via flatbuffers Array", "[hand][native]") { auto hand_pose = std::make_unique(); @@ -199,31 +182,33 @@ TEST_CASE("HandPoseT serialization and deserialization", "[hand][flatbuffers]") hand_pose->joints->mutable_poses()->Mutate(0, joint_pose); hand_pose->is_active = true; - hand_pose->timestamp = std::make_shared(9876543210LL, 1234567890LL); + + // Create HandPoseRecord for serialization (root type) + core::HandPoseRecordT record; + record.data = std::move(hand_pose); + record.timestamp = std::make_unique(9876543210LL, 1234567890LL, 0); // Serialize. - auto offset = core::HandPose::Pack(builder, hand_pose.get()); + auto offset = core::HandPoseRecord::Pack(builder, &record); builder.Finish(offset); // Deserialize. auto buffer = builder.GetBufferPointer(); - auto deserialized = core::GetHandPose(buffer); + auto deserialized = flatbuffers::GetRoot(buffer); - // Verify. - REQUIRE(deserialized->joints() != nullptr); - CHECK(deserialized->joints()->poses()->size() == 26); + // Verify (access data via record). + REQUIRE(deserialized->data() != nullptr); + REQUIRE(deserialized->data()->joints() != nullptr); + CHECK(deserialized->data()->joints()->poses()->size() == 26); - const auto* first_joint = (*deserialized->joints()->poses())[0]; + const auto* first_joint = (*deserialized->data()->joints()->poses())[0]; CHECK(first_joint->pose().position().x() == Catch::Approx(1.5f)); CHECK(first_joint->pose().position().y() == Catch::Approx(2.5f)); CHECK(first_joint->pose().position().z() == Catch::Approx(3.5f)); CHECK(first_joint->is_valid() == true); CHECK(first_joint->radius() == Catch::Approx(0.02f)); - CHECK(deserialized->is_active() == true); - REQUIRE(deserialized->timestamp() != nullptr); - CHECK(deserialized->timestamp()->device_time() == 9876543210LL); - CHECK(deserialized->timestamp()->common_time() == 1234567890LL); + CHECK(deserialized->data()->is_active() == true); } TEST_CASE("HandPoseT can be unpacked from buffer", "[hand][flatbuffers]") @@ -245,16 +230,21 @@ TEST_CASE("HandPoseT can be unpacked from buffer", "[hand][flatbuffers]") } original->is_active = true; - original->timestamp = std::make_shared(1111111111LL, 2222222222LL); - auto offset = core::HandPose::Pack(builder, original.get()); + // Create HandPoseRecord for serialization (root type) + core::HandPoseRecordT record; + record.data = std::move(original); + record.timestamp = std::make_unique(1111111111LL, 2222222222LL, 0); + + auto offset = core::HandPoseRecord::Pack(builder, &record); builder.Finish(offset); - // Unpack to HandPoseT. + // Unpack to HandPoseRecordT, then extract data. auto buffer = builder.GetBufferPointer(); - auto hand_pose_fb = core::GetHandPose(buffer); + auto record_fb = flatbuffers::GetRoot(buffer); auto unpacked = std::make_unique(); - hand_pose_fb->UnPackTo(unpacked.get()); + REQUIRE(record_fb->data() != nullptr); + record_fb->data()->UnPackTo(unpacked.get()); // Verify unpacked data. REQUIRE(unpacked->joints != nullptr); @@ -271,9 +261,6 @@ TEST_CASE("HandPoseT can be unpacked from buffer", "[hand][flatbuffers]") CHECK(joint_25->pose().position().z() == Catch::Approx(75.0f)); CHECK(unpacked->is_active == true); - REQUIRE(unpacked->timestamp != nullptr); - CHECK(unpacked->timestamp->device_time() == 1111111111LL); - CHECK(unpacked->timestamp->common_time() == 2222222222LL); } TEST_CASE("HandPoseT all 26 joints can be set and verified", "[hand][native]") diff --git a/src/core/schema_tests/cpp/test_head.cpp b/src/core/schema_tests/cpp/test_head.cpp index 0219ce47e..e445442c9 100644 --- a/src/core/schema_tests/cpp/test_head.cpp +++ b/src/core/schema_tests/cpp/test_head.cpp @@ -20,7 +20,6 @@ #define VT(field) (field + 2) * 2 static_assert(core::HeadPose::VT_POSE == VT(0)); static_assert(core::HeadPose::VT_IS_VALID == VT(1)); -static_assert(core::HeadPose::VT_TIMESTAMP == VT(2)); // ============================================================================= // Compile-time verification of FlatBuffer field types. @@ -29,7 +28,6 @@ static_assert(core::HeadPose::VT_TIMESTAMP == VT(2)); #define TYPE(field) decltype(std::declval().field()) static_assert(std::is_same_v); static_assert(std::is_same_v); -static_assert(std::is_same_v); TEST_CASE("HeadPoseT can handle is_valid value properly", "[head][native]") @@ -52,7 +50,6 @@ TEST_CASE("HeadPoseT default construction", "[head][native]") // Default values. CHECK(head_pose->pose == nullptr); CHECK(head_pose->is_valid == false); - CHECK(head_pose->timestamp == nullptr); } TEST_CASE("HeadPoseT can store pose data", "[head][native]") @@ -75,20 +72,6 @@ TEST_CASE("HeadPoseT can store pose data", "[head][native]") CHECK(head_pose->pose->orientation().w() == Catch::Approx(1.0f)); } -TEST_CASE("HeadPoseT can store timestamp", "[head][native]") -{ - auto head_pose = std::make_unique(); - - // Set timestamp (XrTime is int64_t). - int64_t test_device_time = 1234567890123456789LL; - int64_t test_common_time = 9876543210LL; - head_pose->timestamp = std::make_shared(test_device_time, test_common_time); - - REQUIRE(head_pose->timestamp != nullptr); - CHECK(head_pose->timestamp->device_time() == test_device_time); - CHECK(head_pose->timestamp->common_time() == test_common_time); -} - TEST_CASE("HeadPoseT can store rotation quaternion", "[head][native]") { auto head_pose = std::make_unique(); @@ -113,25 +96,27 @@ TEST_CASE("HeadPoseT serialization and deserialization", "[head][flatbuffers]") core::Quaternion orientation(0.0f, 0.0f, 0.0f, 1.0f); head_pose->pose = std::make_unique(position, orientation); head_pose->is_valid = true; - head_pose->timestamp = std::make_shared(9876543210LL, 1234567890LL); + + // Create HeadPoseRecord for serialization (root type) + core::HeadPoseRecordT record; + record.data = std::move(head_pose); + record.timestamp = std::make_unique(9876543210LL, 1234567890LL, 0); // Serialize. - auto offset = core::HeadPose::Pack(builder, head_pose.get()); + auto offset = core::HeadPoseRecord::Pack(builder, &record); builder.Finish(offset); // Deserialize. auto buffer = builder.GetBufferPointer(); - auto deserialized = core::GetHeadPose(buffer); - - // Verify. - REQUIRE(deserialized->pose() != nullptr); - CHECK(deserialized->pose()->position().x() == Catch::Approx(1.0f)); - CHECK(deserialized->pose()->position().y() == Catch::Approx(2.0f)); - CHECK(deserialized->pose()->position().z() == Catch::Approx(3.0f)); - CHECK(deserialized->is_valid() == true); - REQUIRE(deserialized->timestamp() != nullptr); - CHECK(deserialized->timestamp()->device_time() == 9876543210LL); - CHECK(deserialized->timestamp()->common_time() == 1234567890LL); + auto deserialized = flatbuffers::GetRoot(buffer); + + // Verify (access data via record). + REQUIRE(deserialized->data() != nullptr); + REQUIRE(deserialized->data()->pose() != nullptr); + CHECK(deserialized->data()->pose()->position().x() == Catch::Approx(1.0f)); + CHECK(deserialized->data()->pose()->position().y() == Catch::Approx(2.0f)); + CHECK(deserialized->data()->pose()->position().z() == Catch::Approx(3.0f)); + CHECK(deserialized->data()->is_valid() == true); } TEST_CASE("HeadPoseT can be unpacked from buffer", "[head][flatbuffers]") @@ -144,16 +129,21 @@ TEST_CASE("HeadPoseT can be unpacked from buffer", "[head][flatbuffers]") core::Quaternion orientation(0.1f, 0.2f, 0.3f, 0.9f); original->pose = std::make_unique(position, orientation); original->is_valid = true; - original->timestamp = std::make_shared(1111111111LL, 2222222222LL); - auto offset = core::HeadPose::Pack(builder, original.get()); + // Create HeadPoseRecord for serialization (root type) + core::HeadPoseRecordT record; + record.data = std::move(original); + record.timestamp = std::make_unique(1111111111LL, 2222222222LL, 0); + + auto offset = core::HeadPoseRecord::Pack(builder, &record); builder.Finish(offset); - // Unpack to HeadPoseT. + // Unpack to HeadPoseRecordT, then extract data. auto buffer = builder.GetBufferPointer(); - auto head_pose_fb = core::GetHeadPose(buffer); + auto record_fb = flatbuffers::GetRoot(buffer); auto unpacked = std::make_unique(); - head_pose_fb->UnPackTo(unpacked.get()); + REQUIRE(record_fb->data() != nullptr); + record_fb->data()->UnPackTo(unpacked.get()); // Verify unpacked data. REQUIRE(unpacked->pose != nullptr); @@ -161,7 +151,4 @@ TEST_CASE("HeadPoseT can be unpacked from buffer", "[head][flatbuffers]") CHECK(unpacked->pose->position().y() == Catch::Approx(6.0f)); CHECK(unpacked->pose->position().z() == Catch::Approx(7.0f)); CHECK(unpacked->is_valid == true); - REQUIRE(unpacked->timestamp != nullptr); - CHECK(unpacked->timestamp->device_time() == 1111111111LL); - CHECK(unpacked->timestamp->common_time() == 2222222222LL); } diff --git a/src/core/schema_tests/cpp/test_locomotion.cpp b/src/core/schema_tests/cpp/test_locomotion.cpp index cd89dee1d..5899ca959 100644 --- a/src/core/schema_tests/cpp/test_locomotion.cpp +++ b/src/core/schema_tests/cpp/test_locomotion.cpp @@ -31,9 +31,8 @@ static_assert(std::is_same_v().angular()), co // ============================================================================= #define VT(field) (field + 2) * 2 -static_assert(core::LocomotionCommand::VT_TIMESTAMP == VT(0)); -static_assert(core::LocomotionCommand::VT_VELOCITY == VT(1)); -static_assert(core::LocomotionCommand::VT_POSE == VT(2)); +static_assert(core::LocomotionCommand::VT_VELOCITY == VT(0)); +static_assert(core::LocomotionCommand::VT_POSE == VT(1)); // ============================================================================= // Twist Tests (struct) @@ -108,7 +107,6 @@ TEST_CASE("LocomotionCommandT default construction", "[locomotion][native]") core::LocomotionCommandT cmd; // Struct pointers should be null by default for table fields. - CHECK(cmd.timestamp == nullptr); CHECK(cmd.velocity == nullptr); CHECK(cmd.pose == nullptr); } @@ -117,17 +115,11 @@ TEST_CASE("LocomotionCommandT can store velocity command", "[locomotion][native] { core::LocomotionCommandT cmd; - // Create timestamp. - cmd.timestamp = std::make_unique(1000000000, 2000000000); - // Create velocity (twist). core::Point linear(1.0f, 0.0f, 0.0f); core::Point angular(0.0f, 0.0f, 0.5f); cmd.velocity = std::make_unique(linear, angular); - CHECK(cmd.timestamp != nullptr); - CHECK(cmd.timestamp->device_time() == 1000000000); - CHECK(cmd.timestamp->common_time() == 2000000000); CHECK(cmd.velocity != nullptr); CHECK(cmd.velocity->linear().x() == Catch::Approx(1.0f)); CHECK(cmd.velocity->angular().z() == Catch::Approx(0.5f)); @@ -138,15 +130,11 @@ TEST_CASE("LocomotionCommandT can store pose command", "[locomotion][native]") { core::LocomotionCommandT cmd; - // Create timestamp. - cmd.timestamp = std::make_unique(3000000000, 4000000000); - // Create pose (for squat/vertical mode). core::Point position(0.0f, 0.0f, -0.2f); // Squat down. core::Quaternion orientation(0.0f, 0.0f, 0.0f, 1.0f); cmd.pose = std::make_unique(position, orientation); - CHECK(cmd.timestamp != nullptr); CHECK(cmd.pose != nullptr); CHECK(cmd.pose->position().z() == Catch::Approx(-0.2f)); CHECK(cmd.velocity == nullptr); // Velocity not set. @@ -156,9 +144,6 @@ TEST_CASE("LocomotionCommandT can store both velocity and pose", "[locomotion][n { core::LocomotionCommandT cmd; - // Create timestamp. - cmd.timestamp = std::make_unique(5000000000, 6000000000); - // Create velocity. core::Point linear(0.5f, 0.0f, 0.0f); core::Point angular(0.0f, 0.0f, 0.1f); @@ -169,7 +154,6 @@ TEST_CASE("LocomotionCommandT can store both velocity and pose", "[locomotion][n core::Quaternion orientation(0.0f, 0.0f, 0.0f, 1.0f); cmd.pose = std::make_unique(position, orientation); - CHECK(cmd.timestamp != nullptr); CHECK(cmd.velocity != nullptr); CHECK(cmd.pose != nullptr); } @@ -183,7 +167,6 @@ TEST_CASE("LocomotionCommand serialization and deserialization", "[locomotion][s // Create native object. core::LocomotionCommandT cmd; - cmd.timestamp = std::make_unique(1234567890, 9876543210); core::Point linear(2.0f, 1.0f, 0.0f); core::Point angular(0.0f, 0.0f, 0.75f); @@ -193,17 +176,19 @@ TEST_CASE("LocomotionCommand serialization and deserialization", "[locomotion][s core::Quaternion orientation(0.0f, 0.0f, 0.0f, 1.0f); cmd.pose = std::make_unique(position, orientation); + // Create LocomotionCommandRecord for serialization (root type) + core::LocomotionCommandRecordT record; + record.data = std::make_unique(cmd); + record.timestamp = std::make_unique(1234567890, 9876543210, 0); + // Serialize. - auto offset = core::LocomotionCommand::Pack(builder, &cmd); + auto offset = core::LocomotionCommandRecord::Pack(builder, &record); builder.Finish(offset); // Deserialize. - auto* deserialized = flatbuffers::GetRoot(builder.GetBufferPointer()); - - // Verify timestamp. - REQUIRE(deserialized->timestamp() != nullptr); - CHECK(deserialized->timestamp()->device_time() == 1234567890); - CHECK(deserialized->timestamp()->common_time() == 9876543210); + auto* deserialized_record = flatbuffers::GetRoot(builder.GetBufferPointer()); + REQUIRE(deserialized_record->data() != nullptr); + auto* deserialized = deserialized_record->data(); // Verify velocity. REQUIRE(deserialized->velocity() != nullptr); @@ -223,23 +208,27 @@ TEST_CASE("LocomotionCommand can be unpacked from buffer", "[locomotion][seriali // Create native object with minimal data. core::LocomotionCommandT cmd; - cmd.timestamp = std::make_unique(100, 200); + + core::LocomotionCommandRecordT record; + record.data = std::make_unique(cmd); + record.timestamp = std::make_unique(100, 200, 0); // Serialize. - auto offset = core::LocomotionCommand::Pack(builder, &cmd); + auto offset = core::LocomotionCommandRecord::Pack(builder, &record); builder.Finish(offset); // Deserialize to table. - auto* table = flatbuffers::GetRoot(builder.GetBufferPointer()); + auto* record_table = flatbuffers::GetRoot(builder.GetBufferPointer()); + REQUIRE(record_table->data() != nullptr); // Unpack to native. auto unpacked = std::make_unique(); - table->UnPackTo(unpacked.get()); + record_table->data()->UnPackTo(unpacked.get()); // Verify. - REQUIRE(unpacked->timestamp != nullptr); - CHECK(unpacked->timestamp->device_time() == 100); - CHECK(unpacked->timestamp->common_time() == 200); + REQUIRE(record_table->timestamp() != nullptr); + CHECK(record_table->timestamp()->sample_time_device_clock() == 100); + CHECK(record_table->timestamp()->sample_time_common_clock() == 200); } TEST_CASE("LocomotionCommand with only velocity (no pose)", "[locomotion][serialize]") @@ -247,17 +236,22 @@ TEST_CASE("LocomotionCommand with only velocity (no pose)", "[locomotion][serial flatbuffers::FlatBufferBuilder builder; core::LocomotionCommandT cmd; - cmd.timestamp = std::make_unique(1000, 2000); core::Point linear(1.0f, 0.0f, 0.0f); core::Point angular(0.0f, 0.0f, 0.0f); cmd.velocity = std::make_unique(linear, angular); // pose is intentionally left null. - auto offset = core::LocomotionCommand::Pack(builder, &cmd); + core::LocomotionCommandRecordT record; + record.data = std::make_unique(cmd); + record.timestamp = std::make_unique(1000, 2000, 0); + + auto offset = core::LocomotionCommandRecord::Pack(builder, &record); builder.Finish(offset); - auto* deserialized = flatbuffers::GetRoot(builder.GetBufferPointer()); + auto* deserialized_record = flatbuffers::GetRoot(builder.GetBufferPointer()); + REQUIRE(deserialized_record->data() != nullptr); + auto* deserialized = deserialized_record->data(); REQUIRE(deserialized->velocity() != nullptr); CHECK(deserialized->pose() == nullptr); @@ -268,17 +262,22 @@ TEST_CASE("LocomotionCommand with only pose (no velocity)", "[locomotion][serial flatbuffers::FlatBufferBuilder builder; core::LocomotionCommandT cmd; - cmd.timestamp = std::make_unique(3000, 4000); core::Point position(0.0f, 0.0f, -0.1f); core::Quaternion orientation(0.0f, 0.0f, 0.0f, 1.0f); cmd.pose = std::make_unique(position, orientation); // velocity is intentionally left null. - auto offset = core::LocomotionCommand::Pack(builder, &cmd); + core::LocomotionCommandRecordT record; + record.data = std::make_unique(cmd); + record.timestamp = std::make_unique(3000, 4000, 0); + + auto offset = core::LocomotionCommandRecord::Pack(builder, &record); builder.Finish(offset); - auto* deserialized = flatbuffers::GetRoot(builder.GetBufferPointer()); + auto* deserialized_record = flatbuffers::GetRoot(builder.GetBufferPointer()); + REQUIRE(deserialized_record->data() != nullptr); + auto* deserialized = deserialized_record->data(); CHECK(deserialized->velocity() == nullptr); REQUIRE(deserialized->pose() != nullptr); @@ -317,7 +316,6 @@ TEST_CASE("LocomotionCommand buffer size is reasonable", "[locomotion][serialize flatbuffers::FlatBufferBuilder builder; core::LocomotionCommandT cmd; - cmd.timestamp = std::make_unique(0, 0); core::Point linear(0.0f, 0.0f, 0.0f); core::Point angular(0.0f, 0.0f, 0.0f); @@ -327,7 +325,11 @@ TEST_CASE("LocomotionCommand buffer size is reasonable", "[locomotion][serialize core::Quaternion orientation(0.0f, 0.0f, 0.0f, 1.0f); cmd.pose = std::make_unique(position, orientation); - auto offset = core::LocomotionCommand::Pack(builder, &cmd); + core::LocomotionCommandRecordT record; + record.data = std::make_unique(cmd); + record.timestamp = std::make_unique(0, 0, 0); + + auto offset = core::LocomotionCommandRecord::Pack(builder, &record); builder.Finish(offset); // Buffer should be reasonably small (under 200 bytes for this simple message). diff --git a/src/core/schema_tests/cpp/test_oak.cpp b/src/core/schema_tests/cpp/test_oak.cpp index d4a3f4099..82994d38b 100644 --- a/src/core/schema_tests/cpp/test_oak.cpp +++ b/src/core/schema_tests/cpp/test_oak.cpp @@ -15,8 +15,7 @@ // ============================================================================= #define VT(field) (field + 2) * 2 -static_assert(core::FrameMetadata::VT_TIMESTAMP == VT(0)); -static_assert(core::FrameMetadata::VT_SEQUENCE_NUMBER == VT(1)); +static_assert(core::FrameMetadata::VT_SEQUENCE_NUMBER == VT(0)); // ============================================================================= // FrameMetadataT Tests (table native type) @@ -25,24 +24,10 @@ TEST_CASE("FrameMetadataT default construction", "[camera][native]") { core::FrameMetadataT metadata; - // Timestamp pointer should be null by default. - CHECK(metadata.timestamp == nullptr); // Integer field should be zero by default. CHECK(metadata.sequence_number == 0); } -TEST_CASE("FrameMetadataT can store timestamp", "[camera][native]") -{ - core::FrameMetadataT metadata; - - // Create timestamp. - metadata.timestamp = std::make_unique(1000000000, 2000000000); - - CHECK(metadata.timestamp != nullptr); - CHECK(metadata.timestamp->device_time() == 1000000000); - CHECK(metadata.timestamp->common_time() == 2000000000); -} - TEST_CASE("FrameMetadataT can store sequence number", "[camera][native]") { core::FrameMetadataT metadata; @@ -57,12 +42,8 @@ TEST_CASE("FrameMetadataT can store full metadata", "[camera][native]") core::FrameMetadataT metadata; // Set all fields. - metadata.timestamp = std::make_unique(3000000000, 4000000000); metadata.sequence_number = 100; - CHECK(metadata.timestamp != nullptr); - CHECK(metadata.timestamp->device_time() == 3000000000); - CHECK(metadata.timestamp->common_time() == 4000000000); CHECK(metadata.sequence_number == 100); } @@ -75,20 +56,20 @@ TEST_CASE("FrameMetadata serialization and deserialization", "[camera][serialize // Create native object. core::FrameMetadataT metadata; - metadata.timestamp = std::make_unique(1234567890, 9876543210); metadata.sequence_number = 999; + core::FrameMetadataRecordT record; + record.data = std::make_unique(metadata); + record.timestamp = std::make_unique(1234567890, 9876543210, 0); + // Serialize. - auto offset = core::FrameMetadata::Pack(builder, &metadata); + auto offset = core::FrameMetadataRecord::Pack(builder, &record); builder.Finish(offset); // Deserialize. - auto* deserialized = flatbuffers::GetRoot(builder.GetBufferPointer()); - - // Verify timestamp. - REQUIRE(deserialized->timestamp() != nullptr); - CHECK(deserialized->timestamp()->device_time() == 1234567890); - CHECK(deserialized->timestamp()->common_time() == 9876543210); + auto* deserialized_record = flatbuffers::GetRoot(builder.GetBufferPointer()); + REQUIRE(deserialized_record->data() != nullptr); + auto* deserialized = deserialized_record->data(); // Verify sequence number. CHECK(deserialized->sequence_number() == 999); @@ -100,42 +81,47 @@ TEST_CASE("FrameMetadata can be unpacked from buffer", "[camera][serialize]") // Create native object with minimal data. core::FrameMetadataT metadata; - metadata.timestamp = std::make_unique(100, 200); metadata.sequence_number = 5; + core::FrameMetadataRecordT record; + record.data = std::make_unique(metadata); + record.timestamp = std::make_unique(100, 200, 0); + // Serialize. - auto offset = core::FrameMetadata::Pack(builder, &metadata); + auto offset = core::FrameMetadataRecord::Pack(builder, &record); builder.Finish(offset); // Deserialize to table. - auto* table = flatbuffers::GetRoot(builder.GetBufferPointer()); + auto* record_table = flatbuffers::GetRoot(builder.GetBufferPointer()); + REQUIRE(record_table->data() != nullptr); // Unpack to native. auto unpacked = std::make_unique(); - table->UnPackTo(unpacked.get()); + record_table->data()->UnPackTo(unpacked.get()); // Verify. - REQUIRE(unpacked->timestamp != nullptr); - CHECK(unpacked->timestamp->device_time() == 100); - CHECK(unpacked->timestamp->common_time() == 200); CHECK(unpacked->sequence_number == 5); } -TEST_CASE("FrameMetadata without timestamp", "[camera][serialize]") +TEST_CASE("FrameMetadataRecord without timestamp", "[camera][serialize]") { flatbuffers::FlatBufferBuilder builder; core::FrameMetadataT metadata; - // timestamp is intentionally left null. metadata.sequence_number = 123; - auto offset = core::FrameMetadata::Pack(builder, &metadata); + core::FrameMetadataRecordT record; + record.data = std::make_unique(metadata); + // timestamp is intentionally left null. + + auto offset = core::FrameMetadataRecord::Pack(builder, &record); builder.Finish(offset); - auto* deserialized = flatbuffers::GetRoot(builder.GetBufferPointer()); + auto* deserialized_record = flatbuffers::GetRoot(builder.GetBufferPointer()); + REQUIRE(deserialized_record->data() != nullptr); - CHECK(deserialized->timestamp() == nullptr); - CHECK(deserialized->sequence_number() == 123); + CHECK(deserialized_record->timestamp() == nullptr); + CHECK(deserialized_record->data()->sequence_number() == 123); } // ============================================================================= @@ -144,11 +130,9 @@ TEST_CASE("FrameMetadata without timestamp", "[camera][serialize]") TEST_CASE("FrameMetadata first frame", "[camera][scenario]") { core::FrameMetadataT metadata; - metadata.timestamp = std::make_unique(0, 1000000); metadata.sequence_number = 0; CHECK(metadata.sequence_number == 0); - CHECK(metadata.timestamp->device_time() == 0); } TEST_CASE("FrameMetadata streaming frames at 30 FPS", "[camera][scenario]") @@ -160,14 +144,9 @@ TEST_CASE("FrameMetadata streaming frames at 30 FPS", "[camera][scenario]") for (int i = 0; i < 5; ++i) { core::FrameMetadataT metadata; - metadata.timestamp = std::make_unique(base_time + i * frame_interval, - base_time + i * frame_interval + 100 // Small offset for - // common_time. - ); metadata.sequence_number = i; CHECK(metadata.sequence_number == i); - CHECK(metadata.timestamp->device_time() == base_time + i * frame_interval); } } @@ -175,18 +154,15 @@ TEST_CASE("FrameMetadata high frequency capture at 120 FPS", "[camera][scenario] { // 120 FPS = 8.33ms interval = 8333333 ns. core::FrameMetadataT metadata; - metadata.timestamp = std::make_unique(8333333, 8333400); metadata.sequence_number = 1; CHECK(metadata.sequence_number == 1); - CHECK(metadata.timestamp->device_time() == 8333333); } TEST_CASE("FrameMetadata sequence rollover scenario", "[camera][scenario]") { // Test near max int32 boundary. core::FrameMetadataT metadata; - metadata.timestamp = std::make_unique(999999999999, 999999999999); metadata.sequence_number = 2147483646; // Near max int32. CHECK(metadata.sequence_number == 2147483646); @@ -208,37 +184,6 @@ TEST_CASE("FrameMetadata with negative sequence number", "[camera][edge]") CHECK(metadata.sequence_number == -1); } -TEST_CASE("FrameMetadata with zero timestamp", "[camera][edge]") -{ - core::FrameMetadataT metadata; - metadata.timestamp = std::make_unique(0, 0); - metadata.sequence_number = 0; - - CHECK(metadata.timestamp->device_time() == 0); - CHECK(metadata.timestamp->common_time() == 0); -} - -TEST_CASE("FrameMetadata with negative timestamp", "[camera][edge]") -{ - // Test with negative timestamp values (valid for relative times). - core::FrameMetadataT metadata; - metadata.timestamp = std::make_unique(-1000, -2000); - - CHECK(metadata.timestamp->device_time() == -1000); - CHECK(metadata.timestamp->common_time() == -2000); -} - -TEST_CASE("FrameMetadata with large timestamp values", "[camera][edge]") -{ - core::FrameMetadataT metadata; - int64_t max_int64 = 9223372036854775807; - metadata.timestamp = std::make_unique(max_int64, max_int64 - 1000); - metadata.sequence_number = 1; - - CHECK(metadata.timestamp->device_time() == max_int64); - CHECK(metadata.timestamp->common_time() == max_int64 - 1000); -} - TEST_CASE("FrameMetadata with max int32 sequence number", "[camera][edge]") { core::FrameMetadataT metadata; @@ -260,10 +205,13 @@ TEST_CASE("FrameMetadata buffer size is reasonable", "[camera][serialize]") flatbuffers::FlatBufferBuilder builder; core::FrameMetadataT metadata; - metadata.timestamp = std::make_unique(0, 0); metadata.sequence_number = 0; - auto offset = core::FrameMetadata::Pack(builder, &metadata); + core::FrameMetadataRecordT record; + record.data = std::make_unique(metadata); + record.timestamp = std::make_unique(0, 0, 0); + + auto offset = core::FrameMetadataRecord::Pack(builder, &record); builder.Finish(offset); // Buffer should be reasonably small (under 100 bytes for this simple message). @@ -283,41 +231,28 @@ TEST_CASE("FrameMetadata can update sequence number", "[camera][native]") } } -TEST_CASE("FrameMetadata can update timestamp", "[camera][native]") -{ - core::FrameMetadataT metadata; - - // Set initial timestamp. - metadata.timestamp = std::make_unique(100, 200); - CHECK(metadata.timestamp->device_time() == 100); - - // Update timestamp. - metadata.timestamp = std::make_unique(300, 400); - CHECK(metadata.timestamp->device_time() == 300); - CHECK(metadata.timestamp->common_time() == 400); -} - TEST_CASE("FrameMetadata roundtrip preserves all data", "[camera][scenario]") { flatbuffers::FlatBufferBuilder builder; // Create comprehensive metadata. core::FrameMetadataT original; - original.timestamp = std::make_unique(5555555555, 6666666666); original.sequence_number = 12345; + core::FrameMetadataRecordT record; + record.data = std::make_unique(original); + record.timestamp = std::make_unique(5555555555, 6666666666, 0); + // Serialize. - auto offset = core::FrameMetadata::Pack(builder, &original); + auto offset = core::FrameMetadataRecord::Pack(builder, &record); builder.Finish(offset); // Unpack to new object. - auto* table = flatbuffers::GetRoot(builder.GetBufferPointer()); + auto* record_table = flatbuffers::GetRoot(builder.GetBufferPointer()); + REQUIRE(record_table->data() != nullptr); core::FrameMetadataT roundtrip; - table->UnPackTo(&roundtrip); + record_table->data()->UnPackTo(&roundtrip); // Verify all data preserved. - REQUIRE(roundtrip.timestamp != nullptr); - CHECK(roundtrip.timestamp->device_time() == 5555555555); - CHECK(roundtrip.timestamp->common_time() == 6666666666); CHECK(roundtrip.sequence_number == 12345); } diff --git a/src/core/schema_tests/cpp/test_pedals.cpp b/src/core/schema_tests/cpp/test_pedals.cpp index 1add1bf2b..a373c6952 100644 --- a/src/core/schema_tests/cpp/test_pedals.cpp +++ b/src/core/schema_tests/cpp/test_pedals.cpp @@ -17,10 +17,9 @@ #define VT(field) (field + 2) * 2 static_assert(core::Generic3AxisPedalOutput::VT_IS_VALID == VT(0)); -static_assert(core::Generic3AxisPedalOutput::VT_TIMESTAMP == VT(1)); -static_assert(core::Generic3AxisPedalOutput::VT_LEFT_PEDAL == VT(2)); -static_assert(core::Generic3AxisPedalOutput::VT_RIGHT_PEDAL == VT(3)); -static_assert(core::Generic3AxisPedalOutput::VT_RUDDER == VT(4)); +static_assert(core::Generic3AxisPedalOutput::VT_LEFT_PEDAL == VT(1)); +static_assert(core::Generic3AxisPedalOutput::VT_RIGHT_PEDAL == VT(2)); +static_assert(core::Generic3AxisPedalOutput::VT_RUDDER == VT(3)); // ============================================================================= // Generic3AxisPedalOutputT Tests (table native type) @@ -31,8 +30,6 @@ TEST_CASE("Generic3AxisPedalOutputT default construction", "[pedals][native]") // is_valid should be false by default. CHECK(output.is_valid == false); - // Timestamp pointer should be null by default. - CHECK(output.timestamp == nullptr); // Float fields should be zero by default. CHECK(output.left_pedal == 0.0f); CHECK(output.right_pedal == 0.0f); @@ -51,18 +48,6 @@ TEST_CASE("Generic3AxisPedalOutputT can handle is_valid value properly", "[pedal CHECK(output.is_valid == true); } -TEST_CASE("Generic3AxisPedalOutputT can store timestamp", "[pedals][native]") -{ - core::Generic3AxisPedalOutputT output; - - // Create timestamp. - output.timestamp = std::make_unique(1000000000, 2000000000); - - CHECK(output.timestamp != nullptr); - CHECK(output.timestamp->device_time() == 1000000000); - CHECK(output.timestamp->common_time() == 2000000000); -} - TEST_CASE("Generic3AxisPedalOutputT can store pedal values", "[pedals][native]") { core::Generic3AxisPedalOutputT output; @@ -82,14 +67,11 @@ TEST_CASE("Generic3AxisPedalOutputT can store full output", "[pedals][native]") // Set all fields. output.is_valid = true; - output.timestamp = std::make_unique(3000000000, 4000000000); output.left_pedal = 1.0f; output.right_pedal = 0.0f; output.rudder = -0.5f; CHECK(output.is_valid == true); - CHECK(output.timestamp != nullptr); - CHECK(output.timestamp->device_time() == 3000000000); CHECK(output.left_pedal == Catch::Approx(1.0f)); CHECK(output.right_pedal == Catch::Approx(0.0f)); CHECK(output.rudder == Catch::Approx(-0.5f)); @@ -105,26 +87,27 @@ TEST_CASE("Generic3AxisPedalOutput serialization and deserialization", "[pedals] // Create native object. core::Generic3AxisPedalOutputT output; output.is_valid = true; - output.timestamp = std::make_unique(1234567890, 9876543210); output.left_pedal = 0.8f; output.right_pedal = 0.2f; output.rudder = 0.33f; + // Create Generic3AxisPedalOutputRecord for serialization (root type) + core::Generic3AxisPedalOutputRecordT record; + record.data = std::make_unique(output); + record.timestamp = std::make_unique(1234567890, 9876543210, 0); + // Serialize. - auto offset = core::Generic3AxisPedalOutput::Pack(builder, &output); + auto offset = core::Generic3AxisPedalOutputRecord::Pack(builder, &record); builder.Finish(offset); // Deserialize. - auto* deserialized = flatbuffers::GetRoot(builder.GetBufferPointer()); + auto* deserialized_record = flatbuffers::GetRoot(builder.GetBufferPointer()); + REQUIRE(deserialized_record->data() != nullptr); + auto* deserialized = deserialized_record->data(); // Verify is_valid. CHECK(deserialized->is_valid() == true); - // Verify timestamp. - REQUIRE(deserialized->timestamp() != nullptr); - CHECK(deserialized->timestamp()->device_time() == 1234567890); - CHECK(deserialized->timestamp()->common_time() == 9876543210); - // Verify pedal values. CHECK(deserialized->left_pedal() == Catch::Approx(0.8f)); CHECK(deserialized->right_pedal() == Catch::Approx(0.2f)); @@ -138,46 +121,52 @@ TEST_CASE("Generic3AxisPedalOutput can be unpacked from buffer", "[pedals][seria // Create native object with minimal data. core::Generic3AxisPedalOutputT output; output.is_valid = true; - output.timestamp = std::make_unique(100, 200); output.left_pedal = 0.5f; + core::Generic3AxisPedalOutputRecordT record; + record.data = std::make_unique(output); + record.timestamp = std::make_unique(100, 200, 0); + // Serialize. - auto offset = core::Generic3AxisPedalOutput::Pack(builder, &output); + auto offset = core::Generic3AxisPedalOutputRecord::Pack(builder, &record); builder.Finish(offset); // Deserialize to table. - auto* table = flatbuffers::GetRoot(builder.GetBufferPointer()); + auto* record_table = flatbuffers::GetRoot(builder.GetBufferPointer()); + REQUIRE(record_table->data() != nullptr); - // Unpack to native. + // Unpack data to native. auto unpacked = std::make_unique(); - table->UnPackTo(unpacked.get()); + record_table->data()->UnPackTo(unpacked.get()); // Verify. CHECK(unpacked->is_valid == true); - REQUIRE(unpacked->timestamp != nullptr); - CHECK(unpacked->timestamp->device_time() == 100); - CHECK(unpacked->timestamp->common_time() == 200); CHECK(unpacked->left_pedal == Catch::Approx(0.5f)); } -TEST_CASE("Generic3AxisPedalOutput without timestamp", "[pedals][serialize]") +TEST_CASE("Generic3AxisPedalOutputRecord with null timestamp", "[pedals][serialize]") { flatbuffers::FlatBufferBuilder builder; core::Generic3AxisPedalOutputT output; - // timestamp is intentionally left null. output.is_valid = true; output.left_pedal = 0.6f; output.right_pedal = 0.4f; output.rudder = 0.0f; - auto offset = core::Generic3AxisPedalOutput::Pack(builder, &output); + core::Generic3AxisPedalOutputRecordT record; + record.data = std::make_unique(output); + // timestamp is intentionally left null. + + auto offset = core::Generic3AxisPedalOutputRecord::Pack(builder, &record); builder.Finish(offset); - auto* deserialized = flatbuffers::GetRoot(builder.GetBufferPointer()); + auto* deserialized_record = flatbuffers::GetRoot(builder.GetBufferPointer()); + REQUIRE(deserialized_record->data() != nullptr); + auto* deserialized = deserialized_record->data(); CHECK(deserialized->is_valid() == true); - CHECK(deserialized->timestamp() == nullptr); + CHECK(deserialized_record->timestamp() == nullptr); CHECK(deserialized->left_pedal() == Catch::Approx(0.6f)); CHECK(deserialized->right_pedal() == Catch::Approx(0.4f)); CHECK(deserialized->rudder() == Catch::Approx(0.0f)); @@ -190,7 +179,6 @@ TEST_CASE("Generic3AxisPedalOutput full forward press", "[pedals][scenario]") { core::Generic3AxisPedalOutputT output; output.is_valid = true; - output.timestamp = std::make_unique(1000000, 1000000); output.left_pedal = 1.0f; output.right_pedal = 1.0f; output.rudder = 0.0f; @@ -205,7 +193,6 @@ TEST_CASE("Generic3AxisPedalOutput left turn with rudder", "[pedals][scenario]") { core::Generic3AxisPedalOutputT output; output.is_valid = true; - output.timestamp = std::make_unique(2000000, 2000000); output.left_pedal = 0.5f; output.right_pedal = 0.5f; output.rudder = -1.0f; // Full left rudder. @@ -217,7 +204,6 @@ TEST_CASE("Generic3AxisPedalOutput right turn with rudder", "[pedals][scenario]" { core::Generic3AxisPedalOutputT output; output.is_valid = true; - output.timestamp = std::make_unique(3000000, 3000000); output.left_pedal = 0.5f; output.right_pedal = 0.5f; output.rudder = 1.0f; // Full right rudder. @@ -230,7 +216,6 @@ TEST_CASE("Generic3AxisPedalOutput differential braking", "[pedals][scenario]") // Simulating differential braking (left brake only). core::Generic3AxisPedalOutputT output; output.is_valid = true; - output.timestamp = std::make_unique(4000000, 4000000); output.left_pedal = 0.0f; // Left brake applied. output.right_pedal = 0.8f; // Right pedal pressed. output.rudder = 0.0f; @@ -243,7 +228,6 @@ TEST_CASE("Generic3AxisPedalOutput neutral position", "[pedals][scenario]") { core::Generic3AxisPedalOutputT output; output.is_valid = true; - output.timestamp = std::make_unique(5000000, 5000000); output.left_pedal = 0.0f; output.right_pedal = 0.0f; output.rudder = 0.0f; @@ -282,17 +266,6 @@ TEST_CASE("Generic3AxisPedalOutput with values greater than 1", "[pedals][edge]" CHECK(output.rudder == Catch::Approx(1.5f)); } -TEST_CASE("Generic3AxisPedalOutput with large timestamp values", "[pedals][edge]") -{ - core::Generic3AxisPedalOutputT output; - output.is_valid = true; - int64_t max_int64 = 9223372036854775807; - output.timestamp = std::make_unique(max_int64, max_int64 - 1000); - output.left_pedal = 0.5f; - - CHECK(output.timestamp->device_time() == max_int64); - CHECK(output.timestamp->common_time() == max_int64 - 1000); -} TEST_CASE("Generic3AxisPedalOutput buffer size is reasonable", "[pedals][serialize]") { @@ -300,12 +273,15 @@ TEST_CASE("Generic3AxisPedalOutput buffer size is reasonable", "[pedals][seriali core::Generic3AxisPedalOutputT output; output.is_valid = true; - output.timestamp = std::make_unique(0, 0); output.left_pedal = 0.0f; output.right_pedal = 0.0f; output.rudder = 0.0f; - auto offset = core::Generic3AxisPedalOutput::Pack(builder, &output); + core::Generic3AxisPedalOutputRecordT record; + record.data = std::make_unique(output); + record.timestamp = std::make_unique(0, 0, 0); + + auto offset = core::Generic3AxisPedalOutputRecord::Pack(builder, &record); builder.Finish(offset); // Buffer should be reasonably small (under 100 bytes for this simple message). @@ -318,19 +294,23 @@ TEST_CASE("Generic3AxisPedalOutput with is_valid false", "[pedals][edge]") core::Generic3AxisPedalOutputT output; output.is_valid = false; - output.timestamp = std::make_unique(1000, 2000); output.left_pedal = 0.5f; output.right_pedal = 0.5f; output.rudder = 0.0f; - auto offset = core::Generic3AxisPedalOutput::Pack(builder, &output); + core::Generic3AxisPedalOutputRecordT record; + record.data = std::make_unique(output); + record.timestamp = std::make_unique(1000, 2000, 0); + + auto offset = core::Generic3AxisPedalOutputRecord::Pack(builder, &record); builder.Finish(offset); - auto* deserialized = flatbuffers::GetRoot(builder.GetBufferPointer()); + auto* deserialized_record = flatbuffers::GetRoot(builder.GetBufferPointer()); + REQUIRE(deserialized_record->data() != nullptr); + auto* deserialized = deserialized_record->data(); // Verify is_valid is false but other data is still present. CHECK(deserialized->is_valid() == false); - REQUIRE(deserialized->timestamp() != nullptr); CHECK(deserialized->left_pedal() == Catch::Approx(0.5f)); CHECK(deserialized->right_pedal() == Catch::Approx(0.5f)); CHECK(deserialized->rudder() == Catch::Approx(0.0f)); diff --git a/src/core/schema_tests/python/test_camera.py b/src/core/schema_tests/python/test_camera.py index 3f515db99..e5694b86c 100644 --- a/src/core/schema_tests/python/test_camera.py +++ b/src/core/schema_tests/python/test_camera.py @@ -4,13 +4,10 @@ """Unit tests for FrameMetadata type in isaacteleop.schema. Tests the following FlatBuffers types: -- FrameMetadata: Table with timestamp and sequence_number +- FrameMetadata: Table with sequence_number """ -from isaacteleop.schema import ( - FrameMetadata, - Timestamp, -) +from isaacteleop.schema import FrameMetadata class TestFrameMetadataConstruction: @@ -20,7 +17,6 @@ def test_default_construction(self): """Test default construction creates FrameMetadata with None/zero fields.""" metadata = FrameMetadata() - assert metadata.timestamp is None assert metadata.sequence_number == 0 def test_repr(self): @@ -31,30 +27,6 @@ def test_repr(self): assert "FrameMetadata" in repr_str -class TestFrameMetadataTimestamp: - """Tests for FrameMetadata timestamp property.""" - - def test_set_timestamp(self): - """Test setting timestamp.""" - metadata = FrameMetadata() - timestamp = Timestamp(device_time=1000000000, common_time=2000000000) - metadata.timestamp = timestamp - - assert metadata.timestamp is not None - assert metadata.timestamp.device_time == 1000000000 - assert metadata.timestamp.common_time == 2000000000 - - def test_large_timestamp_values(self): - """Test with large int64 timestamp values.""" - metadata = FrameMetadata() - max_int64 = 9223372036854775807 - timestamp = Timestamp(device_time=max_int64, common_time=max_int64 - 1000) - metadata.timestamp = timestamp - - assert metadata.timestamp.device_time == max_int64 - assert metadata.timestamp.common_time == max_int64 - 1000 - - class TestFrameMetadataSequenceNumber: """Tests for FrameMetadata sequence_number property.""" @@ -94,12 +66,8 @@ class TestFrameMetadataCombined: def test_full_metadata(self): """Test with all fields set.""" metadata = FrameMetadata() - metadata.timestamp = Timestamp(device_time=1000, common_time=2000) metadata.sequence_number = 100 - assert metadata.timestamp is not None - assert metadata.timestamp.device_time == 1000 - assert metadata.timestamp.common_time == 2000 assert metadata.sequence_number == 100 @@ -109,26 +77,15 @@ class TestFrameMetadataScenarios: def test_first_frame(self): """Test metadata for the first captured frame.""" metadata = FrameMetadata() - metadata.timestamp = Timestamp(device_time=0, common_time=1000000) metadata.sequence_number = 0 assert metadata.sequence_number == 0 - assert metadata.timestamp.device_time == 0 def test_streaming_frames(self): """Test metadata for sequential streaming frames.""" frames = [] - base_device_time = 1000000000 # 1 second in nanoseconds. - frame_interval = 33333333 # ~30 FPS in nanoseconds. - for i in range(5): metadata = FrameMetadata() - metadata.timestamp = Timestamp( - device_time=base_device_time + i * frame_interval, - common_time=base_device_time - + i * frame_interval - + 100, # Small offset. - ) metadata.sequence_number = i frames.append(metadata) @@ -136,17 +93,9 @@ def test_streaming_frames(self): for i, frame in enumerate(frames): assert frame.sequence_number == i - # Verify timestamps are increasing. - for i in range(1, len(frames)): - assert frames[i].timestamp.device_time > frames[i - 1].timestamp.device_time - def test_high_frequency_capture(self): """Test metadata for high-frequency capture (e.g., 120 FPS).""" metadata = FrameMetadata() - metadata.timestamp = Timestamp( - device_time=8333333, # ~8.3ms (120 FPS interval) - common_time=8333400, - ) metadata.sequence_number = 1 assert metadata.sequence_number == 1 @@ -154,9 +103,6 @@ def test_high_frequency_capture(self): def test_sequence_rollover_scenario(self): """Test metadata near sequence number boundaries.""" metadata = FrameMetadata() - metadata.timestamp = Timestamp( - device_time=999999999999, common_time=999999999999 - ) metadata.sequence_number = 2147483646 # Near max int32 assert metadata.sequence_number == 2147483646 @@ -169,22 +115,6 @@ def test_sequence_rollover_scenario(self): class TestFrameMetadataEdgeCases: """Edge case tests for FrameMetadata table.""" - def test_zero_timestamp(self): - """Test with zero timestamp values.""" - metadata = FrameMetadata() - metadata.timestamp = Timestamp(device_time=0, common_time=0) - - assert metadata.timestamp.device_time == 0 - assert metadata.timestamp.common_time == 0 - - def test_negative_timestamp(self): - """Test with negative timestamp values (valid for relative times).""" - metadata = FrameMetadata() - metadata.timestamp = Timestamp(device_time=-1000, common_time=-2000) - - assert metadata.timestamp.device_time == -1000 - assert metadata.timestamp.common_time == -2000 - def test_overwrite_sequence_number(self): """Test overwriting sequence number.""" metadata = FrameMetadata() @@ -193,31 +123,19 @@ def test_overwrite_sequence_number(self): assert metadata.sequence_number == 20 - def test_overwrite_timestamp(self): - """Test overwriting timestamp.""" - metadata = FrameMetadata() - metadata.timestamp = Timestamp(device_time=100, common_time=200) - metadata.timestamp = Timestamp(device_time=300, common_time=400) - - assert metadata.timestamp.device_time == 300 - assert metadata.timestamp.common_time == 400 - - def test_repr_with_timestamp(self): - """Test __repr__ with timestamp set.""" + def test_repr_with_sequence_number(self): + """Test __repr__ with sequence_number set.""" metadata = FrameMetadata() - metadata.timestamp = Timestamp(device_time=123, common_time=456) metadata.sequence_number = 789 repr_str = repr(metadata) assert "FrameMetadata" in repr_str - assert "timestamp" in repr_str assert "sequence_number" in repr_str - def test_repr_without_timestamp(self): - """Test __repr__ without timestamp set.""" + def test_repr_with_default(self): + """Test __repr__ with default values.""" metadata = FrameMetadata() metadata.sequence_number = 42 repr_str = repr(metadata) assert "FrameMetadata" in repr_str - assert "None" in repr_str or "timestamp" in repr_str diff --git a/src/core/schema_tests/python/test_controller.py b/src/core/schema_tests/python/test_controller.py index e08def217..289caed11 100644 --- a/src/core/schema_tests/python/test_controller.py +++ b/src/core/schema_tests/python/test_controller.py @@ -6,9 +6,8 @@ Tests the following FlatBuffers types: - ControllerInputState: Struct with button and axis inputs (immutable) - ControllerPose: Struct with pose and validity (immutable) -- Timestamp: Struct with device and common time timestamps (immutable) +- DeviceDataTimestamp: Struct with device and common time timestamps (immutable) - ControllerSnapshot: Struct representing complete controller state (immutable) -- ControllerData: Root table with both left and right controllers """ import pytest @@ -16,9 +15,8 @@ from isaacteleop.schema import ( ControllerInputState, ControllerPose, - Timestamp, + DeviceDataTimestamp, ControllerSnapshot, - ControllerData, Pose, Point, Quaternion, @@ -91,40 +89,48 @@ def test_repr(self): assert "primary=True" in repr_str -class TestTimestamp: - """Tests for Timestamp struct (immutable).""" +class TestDeviceDataTimestamp: + """Tests for DeviceDataTimestamp struct (immutable).""" def test_default_construction(self): - """Test default construction creates Timestamp with default values.""" - timestamp = Timestamp() + """Test default construction creates DeviceDataTimestamp with default values.""" + timestamp = DeviceDataTimestamp() assert timestamp is not None - assert timestamp.device_time == 0 - assert timestamp.common_time == 0 + assert timestamp.sample_time_device_clock == 0 + assert timestamp.sample_time_common_clock == 0 def test_set_timestamp_values(self): """Test constructing with timestamp values.""" - timestamp = Timestamp(device_time=1234567890123456789, common_time=9876543210) + timestamp = DeviceDataTimestamp( + sample_time_device_clock=1234567890123456789, + sample_time_common_clock=9876543210, + ) - assert timestamp.device_time == 1234567890123456789 - assert timestamp.common_time == 9876543210 + assert timestamp.sample_time_device_clock == 1234567890123456789 + assert timestamp.sample_time_common_clock == 9876543210 def test_large_timestamp_values(self): """Test with large int64 timestamp values.""" max_int64 = 9223372036854775807 - timestamp = Timestamp(device_time=max_int64, common_time=max_int64 - 1000) + timestamp = DeviceDataTimestamp( + sample_time_device_clock=max_int64, + sample_time_common_clock=max_int64 - 1000, + ) - assert timestamp.device_time == max_int64 - assert timestamp.common_time == max_int64 - 1000 + assert timestamp.sample_time_device_clock == max_int64 + assert timestamp.sample_time_common_clock == max_int64 - 1000 def test_repr(self): """Test __repr__ method.""" - timestamp = Timestamp(device_time=1000, common_time=2000) + timestamp = DeviceDataTimestamp( + sample_time_device_clock=1000, sample_time_common_clock=2000 + ) repr_str = repr(timestamp) - assert "Timestamp" in repr_str - assert "device_time=1000" in repr_str - assert "common_time=2000" in repr_str + assert "DeviceDataTimestamp" in repr_str + assert "sample_time_device_clock=1000" in repr_str + assert "sample_time_common_clock=2000" in repr_str class TestControllerPose: @@ -205,7 +211,6 @@ def test_default_construction(self): assert snapshot.grip_pose is not None assert snapshot.aim_pose is not None assert snapshot.inputs is not None - assert snapshot.timestamp is not None assert snapshot.is_active is False def test_complete_snapshot(self): @@ -232,11 +237,8 @@ def test_complete_snapshot(self): trigger_value=1.0, ) - # Create timestamp - timestamp = Timestamp(device_time=1000000000, common_time=2000000000) - # Create snapshot - snapshot = ControllerSnapshot(grip_pose, aim_pose, inputs, True, timestamp) + snapshot = ControllerSnapshot(grip_pose, aim_pose, inputs, True) # Verify all fields assert snapshot.grip_pose.is_valid is True @@ -246,8 +248,6 @@ def test_complete_snapshot(self): assert snapshot.inputs.primary_click is True assert snapshot.inputs.trigger_value == pytest.approx(1.0) assert snapshot.is_active is True - assert snapshot.timestamp.device_time == 1000000000 - assert snapshot.timestamp.common_time == 2000000000 def test_repr_with_default(self): """Test __repr__ with default values.""" @@ -258,25 +258,6 @@ def test_repr_with_default(self): assert "is_active=False" in repr_str -class TestControllerData: - """Tests for ControllerData table.""" - - def test_default_construction(self): - """Test default construction creates ControllerData with None controllers.""" - controller_data = ControllerData() - - assert controller_data is not None - assert controller_data.left_controller is None - assert controller_data.right_controller is None - - def test_repr(self): - """Test __repr__ method.""" - controller_data = ControllerData() - repr_str = repr(controller_data) - - assert "ControllerData" in repr_str - - class TestControllerIntegration: """Integration tests combining multiple controller types.""" @@ -297,10 +278,7 @@ def test_left_and_right_different_states(self): squeeze_value=0.0, trigger_value=0.5, ) - left_timestamp = Timestamp(device_time=1000, common_time=2000) - left_snapshot = ControllerSnapshot( - left_grip, left_aim, left_inputs, True, left_timestamp - ) + left_snapshot = ControllerSnapshot(left_grip, left_aim, left_inputs, True) # Create right controller (inactive) right_snapshot = ControllerSnapshot() @@ -385,14 +363,18 @@ def test_invalid_pose(self): def test_zero_timestamp(self): """Test with zero timestamp values.""" - timestamp = Timestamp(device_time=0, common_time=0) + timestamp = DeviceDataTimestamp( + sample_time_device_clock=0, sample_time_common_clock=0 + ) - assert timestamp.device_time == 0 - assert timestamp.common_time == 0 + assert timestamp.sample_time_device_clock == 0 + assert timestamp.sample_time_common_clock == 0 def test_negative_timestamp(self): """Test with negative timestamp values (valid for relative times).""" - timestamp = Timestamp(device_time=-1000, common_time=-2000) + timestamp = DeviceDataTimestamp( + sample_time_device_clock=-1000, sample_time_common_clock=-2000 + ) - assert timestamp.device_time == -1000 - assert timestamp.common_time == -2000 + assert timestamp.sample_time_device_clock == -1000 + assert timestamp.sample_time_common_clock == -2000 diff --git a/src/core/schema_tests/python/test_full_body.py b/src/core/schema_tests/python/test_full_body.py index 2b1ec5d57..33f9e0fd9 100644 --- a/src/core/schema_tests/python/test_full_body.py +++ b/src/core/schema_tests/python/test_full_body.py @@ -6,7 +6,6 @@ FullBodyPosePicoT is a FlatBuffers table (read-only from Python) that represents full body pose data: - joints: BodyJointsPico struct containing 24 BodyJointPose entries (XR_BD_body_tracking) - is_active: Whether the body tracking is active -- timestamp: Timestamp struct with device and common time BodyJointsPico is a struct with a fixed-size array of 24 BodyJointPose entries. @@ -153,7 +152,6 @@ def test_default_construction(self): assert body_pose is not None assert body_pose.joints is None assert body_pose.is_active is False - assert body_pose.timestamp is None class TestFullBodyPosePicoTRepr: diff --git a/src/core/schema_tests/python/test_hand.py b/src/core/schema_tests/python/test_hand.py index f9cccf634..43bbdccc5 100644 --- a/src/core/schema_tests/python/test_hand.py +++ b/src/core/schema_tests/python/test_hand.py @@ -6,7 +6,6 @@ HandPoseT is a FlatBuffers table (read-only from Python) that represents hand pose data: - joints: HandJoints struct containing 26 HandJointPose entries (XR_HAND_JOINT_COUNT_EXT) - is_active: Whether the hand pose data is active -- timestamp: Timestamp struct with device and common time HandJoints is a struct with a fixed-size array of 26 HandJointPose entries. @@ -156,7 +155,6 @@ def test_default_construction(self): assert hand_pose is not None assert hand_pose.joints is None assert hand_pose.is_active is False - assert hand_pose.timestamp is None class TestHandPoseTRepr: diff --git a/src/core/schema_tests/python/test_head.py b/src/core/schema_tests/python/test_head.py index df47e8abe..51ce94303 100644 --- a/src/core/schema_tests/python/test_head.py +++ b/src/core/schema_tests/python/test_head.py @@ -6,7 +6,6 @@ HeadPoseT is a FlatBuffers table (read-only from Python) that represents head pose data: - pose: The Pose struct (position and orientation) - is_valid: Whether the head pose data is valid -- timestamp: Timestamp struct with device and common time Note: Python code should only READ this data (created by C++ trackers), not modify it. """ @@ -24,7 +23,6 @@ def test_default_construction(self): assert head_pose is not None assert head_pose.pose is None assert head_pose.is_valid is False - assert head_pose.timestamp is None class TestHeadPoseTRepr: diff --git a/src/core/schema_tests/python/test_locomotion.py b/src/core/schema_tests/python/test_locomotion.py index f60bc069a..300c01af6 100644 --- a/src/core/schema_tests/python/test_locomotion.py +++ b/src/core/schema_tests/python/test_locomotion.py @@ -5,7 +5,7 @@ Tests the following FlatBuffers types: - Twist: Struct with linear and angular velocity (Point types) -- LocomotionCommand: Table with timestamp, velocity (Twist), and pose +- LocomotionCommand: Table with velocity (Twist) and pose """ import pytest @@ -16,7 +16,6 @@ Point, Pose, Quaternion, - Timestamp, ) @@ -144,7 +143,6 @@ def test_default_construction(self): """Test default construction creates LocomotionCommand with None fields.""" cmd = LocomotionCommand() - assert cmd.timestamp is None assert cmd.velocity is None assert cmd.pose is None @@ -156,30 +154,6 @@ def test_repr(self): assert "LocomotionCommand" in repr_str -class TestLocomotionCommandTimestamp: - """Tests for LocomotionCommand timestamp property.""" - - def test_set_timestamp(self): - """Test setting timestamp.""" - cmd = LocomotionCommand() - timestamp = Timestamp(device_time=1000000000, common_time=2000000000) - cmd.timestamp = timestamp - - assert cmd.timestamp is not None - assert cmd.timestamp.device_time == 1000000000 - assert cmd.timestamp.common_time == 2000000000 - - def test_large_timestamp_values(self): - """Test with large int64 timestamp values.""" - cmd = LocomotionCommand() - max_int64 = 9223372036854775807 - timestamp = Timestamp(device_time=max_int64, common_time=max_int64 - 1000) - cmd.timestamp = timestamp - - assert cmd.timestamp.device_time == max_int64 - assert cmd.timestamp.common_time == max_int64 - 1000 - - class TestLocomotionCommandVelocity: """Tests for LocomotionCommand velocity property.""" @@ -245,31 +219,25 @@ class TestLocomotionCommandCombined: def test_velocity_only(self): """Test command with only velocity set (no pose).""" cmd = LocomotionCommand() - cmd.timestamp = Timestamp(device_time=1000, common_time=2000) cmd.velocity = Twist(Point(1.0, 0.0, 0.0), Point(0.0, 0.0, 0.0)) - assert cmd.timestamp is not None assert cmd.velocity is not None assert cmd.pose is None def test_pose_only(self): """Test command with only pose set (no velocity).""" cmd = LocomotionCommand() - cmd.timestamp = Timestamp(device_time=3000, common_time=4000) cmd.pose = Pose(Point(0.0, 0.0, -0.1), Quaternion(0.0, 0.0, 0.0, 1.0)) - assert cmd.timestamp is not None assert cmd.velocity is None assert cmd.pose is not None def test_both_velocity_and_pose(self): """Test command with both velocity and pose set.""" cmd = LocomotionCommand() - cmd.timestamp = Timestamp(device_time=5000, common_time=6000) cmd.velocity = Twist(Point(0.5, 0.0, 0.0), Point(0.0, 0.0, 0.1)) cmd.pose = Pose(Point(0.0, 0.0, 0.05), Quaternion(0.0, 0.0, 0.0, 1.0)) - assert cmd.timestamp is not None assert cmd.velocity is not None assert cmd.pose is not None assert cmd.velocity.linear.x == pytest.approx(0.5) @@ -282,8 +250,6 @@ class TestLocomotionCommandFootPedalScenarios: def test_forward_motion(self): """Test typical forward motion command from foot pedal.""" cmd = LocomotionCommand() - cmd.timestamp = Timestamp(device_time=1000000, common_time=1000000) - # Forward velocity from foot pedal cmd.velocity = Twist(Point(0.5, 0.0, 0.0), Point(0.0, 0.0, 0.0)) assert cmd.velocity.linear.x == pytest.approx(0.5) @@ -293,8 +259,6 @@ def test_forward_motion(self): def test_turning_motion(self): """Test turning motion command from foot pedal.""" cmd = LocomotionCommand() - cmd.timestamp = Timestamp(device_time=2000000, common_time=2000000) - # Forward + turning cmd.velocity = Twist(Point(0.3, 0.0, 0.0), Point(0.0, 0.0, 0.5)) assert cmd.velocity.linear.x == pytest.approx(0.3) @@ -303,8 +267,6 @@ def test_turning_motion(self): def test_squat_mode(self): """Test squat mode command (vertical mode) from foot pedal.""" cmd = LocomotionCommand() - cmd.timestamp = Timestamp(device_time=3000000, common_time=3000000) - # Squat down pose cmd.pose = Pose(Point(0.0, 0.0, -0.15), Quaternion(0.0, 0.0, 0.0, 1.0)) assert cmd.pose.position.z == pytest.approx(-0.15) @@ -312,7 +274,6 @@ def test_squat_mode(self): def test_stationary(self): """Test stationary command (zero velocity).""" cmd = LocomotionCommand() - cmd.timestamp = Timestamp(device_time=4000000, common_time=4000000) cmd.velocity = Twist(Point(0.0, 0.0, 0.0), Point(0.0, 0.0, 0.0)) assert cmd.velocity.linear.x == pytest.approx(0.0) @@ -356,22 +317,6 @@ def test_max_velocities(self): class TestLocomotionCommandEdgeCases: """Edge case tests for LocomotionCommand table.""" - def test_zero_timestamp(self): - """Test with zero timestamp values.""" - cmd = LocomotionCommand() - cmd.timestamp = Timestamp(device_time=0, common_time=0) - - assert cmd.timestamp.device_time == 0 - assert cmd.timestamp.common_time == 0 - - def test_negative_timestamp(self): - """Test with negative timestamp values (valid for relative times).""" - cmd = LocomotionCommand() - cmd.timestamp = Timestamp(device_time=-1000, common_time=-2000) - - assert cmd.timestamp.device_time == -1000 - assert cmd.timestamp.common_time == -2000 - def test_overwrite_velocity(self): """Test overwriting velocity field.""" cmd = LocomotionCommand() diff --git a/src/core/schema_tests/python/test_pedals.py b/src/core/schema_tests/python/test_pedals.py index a09834413..f54a230b4 100644 --- a/src/core/schema_tests/python/test_pedals.py +++ b/src/core/schema_tests/python/test_pedals.py @@ -4,15 +4,12 @@ """Unit tests for Generic3AxisPedalOutput type in isaacteleop.schema. Tests the following FlatBuffers types: -- Generic3AxisPedalOutput: Table with is_valid, timestamp, left_pedal, right_pedal, and rudder +- Generic3AxisPedalOutput: Table with is_valid, left_pedal, right_pedal, and rudder """ import pytest -from isaacteleop.schema import ( - Generic3AxisPedalOutput, - Timestamp, -) +from isaacteleop.schema import Generic3AxisPedalOutput class TestGeneric3AxisPedalOutputConstruction: @@ -23,7 +20,6 @@ def test_default_construction(self): output = Generic3AxisPedalOutput() assert output.is_valid is False - assert output.timestamp is None assert output.left_pedal == 0.0 assert output.right_pedal == 0.0 assert output.rudder == 0.0 @@ -58,30 +54,6 @@ def test_set_is_valid_to_false(self): assert output.is_valid is False -class TestGeneric3AxisPedalOutputTimestamp: - """Tests for Generic3AxisPedalOutput timestamp property.""" - - def test_set_timestamp(self): - """Test setting timestamp.""" - output = Generic3AxisPedalOutput() - timestamp = Timestamp(device_time=1000000000, common_time=2000000000) - output.timestamp = timestamp - - assert output.timestamp is not None - assert output.timestamp.device_time == 1000000000 - assert output.timestamp.common_time == 2000000000 - - def test_large_timestamp_values(self): - """Test with large int64 timestamp values.""" - output = Generic3AxisPedalOutput() - max_int64 = 9223372036854775807 - timestamp = Timestamp(device_time=max_int64, common_time=max_int64 - 1000) - output.timestamp = timestamp - - assert output.timestamp.device_time == max_int64 - assert output.timestamp.common_time == max_int64 - 1000 - - class TestGeneric3AxisPedalOutputPedals: """Tests for Generic3AxisPedalOutput pedal properties.""" @@ -125,14 +97,11 @@ def test_full_output(self): """Test with all fields set.""" output = Generic3AxisPedalOutput() output.is_valid = True - output.timestamp = Timestamp(device_time=1000, common_time=2000) output.left_pedal = 1.0 output.right_pedal = 0.0 output.rudder = -0.5 assert output.is_valid is True - assert output.timestamp is not None - assert output.timestamp.device_time == 1000 assert output.left_pedal == pytest.approx(1.0) assert output.right_pedal == pytest.approx(0.0) assert output.rudder == pytest.approx(-0.5) @@ -145,7 +114,6 @@ def test_full_forward_press(self): """Test full forward press on both pedals.""" output = Generic3AxisPedalOutput() output.is_valid = True - output.timestamp = Timestamp(device_time=1000000, common_time=1000000) output.left_pedal = 1.0 output.right_pedal = 1.0 output.rudder = 0.0 @@ -159,7 +127,6 @@ def test_left_turn_with_rudder(self): """Test left turn using rudder.""" output = Generic3AxisPedalOutput() output.is_valid = True - output.timestamp = Timestamp(device_time=2000000, common_time=2000000) output.left_pedal = 0.5 output.right_pedal = 0.5 output.rudder = -1.0 # Full left rudder. @@ -170,7 +137,6 @@ def test_right_turn_with_rudder(self): """Test right turn using rudder.""" output = Generic3AxisPedalOutput() output.is_valid = True - output.timestamp = Timestamp(device_time=3000000, common_time=3000000) output.left_pedal = 0.5 output.right_pedal = 0.5 output.rudder = 1.0 # Full right rudder. @@ -181,7 +147,6 @@ def test_differential_braking(self): """Test differential braking scenario.""" output = Generic3AxisPedalOutput() output.is_valid = True - output.timestamp = Timestamp(device_time=4000000, common_time=4000000) output.left_pedal = 0.0 # Left brake applied. output.right_pedal = 0.8 # Right pedal pressed. output.rudder = 0.0 @@ -193,7 +158,6 @@ def test_neutral_position(self): """Test neutral/idle position.""" output = Generic3AxisPedalOutput() output.is_valid = True - output.timestamp = Timestamp(device_time=5000000, common_time=5000000) output.left_pedal = 0.0 output.right_pedal = 0.0 output.rudder = 0.0 @@ -206,22 +170,6 @@ def test_neutral_position(self): class TestGeneric3AxisPedalOutputEdgeCases: """Edge case tests for Generic3AxisPedalOutput table.""" - def test_zero_timestamp(self): - """Test with zero timestamp values.""" - output = Generic3AxisPedalOutput() - output.timestamp = Timestamp(device_time=0, common_time=0) - - assert output.timestamp.device_time == 0 - assert output.timestamp.common_time == 0 - - def test_negative_timestamp(self): - """Test with negative timestamp values (valid for relative times).""" - output = Generic3AxisPedalOutput() - output.timestamp = Timestamp(device_time=-1000, common_time=-2000) - - assert output.timestamp.device_time == -1000 - assert output.timestamp.common_time == -2000 - def test_negative_pedal_values(self): """Test with negative pedal values (edge case).""" output = Generic3AxisPedalOutput() @@ -268,27 +216,16 @@ def test_overwrite_rudder(self): assert output.rudder == pytest.approx(-0.8) - def test_overwrite_timestamp(self): - """Test overwriting timestamp.""" - output = Generic3AxisPedalOutput() - output.timestamp = Timestamp(device_time=100, common_time=200) - output.timestamp = Timestamp(device_time=300, common_time=400) - - assert output.timestamp.device_time == 300 - assert output.timestamp.common_time == 400 - def test_is_valid_false_with_data(self): """Test is_valid=False doesn't prevent storing data.""" output = Generic3AxisPedalOutput() output.is_valid = False - output.timestamp = Timestamp(device_time=1000, common_time=2000) output.left_pedal = 0.5 output.right_pedal = 0.5 output.rudder = 0.0 # Data is present even when is_valid is False. assert output.is_valid is False - assert output.timestamp is not None assert output.left_pedal == pytest.approx(0.5) assert output.right_pedal == pytest.approx(0.5) assert output.rudder == pytest.approx(0.0) diff --git a/src/core/synchronization/CMakeLists.txt b/src/core/synchronization/CMakeLists.txt new file mode 100644 index 000000000..72dd1d50f --- /dev/null +++ b/src/core/synchronization/CMakeLists.txt @@ -0,0 +1,6 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.20) + +add_subdirectory(clock_sync) diff --git a/src/core/synchronization/clock_sync/CMakeLists.txt b/src/core/synchronization/clock_sync/CMakeLists.txt new file mode 100644 index 000000000..01d2d550c --- /dev/null +++ b/src/core/synchronization/clock_sync/CMakeLists.txt @@ -0,0 +1,102 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.20) + +# ============================================================================== +# Clock Sync - types (header-only) + client library + executables + Python +# ============================================================================== + +# Header-only library for shared types (clock_types.hpp, platform.hpp) +add_library(clock_sync_types INTERFACE) + +target_include_directories(clock_sync_types + INTERFACE + $ + $ +) + +# On Windows, link Winsock2 and the high-resolution timer API +if(WIN32) + target_link_libraries(clock_sync_types INTERFACE ws2_32 winmm) +endif() + +add_library(utilities::clock_sync_types ALIAS clock_sync_types) + +# Static library for the ClockClient class +add_library(clock_sync_client STATIC + clock_client_lib.cpp +) + +target_link_libraries(clock_sync_client + PUBLIC + utilities::clock_sync_types +) + +target_include_directories(clock_sync_client + PUBLIC + $ + $ +) + +add_library(utilities::clock_sync_client ALIAS clock_sync_client) + +# Clock server executable +add_executable(clock_server clock_server.cpp) +target_link_libraries(clock_server PRIVATE utilities::clock_sync_types) + +# Clock client executable (monotonic timestamps, no XR) +add_executable(clock_client clock_client.cpp) +target_link_libraries(clock_client PRIVATE utilities::clock_sync_client) + +# ------------------------------------------------------------------------------ +# Optional: XR time translator library + XR client executable +# Requires oxr::oxr_core (OpenXR session management + time conversion utilities) +# ------------------------------------------------------------------------------ + +set(CLOCK_SYNC_HAS_XR OFF) + +if(TARGET oxr_core) + add_library(clock_sync_xr STATIC + xr_clock_translator.cpp + ) + + target_link_libraries(clock_sync_xr + PUBLIC + utilities::clock_sync_types + oxr::oxr_core + ) + + target_include_directories(clock_sync_xr + PUBLIC + $ + $ + ) + + add_library(utilities::clock_sync_xr ALIAS clock_sync_xr) + + add_executable(clock_client_xr clock_client_xr.cpp) + target_link_libraries(clock_client_xr + PRIVATE + utilities::clock_sync_client + utilities::clock_sync_xr + ) + + set(CLOCK_SYNC_HAS_XR ON) +endif() + +# Install +set(_CLOCK_SYNC_INSTALL_TARGETS clock_server clock_client clock_sync_client) +if(CLOCK_SYNC_HAS_XR) + list(APPEND _CLOCK_SYNC_INSTALL_TARGETS clock_client_xr clock_sync_xr) +endif() + +install(TARGETS ${_CLOCK_SYNC_INSTALL_TARGETS} + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + INCLUDES DESTINATION include +) + +install(DIRECTORY inc/clock_sync + DESTINATION include +) diff --git a/src/core/synchronization/clock_sync/clock_client.cpp b/src/core/synchronization/clock_sync/clock_client.cpp new file mode 100644 index 000000000..6a0168dd8 --- /dev/null +++ b/src/core/synchronization/clock_sync/clock_client.cpp @@ -0,0 +1,152 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "clock_sync/clock_client.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace +{ + +volatile sig_atomic_t g_running = 1; + +void signal_handler(int /*sig*/) +{ + g_running = 0; +} + +void print_header() +{ + std::cout << std::left << std::setw(8) << "seq" << std::setw(18) << "rtt_us" << std::setw(18) << "offset_us" + << std::setw(18) << "t1_ns" << std::setw(18) << "t2_ns" << std::setw(18) << "t3_ns" << std::setw(18) + << "t4_ns" + << "\n"; + std::cout << std::string(116, '-') << "\n"; +} + +void print_measurement(uint64_t seq, const core::ClockMeasurement& m) +{ + std::cout << std::left << std::setw(8) << seq << std::setw(18) << std::fixed << std::setprecision(3) + << core::ns_to_sec(m.rtt()) * 1e6 << std::setw(18) << core::ns_to_sec(m.offset()) * 1e6 << std::setw(18) + << m.t1 << std::setw(18) << m.t2 << std::setw(18) << m.t3 << std::setw(18) << m.t4 << "\n"; +} + +void print_summary(const std::vector& measurements, uint64_t sent) +{ + if (measurements.empty()) + return; + + core::clock_ns_t min_rtt = measurements[0].rtt(); + core::clock_ns_t max_rtt = min_rtt; + double sum_rtt = 0; + double sum_offset = 0; + + for (const auto& m : measurements) + { + core::clock_ns_t r = m.rtt(); + if (r < min_rtt) + min_rtt = r; + if (r > max_rtt) + max_rtt = r; + sum_rtt += static_cast(r); + sum_offset += static_cast(m.offset()); + } + + double avg_rtt = sum_rtt / static_cast(measurements.size()); + double avg_offset = sum_offset / static_cast(measurements.size()); + uint64_t lost = sent - measurements.size(); + + std::cout << "\n--- Summary ---\n"; + std::cout << "Packets: " << sent << " sent, " << measurements.size() << " received, " << lost << " lost\n"; + std::cout << std::fixed << std::setprecision(3); + std::cout << "RTT (us): min=" << core::ns_to_sec(min_rtt) * 1e6 << " avg=" << avg_rtt / 1e3 + << " max=" << core::ns_to_sec(max_rtt) * 1e6 << "\n"; + std::cout << "Offset (us): avg=" << avg_offset / 1e3 << "\n"; +} + +} // namespace + +int main(int argc, char* argv[]) +{ + if (argc < 2) + { + std::cerr << "Usage: clock_client [port] [interval_ms] [count]\n" + << " Client uses CLOCK_MONOTONIC; server uses CLOCK_MONOTONIC_RAW.\n"; + return 1; + } + + const char* server_ip = argv[1]; + uint16_t port = (argc > 2) ? static_cast(std::atoi(argv[2])) : core::kDefaultClockPort; + int interval_ms = (argc > 3) ? std::atoi(argv[3]) : 1000; + int count = (argc > 4) ? std::atoi(argv[4]) : 0; + + std::signal(SIGINT, signal_handler); + std::signal(SIGTERM, signal_handler); + + std::vector measurements; + std::mutex mu; + std::condition_variable cv; + uint64_t seq = 0; + + auto on_measurement = [&](const core::ClockMeasurement& m) + { + std::lock_guard lock(mu); + measurements.push_back(m); + print_measurement(seq, m); + ++seq; + cv.notify_one(); + }; + + std::shared_ptr client; + try + { + client = core::ClockClient::Create(server_ip, port, on_measurement); + } + catch (const std::exception& e) + { + std::cerr << e.what() << std::endl; + return 1; + } + + std::cout << "[ClockClient] Pinging " << server_ip << ":" << port << " every " << interval_ms << " ms" + << " (client: CLOCK_MONOTONIC, server: CLOCK_MONOTONIC_RAW)\n\n"; + + print_header(); + + uint64_t sent = 0; + + if (count > 0) + { + client->start(interval_ms, count); + sent = static_cast(count); + + std::unique_lock lock(mu); + cv.wait_for(lock, std::chrono::milliseconds(count * interval_ms + 2000), + [&] { return seq >= static_cast(count) || !g_running; }); + } + else + { + client->start(interval_ms, 0); + + while (g_running) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + client->stop(); + sent = seq + 1; + } + + { + std::lock_guard lock(mu); + print_summary(measurements, sent); + } + + return 0; +} diff --git a/src/core/synchronization/clock_sync/clock_client_lib.cpp b/src/core/synchronization/clock_sync/clock_client_lib.cpp new file mode 100644 index 000000000..84e30b8a4 --- /dev/null +++ b/src/core/synchronization/clock_sync/clock_client_lib.cpp @@ -0,0 +1,191 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "clock_sync/clock_client.hpp" + +#include +#include +#include + +namespace core +{ + +// ============================================================================ +// Factory +// ============================================================================ + +std::shared_ptr ClockClient::Create(const std::string& server_ip, + uint16_t port, + MeasurementCallback callback, + ClockTranslator translator) +{ + auto client = std::shared_ptr(new ClockClient()); + client->translator_ = std::move(translator); + client->callback_ = std::move(callback); + client->initialize(server_ip, port); + return client; +} + +// ============================================================================ +// Lifecycle +// ============================================================================ + +void ClockClient::initialize(const std::string& server_ip, uint16_t port) +{ + ensure_winsock(); + + sock_ = socket(AF_INET, SOCK_DGRAM, 0); + if (sock_ == kInvalidSocket) + { + throw std::runtime_error("[ClockClient] Failed to create socket"); + } + + set_recv_timeout(sock_, 100); + + std::memset(&server_addr_, 0, sizeof(server_addr_)); + server_addr_.sin_family = AF_INET; + server_addr_.sin_port = htons(port); + if (inet_pton(AF_INET, server_ip.c_str(), &server_addr_.sin_addr) <= 0) + { + close_socket(sock_); + sock_ = kInvalidSocket; + throw std::runtime_error("[ClockClient] Invalid server address: " + server_ip); + } + + alive_.store(true, std::memory_order_release); + receiver_ = std::thread(&ClockClient::receiver_loop, this); +} + +ClockClient::~ClockClient() +{ + stop(); + + alive_.store(false, std::memory_order_release); + if (receiver_.joinable()) + { + receiver_.join(); + } + + if (sock_ != kInvalidSocket) + { + close_socket(sock_); + } +} + +// ============================================================================ +// Callback +// ============================================================================ + +void ClockClient::set_callback(MeasurementCallback cb) +{ + std::lock_guard lock(callback_mutex_); + callback_ = std::move(cb); +} + +// ============================================================================ +// Non-blocking ping +// ============================================================================ + +void ClockClient::ping() +{ + uint8_t buf[sizeof(ClockPacket)]; + ClockPacket req; + req.t1 = get_monotonic_ns(); + req.serialize(buf); + + sendto(sock_, reinterpret_cast(buf), sizeof(ClockPacket), 0, + reinterpret_cast(&server_addr_), sizeof(server_addr_)); +} + +// ============================================================================ +// Receiver thread +// ============================================================================ + +void ClockClient::receiver_loop() +{ + uint8_t buf[sizeof(ClockPacket)]; + + while (alive_.load(std::memory_order_acquire)) + { + sockaddr_in from{}; + socklen_t from_len = sizeof(from); + ssize_t n = + recvfrom(sock_, reinterpret_cast(buf), sizeof(buf), 0, reinterpret_cast(&from), &from_len); + + clock_ns_t t4 = get_monotonic_ns(); + + if (n < 0) + { + continue; + } + + if (static_cast(n) < sizeof(ClockPacket)) + { + continue; + } + + auto reply = ClockPacket::deserialize(buf); + ClockMeasurement m{ reply.t1, reply.t2, reply.t3, t4 }; + + if (translator_) + { + m.t1 = translator_(m.t1); + m.t4 = translator_(m.t4); + } + + MeasurementCallback cb; + { + std::lock_guard lock(callback_mutex_); + cb = callback_; + } + + if (cb) + { + cb(m); + } + } +} + +// ============================================================================ +// Periodic sender +// ============================================================================ + +void ClockClient::start(int interval_ms, int count) +{ + if (sending_.load(std::memory_order_relaxed)) + { + throw std::runtime_error("[ClockClient] Periodic pinging already active"); + } + + sending_.store(true, std::memory_order_release); + sender_ = std::thread(&ClockClient::sender_loop, this, interval_ms, count); +} + +void ClockClient::stop() +{ + sending_.store(false, std::memory_order_release); + if (sender_.joinable()) + { + sender_.join(); + } +} + +void ClockClient::sender_loop(int interval_ms, int count) +{ + int done = 0; + + while (sending_.load(std::memory_order_acquire) && (count == 0 || done < count)) + { + ping(); + ++done; + + if (sending_.load(std::memory_order_acquire) && (count == 0 || done < count)) + { + std::this_thread::sleep_for(std::chrono::milliseconds(interval_ms)); + } + } + + sending_.store(false, std::memory_order_release); +} + +} // namespace core diff --git a/src/core/synchronization/clock_sync/clock_client_xr.cpp b/src/core/synchronization/clock_sync/clock_client_xr.cpp new file mode 100644 index 000000000..54ec70606 --- /dev/null +++ b/src/core/synchronization/clock_sync/clock_client_xr.cpp @@ -0,0 +1,165 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "clock_sync/clock_client.hpp" +#include "clock_sync/xr_clock_translator.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace +{ + +volatile sig_atomic_t g_running = 1; + +void signal_handler(int /*sig*/) +{ + g_running = 0; +} + +void print_header() +{ + std::cout << std::left << std::setw(8) << "seq" << std::setw(18) << "rtt_us" << std::setw(18) << "offset_us" + << std::setw(18) << "t1_ns" << std::setw(18) << "t2_ns" << std::setw(18) << "t3_ns" << std::setw(18) + << "t4_ns" + << "\n"; + std::cout << std::string(116, '-') << "\n"; +} + +void print_measurement(uint64_t seq, const core::ClockMeasurement& m) +{ + std::cout << std::left << std::setw(8) << seq << std::setw(18) << std::fixed << std::setprecision(3) + << core::ns_to_sec(m.rtt()) * 1e6 << std::setw(18) << core::ns_to_sec(m.offset()) * 1e6 << std::setw(18) + << m.t1 << std::setw(18) << m.t2 << std::setw(18) << m.t3 << std::setw(18) << m.t4 << "\n"; +} + +void print_summary(const std::vector& measurements, uint64_t sent) +{ + if (measurements.empty()) + return; + + core::clock_ns_t min_rtt = measurements[0].rtt(); + core::clock_ns_t max_rtt = min_rtt; + double sum_rtt = 0; + double sum_offset = 0; + + for (const auto& m : measurements) + { + core::clock_ns_t r = m.rtt(); + if (r < min_rtt) + min_rtt = r; + if (r > max_rtt) + max_rtt = r; + sum_rtt += static_cast(r); + sum_offset += static_cast(m.offset()); + } + + double avg_rtt = sum_rtt / static_cast(measurements.size()); + double avg_offset = sum_offset / static_cast(measurements.size()); + uint64_t lost = sent - measurements.size(); + + std::cout << "\n--- Summary ---\n"; + std::cout << "Packets: " << sent << " sent, " << measurements.size() << " received, " << lost << " lost\n"; + std::cout << std::fixed << std::setprecision(3); + std::cout << "RTT (us): min=" << core::ns_to_sec(min_rtt) * 1e6 << " avg=" << avg_rtt / 1e3 + << " max=" << core::ns_to_sec(max_rtt) * 1e6 << "\n"; + std::cout << "Offset (us): avg=" << avg_offset / 1e3 << "\n"; +} + +} // namespace + +int main(int argc, char* argv[]) +{ + if (argc < 2) + { + std::cerr << "Usage: clock_client_xr [port] [interval_ms] [count]\n" + << " Client timestamps are translated to XrTime via OpenXR.\n" + << " Server uses CLOCK_MONOTONIC_RAW.\n"; + return 1; + } + + const char* server_ip = argv[1]; + uint16_t port = (argc > 2) ? static_cast(std::atoi(argv[2])) : core::kDefaultClockPort; + int interval_ms = (argc > 3) ? std::atoi(argv[3]) : 1000; + int count = (argc > 4) ? std::atoi(argv[4]) : 0; + + std::signal(SIGINT, signal_handler); + std::signal(SIGTERM, signal_handler); + + std::shared_ptr xr_translator; + try + { + xr_translator = core::XrClockTranslator::Create(); + } + catch (const std::exception& e) + { + std::cerr << "[ClockClientXR] Failed to create XR translator: " << e.what() << std::endl; + return 1; + } + + std::vector measurements; + std::mutex mu; + std::condition_variable cv; + uint64_t seq = 0; + + auto on_measurement = [&](const core::ClockMeasurement& m) + { + std::lock_guard lock(mu); + measurements.push_back(m); + print_measurement(seq, m); + ++seq; + cv.notify_one(); + }; + + std::shared_ptr client; + try + { + client = core::ClockClient::Create(server_ip, port, on_measurement, xr_translator->make_translator()); + } + catch (const std::exception& e) + { + std::cerr << e.what() << std::endl; + return 1; + } + + std::cout << "[ClockClientXR] Pinging " << server_ip << ":" << port << " every " << interval_ms << " ms" + << " (client: XrTime, server: CLOCK_MONOTONIC_RAW)\n\n"; + + print_header(); + + uint64_t sent = 0; + + if (count > 0) + { + client->start(interval_ms, count); + sent = static_cast(count); + + std::unique_lock lock(mu); + cv.wait_for(lock, std::chrono::milliseconds(count * interval_ms + 2000), + [&] { return seq >= static_cast(count) || !g_running; }); + } + else + { + client->start(interval_ms, 0); + + while (g_running) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + client->stop(); + sent = seq + 1; + } + + { + std::lock_guard lock(mu); + print_summary(measurements, sent); + } + + return 0; +} diff --git a/src/core/synchronization/clock_sync/clock_server.cpp b/src/core/synchronization/clock_sync/clock_server.cpp new file mode 100644 index 000000000..cd789d165 --- /dev/null +++ b/src/core/synchronization/clock_sync/clock_server.cpp @@ -0,0 +1,107 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "clock_sync/clock_types.hpp" +#include "clock_sync/platform.hpp" + +#include +#include +#include +#include + +namespace +{ + +volatile sig_atomic_t g_running = 1; + +void signal_handler(int /*sig*/) +{ + g_running = 0; +} + +} // namespace + +int main(int argc, char* argv[]) +{ + uint16_t port = core::kDefaultClockPort; + if (argc > 1) + { + port = static_cast(std::atoi(argv[1])); + } + + std::signal(SIGINT, signal_handler); + std::signal(SIGTERM, signal_handler); + + core::ensure_winsock(); + + core::socket_t sock = socket(AF_INET, SOCK_DGRAM, 0); + if (sock == core::kInvalidSocket) + { + std::cerr << "[ClockServer] Failed to create socket" << std::endl; + return 1; + } + +#ifdef _WIN32 + char opt = 1; +#else + int opt = 1; +#endif + setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)); + + sockaddr_in addr{}; + addr.sin_family = AF_INET; + addr.sin_addr.s_addr = INADDR_ANY; + addr.sin_port = htons(port); + + if (bind(sock, reinterpret_cast(&addr), sizeof(addr)) < 0) + { + std::cerr << "[ClockServer] Failed to bind to port " << port << std::endl; + core::close_socket(sock); + return 1; + } + +#ifdef _WIN32 + std::cout << "[ClockServer] Listening on UDP port " << port << " (QPC)" << std::endl; +#else + std::cout << "[ClockServer] Listening on UDP port " << port << " (CLOCK_MONOTONIC_RAW)" << std::endl; +#endif + + uint8_t buf[sizeof(core::ClockPacket)]; + sockaddr_in client_addr{}; + socklen_t client_len = sizeof(client_addr); + + while (g_running) + { + auto n = recvfrom( + sock, reinterpret_cast(buf), sizeof(buf), 0, reinterpret_cast(&client_addr), &client_len); + + core::clock_ns_t t2 = core::get_monotonic_raw_ns(); + + if (n < 0) + { + if (core::is_socket_interrupted_error()) + continue; + std::cerr << "[ClockServer] recvfrom error" << std::endl; + continue; + } + + if (static_cast(n) < sizeof(core::ClockPacket)) + { + std::cerr << "[ClockServer] Ignoring short packet (" << n << " bytes)" << std::endl; + continue; + } + + auto pkt = core::ClockPacket::deserialize(buf); + pkt.t2 = t2; + + pkt.t3 = core::get_monotonic_raw_ns(); + pkt.serialize(buf); + + sendto(sock, reinterpret_cast(buf), sizeof(core::ClockPacket), 0, + reinterpret_cast(&client_addr), client_len); + } + + std::cout << "\n[ClockServer] Shutting down." << std::endl; + core::close_socket(sock); + return 0; +} diff --git a/src/core/synchronization/clock_sync/inc/clock_sync/clock_client.hpp b/src/core/synchronization/clock_sync/inc/clock_sync/clock_client.hpp new file mode 100644 index 000000000..6cb1e70a2 --- /dev/null +++ b/src/core/synchronization/clock_sync/inc/clock_sync/clock_client.hpp @@ -0,0 +1,126 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "clock_types.hpp" +#include "platform.hpp" + +#include +#include +#include +#include +#include +#include + +namespace core +{ + +/*! + * @brief Asynchronous UDP client for measuring clock offset and RTT against a + * ClockServer. + * + * Internally the client always timestamps with CLOCK_MONOTONIC (QPC on + * Windows). An optional ClockTranslator can be provided to convert the + * client-side timestamps (t1, t4) into another time domain (e.g. XrTime) + * before delivering the measurement through the callback. + * + * Usage (monotonic): + * @code + * auto client = ClockClient::Create("192.168.1.100", 5555, + * [](const ClockMeasurement& m) { + * std::cout << "rtt=" << m.rtt() << " offset=" << m.offset() << "\n"; + * }); + * client->ping(); + * client.reset(); + * @endcode + * + * Usage (with translator, e.g. XrTime): + * @code + * auto xr = XrClockTranslator::Create(); + * auto client = ClockClient::Create("192.168.1.100", 5555, + * my_callback, xr->make_translator()); + * @endcode + */ +class ClockClient +{ +public: + using MeasurementCallback = std::function; + + /*! + * @brief Factory method. + * @param server_ip IPv4 address of the clock server. + * @param port UDP port (default kDefaultClockPort). + * @param callback Called from the receiver thread for every reply. + * @param translator Optional translator applied to client-side timestamps + * (t1, t4) before delivering the measurement. Pass + * nullptr for raw CLOCK_MONOTONIC nanoseconds. + * @return A shared_ptr to the new client. The receiver thread is already running. + * @throws std::runtime_error if the socket cannot be created or the address is invalid. + */ + static std::shared_ptr Create(const std::string& server_ip, + uint16_t port = kDefaultClockPort, + MeasurementCallback callback = nullptr, + ClockTranslator translator = nullptr); + + ~ClockClient(); + + ClockClient(const ClockClient&) = delete; + ClockClient& operator=(const ClockClient&) = delete; + + /// Replace the measurement callback. Thread-safe. + void set_callback(MeasurementCallback cb); + + /*! + * @brief Send a single clock request (non-blocking). + * + * The result will be delivered asynchronously through the callback when + * (and if) the server replies. Lost packets simply produce no callback. + */ + void ping(); + + /*! + * @brief Start periodic pinging on a background thread. + * @param interval_ms Milliseconds between consecutive pings. + * @param count Number of pings (0 = unlimited until `stop()`). + * @throws std::runtime_error if periodic pinging is already active. + */ + void start(int interval_ms, int count = 0); + + /*! + * @brief Stop periodic pinging. The receiver thread keeps running. + * + * Safe to call even if not running (no-op). + */ + void stop(); + + /// @return true while the periodic sender thread is active. + bool is_running() const + { + return sending_.load(std::memory_order_relaxed); + } + +private: + ClockClient() = default; + void initialize(const std::string& server_ip, uint16_t port); + void receiver_loop(); + void sender_loop(int interval_ms, int count); + + socket_t sock_{ kInvalidSocket }; + ::sockaddr_in server_addr_{}; + + ClockTranslator translator_; + + // Receiver thread — lives for the entire lifetime of the object + std::atomic alive_{ false }; + std::thread receiver_; + + // Optional periodic sender thread + std::atomic sending_{ false }; + std::thread sender_; + + std::mutex callback_mutex_; + MeasurementCallback callback_; +}; + +} // namespace core diff --git a/src/core/synchronization/clock_sync/inc/clock_sync/clock_types.hpp b/src/core/synchronization/clock_sync/inc/clock_sync/clock_types.hpp new file mode 100644 index 000000000..f0d558261 --- /dev/null +++ b/src/core/synchronization/clock_sync/inc/clock_sync/clock_types.hpp @@ -0,0 +1,170 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include +#include +#include + +#ifdef _WIN32 +# ifndef WIN32_LEAN_AND_MEAN +# define WIN32_LEAN_AND_MEAN +# endif +# ifndef NOMINMAX +# define NOMINMAX +# endif +# include +#else +# include +#endif + +namespace core +{ + +/// All timestamps are int64_t nanoseconds. +using clock_ns_t = int64_t; + +/// Optional translator that converts a monotonic timestamp (ns) to another +/// time domain (e.g. XrTime). When null, the client reports raw monotonic. +using ClockTranslator = std::function; + +constexpr uint16_t kDefaultClockPort = 5555; + +// ============================================================================ +// Wire Protocol +// ============================================================================ + +/// Fixed-size packet exchanged between client and server. +/// +/// Client -> Server: only t1 is meaningful (client transmit time). +/// Server -> Client: t1 is echoed back, t2 = server receive, t3 = server transmit. +/// Client records t4 (client receive) locally on arrival. +struct ClockPacket +{ + clock_ns_t t1{ 0 }; // client transmit timestamp + clock_ns_t t2{ 0 }; // server receive timestamp + clock_ns_t t3{ 0 }; // server transmit timestamp + + void serialize(uint8_t* buf) const + { + std::memcpy(buf, this, sizeof(ClockPacket)); + } + + static ClockPacket deserialize(const uint8_t* buf) + { + ClockPacket pkt; + std::memcpy(&pkt, buf, sizeof(ClockPacket)); + return pkt; + } +}; + +static_assert(sizeof(ClockPacket) == 3 * sizeof(clock_ns_t), "ClockPacket must be tightly packed"); + +// ============================================================================ +// Timestamp helpers +// ============================================================================ + +#ifdef _WIN32 + +namespace detail +{ + +/// Cached QPC frequency (counts per second). Initialised once on first use. +inline int64_t qpc_frequency() +{ + static int64_t freq = []() + { + LARGE_INTEGER f; + QueryPerformanceFrequency(&f); + return f.QuadPart; + }(); + return freq; +} + +/// Read QueryPerformanceCounter and convert to nanoseconds. +inline clock_ns_t qpc_ns() +{ + LARGE_INTEGER counter; + QueryPerformanceCounter(&counter); + return static_cast(counter.QuadPart) * 1'000'000'000LL / qpc_frequency(); +} + +} // namespace detail + +/// Read CLOCK_MONOTONIC equivalent (QPC) as nanoseconds. +inline clock_ns_t get_monotonic_ns() +{ + return detail::qpc_ns(); +} + +/// Read hardware counter (QPC) as nanoseconds — server-side default. +inline clock_ns_t get_monotonic_raw_ns() +{ + return detail::qpc_ns(); +} + +#else // POSIX + +namespace detail +{ + +inline clock_ns_t posix_clock_ns(clockid_t clk) +{ + struct timespec ts; + if (clock_gettime(clk, &ts) != 0) + { + throw std::runtime_error("clock_gettime failed: " + std::string(std::strerror(errno))); + } + return static_cast(ts.tv_sec) * 1'000'000'000LL + static_cast(ts.tv_nsec); +} + +} // namespace detail + +/// Read CLOCK_MONOTONIC as nanoseconds. +inline clock_ns_t get_monotonic_ns() +{ + return detail::posix_clock_ns(CLOCK_MONOTONIC); +} + +/// Read CLOCK_MONOTONIC_RAW as nanoseconds — server-side default. +inline clock_ns_t get_monotonic_raw_ns() +{ + return detail::posix_clock_ns(CLOCK_MONOTONIC_RAW); +} + +#endif + +/// Convenience: convert nanoseconds to fractional seconds. +inline double ns_to_sec(clock_ns_t ns) +{ + return static_cast(ns) / 1e9; +} + +// ============================================================================ +// Measurement record (client-side) +// ============================================================================ + +struct ClockMeasurement +{ + clock_ns_t t1{ 0 }; // client transmit + clock_ns_t t2{ 0 }; // server receive + clock_ns_t t3{ 0 }; // server transmit + clock_ns_t t4{ 0 }; // client receive + + /// Round-trip time: (t4 - t1) - (t3 - t2) + clock_ns_t rtt() const + { + return (t4 - t1) - (t3 - t2); + } + + /// Clock offset: ((t2 - t1) + (t3 - t4)) / 2 + clock_ns_t offset() const + { + return ((t2 - t1) + (t3 - t4)) / 2; + } +}; + +} // namespace core diff --git a/src/core/synchronization/clock_sync/inc/clock_sync/platform.hpp b/src/core/synchronization/clock_sync/inc/clock_sync/platform.hpp new file mode 100644 index 000000000..5f2f8a3a0 --- /dev/null +++ b/src/core/synchronization/clock_sync/inc/clock_sync/platform.hpp @@ -0,0 +1,132 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +// ============================================================================ +// Platform-specific socket and networking abstractions +// ============================================================================ + +#ifdef _WIN32 + +# ifndef WIN32_LEAN_AND_MEAN +# define WIN32_LEAN_AND_MEAN +# endif +# ifndef NOMINMAX +# define NOMINMAX +# endif +# include +# include + +namespace core +{ + +using socket_t = SOCKET; +using socklen_t = int; +using ssize_t = int; + +constexpr socket_t kInvalidSocket = INVALID_SOCKET; + +inline void close_socket(socket_t s) +{ + closesocket(s); +} + +inline int last_socket_error() +{ + return WSAGetLastError(); +} + +inline bool is_socket_timeout_error() +{ + return WSAGetLastError() == WSAETIMEDOUT; +} + +inline bool is_socket_interrupted_error() +{ + return WSAGetLastError() == WSAEINTR; +} + +/// RAII wrapper for WSAStartup / WSACleanup. +struct WinsockInit +{ + WinsockInit() + { + WSADATA wsa; + WSAStartup(MAKEWORD(2, 2), &wsa); + } + ~WinsockInit() + { + WSACleanup(); + } + WinsockInit(const WinsockInit&) = delete; + WinsockInit& operator=(const WinsockInit&) = delete; +}; + +/// Call once before any socket operations. Thread-safe (static local). +inline void ensure_winsock() +{ + static WinsockInit init; +} + +/// Set SO_RCVTIMEO. On Windows the value is a DWORD in milliseconds. +inline void set_recv_timeout(socket_t sock, int ms) +{ + DWORD timeout_ms = static_cast(ms); + setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, reinterpret_cast(&timeout_ms), sizeof(timeout_ms)); +} + +} // namespace core + +#else // POSIX + +# include +# include +# include + +# include +# include + +namespace core +{ + +using socket_t = int; + +constexpr socket_t kInvalidSocket = -1; + +inline void close_socket(socket_t s) +{ + close(s); +} + +inline int last_socket_error() +{ + return errno; +} + +inline bool is_socket_timeout_error() +{ + return errno == EAGAIN || errno == EWOULDBLOCK; +} + +inline bool is_socket_interrupted_error() +{ + return errno == EINTR; +} + +inline void ensure_winsock() +{ +} // no-op on POSIX + +/// Set SO_RCVTIMEO. On POSIX the value is a struct timeval. +inline void set_recv_timeout(socket_t sock, int ms) +{ + struct timeval tv; + tv.tv_sec = ms / 1000; + tv.tv_usec = (ms % 1000) * 1000; + setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)); +} + +} // namespace core + +#endif diff --git a/src/core/synchronization/clock_sync/inc/clock_sync/xr_clock_translator.hpp b/src/core/synchronization/clock_sync/inc/clock_sync/xr_clock_translator.hpp new file mode 100644 index 000000000..8fb7f2632 --- /dev/null +++ b/src/core/synchronization/clock_sync/inc/clock_sync/xr_clock_translator.hpp @@ -0,0 +1,47 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "clock_types.hpp" + +#include +#include +#include + +namespace core +{ + +/// Translates CLOCK_MONOTONIC timestamps to XrTime using the OpenXR runtime. +/// +/// Creates a minimal OpenXR instance (headless, time-conversion only) and uses +/// xrConvertTimespecTimeToTimeKHR (Linux) or +/// xrConvertWin32PerformanceCounterToTimeKHR (Windows) to translate. +class XrClockTranslator : public std::enable_shared_from_this +{ +public: + /** @brief Create an XrClockTranslator backed by a headless OpenXR instance. + * @param app_name Application name for xrCreateInstance. + * @param extra_extensions Additional extensions to request. + * @throws std::runtime_error on failure. + */ + static std::shared_ptr Create(const std::string& app_name = "ClockSyncXr", + const std::vector& extra_extensions = {}); + ~XrClockTranslator(); + + XrClockTranslator(const XrClockTranslator&) = delete; + XrClockTranslator& operator=(const XrClockTranslator&) = delete; + + /// Translate a CLOCK_MONOTONIC nanosecond timestamp to XrTime (nanoseconds). + clock_ns_t translate(clock_ns_t monotonic_ns) const; + + /// Create a ClockTranslator function suitable for ClockClient. + ClockTranslator make_translator(); + +private: + XrClockTranslator(); + struct Impl; + std::unique_ptr impl_; +}; + +} // namespace core diff --git a/src/core/synchronization/clock_sync/xr_clock_translator.cpp b/src/core/synchronization/clock_sync/xr_clock_translator.cpp new file mode 100644 index 000000000..db50f59f2 --- /dev/null +++ b/src/core/synchronization/clock_sync/xr_clock_translator.cpp @@ -0,0 +1,67 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "clock_sync/xr_clock_translator.hpp" + +#include +#include + +#include + +namespace core +{ + +// ============================================================================ +// XrClockTranslator::Impl +// ============================================================================ + +struct XrClockTranslator::Impl +{ + std::unique_ptr session; + std::unique_ptr time_converter; +}; + +// ============================================================================ +// Construction +// ============================================================================ + +XrClockTranslator::XrClockTranslator() : impl_(std::make_unique()) +{ +} + +XrClockTranslator::~XrClockTranslator() = default; + +std::shared_ptr XrClockTranslator::Create(const std::string& app_name, + const std::vector& extra_extensions) +{ + auto translator = std::shared_ptr(new XrClockTranslator()); + + // Combine time conversion extensions with any extra extensions + auto extensions = XrTimeConverter::get_required_extensions(); + extensions.insert(extensions.end(), extra_extensions.begin(), extra_extensions.end()); + + translator->impl_->session = std::make_unique(app_name, extensions); + translator->impl_->time_converter = + std::make_unique(translator->impl_->session->get_handles()); + + std::cout << "[XrClockTranslator] Initialised (translating CLOCK_MONOTONIC -> XrTime)" << std::endl; + return translator; +} + +// ============================================================================ +// Translation +// ============================================================================ + +clock_ns_t XrClockTranslator::translate(clock_ns_t monotonic_ns) const +{ + XrTime xr_time = impl_->time_converter->convert_monotonic_ns_to_xrtime(monotonic_ns); + return static_cast(xr_time); +} + +ClockTranslator XrClockTranslator::make_translator() +{ + auto self = shared_from_this(); + return [self](clock_ns_t monotonic_ns) -> clock_ns_t { return self->translate(monotonic_ns); }; +} + +} // namespace core diff --git a/src/core/teleop_session_manager/README.md b/src/core/teleop_session_manager/README.md index fc41ba8f6..c6d03212a 100644 --- a/src/core/teleop_session_manager/README.md +++ b/src/core/teleop_session_manager/README.md @@ -135,11 +135,12 @@ pipeline = gripper.connect({...}) while True: deviceio_session.update() # Manual data injection needed for new sources - controller_data = controller_tracker.get_controller_data(deviceio_session) + left_snapshot = controller_tracker.get_left_controller(deviceio_session) + right_snapshot = controller_tracker.get_right_controller(deviceio_session) inputs = { "controllers": { - "deviceio_controller_left": [controller_data.left_controller], - "deviceio_controller_right": [controller_data.right_controller] + "deviceio_controller_left": [left_snapshot], + "deviceio_controller_right": [right_snapshot] } } result = pipeline(inputs) diff --git a/src/core/teleop_session_manager_tests/python/test_teleop_session.py b/src/core/teleop_session_manager_tests/python/test_teleop_session.py index 004910363..60f0bf404 100644 --- a/src/core/teleop_session_manager_tests/python/test_teleop_session.py +++ b/src/core/teleop_session_manager_tests/python/test_teleop_session.py @@ -71,22 +71,18 @@ def get_required_extensions(self): return ["XR_EXT_hand_tracking"] -class _MockControllerData: - """Mock controller data returned by ControllerTracker.""" - - def __init__(self): - self.left_controller = 3.0 - self.right_controller = 4.0 - - class ControllerTracker: """Mock controller tracker for testing.""" def __init__(self): - self._data = _MockControllerData() + self._left_controller = 3.0 + self._right_controller = 4.0 + + def get_left_controller(self, session): + return self._left_controller - def get_controller_data(self, session): - return self._data + def get_right_controller(self, session): + return self._right_controller def get_required_extensions(self): return ["XR_EXT_controller_interaction"] @@ -181,14 +177,15 @@ def __init__(self, name: str = "controllers"): def poll_tracker(self, deviceio_session): source_inputs = self.input_spec() - controller_data = self._tracker.get_controller_data(deviceio_session) + left_snapshot = self._tracker.get_left_controller(deviceio_session) + right_snapshot = self._tracker.get_right_controller(deviceio_session) result = {} for input_name, group_type in source_inputs.items(): tg = TensorGroup(group_type) if "left" in input_name: - tg[0] = controller_data.left_controller + tg[0] = left_snapshot elif "right" in input_name: - tg[0] = controller_data.right_controller + tg[0] = right_snapshot result[input_name] = tg return result diff --git a/src/plugins/controller_synthetic_hands/synthetic_hands_plugin.cpp b/src/plugins/controller_synthetic_hands/synthetic_hands_plugin.cpp index 64de742e4..785ccf731 100644 --- a/src/plugins/controller_synthetic_hands/synthetic_hands_plugin.cpp +++ b/src/plugins/controller_synthetic_hands/synthetic_hands_plugin.cpp @@ -73,32 +73,12 @@ void SyntheticHandsPlugin::worker_thread() continue; } - // Get controller data from tracker - const auto& controller_data = m_controller_tracker->get_controller_data(*m_deviceio_session); + const auto& left_ctrl = m_controller_tracker->get_left_controller(*m_deviceio_session); + const auto& right_ctrl = m_controller_tracker->get_right_controller(*m_deviceio_session); + XrTime time = m_controller_tracker->get_last_update_time(*m_deviceio_session); - // Get timestamp from controller data - XrTime time = 0; - if (controller_data.left_controller && controller_data.left_controller->is_active()) - { - time = controller_data.left_controller->timestamp().device_time(); - } - else if (controller_data.right_controller && controller_data.right_controller->is_active()) - { - time = controller_data.right_controller->timestamp().device_time(); - } - - // Get target curl values from trigger inputs - float left_target = 0.0f; - float right_target = 0.0f; - - if (controller_data.left_controller) - { - left_target = controller_data.left_controller->inputs().trigger_value(); - } - if (controller_data.right_controller) - { - right_target = controller_data.right_controller->inputs().trigger_value(); - } + float left_target = left_ctrl.inputs().trigger_value(); + float right_target = right_ctrl.inputs().trigger_value(); // Smoothly interpolate float curl_delta = CURL_SPEED * FRAME_TIME; @@ -120,12 +100,12 @@ void SyntheticHandsPlugin::worker_thread() m_right_curl = right_curl_current; } - if (m_left_enabled && controller_data.left_controller) + if (m_left_enabled && left_ctrl.is_active()) { bool grip_valid = false; bool aim_valid = false; - oxr_utils::get_grip_pose(*controller_data.left_controller, grip_valid); - XrPosef wrist = oxr_utils::get_aim_pose(*controller_data.left_controller, aim_valid); + oxr_utils::get_grip_pose(left_ctrl, grip_valid); + XrPosef wrist = oxr_utils::get_aim_pose(left_ctrl, aim_valid); if (grip_valid && aim_valid) { @@ -134,12 +114,12 @@ void SyntheticHandsPlugin::worker_thread() } } - if (m_right_enabled && controller_data.right_controller) + if (m_right_enabled && right_ctrl.is_active()) { bool grip_valid = false; bool aim_valid = false; - oxr_utils::get_grip_pose(*controller_data.right_controller, grip_valid); - XrPosef wrist = oxr_utils::get_aim_pose(*controller_data.right_controller, aim_valid); + oxr_utils::get_grip_pose(right_ctrl, grip_valid); + XrPosef wrist = oxr_utils::get_aim_pose(right_ctrl, aim_valid); if (grip_valid && aim_valid) { diff --git a/src/plugins/generic_3axis_pedal/generic_3axis_pedal_plugin.cpp b/src/plugins/generic_3axis_pedal/generic_3axis_pedal_plugin.cpp index 7f359764b..e5a0b0ec4 100644 --- a/src/plugins/generic_3axis_pedal/generic_3axis_pedal_plugin.cpp +++ b/src/plugins/generic_3axis_pedal/generic_3axis_pedal_plugin.cpp @@ -141,14 +141,14 @@ void Generic3AxisPedalPlugin::push_current_state() out.left_pedal = static_cast(axes_[0]); out.right_pedal = static_cast(axes_[1]); out.rudder = static_cast(axes_[2]); + auto now = std::chrono::steady_clock::now(); auto ns = std::chrono::duration_cast(now.time_since_epoch()).count(); - out.timestamp = std::make_shared(ns, ns); flatbuffers::FlatBufferBuilder builder(kMaxFlatbufferSize); auto offset = core::Generic3AxisPedalOutput::Pack(builder, &out); builder.Finish(offset); - pusher_.push_buffer(builder.GetBufferPointer(), builder.GetSize()); + pusher_.push_buffer(builder.GetBufferPointer(), builder.GetSize(), ns, ns); } } // namespace generic_3axis_pedal diff --git a/src/plugins/manus/core/manus_hand_tracking_plugin.cpp b/src/plugins/manus/core/manus_hand_tracking_plugin.cpp index 6e2d680eb..da04442c8 100644 --- a/src/plugins/manus/core/manus_hand_tracking_plugin.cpp +++ b/src/plugins/manus/core/manus_hand_tracking_plugin.cpp @@ -342,19 +342,9 @@ void ManusTracker::inject_hand_data() right_nodes = m_right_hand_nodes; } - // Get controller data from DeviceIOSession - const auto& controller_data = m_controller_tracker->get_controller_data(*m_deviceio_session); - - // Get timestamp from controller data for injection - XrTime time = 0; - if (controller_data.left_controller && controller_data.left_controller->is_active()) - { - time = controller_data.left_controller->timestamp().device_time(); - } - else if (controller_data.right_controller && controller_data.right_controller->is_active()) - { - time = controller_data.right_controller->timestamp().device_time(); - } + const auto& left_ctrl = m_controller_tracker->get_left_controller(*m_deviceio_session); + const auto& right_ctrl = m_controller_tracker->get_right_controller(*m_deviceio_session); + XrTime time = m_controller_tracker->get_last_update_time(*m_deviceio_session); auto process_hand = [&](const std::vector& nodes, bool is_left) { @@ -367,14 +357,12 @@ void ManusTracker::inject_hand_data() XrPosef root_pose = { { 0.0f, 0.0f, 0.0f, 1.0f }, { 0.0f, 0.0f, 0.0f } }; bool is_root_tracked = false; - // Get controller snapshot for this hand - const core::ControllerSnapshot* snapshot = - is_left ? controller_data.left_controller.get() : controller_data.right_controller.get(); + const core::ControllerSnapshot& snapshot = is_left ? left_ctrl : right_ctrl; - if (snapshot) + if (snapshot.is_active()) { bool aim_valid = false; - XrPosef raw_pose = oxr_utils::get_aim_pose(*snapshot, aim_valid); + XrPosef raw_pose = oxr_utils::get_aim_pose(snapshot, aim_valid); if (aim_valid) { diff --git a/src/plugins/oak/core/frame_sink.cpp b/src/plugins/oak/core/frame_sink.cpp index eb93d0ff5..c03d7edc8 100644 --- a/src/plugins/oak/core/frame_sink.cpp +++ b/src/plugins/oak/core/frame_sink.cpp @@ -27,12 +27,12 @@ MetadataPusher::MetadataPusher(const std::string& collection_id) { } -void MetadataPusher::push(const core::FrameMetadataT& data) +void MetadataPusher::push(const core::FrameMetadataT& data, int64_t device_time_ns, int64_t common_time_ns) { flatbuffers::FlatBufferBuilder builder(m_pusher.config().max_flatbuffer_size); auto offset = core::FrameMetadata::Pack(builder, &data); builder.Finish(offset); - m_pusher.push_buffer(builder.GetBufferPointer(), builder.GetSize()); + m_pusher.push_buffer(builder.GetBufferPointer(), builder.GetSize(), device_time_ns, common_time_ns); } // ============================================================================= @@ -62,7 +62,7 @@ void FrameSink::on_frame(const OakFrame& frame) if (m_pusher) { - m_pusher->push(frame.metadata); + m_pusher->push(frame.metadata, frame.device_time_ns, frame.common_time_ns); } } diff --git a/src/plugins/oak/core/frame_sink.hpp b/src/plugins/oak/core/frame_sink.hpp index 3d668d34e..3c3c5f0e0 100644 --- a/src/plugins/oak/core/frame_sink.hpp +++ b/src/plugins/oak/core/frame_sink.hpp @@ -35,9 +35,11 @@ class MetadataPusher /** * @brief Push a FrameMetadata message. * @param data The FrameMetadataT native object to serialize and push. + * @param device_time_ns Device clock timestamp in nanoseconds. + * @param common_time_ns Common clock (monotonic) timestamp in nanoseconds. * @throws std::runtime_error if the push fails. */ - void push(const core::FrameMetadataT& data); + void push(const core::FrameMetadataT& data, int64_t device_time_ns, int64_t common_time_ns); private: static constexpr size_t MAX_FLATBUFFER_SIZE = 128; diff --git a/src/plugins/oak/core/oak_camera.cpp b/src/plugins/oak/core/oak_camera.cpp index a2b4f4b46..741e0ebc0 100644 --- a/src/plugins/oak/core/oak_camera.cpp +++ b/src/plugins/oak/core/oak_camera.cpp @@ -86,8 +86,9 @@ std::optional OakCamera::get_frame() OakFrame frame; frame.h264_data = std::vector(data.begin(), data.end()); - frame.metadata.timestamp = std::make_shared(device_time_ns, common_time_ns); frame.metadata.sequence_number = static_cast(packet->getSequenceNum()); + frame.device_time_ns = device_time_ns; + frame.common_time_ns = common_time_ns; static std::chrono::steady_clock::time_point last_log_time{}; if (packet->getTimestamp() - last_log_time >= std::chrono::seconds(5)) diff --git a/src/plugins/oak/core/oak_camera.hpp b/src/plugins/oak/core/oak_camera.hpp index 4f5cca78d..d67b9b8b4 100644 --- a/src/plugins/oak/core/oak_camera.hpp +++ b/src/plugins/oak/core/oak_camera.hpp @@ -37,8 +37,12 @@ struct OakFrame /// H.264 encoded frame data std::vector h264_data; - /// Frame metadata (timestamp + sequence number) from oak.fbs + /// Frame metadata (sequence number) from oak.fbs core::FrameMetadataT metadata; + + /// Timestamps stored externally (no longer embedded in FrameMetadataT) + int64_t device_time_ns = 0; + int64_t common_time_ns = 0; }; /**