From 377de2c2cd256d68e33520a381b1a4a3560c0913 Mon Sep 17 00:00:00 2001 From: Devdeep Ray Date: Thu, 19 Feb 2026 22:35:02 -0800 Subject: [PATCH 1/6] Update timestamp schema, and decouple from data schema --- .../oxr/python/test_controller_tracker.py | 17 +- .../retargeting/python/sources_example.py | 9 +- examples/schemaio/frame_metadata_printer.cpp | 16 +- examples/schemaio/pedal_pusher.cpp | 7 +- src/core/deviceio/cpp/controller_tracker.cpp | 69 +++++---- .../cpp/frame_metadata_tracker_oak.cpp | 32 ++-- .../deviceio/cpp/full_body_tracker_pico.cpp | 25 ++- .../cpp/generic_3axis_pedal_tracker.cpp | 32 ++-- src/core/deviceio/cpp/hand_tracker.cpp | 34 ++-- src/core/deviceio/cpp/head_tracker.cpp | 19 ++- .../cpp/inc/deviceio/controller_tracker.hpp | 37 +++-- .../deviceio/frame_metadata_tracker_oak.hpp | 1 + .../inc/deviceio/full_body_tracker_pico.hpp | 11 +- .../deviceio/generic_3axis_pedal_tracker.hpp | 1 + .../cpp/inc/deviceio/hand_tracker.hpp | 20 ++- .../cpp/inc/deviceio/head_tracker.hpp | 14 +- .../cpp/inc/deviceio/schema_tracker.hpp | 11 +- .../deviceio/cpp/inc/deviceio/tracker.hpp | 29 +++- .../deviceio/python/deviceio_bindings.cpp | 19 ++- src/core/deviceio/python/deviceio_init.py | 6 +- src/core/mcap/cpp/inc/mcap/recorder.hpp | 29 ++-- src/core/mcap/cpp/mcap_recorder.cpp | 75 +++++---- src/core/mcap/python/mcap_bindings.cpp | 2 +- .../mcap_tests/cpp/test_mcap_recorder.cpp | 21 ++- .../oxr_utils/cpp/inc/oxr_utils/oxr_time.hpp | 44 ++++++ .../cpp/inc/pusherio/schema_pusher.hpp | 19 ++- src/core/pusherio/cpp/schema_pusher.cpp | 20 ++- .../controllers_source.py | 14 +- .../deviceio_source_nodes/hands_source.py | 1 - .../deviceio_source_nodes/head_source.py | 3 +- .../python/tensor_types/standard_types.py | 4 - .../python/utilities/hand_transform.py | 7 +- .../python/utilities/head_transform.py | 9 +- .../python/test_sources.py | 6 +- .../python/test_transforms.py | 19 +-- src/core/schema/fbs/controller.fbs | 16 +- src/core/schema/fbs/full_body.fbs | 13 +- src/core/schema/fbs/hand.fbs | 9 +- src/core/schema/fbs/hands.fbs | 18 --- src/core/schema/fbs/head.fbs | 9 +- src/core/schema/fbs/locomotion.fbs | 15 +- src/core/schema/fbs/oak.fbs | 11 +- src/core/schema/fbs/pedals.fbs | 15 +- src/core/schema/fbs/timestamp.fbs | 33 ++-- src/core/schema/python/controller_bindings.h | 55 +++---- src/core/schema/python/full_body_bindings.h | 11 +- src/core/schema/python/hand_bindings.h | 12 +- src/core/schema/python/head_bindings.h | 12 +- src/core/schema/python/locomotion_bindings.h | 15 +- src/core/schema/python/oak_bindings.h | 24 +-- src/core/schema/python/pedals_bindings.h | 12 -- src/core/schema/python/schema_init.py | 8 +- src/core/schema_tests/cpp/test_controller.cpp | 116 +++++--------- src/core/schema_tests/cpp/test_full_body.cpp | 77 ++++------ src/core/schema_tests/cpp/test_hand.cpp | 59 +++---- src/core/schema_tests/cpp/test_head.cpp | 63 +++----- src/core/schema_tests/cpp/test_locomotion.cpp | 84 +++++----- src/core/schema_tests/cpp/test_oak.cpp | 145 +++++------------- src/core/schema_tests/cpp/test_pedals.cpp | 108 ++++++------- src/core/schema_tests/python/test_camera.py | 94 +----------- .../schema_tests/python/test_controller.py | 82 ++++------ .../schema_tests/python/test_full_body.py | 2 - src/core/schema_tests/python/test_hand.py | 2 - src/core/schema_tests/python/test_head.py | 2 - .../schema_tests/python/test_locomotion.py | 57 +------ src/core/schema_tests/python/test_pedals.py | 67 +------- src/core/teleop_session_manager/README.md | 7 +- .../python/test_teleop_session.py | 25 ++- .../synthetic_hands_plugin.cpp | 42 ++--- .../generic_3axis_pedal_plugin.cpp | 4 +- .../manus/core/manus_hand_tracking_plugin.cpp | 24 +-- src/plugins/oak/core/frame_sink.cpp | 6 +- src/plugins/oak/core/frame_sink.hpp | 4 +- src/plugins/oak/core/oak_camera.cpp | 3 +- src/plugins/oak/core/oak_camera.hpp | 6 +- 75 files changed, 817 insertions(+), 1232 deletions(-) delete mode 100644 src/core/schema/fbs/hands.fbs 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/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..88182379d 100644 --- a/src/core/deviceio/cpp/frame_metadata_tracker_oak.cpp +++ b/src/core/deviceio/cpp/frame_metadata_tracker_oak.cpp @@ -24,27 +24,31 @@ class FrameMetadataTrackerOak::Impl : public ITrackerImpl { } - bool update(XrTime /* time */) override + bool update(XrTime time) override { - // Try to read new data from tensor stream if (m_schema_reader.read_buffer(m_buffer)) { - auto fb = GetFrameMetadata(m_buffer.data()); + auto fb = flatbuffers::GetRoot(m_buffer.data()); if (fb) { fb->UnPackTo(&m_data); + m_last_timestamp = DeviceDataTimestamp(time, time, 0); return true; } } - // Return true even if no new data - we're still running return true; } - Timestamp serialize(flatbuffers::FlatBufferBuilder& builder) const override + DeviceDataTimestamp serialize(flatbuffers::FlatBufferBuilder& builder, size_t /* channel_index */) const override { - auto offset = FrameMetadata::Pack(builder, &m_data); - builder.Finish(offset); - return m_data.timestamp ? *m_data.timestamp : Timestamp{}; + 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; } const FrameMetadataT& get_data() const @@ -56,6 +60,7 @@ class FrameMetadataTrackerOak::Impl : public ITrackerImpl SchemaTracker m_schema_reader; std::vector m_buffer; FrameMetadataT m_data; + DeviceDataTimestamp m_last_timestamp{}; }; // ============================================================================ @@ -82,13 +87,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..58092b3f5 100644 --- a/src/core/deviceio/cpp/generic_3axis_pedal_tracker.cpp +++ b/src/core/deviceio/cpp/generic_3axis_pedal_tracker.cpp @@ -23,28 +23,32 @@ class Generic3AxisPedalTracker::Impl : public ITrackerImpl { } - bool update(XrTime /* time */) override + bool update(XrTime time) override { - // Try to read new data from tensor stream if (m_schema_reader.read_buffer(m_buffer)) { - auto fb = GetGeneric3AxisPedalOutput(m_buffer.data()); + auto fb = flatbuffers::GetRoot(m_buffer.data()); if (fb) { fb->UnPackTo(&m_data); + m_last_timestamp = DeviceDataTimestamp(time, time, 0); return true; } } - // Return true even if no new data - we're still running, but invalid data. 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 offset = Generic3AxisPedalOutput::Pack(builder, &m_data); - builder.Finish(offset); - return m_data.timestamp ? *m_data.timestamp : Timestamp{}; + 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; } const Generic3AxisPedalOutputT& get_data() const @@ -56,6 +60,7 @@ class Generic3AxisPedalTracker::Impl : public ITrackerImpl SchemaTracker m_schema_reader; std::vector m_buffer; Generic3AxisPedalOutputT m_data; + DeviceDataTimestamp m_last_timestamp{}; }; // ============================================================================ @@ -82,13 +87,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..c8d64226a 100644 --- a/src/core/deviceio/cpp/inc/deviceio/schema_tracker.hpp +++ b/src/core/deviceio/cpp/inc/deviceio/schema_tracker.hpp @@ -98,10 +98,13 @@ struct SchemaTrackerConfig * return false; * } * - * Timestamp serialize(flatbuffers::FlatBufferBuilder& builder) const override { - * auto offset = LocomotionCommand::Pack(builder, &data_); - * builder.Finish(offset); - * return data_.timestamp ? *data_.timestamp : Timestamp{}; + * DeviceDataTimestamp serialize(flatbuffers::FlatBufferBuilder& builder, size_t) const override { + * auto data_offset = LocomotionCommand::Pack(builder, &data_); + * LocomotionCommandRecordBuilder rb(builder); + * rb.add_data(data_offset); + * rb.add_timestamp(&m_last_timestamp); + * builder.Finish(rb.Finish()); + * return m_last_timestamp; * } * * const LocomotionCommandT& get_data() const { return data_; } diff --git a/src/core/deviceio/cpp/inc/deviceio/tracker.hpp b/src/core/deviceio/cpp/inc/deviceio/tracker.hpp index 8670d6308..a0a25dc93 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 @@ -30,12 +30,17 @@ 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 Timestamp serialize(flatbuffers::FlatBufferBuilder& builder) const = 0; + virtual DeviceDataTimestamp serialize(flatbuffers::FlatBufferBuilder& builder, size_t channel_index) const = 0; }; // Base interface for all trackers @@ -53,16 +58,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/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..a95593b47 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,31 +89,25 @@ 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; - } - flatbuffers::FlatBufferBuilder builder(256); - Timestamp timestamp = tracker_impl.serialize(builder); + DeviceDataTimestamp timestamp = tracker_impl.serialize(builder, binding.channel_index); - // Use tracker timestamp for log time, fall back to system time if 0 mcap::Timestamp log_time; - if (timestamp.device_time() != 0) + if (timestamp.sample_time_common_clock() != 0) { - // XrTime is in nanoseconds, same as MCAP Timestamp - log_time = static_cast(timestamp.device_time()); + log_time = static_cast(timestamp.sample_time_common_clock()); } else { @@ -117,7 +117,7 @@ class McapRecorder::Impl } mcap::Message msg; - msg.channelId = it->second; + msg.channelId = binding.mcap_channel_id; msg.logTime = log_time; msg.publishTime = log_time; msg.sequence = static_cast(message_count_); @@ -136,23 +136,22 @@ class McapRecorder::Impl } 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..5006e0d3d 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,12 +59,9 @@ 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 + 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..242a4871d 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") @@ -328,13 +326,12 @@ 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 + [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 +477,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 +491,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 +537,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 +553,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 +608,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 +696,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 +703,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..8ca7d3320 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,40 @@ 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 +203,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 +229,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 +240,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 +250,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 +270,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 +355,14 @@ 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/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; }; /** From 2716d9bf7bc68b84e6b7db184e8acbe088f7dff5 Mon Sep 17 00:00:00 2001 From: Devdeep Ray Date: Thu, 19 Feb 2026 23:23:10 -0800 Subject: [PATCH 2/6] Fix schema tracker to have the proper timestamps passed through --- .../cpp/frame_metadata_tracker_oak.cpp | 48 +++++++++++-- .../cpp/generic_3axis_pedal_tracker.cpp | 53 ++++++++++++--- .../cpp/inc/deviceio/schema_tracker.hpp | 67 ++++++++++++++----- .../deviceio/cpp/inc/deviceio/tracker.hpp | 24 +++++++ src/core/deviceio/cpp/schema_tracker.cpp | 62 +++++++++-------- src/core/mcap/cpp/mcap_recorder.cpp | 61 +++++++++-------- 6 files changed, 227 insertions(+), 88 deletions(-) diff --git a/src/core/deviceio/cpp/frame_metadata_tracker_oak.cpp b/src/core/deviceio/cpp/frame_metadata_tracker_oak.cpp index 88182379d..c18b5edac 100644 --- a/src/core/deviceio/cpp/frame_metadata_tracker_oak.cpp +++ b/src/core/deviceio/cpp/frame_metadata_tracker_oak.cpp @@ -24,18 +24,30 @@ class FrameMetadataTrackerOak::Impl : public ITrackerImpl { } - bool update(XrTime time) override + bool update(XrTime /* time */) override { - if (m_schema_reader.read_buffer(m_buffer)) + m_pending_records.clear(); + + std::vector raw_samples; + m_schema_reader.read_all_samples(raw_samples); + + for (auto& sample : raw_samples) { - auto fb = flatbuffers::GetRoot(m_buffer.data()); + auto fb = flatbuffers::GetRoot(sample.buffer.data()); if (fb) { - fb->UnPackTo(&m_data); - m_last_timestamp = DeviceDataTimestamp(time, time, 0); - return true; + FrameMetadataT parsed; + fb->UnPackTo(&parsed); + m_pending_records.push_back({ std::move(parsed), sample.timestamp }); } } + + if (!m_pending_records.empty()) + { + m_data = m_pending_records.back().data; + m_last_timestamp = m_pending_records.back().timestamp; + } + return true; } @@ -51,16 +63,38 @@ class FrameMetadataTrackerOak::Impl : public ITrackerImpl return m_last_timestamp; } + void serialize_all(size_t /* channel_index */, const RecordCallback& callback) const override + { + for (const auto& record : m_pending_records) + { + flatbuffers::FlatBufferBuilder builder(256); + auto data_offset = FrameMetadata::Pack(builder, &record.data); + + FrameMetadataRecordBuilder record_builder(builder); + record_builder.add_data(data_offset); + record_builder.add_timestamp(&record.timestamp); + builder.Finish(record_builder.Finish()); + + callback(record.timestamp, builder.GetBufferPointer(), builder.GetSize()); + } + } + const FrameMetadataT& get_data() const { return m_data; } private: + struct PendingRecord + { + FrameMetadataT data; + DeviceDataTimestamp timestamp; + }; + SchemaTracker m_schema_reader; - std::vector m_buffer; FrameMetadataT m_data; DeviceDataTimestamp m_last_timestamp{}; + std::vector m_pending_records; }; // ============================================================================ diff --git a/src/core/deviceio/cpp/generic_3axis_pedal_tracker.cpp b/src/core/deviceio/cpp/generic_3axis_pedal_tracker.cpp index 58092b3f5..fb8aed351 100644 --- a/src/core/deviceio/cpp/generic_3axis_pedal_tracker.cpp +++ b/src/core/deviceio/cpp/generic_3axis_pedal_tracker.cpp @@ -23,19 +23,34 @@ class Generic3AxisPedalTracker::Impl : public ITrackerImpl { } - bool update(XrTime time) override + bool update(XrTime /* time */) override { - if (m_schema_reader.read_buffer(m_buffer)) + m_pending_records.clear(); + + std::vector raw_samples; + m_schema_reader.read_all_samples(raw_samples); + + for (auto& sample : raw_samples) { - auto fb = flatbuffers::GetRoot(m_buffer.data()); + auto fb = flatbuffers::GetRoot(sample.buffer.data()); if (fb) { - fb->UnPackTo(&m_data); - m_last_timestamp = DeviceDataTimestamp(time, time, 0); - return true; + Generic3AxisPedalOutputT parsed; + fb->UnPackTo(&parsed); + m_pending_records.push_back({ std::move(parsed), sample.timestamp }); } } - m_data.is_valid = false; + + if (!m_pending_records.empty()) + { + m_data = m_pending_records.back().data; + m_last_timestamp = m_pending_records.back().timestamp; + } + else + { + m_data.is_valid = false; + } + return true; } @@ -51,16 +66,38 @@ class Generic3AxisPedalTracker::Impl : public ITrackerImpl return m_last_timestamp; } + void serialize_all(size_t /* channel_index */, const RecordCallback& callback) const override + { + for (const auto& record : m_pending_records) + { + flatbuffers::FlatBufferBuilder builder(256); + auto data_offset = Generic3AxisPedalOutput::Pack(builder, &record.data); + + Generic3AxisPedalOutputRecordBuilder record_builder(builder); + record_builder.add_data(data_offset); + record_builder.add_timestamp(&record.timestamp); + builder.Finish(record_builder.Finish()); + + callback(record.timestamp, builder.GetBufferPointer(), builder.GetSize()); + } + } + const Generic3AxisPedalOutputT& get_data() const { return m_data; } private: + struct PendingRecord + { + Generic3AxisPedalOutputT data; + DeviceDataTimestamp timestamp; + }; + SchemaTracker m_schema_reader; - std::vector m_buffer; Generic3AxisPedalOutputT m_data; DeviceDataTimestamp m_last_timestamp{}; + std::vector m_pending_records; }; // ============================================================================ diff --git a/src/core/deviceio/cpp/inc/deviceio/schema_tracker.hpp b/src/core/deviceio/cpp/inc/deviceio/schema_tracker.hpp index c8d64226a..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,32 +88,51 @@ struct SchemaTrackerConfig * Impl(const OpenXRSessionHandles& handles, SchemaTrackerConfig config) * : m_schema_reader(handles, std::move(config)) {} * - * bool update(XrTime time) override { - * if (m_schema_reader.read_buffer(buffer_)) { - * auto fb = GetLocomotionCommand(buffer_.data()); + * bool update(XrTime) override { + * m_pending.clear(); + * std::vector 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; * } * * DeviceDataTimestamp serialize(flatbuffers::FlatBufferBuilder& builder, size_t) const override { - * auto data_offset = LocomotionCommand::Pack(builder, &data_); + * auto offset = LocomotionCommand::Pack(builder, &data_); + * auto& ts = m_pending.empty() ? DeviceDataTimestamp{} : m_pending.back().timestamp; * LocomotionCommandRecordBuilder rb(builder); - * rb.add_data(data_offset); - * rb.add_timestamp(&m_last_timestamp); + * rb.add_data(offset); + * rb.add_timestamp(&ts); * builder.Finish(rb.Finish()); - * return m_last_timestamp; + * return ts; + * } + * + * void serialize_all(size_t, const RecordCallback& cb) const override { + * for (const auto& r : m_pending) { + * flatbuffers::FlatBufferBuilder b(256); + * auto offset = LocomotionCommand::Pack(b, &r.data); + * LocomotionCommandRecordBuilder rb(b); + * rb.add_data(offset); + * rb.add_timestamp(&r.timestamp); + * b.Finish(rb.Finish()); + * cb(r.timestamp, b.GetBufferPointer(), b.GetSize()); + * } * } * * const LocomotionCommandT& get_data() const { return data_; } * * private: + * struct Pending { LocomotionCommandT data; DeviceDataTimestamp timestamp; }; * SchemaTracker m_schema_reader; - * std::vector buffer_; * LocomotionCommandT data_; + * std::vector m_pending; * }; * }; * @endcode @@ -145,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. @@ -164,9 +194,10 @@ class SchemaTracker private: void initialize_tensor_data_functions(); void create_tensor_list(); + bool ensure_collection(); void poll_for_updates(); std::optional 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 a0a25dc93..820659a42 100644 --- a/src/core/deviceio/cpp/inc/deviceio/tracker.hpp +++ b/src/core/deviceio/cpp/inc/deviceio/tracker.hpp @@ -7,6 +7,7 @@ #include #include +#include #include #include #include @@ -41,6 +42,29 @@ class ITrackerImpl * @return DeviceDataTimestamp for MCAP log time. */ virtual DeviceDataTimestamp serialize(flatbuffers::FlatBufferBuilder& builder, size_t channel_index) const = 0; + + /** + * @brief Callback type for serialize_all: receives timestamp, raw buffer pointer, and size. + */ + using RecordCallback = std::function; + + /** + * @brief Serialize all pending records for a channel, invoking the callback for each. + * + * The default implementation calls serialize() once, which is correct for + * direct OpenXR trackers that always have exactly one state per update. + * SchemaTracker-based trackers override this to emit every queued tensor + * sample so that no data is lost in MCAP recording. + * + * @param channel_index Which record channel to serialize. + * @param callback Invoked once per record with (timestamp, data_ptr, data_size). + */ + virtual void serialize_all(size_t channel_index, const RecordCallback& callback) const + { + flatbuffers::FlatBufferBuilder builder(256); + DeviceDataTimestamp ts = serialize(builder, channel_index); + callback(ts, builder.GetBufferPointer(), builder.GetSize()); + } }; // Base interface for all trackers diff --git a/src/core/deviceio/cpp/schema_tracker.cpp b/src/core/deviceio/cpp/schema_tracker.cpp 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/mcap/cpp/mcap_recorder.cpp b/src/core/mcap/cpp/mcap_recorder.cpp index a95593b47..b3c96d96d 100644 --- a/src/core/mcap/cpp/mcap_recorder.cpp +++ b/src/core/mcap/cpp/mcap_recorder.cpp @@ -101,38 +101,43 @@ class McapRecorder::Impl bool record_channel(const ChannelBinding& binding, const ITrackerImpl& tracker_impl) { - flatbuffers::FlatBufferBuilder builder(256); - DeviceDataTimestamp timestamp = tracker_impl.serialize(builder, binding.channel_index); + bool success = true; - 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()); - } + 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()); + } - 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(builder.GetBufferPointer()); - msg.dataSize = builder.GetSize(); + 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; - auto status = writer_.write(msg); - if (!status.ok()) - { - std::cerr << "McapRecorder: Failed to write message: " << status.message << std::endl; - return false; - } + auto status = writer_.write(msg); + if (!status.ok()) + { + std::cerr << "McapRecorder: Failed to write message: " << status.message << std::endl; + success = false; + } + + ++message_count_; + }); - ++message_count_; - return true; + return success; } std::string filename_; From b0ee3a86595c82b8a749560a6cc6d5ab646e50bc Mon Sep 17 00:00:00 2001 From: Devdeep Ray Date: Fri, 20 Feb 2026 09:03:17 -0800 Subject: [PATCH 3/6] Add tracked data and device output timestamp --- examples/oxr/cpp/oxr_session_sharing.cpp | 8 +- examples/oxr/cpp/oxr_simple_api_demo.cpp | 10 +- examples/schemaio/frame_metadata_printer.cpp | 3 +- examples/schemaio/pedal_printer.cpp | 3 +- src/core/deviceio/cpp/controller_tracker.cpp | 46 ++++----- .../cpp/frame_metadata_tracker_oak.cpp | 27 ++++-- .../deviceio/cpp/full_body_tracker_pico.cpp | 39 ++++---- .../cpp/generic_3axis_pedal_tracker.cpp | 29 +++--- src/core/deviceio/cpp/hand_tracker.cpp | 38 +++++--- src/core/deviceio/cpp/head_tracker.cpp | 33 ++++--- .../cpp/inc/deviceio/controller_tracker.hpp | 24 ++--- .../deviceio/frame_metadata_tracker_oak.hpp | 4 +- .../inc/deviceio/full_body_tracker_pico.hpp | 4 +- .../deviceio/generic_3axis_pedal_tracker.hpp | 4 +- .../cpp/inc/deviceio/hand_tracker.hpp | 20 ++-- .../cpp/inc/deviceio/head_tracker.hpp | 10 +- .../deviceio/python/deviceio_bindings.cpp | 93 +++++++++++++++---- src/core/deviceio/python/deviceio_init.py | 26 +++++- src/core/schema/fbs/controller.fbs | 6 ++ src/core/schema/fbs/full_body.fbs | 6 ++ src/core/schema/fbs/hand.fbs | 6 ++ src/core/schema/fbs/head.fbs | 6 ++ src/core/schema/fbs/locomotion.fbs | 6 ++ src/core/schema/fbs/oak.fbs | 6 ++ src/core/schema/fbs/pedals.fbs | 6 ++ src/core/schema/fbs/timestamp.fbs | 19 ++++ .../synthetic_hands_plugin.cpp | 8 +- .../manus/core/manus_hand_tracking_plugin.cpp | 8 +- 28 files changed, 336 insertions(+), 162 deletions(-) diff --git a/examples/oxr/cpp/oxr_session_sharing.cpp b/examples/oxr/cpp/oxr_session_sharing.cpp index 021f81bd0..baca1cdbe 100644 --- a/examples/oxr/cpp/oxr_session_sharing.cpp +++ b/examples/oxr/cpp/oxr_session_sharing.cpp @@ -95,11 +95,11 @@ try if (i % 3 == 0) { std::cout << "Frame " << i << ": " - << "Hands=" << (left.is_active ? "ACTIVE" : "INACTIVE") << " | " - << "Head=" << (head.is_valid ? "VALID" : "INVALID"); - if (head.is_valid && head.pose) + << "Hands=" << (left.data->is_active ? "ACTIVE" : "INACTIVE") << " | " + << "Head=" << (head.data->is_valid ? "VALID" : "INVALID"); + if (head.data->is_valid && head.data->pose) { - const auto& pos = head.pose->position(); + const auto& pos = head.data->pose->position(); std::cout << " [" << pos.x() << ", " << pos.y() << ", " << pos.z() << "]"; } std::cout << std::endl; diff --git a/examples/oxr/cpp/oxr_simple_api_demo.cpp b/examples/oxr/cpp/oxr_simple_api_demo.cpp index 28f5e900d..79ebd5c46 100644 --- a/examples/oxr/cpp/oxr_simple_api_demo.cpp +++ b/examples/oxr/cpp/oxr_simple_api_demo.cpp @@ -87,13 +87,13 @@ try const auto& head = head_tracker->get_head(*session); std::cout << "Frame " << i << ":" << std::endl; - std::cout << " Left hand: " << (left.is_active ? "ACTIVE" : "INACTIVE") << std::endl; - std::cout << " Right hand: " << (right.is_active ? "ACTIVE" : "INACTIVE") << std::endl; - std::cout << " Head pose: " << (head.is_valid ? "VALID" : "INVALID") << std::endl; + std::cout << " Left hand: " << (left.data->is_active ? "ACTIVE" : "INACTIVE") << std::endl; + std::cout << " Right hand: " << (right.data->is_active ? "ACTIVE" : "INACTIVE") << std::endl; + std::cout << " Head pose: " << (head.data->is_valid ? "VALID" : "INVALID") << std::endl; - if (head.is_valid && head.pose) + if (head.data->is_valid && head.data->pose) { - const auto& pos = head.pose->position(); + const auto& pos = head.data->pose->position(); std::cout << " Position: [" << pos.x() << ", " << pos.y() << ", " << pos.z() << "]" << std::endl; } std::cout << std::endl; diff --git a/examples/schemaio/frame_metadata_printer.cpp b/examples/schemaio/frame_metadata_printer.cpp index f90e728ce..c25984549 100644 --- a/examples/schemaio/frame_metadata_printer.cpp +++ b/examples/schemaio/frame_metadata_printer.cpp @@ -114,7 +114,8 @@ try } // Print when we have new data (sequence_number changed) - const auto& data = tracker->get_data(*session); + const auto& tracked = tracker->get_data(*session); + const auto& data = *tracked.data; if (data.sequence_number != last_printed_sequence) { print_frame_metadata(data, ++received_count); diff --git a/examples/schemaio/pedal_printer.cpp b/examples/schemaio/pedal_printer.cpp index 40eb6cd50..441aa8668 100644 --- a/examples/schemaio/pedal_printer.cpp +++ b/examples/schemaio/pedal_printer.cpp @@ -76,7 +76,8 @@ try } // Print current data if available - const auto& data = tracker->get_data(*session); + const auto& tracked = tracker->get_data(*session); + const auto& data = *tracked.data; if (data.is_valid) { print_pedal_data(data, received_count++); diff --git a/src/core/deviceio/cpp/controller_tracker.cpp b/src/core/deviceio/cpp/controller_tracker.cpp index dbf86adb2..192799d26 100644 --- a/src/core/deviceio/cpp/controller_tracker.cpp +++ b/src/core/deviceio/cpp/controller_tracker.cpp @@ -6,6 +6,7 @@ #include "inc/deviceio/deviceio_session.hpp" #include +#include #include #include #include @@ -259,6 +260,11 @@ ControllerTracker::Impl::Impl(const OpenXRSessionHandles& handles) throw std::runtime_error("Failed to attach action sets: " + std::to_string(result)); } + left_tracked_.data = std::make_shared(); + left_tracked_.timestamp = std::make_shared(); + right_tracked_.data = std::make_shared(); + right_tracked_.timestamp = std::make_shared(); + std::cout << "ControllerTracker initialized (left + right)" << std::endl; } @@ -335,39 +341,38 @@ bool ControllerTracker::Impl::update(XrTime time) snapshot_out = ControllerSnapshot(grip_pose, aim_pose, inputs, is_active); }; - update_controller(left_hand_path_, left_grip_space_, left_aim_space_, left_controller_); - update_controller(right_hand_path_, right_grip_space_, right_aim_space_, right_controller_); - - last_timestamp_ = DeviceDataTimestamp(time, time, 0); + update_controller(left_hand_path_, left_grip_space_, left_aim_space_, *left_tracked_.data); + update_controller(right_hand_path_, right_grip_space_, right_aim_space_, *right_tracked_.data); - return left_controller_.is_active() || right_controller_.is_active(); -} + auto now_ns = + std::chrono::duration_cast(std::chrono::steady_clock::now().time_since_epoch()).count(); + *left_tracked_.timestamp = DeviceOutputTimestamp(now_ns, time, 0); + *right_tracked_.timestamp = DeviceOutputTimestamp(now_ns, time, 0); + last_record_timestamp_ = DeviceDataTimestamp(time, time, 0); -const ControllerSnapshot& ControllerTracker::Impl::get_left_controller() const -{ - return left_controller_; + return left_tracked_.data->is_active() || right_tracked_.data->is_active(); } -const ControllerSnapshot& ControllerTracker::Impl::get_right_controller() const +const ControllerSnapshotTrackedT& ControllerTracker::Impl::get_left_controller() const { - return right_controller_; + return left_tracked_; } -XrTime ControllerTracker::Impl::get_last_update_time() const +const ControllerSnapshotTrackedT& ControllerTracker::Impl::get_right_controller() const { - return last_timestamp_.sample_time_device_clock(); + return right_tracked_; } DeviceDataTimestamp ControllerTracker::Impl::serialize(flatbuffers::FlatBufferBuilder& builder, size_t channel_index) const { - const ControllerSnapshot& snapshot = (channel_index == 0) ? left_controller_ : right_controller_; + const ControllerSnapshot& snapshot = (channel_index == 0) ? *left_tracked_.data : *right_tracked_.data; ControllerSnapshotRecordBuilder record_builder(builder); record_builder.add_data(&snapshot); - record_builder.add_timestamp(&last_timestamp_); + record_builder.add_timestamp(&last_record_timestamp_); builder.Finish(record_builder.Finish()); - return last_timestamp_; + return last_record_timestamp_; } // ============================================================================ @@ -380,21 +385,16 @@ std::vector ControllerTracker::get_required_extensions() const return {}; } -const ControllerSnapshot& ControllerTracker::get_left_controller(const DeviceIOSession& session) const +const ControllerSnapshotTrackedT& ControllerTracker::get_left_controller(const DeviceIOSession& session) const { return static_cast(session.get_tracker_impl(*this)).get_left_controller(); } -const ControllerSnapshot& ControllerTracker::get_right_controller(const DeviceIOSession& session) const +const ControllerSnapshotTrackedT& ControllerTracker::get_right_controller(const DeviceIOSession& session) const { return static_cast(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_last_update_time(); -} - std::shared_ptr ControllerTracker::create_tracker(const OpenXRSessionHandles& handles) const { return std::make_shared(handles); diff --git a/src/core/deviceio/cpp/frame_metadata_tracker_oak.cpp b/src/core/deviceio/cpp/frame_metadata_tracker_oak.cpp index c18b5edac..b60cfb900 100644 --- a/src/core/deviceio/cpp/frame_metadata_tracker_oak.cpp +++ b/src/core/deviceio/cpp/frame_metadata_tracker_oak.cpp @@ -8,6 +8,7 @@ #include #include +#include #include namespace core @@ -22,6 +23,8 @@ class FrameMetadataTrackerOak::Impl : public ITrackerImpl public: Impl(const OpenXRSessionHandles& handles, SchemaTrackerConfig config) : m_schema_reader(handles, std::move(config)) { + m_tracked.data = std::make_shared(); + m_tracked.timestamp = std::make_shared(); } bool update(XrTime /* time */) override @@ -44,8 +47,12 @@ class FrameMetadataTrackerOak::Impl : public ITrackerImpl if (!m_pending_records.empty()) { - m_data = m_pending_records.back().data; - m_last_timestamp = m_pending_records.back().timestamp; + *m_tracked.data = m_pending_records.back().data; + auto now_ns = + std::chrono::duration_cast(std::chrono::steady_clock::now().time_since_epoch()) + .count(); + *m_tracked.timestamp = DeviceOutputTimestamp(now_ns, 0, 0); + m_last_record_timestamp = m_pending_records.back().timestamp; } return true; @@ -53,14 +60,14 @@ class FrameMetadataTrackerOak::Impl : public ITrackerImpl DeviceDataTimestamp serialize(flatbuffers::FlatBufferBuilder& builder, size_t /* channel_index */) const override { - auto data_offset = FrameMetadata::Pack(builder, &m_data); + auto data_offset = FrameMetadata::Pack(builder, m_tracked.data.get()); FrameMetadataRecordBuilder record_builder(builder); record_builder.add_data(data_offset); - record_builder.add_timestamp(&m_last_timestamp); + record_builder.add_timestamp(&m_last_record_timestamp); builder.Finish(record_builder.Finish()); - return m_last_timestamp; + return m_last_record_timestamp; } void serialize_all(size_t /* channel_index */, const RecordCallback& callback) const override @@ -79,9 +86,9 @@ class FrameMetadataTrackerOak::Impl : public ITrackerImpl } } - const FrameMetadataT& get_data() const + const FrameMetadataTrackedT& get_data() const { - return m_data; + return m_tracked; } private: @@ -92,8 +99,8 @@ class FrameMetadataTrackerOak::Impl : public ITrackerImpl }; SchemaTracker m_schema_reader; - FrameMetadataT m_data; - DeviceDataTimestamp m_last_timestamp{}; + FrameMetadataTrackedT m_tracked; + DeviceDataTimestamp m_last_record_timestamp{}; std::vector m_pending_records; }; @@ -140,7 +147,7 @@ const SchemaTrackerConfig& FrameMetadataTrackerOak::get_config() const return m_config; } -const FrameMetadataT& FrameMetadataTrackerOak::get_data(const DeviceIOSession& session) const +const FrameMetadataTrackedT& FrameMetadataTrackerOak::get_data(const DeviceIOSession& session) const { return static_cast(session.get_tracker_impl(*this)).get_data(); } diff --git a/src/core/deviceio/cpp/full_body_tracker_pico.cpp b/src/core/deviceio/cpp/full_body_tracker_pico.cpp index 12072fb17..35476199e 100644 --- a/src/core/deviceio/cpp/full_body_tracker_pico.cpp +++ b/src/core/deviceio/cpp/full_body_tracker_pico.cpp @@ -6,6 +6,7 @@ #include "inc/deviceio/deviceio_session.hpp" #include +#include #include #include @@ -26,13 +27,13 @@ class FullBodyTrackerPicoImpl : public ITrackerImpl bool update(XrTime time) override; DeviceDataTimestamp serialize(flatbuffers::FlatBufferBuilder& builder, size_t channel_index) const override; - const FullBodyPosePicoT& get_body_pose() const; + const FullBodyPosePicoTrackedT& get_body_pose() const; private: XrSpace base_space_; XrBodyTrackerBD body_tracker_; - FullBodyPosePicoT body_pose_; - DeviceDataTimestamp last_timestamp_{}; + FullBodyPosePicoTrackedT tracked_; + DeviceDataTimestamp last_record_timestamp_{}; // Extension function pointers PFN_xrCreateBodyTrackerBD pfn_create_body_tracker_; @@ -100,7 +101,10 @@ FullBodyTrackerPicoImpl::FullBodyTrackerPicoImpl(const OpenXRSessionHandles& han throw std::runtime_error("Failed to create body tracker: " + std::to_string(result)); } - body_pose_.is_active = false; + tracked_.data = std::make_shared(); + tracked_.timestamp = std::make_shared(); + + tracked_.data->is_active = false; std::cout << "FullBodyTrackerPico initialized (24 joints)" << std::endl; } @@ -134,19 +138,22 @@ bool FullBodyTrackerPicoImpl::update(XrTime time) XrResult result = pfn_locate_body_joints_(body_tracker_, &locate_info, &locations); if (XR_FAILED(result)) { - body_pose_.is_active = false; + tracked_.data->is_active = false; return false; } // allJointPosesTracked indicates if all joint poses are valid - body_pose_.is_active = locations.allJointPosesTracked; + tracked_.data->is_active = locations.allJointPosesTracked; - last_timestamp_ = DeviceDataTimestamp(time, time, 0); + auto now_ns = + std::chrono::duration_cast(std::chrono::steady_clock::now().time_since_epoch()).count(); + *tracked_.timestamp = DeviceOutputTimestamp(now_ns, time, 0); + last_record_timestamp_ = DeviceDataTimestamp(time, time, 0); // Ensure joints struct is allocated - if (!body_pose_.joints) + if (!tracked_.data->joints) { - body_pose_.joints = std::make_unique(); + tracked_.data->joints = std::make_unique(); } for (uint32_t i = 0; i < XR_BODY_JOINT_COUNT_BD; ++i) @@ -164,28 +171,28 @@ bool FullBodyTrackerPicoImpl::update(XrTime time) // Create BodyJointPose and set it in the array BodyJointPose joint_pose(pose, is_valid); - body_pose_.joints->mutable_joints()->Mutate(i, joint_pose); + tracked_.data->joints->mutable_joints()->Mutate(i, joint_pose); } return true; } -const FullBodyPosePicoT& FullBodyTrackerPicoImpl::get_body_pose() const +const FullBodyPosePicoTrackedT& FullBodyTrackerPicoImpl::get_body_pose() const { - return body_pose_; + return tracked_; } DeviceDataTimestamp FullBodyTrackerPicoImpl::serialize(flatbuffers::FlatBufferBuilder& builder, size_t /* channel_index */) const { - auto data_offset = FullBodyPosePico::Pack(builder, &body_pose_); + auto data_offset = FullBodyPosePico::Pack(builder, tracked_.data.get()); FullBodyPosePicoRecordBuilder record_builder(builder); record_builder.add_data(data_offset); - record_builder.add_timestamp(&last_timestamp_); + record_builder.add_timestamp(&last_record_timestamp_); builder.Finish(record_builder.Finish()); - return last_timestamp_; + return last_record_timestamp_; } // ============================================================================ @@ -197,7 +204,7 @@ std::vector FullBodyTrackerPico::get_required_extensions() const return { XR_BD_BODY_TRACKING_EXTENSION_NAME }; } -const FullBodyPosePicoT& FullBodyTrackerPico::get_body_pose(const DeviceIOSession& session) const +const FullBodyPosePicoTrackedT& FullBodyTrackerPico::get_body_pose(const DeviceIOSession& session) const { return static_cast(session.get_tracker_impl(*this)).get_body_pose(); } diff --git a/src/core/deviceio/cpp/generic_3axis_pedal_tracker.cpp b/src/core/deviceio/cpp/generic_3axis_pedal_tracker.cpp index fb8aed351..f121fefb5 100644 --- a/src/core/deviceio/cpp/generic_3axis_pedal_tracker.cpp +++ b/src/core/deviceio/cpp/generic_3axis_pedal_tracker.cpp @@ -7,6 +7,7 @@ #include +#include #include namespace core @@ -21,6 +22,8 @@ class Generic3AxisPedalTracker::Impl : public ITrackerImpl public: Impl(const OpenXRSessionHandles& handles, SchemaTrackerConfig config) : m_schema_reader(handles, std::move(config)) { + m_tracked.data = std::make_shared(); + m_tracked.timestamp = std::make_shared(); } bool update(XrTime /* time */) override @@ -43,12 +46,16 @@ class Generic3AxisPedalTracker::Impl : public ITrackerImpl if (!m_pending_records.empty()) { - m_data = m_pending_records.back().data; - m_last_timestamp = m_pending_records.back().timestamp; + *m_tracked.data = m_pending_records.back().data; + auto now_ns = + std::chrono::duration_cast(std::chrono::steady_clock::now().time_since_epoch()) + .count(); + *m_tracked.timestamp = DeviceOutputTimestamp(now_ns, 0, 0); + m_last_record_timestamp = m_pending_records.back().timestamp; } else { - m_data.is_valid = false; + m_tracked.data->is_valid = false; } return true; @@ -56,14 +63,14 @@ class Generic3AxisPedalTracker::Impl : public ITrackerImpl DeviceDataTimestamp serialize(flatbuffers::FlatBufferBuilder& builder, size_t /* channel_index */) const override { - auto data_offset = Generic3AxisPedalOutput::Pack(builder, &m_data); + auto data_offset = Generic3AxisPedalOutput::Pack(builder, m_tracked.data.get()); Generic3AxisPedalOutputRecordBuilder record_builder(builder); record_builder.add_data(data_offset); - record_builder.add_timestamp(&m_last_timestamp); + record_builder.add_timestamp(&m_last_record_timestamp); builder.Finish(record_builder.Finish()); - return m_last_timestamp; + return m_last_record_timestamp; } void serialize_all(size_t /* channel_index */, const RecordCallback& callback) const override @@ -82,9 +89,9 @@ class Generic3AxisPedalTracker::Impl : public ITrackerImpl } } - const Generic3AxisPedalOutputT& get_data() const + const Generic3AxisPedalOutputTrackedT& get_data() const { - return m_data; + return m_tracked; } private: @@ -95,8 +102,8 @@ class Generic3AxisPedalTracker::Impl : public ITrackerImpl }; SchemaTracker m_schema_reader; - Generic3AxisPedalOutputT m_data; - DeviceDataTimestamp m_last_timestamp{}; + Generic3AxisPedalOutputTrackedT m_tracked; + DeviceDataTimestamp m_last_record_timestamp{}; std::vector m_pending_records; }; @@ -143,7 +150,7 @@ const SchemaTrackerConfig& Generic3AxisPedalTracker::get_config() const return m_config; } -const Generic3AxisPedalOutputT& Generic3AxisPedalTracker::get_data(const DeviceIOSession& session) const +const Generic3AxisPedalOutputTrackedT& Generic3AxisPedalTracker::get_data(const DeviceIOSession& session) const { return static_cast(session.get_tracker_impl(*this)).get_data(); } diff --git a/src/core/deviceio/cpp/hand_tracker.cpp b/src/core/deviceio/cpp/hand_tracker.cpp index 13e24d4c3..62aea74be 100644 --- a/src/core/deviceio/cpp/hand_tracker.cpp +++ b/src/core/deviceio/cpp/hand_tracker.cpp @@ -6,6 +6,7 @@ #include "inc/deviceio/deviceio_session.hpp" #include +#include #include #include @@ -80,24 +81,29 @@ HandTracker::Impl::Impl(const OpenXRSessionHandles& handles) throw std::runtime_error("Failed to create right hand tracker: " + std::to_string(result)); } - left_hand_.is_active = false; - right_hand_.is_active = false; + left_tracked_.data = std::make_shared(); + left_tracked_.timestamp = std::make_shared(); + right_tracked_.data = std::make_shared(); + right_tracked_.timestamp = std::make_shared(); + + left_tracked_.data->is_active = false; + right_tracked_.data->is_active = false; std::cout << "HandTracker initialized (left + right)" << std::endl; } DeviceDataTimestamp HandTracker::Impl::serialize(flatbuffers::FlatBufferBuilder& builder, size_t channel_index) const { - const HandPoseT& hand = (channel_index == 0) ? left_hand_ : right_hand_; + const HandPoseT& hand = (channel_index == 0) ? *left_tracked_.data : *right_tracked_.data; auto data_offset = HandPose::Pack(builder, &hand); HandPoseRecordBuilder record_builder(builder); record_builder.add_data(data_offset); - record_builder.add_timestamp(&last_timestamp_); + record_builder.add_timestamp(&last_record_timestamp_); builder.Finish(record_builder.Finish()); - return last_timestamp_; + return last_record_timestamp_; } HandTracker::Impl::~Impl() @@ -120,23 +126,27 @@ HandTracker::Impl::~Impl() // Override from ITrackerImpl 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_); + bool left_ok = update_hand(left_hand_tracker_, time, *left_tracked_.data); + bool right_ok = update_hand(right_hand_tracker_, time, *right_tracked_.data); - last_timestamp_ = DeviceDataTimestamp(time, time, 0); + auto now_ns = + std::chrono::duration_cast(std::chrono::steady_clock::now().time_since_epoch()).count(); + *left_tracked_.timestamp = DeviceOutputTimestamp(now_ns, time, 0); + *right_tracked_.timestamp = DeviceOutputTimestamp(now_ns, time, 0); + last_record_timestamp_ = DeviceDataTimestamp(time, time, 0); // Return true if at least one hand updated successfully return left_ok || right_ok; } -const HandPoseT& HandTracker::Impl::get_left_hand() const +const HandPoseTrackedT& HandTracker::Impl::get_left_hand() const { - return left_hand_; + return left_tracked_; } -const HandPoseT& HandTracker::Impl::get_right_hand() const +const HandPoseTrackedT& HandTracker::Impl::get_right_hand() const { - return right_hand_; + return right_tracked_; } bool HandTracker::Impl::update_hand(XrHandTrackerEXT tracker, XrTime time, HandPoseT& out_data) @@ -197,12 +207,12 @@ std::vector HandTracker::get_required_extensions() const return { XR_EXT_HAND_TRACKING_EXTENSION_NAME }; } -const HandPoseT& HandTracker::get_left_hand(const DeviceIOSession& session) const +const HandPoseTrackedT& HandTracker::get_left_hand(const DeviceIOSession& session) const { return static_cast(session.get_tracker_impl(*this)).get_left_hand(); } -const HandPoseT& HandTracker::get_right_hand(const DeviceIOSession& session) const +const HandPoseTrackedT& HandTracker::get_right_hand(const DeviceIOSession& session) const { return static_cast(session.get_tracker_impl(*this)).get_right_hand(); } diff --git a/src/core/deviceio/cpp/head_tracker.cpp b/src/core/deviceio/cpp/head_tracker.cpp index 821d9c0e5..055b35cbe 100644 --- a/src/core/deviceio/cpp/head_tracker.cpp +++ b/src/core/deviceio/cpp/head_tracker.cpp @@ -5,6 +5,7 @@ #include "inc/deviceio/deviceio_session.hpp" +#include #include #include @@ -24,9 +25,10 @@ HeadTracker::Impl::Impl(const OpenXRSessionHandles& handles) handles.session, { .type = XR_TYPE_REFERENCE_SPACE_CREATE_INFO, .referenceSpaceType = XR_REFERENCE_SPACE_TYPE_VIEW, - .poseInReferenceSpace = { .orientation = { 0, 0, 0, 1 } } })), - head_{} + .poseInReferenceSpace = { .orientation = { 0, 0, 0, 1 } } })) { + tracked_.data = std::make_shared(); + tracked_.timestamp = std::make_shared(); } // Override from ITrackerImpl @@ -38,7 +40,7 @@ bool HeadTracker::Impl::update(XrTime time) if (XR_FAILED(result)) { - head_.is_valid = false; + tracked_.data->is_valid = false; return false; } @@ -46,42 +48,45 @@ bool HeadTracker::Impl::update(XrTime time) bool position_valid = (location.locationFlags & XR_SPACE_LOCATION_POSITION_VALID_BIT) != 0; bool orientation_valid = (location.locationFlags & XR_SPACE_LOCATION_ORIENTATION_VALID_BIT) != 0; - head_.is_valid = position_valid && orientation_valid; + tracked_.data->is_valid = position_valid && orientation_valid; - last_timestamp_ = DeviceDataTimestamp(time, time, 0); + auto now_ns = + std::chrono::duration_cast(std::chrono::steady_clock::now().time_since_epoch()).count(); + *tracked_.timestamp = DeviceOutputTimestamp(now_ns, time, 0); + last_record_timestamp_ = DeviceDataTimestamp(time, time, 0); - if (head_.is_valid) + if (tracked_.data->is_valid) { // Create pose from position and orientation using FlatBuffers structs Point position(location.pose.position.x, location.pose.position.y, location.pose.position.z); Quaternion orientation(location.pose.orientation.x, location.pose.orientation.y, location.pose.orientation.z, location.pose.orientation.w); - head_.pose = std::make_shared(position, orientation); + tracked_.data->pose = std::make_shared(position, orientation); } else { // Invalid - reset pose - head_.pose.reset(); + tracked_.data->pose.reset(); } return true; } -const HeadPoseT& HeadTracker::Impl::get_head() const +const HeadPoseTrackedT& HeadTracker::Impl::get_head() const { - return head_; + return tracked_; } DeviceDataTimestamp HeadTracker::Impl::serialize(flatbuffers::FlatBufferBuilder& builder, size_t /* channel_index */) const { - auto data_offset = HeadPose::Pack(builder, &head_); + auto data_offset = HeadPose::Pack(builder, tracked_.data.get()); HeadPoseRecordBuilder record_builder(builder); record_builder.add_data(data_offset); - record_builder.add_timestamp(&last_timestamp_); + record_builder.add_timestamp(&last_record_timestamp_); builder.Finish(record_builder.Finish()); - return last_timestamp_; + return last_record_timestamp_; } // ============================================================================ @@ -94,7 +99,7 @@ std::vector HeadTracker::get_required_extensions() const return {}; } -const HeadPoseT& HeadTracker::get_head(const DeviceIOSession& session) const +const HeadPoseTrackedT& HeadTracker::get_head(const DeviceIOSession& session) const { return static_cast(session.get_tracker_impl(*this)).get_head(); } diff --git a/src/core/deviceio/cpp/inc/deviceio/controller_tracker.hpp b/src/core/deviceio/cpp/inc/deviceio/controller_tracker.hpp index 5b50dd71f..29d926d7e 100644 --- a/src/core/deviceio/cpp/inc/deviceio/controller_tracker.hpp +++ b/src/core/deviceio/cpp/inc/deviceio/controller_tracker.hpp @@ -43,12 +43,9 @@ class ControllerTracker : public ITracker 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; + // Query methods - public API for getting individual controller data (returns tracked output with timestamp) + const ControllerSnapshotTrackedT& get_left_controller(const DeviceIOSession& session) const; + const ControllerSnapshotTrackedT& get_right_controller(const DeviceIOSession& session) const; private: static constexpr const char* TRACKER_NAME = "ControllerTracker"; @@ -66,9 +63,8 @@ class ControllerTracker : public ITracker DeviceDataTimestamp serialize(flatbuffers::FlatBufferBuilder& builder, size_t channel_index) const override; - const ControllerSnapshot& get_left_controller() const; - const ControllerSnapshot& get_right_controller() const; - XrTime get_last_update_time() const; + const ControllerSnapshotTrackedT& get_left_controller() const; + const ControllerSnapshotTrackedT& get_right_controller() const; private: const OpenXRCoreFunctions core_funcs_; @@ -97,12 +93,12 @@ class ControllerTracker : public ITracker XrSpacePtr left_aim_space_; XrSpacePtr right_aim_space_; - // Controller snapshots stored separately - ControllerSnapshot left_controller_{}; - ControllerSnapshot right_controller_{}; + // Tracked output (data + DeviceOutputTimestamp) + ControllerSnapshotTrackedT left_tracked_; + ControllerSnapshotTrackedT right_tracked_; - // Timestamp from last update - DeviceDataTimestamp last_timestamp_{}; + // Record timestamp for MCAP + DeviceDataTimestamp last_record_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 2e2c3b111..ab8a6bed7 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 @@ -51,9 +51,9 @@ class FrameMetadataTrackerOak : public ITracker std::vector get_record_channels() const override; /*! - * @brief Get the current frame metadata. + * @brief Get the current frame metadata (returns tracked output with timestamp). */ - const FrameMetadataT& get_data(const DeviceIOSession& session) const; + const FrameMetadataTrackedT& get_data(const DeviceIOSession& session) const; protected: const SchemaTrackerConfig& get_config() const; 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 e6167b0d4..69f375278 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 @@ -46,8 +46,8 @@ class FullBodyTrackerPico : public ITracker return { "full_body" }; } - // Query method - public API for getting body pose data - const FullBodyPosePicoT& get_body_pose(const DeviceIOSession& session) const; + // Query method - public API for getting body pose data (returns tracked output with timestamp) + const FullBodyPosePicoTrackedT& get_body_pose(const DeviceIOSession& session) const; private: std::shared_ptr create_tracker(const OpenXRSessionHandles& handles) const override; 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 3182da13c..1256dc5ca 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 @@ -51,9 +51,9 @@ class Generic3AxisPedalTracker : public ITracker std::vector get_record_channels() const override; /*! - * @brief Get the current foot pedal data. + * @brief Get the current foot pedal data (returns tracked output with timestamp). */ - const Generic3AxisPedalOutputT& get_data(const DeviceIOSession& session) const; + const Generic3AxisPedalOutputTrackedT& get_data(const DeviceIOSession& session) const; protected: /*! diff --git a/src/core/deviceio/cpp/inc/deviceio/hand_tracker.hpp b/src/core/deviceio/cpp/inc/deviceio/hand_tracker.hpp index 561f2f055..e5cd004c8 100644 --- a/src/core/deviceio/cpp/inc/deviceio/hand_tracker.hpp +++ b/src/core/deviceio/cpp/inc/deviceio/hand_tracker.hpp @@ -41,9 +41,9 @@ class HandTracker : public ITracker return { "left_hand", "right_hand" }; } - // Query methods - public API for getting hand data - const HandPoseT& get_left_hand(const DeviceIOSession& session) const; - const HandPoseT& get_right_hand(const DeviceIOSession& session) const; + // Query methods - public API for getting hand data (returns tracked output with timestamp) + const HandPoseTrackedT& get_left_hand(const DeviceIOSession& session) const; + const HandPoseTrackedT& get_right_hand(const DeviceIOSession& session) const; // Get joint name for debugging static std::string get_joint_name(uint32_t joint_index); @@ -67,8 +67,8 @@ class HandTracker : public ITracker DeviceDataTimestamp serialize(flatbuffers::FlatBufferBuilder& builder, size_t channel_index) const override; - const HandPoseT& get_left_hand() const; - const HandPoseT& get_right_hand() const; + const HandPoseTrackedT& get_left_hand() const; + const HandPoseTrackedT& get_right_hand() const; private: bool update_hand(XrHandTrackerEXT tracker, XrTime time, HandPoseT& out_data); @@ -79,12 +79,12 @@ class HandTracker : public ITracker XrHandTrackerEXT left_hand_tracker_; XrHandTrackerEXT right_hand_tracker_; - // Hand data - HandPoseT left_hand_; - HandPoseT right_hand_; + // Tracked output (data + DeviceOutputTimestamp) + HandPoseTrackedT left_tracked_; + HandPoseTrackedT right_tracked_; - // Timestamp from last update - DeviceDataTimestamp last_timestamp_{}; + // Record timestamp for MCAP + DeviceDataTimestamp last_record_timestamp_{}; // Extension function pointers PFN_xrCreateHandTrackerEXT pfn_create_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 833276e57..c80f3856b 100644 --- a/src/core/deviceio/cpp/inc/deviceio/head_tracker.hpp +++ b/src/core/deviceio/cpp/inc/deviceio/head_tracker.hpp @@ -42,8 +42,8 @@ class HeadTracker : public ITracker return { "head" }; } - // Query methods - public API for getting head data - const HeadPoseT& get_head(const DeviceIOSession& session) const; + // Query methods - public API for getting head data (returns tracked output with timestamp) + const HeadPoseTrackedT& get_head(const DeviceIOSession& session) const; private: static constexpr const char* TRACKER_NAME = "HeadTracker"; @@ -61,14 +61,14 @@ class HeadTracker : public ITracker DeviceDataTimestamp serialize(flatbuffers::FlatBufferBuilder& builder, size_t channel_index) const override; - const HeadPoseT& get_head() const; + const HeadPoseTrackedT& get_head() const; private: const OpenXRCoreFunctions core_funcs_; XrSpace base_space_; XrSpacePtr view_space_; - HeadPoseT head_; - DeviceDataTimestamp last_timestamp_{}; + HeadPoseTrackedT tracked_; + DeviceDataTimestamp last_record_timestamp_{}; }; }; diff --git a/src/core/deviceio/python/deviceio_bindings.cpp b/src/core/deviceio/python/deviceio_bindings.cpp index 998374b23..d8f74b30b 100644 --- a/src/core/deviceio/python/deviceio_bindings.cpp +++ b/src/core/deviceio/python/deviceio_bindings.cpp @@ -15,6 +15,72 @@ PYBIND11_MODULE(_deviceio, m) { m.doc() = "Isaac Teleop DeviceIO - Device I/O Module"; + // DeviceOutputTimestamp (FlatBuffer struct for tracker output timestamps) + py::class_(m, "DeviceOutputTimestamp") + .def_property_readonly("query_time_common_clock", &core::DeviceOutputTimestamp::query_time_common_clock) + .def_property_readonly("target_time_common_clock", &core::DeviceOutputTimestamp::target_time_common_clock) + .def_property_readonly("target_time_consumer_clock", &core::DeviceOutputTimestamp::target_time_consumer_clock) + .def("__repr__", + [](const core::DeviceOutputTimestamp& self) + { + return "DeviceOutputTimestamp(query_time=" + std::to_string(self.query_time_common_clock()) + + ", target_time=" + std::to_string(self.target_time_common_clock()) + + ", target_time_consumer=" + std::to_string(self.target_time_consumer_clock()) + ")"; + }); + + // TrackedT wrapper types (pair data + DeviceOutputTimestamp from each tracker) + py::class_(m, "HandPoseTrackedT") + .def_property_readonly( + "data", [](const core::HandPoseTrackedT& self) -> const core::HandPoseT* { return self.data.get(); }, + py::return_value_policy::reference_internal) + .def_property_readonly( + "timestamp", + [](const core::HandPoseTrackedT& self) -> const core::DeviceOutputTimestamp* { return self.timestamp.get(); }, + py::return_value_policy::reference_internal); + + py::class_(m, "HeadPoseTrackedT") + .def_property_readonly( + "data", [](const core::HeadPoseTrackedT& self) -> const core::HeadPoseT* { return self.data.get(); }, + py::return_value_policy::reference_internal) + .def_property_readonly( + "timestamp", + [](const core::HeadPoseTrackedT& self) -> const core::DeviceOutputTimestamp* { return self.timestamp.get(); }, + py::return_value_policy::reference_internal); + + py::class_(m, "ControllerSnapshotTrackedT") + .def_property_readonly( + "data", + [](const core::ControllerSnapshotTrackedT& self) -> const core::ControllerSnapshot* + { return self.data.get(); }, + py::return_value_policy::reference_internal) + .def_property_readonly( + "timestamp", + [](const core::ControllerSnapshotTrackedT& self) -> const core::DeviceOutputTimestamp* + { return self.timestamp.get(); }, + py::return_value_policy::reference_internal); + + py::class_(m, "FrameMetadataTrackedT") + .def_property_readonly( + "data", + [](const core::FrameMetadataTrackedT& self) -> const core::FrameMetadataT* { return self.data.get(); }, + py::return_value_policy::reference_internal) + .def_property_readonly( + "timestamp", + [](const core::FrameMetadataTrackedT& self) -> const core::DeviceOutputTimestamp* + { return self.timestamp.get(); }, + py::return_value_policy::reference_internal); + + py::class_(m, "FullBodyPosePicoTrackedT") + .def_property_readonly( + "data", + [](const core::FullBodyPosePicoTrackedT& self) -> const core::FullBodyPosePicoT* { return self.data.get(); }, + py::return_value_policy::reference_internal) + .def_property_readonly( + "timestamp", + [](const core::FullBodyPosePicoTrackedT& self) -> const core::DeviceOutputTimestamp* + { return self.timestamp.get(); }, + py::return_value_policy::reference_internal); + // ITracker interface (base class) py::class_>(m, "ITracker").def("get_name", &core::ITracker::get_name); @@ -23,12 +89,12 @@ PYBIND11_MODULE(_deviceio, m) .def(py::init<>()) .def( "get_left_hand", - [](core::HandTracker& self, PyDeviceIOSession& session) -> const core::HandPoseT& + [](core::HandTracker& self, PyDeviceIOSession& session) -> const core::HandPoseTrackedT& { return self.get_left_hand(session.native()); }, py::arg("session"), py::return_value_policy::reference_internal) .def( "get_right_hand", - [](core::HandTracker& self, PyDeviceIOSession& session) -> const core::HandPoseT& + [](core::HandTracker& self, PyDeviceIOSession& session) -> const core::HandPoseTrackedT& { return self.get_right_hand(session.native()); }, py::arg("session"), py::return_value_policy::reference_internal) .def_static("get_joint_name", &core::HandTracker::get_joint_name); @@ -38,7 +104,7 @@ PYBIND11_MODULE(_deviceio, m) .def(py::init<>()) .def( "get_head", - [](core::HeadTracker& self, PyDeviceIOSession& session) -> const core::HeadPoseT& + [](core::HeadTracker& self, PyDeviceIOSession& session) -> const core::HeadPoseTrackedT& { return self.get_head(session.native()); }, py::arg("session"), py::return_value_policy::reference_internal); @@ -47,19 +113,14 @@ PYBIND11_MODULE(_deviceio, m) .def(py::init<>()) .def( "get_left_controller", - [](core::ControllerTracker& self, PyDeviceIOSession& session) -> const core::ControllerSnapshot& + [](core::ControllerTracker& self, PyDeviceIOSession& session) -> const core::ControllerSnapshotTrackedT& { return self.get_left_controller(session.native()); }, - py::arg("session"), py::return_value_policy::reference_internal, "Get the left controller snapshot") + py::arg("session"), py::return_value_policy::reference_internal, "Get the left controller tracked output") .def( "get_right_controller", - [](core::ControllerTracker& self, PyDeviceIOSession& session) -> const core::ControllerSnapshot& + [](core::ControllerTracker& self, PyDeviceIOSession& session) -> const core::ControllerSnapshotTrackedT& { 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"); + py::arg("session"), py::return_value_policy::reference_internal, "Get the right controller tracked output"); // FrameMetadataTrackerOak class py::class_>( @@ -69,10 +130,10 @@ PYBIND11_MODULE(_deviceio, m) "Construct a FrameMetadataTrackerOak for the given tensor collection ID") .def( "get_data", - [](core::FrameMetadataTrackerOak& self, PyDeviceIOSession& session) -> const core::FrameMetadataT& + [](core::FrameMetadataTrackerOak& self, PyDeviceIOSession& session) -> const core::FrameMetadataTrackedT& { return self.get_data(session.native()); }, py::arg("session"), py::return_value_policy::reference_internal, - "Get the current frame metadata (timestamp and sequence_number)"); + "Get the current frame metadata tracked output (data + timestamp)"); // FullBodyTrackerPico class (PICO XR_BD_body_tracking extension) py::class_>( @@ -80,10 +141,10 @@ PYBIND11_MODULE(_deviceio, m) .def(py::init<>()) .def( "get_body_pose", - [](core::FullBodyTrackerPico& self, PyDeviceIOSession& session) -> const core::FullBodyPosePicoT& + [](core::FullBodyTrackerPico& self, PyDeviceIOSession& session) -> const core::FullBodyPosePicoTrackedT& { return self.get_body_pose(session.native()); }, py::arg("session"), py::return_value_policy::reference_internal, - "Get full body pose data (24 joints from pelvis to hands)"); + "Get full body pose tracked output (24 joints from pelvis to hands)"); // DeviceIOSession class (bound via wrapper for context management) // Other C++ modules (like mcap) should include and accept diff --git a/src/core/deviceio/python/deviceio_init.py b/src/core/deviceio/python/deviceio_init.py index 585ee7f42..c5d19670a 100644 --- a/src/core/deviceio/python/deviceio_init.py +++ b/src/core/deviceio/python/deviceio_init.py @@ -5,11 +5,15 @@ This module provides trackers and teleop session functionality. -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_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. +Tracker getters return TrackedT wrapper types containing `.data` (the raw data) +and `.timestamp` (DeviceOutputTimestamp with query/target times): + HandTracker.get_left_hand(session) / get_right_hand(session) -> HandPoseTrackedT + HeadTracker.get_head(session) -> HeadPoseTrackedT + ControllerTracker.get_left_controller(session) / get_right_controller(session) -> ControllerSnapshotTrackedT + FrameMetadataTrackerOak.get_data(session) -> FrameMetadataTrackedT + FullBodyTrackerPico.get_body_pose(session) -> FullBodyPosePicoTrackedT + +Import raw data types from isaacteleop.schema if you need to work with pose types. """ from ._deviceio import ( @@ -20,6 +24,12 @@ FrameMetadataTrackerOak, FullBodyTrackerPico, DeviceIOSession, + DeviceOutputTimestamp, + HandPoseTrackedT, + HeadPoseTrackedT, + ControllerSnapshotTrackedT, + FrameMetadataTrackedT, + FullBodyPosePicoTrackedT, NUM_JOINTS, JOINT_PALM, JOINT_WRIST, @@ -44,7 +54,13 @@ "ControllerPose", "ControllerSnapshot", "DeviceDataTimestamp", + "DeviceOutputTimestamp", "FrameMetadata", + "HandPoseTrackedT", + "HeadPoseTrackedT", + "ControllerSnapshotTrackedT", + "FrameMetadataTrackedT", + "FullBodyPosePicoTrackedT", "ITracker", "HandTracker", "HeadTracker", diff --git a/src/core/schema/fbs/controller.fbs b/src/core/schema/fbs/controller.fbs index 24ceaa4b0..a9f8eaab1 100644 --- a/src/core/schema/fbs/controller.fbs +++ b/src/core/schema/fbs/controller.fbs @@ -50,4 +50,10 @@ table ControllerSnapshotRecord { timestamp: DeviceDataTimestamp (id: 1); } +// Tracker output wrapper: pairs a ControllerSnapshot with a DeviceOutputTimestamp. +table ControllerSnapshotTracked { + data: ControllerSnapshot (id: 0); + timestamp: DeviceOutputTimestamp (id: 1); +} + root_type ControllerSnapshotRecord; diff --git a/src/core/schema/fbs/full_body.fbs b/src/core/schema/fbs/full_body.fbs index 82f7c0491..594b5c21d 100644 --- a/src/core/schema/fbs/full_body.fbs +++ b/src/core/schema/fbs/full_body.fbs @@ -68,4 +68,10 @@ table FullBodyPosePicoRecord { timestamp: DeviceDataTimestamp (id: 1); } +// Tracker output wrapper: pairs a FullBodyPosePico with a DeviceOutputTimestamp. +table FullBodyPosePicoTracked { + data: FullBodyPosePico (id: 0); + timestamp: DeviceOutputTimestamp (id: 1); +} + root_type FullBodyPosePicoRecord; diff --git a/src/core/schema/fbs/hand.fbs b/src/core/schema/fbs/hand.fbs index 16c5eaa3f..b1744443e 100644 --- a/src/core/schema/fbs/hand.fbs +++ b/src/core/schema/fbs/hand.fbs @@ -39,4 +39,10 @@ table HandPoseRecord { timestamp: DeviceDataTimestamp (id: 1); } +// Tracker output wrapper: pairs a HandPose with a DeviceOutputTimestamp. +table HandPoseTracked { + data: HandPose (id: 0); + timestamp: DeviceOutputTimestamp (id: 1); +} + root_type HandPoseRecord; diff --git a/src/core/schema/fbs/head.fbs b/src/core/schema/fbs/head.fbs index 2efa3e631..f6fbbffb4 100644 --- a/src/core/schema/fbs/head.fbs +++ b/src/core/schema/fbs/head.fbs @@ -21,4 +21,10 @@ table HeadPoseRecord { timestamp: DeviceDataTimestamp (id: 1); } +// Tracker output wrapper: pairs a HeadPose with a DeviceOutputTimestamp. +table HeadPoseTracked { + data: HeadPose (id: 0); + timestamp: DeviceOutputTimestamp (id: 1); +} + root_type HeadPoseRecord; diff --git a/src/core/schema/fbs/locomotion.fbs b/src/core/schema/fbs/locomotion.fbs index 9e905cfc3..4cc798ade 100644 --- a/src/core/schema/fbs/locomotion.fbs +++ b/src/core/schema/fbs/locomotion.fbs @@ -22,4 +22,10 @@ table LocomotionCommandRecord { timestamp: DeviceDataTimestamp (id: 1); } +// Tracker output wrapper: pairs a LocomotionCommand with a DeviceOutputTimestamp. +table LocomotionCommandTracked { + data: LocomotionCommand (id: 0); + timestamp: DeviceOutputTimestamp (id: 1); +} + root_type LocomotionCommandRecord; diff --git a/src/core/schema/fbs/oak.fbs b/src/core/schema/fbs/oak.fbs index 14fd7add2..8cba5400b 100644 --- a/src/core/schema/fbs/oak.fbs +++ b/src/core/schema/fbs/oak.fbs @@ -16,4 +16,10 @@ table FrameMetadataRecord { timestamp: DeviceDataTimestamp (id: 1); } +// Tracker output wrapper: pairs a FrameMetadata with a DeviceOutputTimestamp. +table FrameMetadataTracked { + data: FrameMetadata (id: 0); + timestamp: DeviceOutputTimestamp (id: 1); +} + root_type FrameMetadataRecord; diff --git a/src/core/schema/fbs/pedals.fbs b/src/core/schema/fbs/pedals.fbs index 9e79c8410..5028b9516 100644 --- a/src/core/schema/fbs/pedals.fbs +++ b/src/core/schema/fbs/pedals.fbs @@ -27,4 +27,10 @@ table Generic3AxisPedalOutputRecord { timestamp: DeviceDataTimestamp (id: 1); } +// Tracker output wrapper: pairs a Generic3AxisPedalOutput with a DeviceOutputTimestamp. +table Generic3AxisPedalOutputTracked { + data: Generic3AxisPedalOutput (id: 0); + timestamp: DeviceOutputTimestamp (id: 1); +} + root_type Generic3AxisPedalOutputRecord; diff --git a/src/core/schema/fbs/timestamp.fbs b/src/core/schema/fbs/timestamp.fbs index 68ceaf69b..d10681b80 100644 --- a/src/core/schema/fbs/timestamp.fbs +++ b/src/core/schema/fbs/timestamp.fbs @@ -24,3 +24,22 @@ struct DeviceDataTimestamp { // Useful for measuring pipeline latency (available - sample). available_time_common_clock: int64; } + +// Timestamp attached to tracker output (what the consumer receives). +// +// All timestamps are in nanoseconds. The "common clock" is the system monotonic +// clock (CLOCK_MONOTONIC on Linux, QueryPerformanceCounter on Windows). +// The "consumer clock" is the application-domain clock, which may differ from +// real time (e.g., a simulation running slower than real time). +struct DeviceOutputTimestamp { + // When the tracker query was executed, in the common clock domain. + query_time_common_clock: int64; + + // The target prediction time requested, in the common clock domain. + // Zero if not applicable (e.g., for historical tensor data). + target_time_common_clock: int64; + + // The target time in the consumer's application-domain clock. + // TODO: Not yet wired — will be passed into update() in a future change. + target_time_consumer_clock: int64; +} diff --git a/src/plugins/controller_synthetic_hands/synthetic_hands_plugin.cpp b/src/plugins/controller_synthetic_hands/synthetic_hands_plugin.cpp index 785ccf731..bf460cc0a 100644 --- a/src/plugins/controller_synthetic_hands/synthetic_hands_plugin.cpp +++ b/src/plugins/controller_synthetic_hands/synthetic_hands_plugin.cpp @@ -73,9 +73,11 @@ void SyntheticHandsPlugin::worker_thread() continue; } - 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); + const auto& left_tracked = m_controller_tracker->get_left_controller(*m_deviceio_session); + const auto& right_tracked = m_controller_tracker->get_right_controller(*m_deviceio_session); + const auto& left_ctrl = *left_tracked.data; + const auto& right_ctrl = *right_tracked.data; + XrTime time = left_tracked.timestamp->target_time_common_clock(); float left_target = left_ctrl.inputs().trigger_value(); float right_target = right_ctrl.inputs().trigger_value(); diff --git a/src/plugins/manus/core/manus_hand_tracking_plugin.cpp b/src/plugins/manus/core/manus_hand_tracking_plugin.cpp index da04442c8..ca2d48811 100644 --- a/src/plugins/manus/core/manus_hand_tracking_plugin.cpp +++ b/src/plugins/manus/core/manus_hand_tracking_plugin.cpp @@ -342,9 +342,9 @@ void ManusTracker::inject_hand_data() right_nodes = m_right_hand_nodes; } - 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); + const auto& left_tracked = m_controller_tracker->get_left_controller(*m_deviceio_session); + const auto& right_tracked = m_controller_tracker->get_right_controller(*m_deviceio_session); + XrTime time = left_tracked.timestamp->target_time_common_clock(); auto process_hand = [&](const std::vector& nodes, bool is_left) { @@ -357,7 +357,7 @@ 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; - const core::ControllerSnapshot& snapshot = is_left ? left_ctrl : right_ctrl; + const core::ControllerSnapshot& snapshot = is_left ? *left_tracked.data : *right_tracked.data; if (snapshot.is_active()) { From 53b7c0ac71a5dade39b7a9f170da996dfaf95af5 Mon Sep 17 00:00:00 2001 From: Devdeep Ray Date: Fri, 20 Feb 2026 11:43:51 -0800 Subject: [PATCH 4/6] Revert "Add tracked data and device output timestamp" This reverts commit b0ee3a86595c82b8a749560a6cc6d5ab646e50bc. --- examples/oxr/cpp/oxr_session_sharing.cpp | 8 +- examples/oxr/cpp/oxr_simple_api_demo.cpp | 10 +- examples/schemaio/frame_metadata_printer.cpp | 3 +- examples/schemaio/pedal_printer.cpp | 3 +- src/core/deviceio/cpp/controller_tracker.cpp | 46 ++++----- .../cpp/frame_metadata_tracker_oak.cpp | 27 ++---- .../deviceio/cpp/full_body_tracker_pico.cpp | 39 ++++---- .../cpp/generic_3axis_pedal_tracker.cpp | 29 +++--- src/core/deviceio/cpp/hand_tracker.cpp | 38 +++----- src/core/deviceio/cpp/head_tracker.cpp | 33 +++---- .../cpp/inc/deviceio/controller_tracker.hpp | 24 +++-- .../deviceio/frame_metadata_tracker_oak.hpp | 4 +- .../inc/deviceio/full_body_tracker_pico.hpp | 4 +- .../deviceio/generic_3axis_pedal_tracker.hpp | 4 +- .../cpp/inc/deviceio/hand_tracker.hpp | 20 ++-- .../cpp/inc/deviceio/head_tracker.hpp | 10 +- .../deviceio/python/deviceio_bindings.cpp | 93 ++++--------------- src/core/deviceio/python/deviceio_init.py | 26 +----- src/core/schema/fbs/controller.fbs | 6 -- src/core/schema/fbs/full_body.fbs | 6 -- src/core/schema/fbs/hand.fbs | 6 -- src/core/schema/fbs/head.fbs | 6 -- src/core/schema/fbs/locomotion.fbs | 6 -- src/core/schema/fbs/oak.fbs | 6 -- src/core/schema/fbs/pedals.fbs | 6 -- src/core/schema/fbs/timestamp.fbs | 19 ---- .../synthetic_hands_plugin.cpp | 8 +- .../manus/core/manus_hand_tracking_plugin.cpp | 8 +- 28 files changed, 162 insertions(+), 336 deletions(-) diff --git a/examples/oxr/cpp/oxr_session_sharing.cpp b/examples/oxr/cpp/oxr_session_sharing.cpp index baca1cdbe..021f81bd0 100644 --- a/examples/oxr/cpp/oxr_session_sharing.cpp +++ b/examples/oxr/cpp/oxr_session_sharing.cpp @@ -95,11 +95,11 @@ try if (i % 3 == 0) { std::cout << "Frame " << i << ": " - << "Hands=" << (left.data->is_active ? "ACTIVE" : "INACTIVE") << " | " - << "Head=" << (head.data->is_valid ? "VALID" : "INVALID"); - if (head.data->is_valid && head.data->pose) + << "Hands=" << (left.is_active ? "ACTIVE" : "INACTIVE") << " | " + << "Head=" << (head.is_valid ? "VALID" : "INVALID"); + if (head.is_valid && head.pose) { - const auto& pos = head.data->pose->position(); + const auto& pos = head.pose->position(); std::cout << " [" << pos.x() << ", " << pos.y() << ", " << pos.z() << "]"; } std::cout << std::endl; diff --git a/examples/oxr/cpp/oxr_simple_api_demo.cpp b/examples/oxr/cpp/oxr_simple_api_demo.cpp index 79ebd5c46..28f5e900d 100644 --- a/examples/oxr/cpp/oxr_simple_api_demo.cpp +++ b/examples/oxr/cpp/oxr_simple_api_demo.cpp @@ -87,13 +87,13 @@ try const auto& head = head_tracker->get_head(*session); std::cout << "Frame " << i << ":" << std::endl; - std::cout << " Left hand: " << (left.data->is_active ? "ACTIVE" : "INACTIVE") << std::endl; - std::cout << " Right hand: " << (right.data->is_active ? "ACTIVE" : "INACTIVE") << std::endl; - std::cout << " Head pose: " << (head.data->is_valid ? "VALID" : "INVALID") << std::endl; + std::cout << " Left hand: " << (left.is_active ? "ACTIVE" : "INACTIVE") << std::endl; + std::cout << " Right hand: " << (right.is_active ? "ACTIVE" : "INACTIVE") << std::endl; + std::cout << " Head pose: " << (head.is_valid ? "VALID" : "INVALID") << std::endl; - if (head.data->is_valid && head.data->pose) + if (head.is_valid && head.pose) { - const auto& pos = head.data->pose->position(); + const auto& pos = head.pose->position(); std::cout << " Position: [" << pos.x() << ", " << pos.y() << ", " << pos.z() << "]" << std::endl; } std::cout << std::endl; diff --git a/examples/schemaio/frame_metadata_printer.cpp b/examples/schemaio/frame_metadata_printer.cpp index c25984549..f90e728ce 100644 --- a/examples/schemaio/frame_metadata_printer.cpp +++ b/examples/schemaio/frame_metadata_printer.cpp @@ -114,8 +114,7 @@ try } // Print when we have new data (sequence_number changed) - const auto& tracked = tracker->get_data(*session); - const auto& data = *tracked.data; + const auto& data = tracker->get_data(*session); if (data.sequence_number != last_printed_sequence) { print_frame_metadata(data, ++received_count); diff --git a/examples/schemaio/pedal_printer.cpp b/examples/schemaio/pedal_printer.cpp index 441aa8668..40eb6cd50 100644 --- a/examples/schemaio/pedal_printer.cpp +++ b/examples/schemaio/pedal_printer.cpp @@ -76,8 +76,7 @@ try } // Print current data if available - const auto& tracked = tracker->get_data(*session); - const auto& data = *tracked.data; + const auto& data = tracker->get_data(*session); if (data.is_valid) { print_pedal_data(data, received_count++); diff --git a/src/core/deviceio/cpp/controller_tracker.cpp b/src/core/deviceio/cpp/controller_tracker.cpp index 192799d26..dbf86adb2 100644 --- a/src/core/deviceio/cpp/controller_tracker.cpp +++ b/src/core/deviceio/cpp/controller_tracker.cpp @@ -6,7 +6,6 @@ #include "inc/deviceio/deviceio_session.hpp" #include -#include #include #include #include @@ -260,11 +259,6 @@ ControllerTracker::Impl::Impl(const OpenXRSessionHandles& handles) throw std::runtime_error("Failed to attach action sets: " + std::to_string(result)); } - left_tracked_.data = std::make_shared(); - left_tracked_.timestamp = std::make_shared(); - right_tracked_.data = std::make_shared(); - right_tracked_.timestamp = std::make_shared(); - std::cout << "ControllerTracker initialized (left + right)" << std::endl; } @@ -341,38 +335,39 @@ bool ControllerTracker::Impl::update(XrTime time) snapshot_out = ControllerSnapshot(grip_pose, aim_pose, inputs, is_active); }; - update_controller(left_hand_path_, left_grip_space_, left_aim_space_, *left_tracked_.data); - update_controller(right_hand_path_, right_grip_space_, right_aim_space_, *right_tracked_.data); + 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); - auto now_ns = - std::chrono::duration_cast(std::chrono::steady_clock::now().time_since_epoch()).count(); - *left_tracked_.timestamp = DeviceOutputTimestamp(now_ns, time, 0); - *right_tracked_.timestamp = DeviceOutputTimestamp(now_ns, time, 0); - last_record_timestamp_ = DeviceDataTimestamp(time, time, 0); + return left_controller_.is_active() || right_controller_.is_active(); +} - return left_tracked_.data->is_active() || right_tracked_.data->is_active(); +const ControllerSnapshot& ControllerTracker::Impl::get_left_controller() const +{ + return left_controller_; } -const ControllerSnapshotTrackedT& ControllerTracker::Impl::get_left_controller() const +const ControllerSnapshot& ControllerTracker::Impl::get_right_controller() const { - return left_tracked_; + return right_controller_; } -const ControllerSnapshotTrackedT& ControllerTracker::Impl::get_right_controller() const +XrTime ControllerTracker::Impl::get_last_update_time() const { - return right_tracked_; + 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_tracked_.data : *right_tracked_.data; + const ControllerSnapshot& snapshot = (channel_index == 0) ? left_controller_ : right_controller_; ControllerSnapshotRecordBuilder record_builder(builder); record_builder.add_data(&snapshot); - record_builder.add_timestamp(&last_record_timestamp_); + record_builder.add_timestamp(&last_timestamp_); builder.Finish(record_builder.Finish()); - return last_record_timestamp_; + return last_timestamp_; } // ============================================================================ @@ -385,16 +380,21 @@ std::vector ControllerTracker::get_required_extensions() const return {}; } -const ControllerSnapshotTrackedT& ControllerTracker::get_left_controller(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 ControllerSnapshotTrackedT& ControllerTracker::get_right_controller(const DeviceIOSession& session) const +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_last_update_time(); +} + std::shared_ptr ControllerTracker::create_tracker(const OpenXRSessionHandles& handles) const { return std::make_shared(handles); diff --git a/src/core/deviceio/cpp/frame_metadata_tracker_oak.cpp b/src/core/deviceio/cpp/frame_metadata_tracker_oak.cpp index b60cfb900..c18b5edac 100644 --- a/src/core/deviceio/cpp/frame_metadata_tracker_oak.cpp +++ b/src/core/deviceio/cpp/frame_metadata_tracker_oak.cpp @@ -8,7 +8,6 @@ #include #include -#include #include namespace core @@ -23,8 +22,6 @@ class FrameMetadataTrackerOak::Impl : public ITrackerImpl public: Impl(const OpenXRSessionHandles& handles, SchemaTrackerConfig config) : m_schema_reader(handles, std::move(config)) { - m_tracked.data = std::make_shared(); - m_tracked.timestamp = std::make_shared(); } bool update(XrTime /* time */) override @@ -47,12 +44,8 @@ class FrameMetadataTrackerOak::Impl : public ITrackerImpl if (!m_pending_records.empty()) { - *m_tracked.data = m_pending_records.back().data; - auto now_ns = - std::chrono::duration_cast(std::chrono::steady_clock::now().time_since_epoch()) - .count(); - *m_tracked.timestamp = DeviceOutputTimestamp(now_ns, 0, 0); - m_last_record_timestamp = m_pending_records.back().timestamp; + m_data = m_pending_records.back().data; + m_last_timestamp = m_pending_records.back().timestamp; } return true; @@ -60,14 +53,14 @@ class FrameMetadataTrackerOak::Impl : public ITrackerImpl DeviceDataTimestamp serialize(flatbuffers::FlatBufferBuilder& builder, size_t /* channel_index */) const override { - auto data_offset = FrameMetadata::Pack(builder, m_tracked.data.get()); + auto data_offset = FrameMetadata::Pack(builder, &m_data); FrameMetadataRecordBuilder record_builder(builder); record_builder.add_data(data_offset); - record_builder.add_timestamp(&m_last_record_timestamp); + record_builder.add_timestamp(&m_last_timestamp); builder.Finish(record_builder.Finish()); - return m_last_record_timestamp; + return m_last_timestamp; } void serialize_all(size_t /* channel_index */, const RecordCallback& callback) const override @@ -86,9 +79,9 @@ class FrameMetadataTrackerOak::Impl : public ITrackerImpl } } - const FrameMetadataTrackedT& get_data() const + const FrameMetadataT& get_data() const { - return m_tracked; + return m_data; } private: @@ -99,8 +92,8 @@ class FrameMetadataTrackerOak::Impl : public ITrackerImpl }; SchemaTracker m_schema_reader; - FrameMetadataTrackedT m_tracked; - DeviceDataTimestamp m_last_record_timestamp{}; + FrameMetadataT m_data; + DeviceDataTimestamp m_last_timestamp{}; std::vector m_pending_records; }; @@ -147,7 +140,7 @@ const SchemaTrackerConfig& FrameMetadataTrackerOak::get_config() const return m_config; } -const FrameMetadataTrackedT& FrameMetadataTrackerOak::get_data(const DeviceIOSession& session) const +const FrameMetadataT& FrameMetadataTrackerOak::get_data(const DeviceIOSession& session) const { return static_cast(session.get_tracker_impl(*this)).get_data(); } diff --git a/src/core/deviceio/cpp/full_body_tracker_pico.cpp b/src/core/deviceio/cpp/full_body_tracker_pico.cpp index 35476199e..12072fb17 100644 --- a/src/core/deviceio/cpp/full_body_tracker_pico.cpp +++ b/src/core/deviceio/cpp/full_body_tracker_pico.cpp @@ -6,7 +6,6 @@ #include "inc/deviceio/deviceio_session.hpp" #include -#include #include #include @@ -27,13 +26,13 @@ class FullBodyTrackerPicoImpl : public ITrackerImpl bool update(XrTime time) override; DeviceDataTimestamp serialize(flatbuffers::FlatBufferBuilder& builder, size_t channel_index) const override; - const FullBodyPosePicoTrackedT& get_body_pose() const; + const FullBodyPosePicoT& get_body_pose() const; private: XrSpace base_space_; XrBodyTrackerBD body_tracker_; - FullBodyPosePicoTrackedT tracked_; - DeviceDataTimestamp last_record_timestamp_{}; + FullBodyPosePicoT body_pose_; + DeviceDataTimestamp last_timestamp_{}; // Extension function pointers PFN_xrCreateBodyTrackerBD pfn_create_body_tracker_; @@ -101,10 +100,7 @@ FullBodyTrackerPicoImpl::FullBodyTrackerPicoImpl(const OpenXRSessionHandles& han throw std::runtime_error("Failed to create body tracker: " + std::to_string(result)); } - tracked_.data = std::make_shared(); - tracked_.timestamp = std::make_shared(); - - tracked_.data->is_active = false; + body_pose_.is_active = false; std::cout << "FullBodyTrackerPico initialized (24 joints)" << std::endl; } @@ -138,22 +134,19 @@ bool FullBodyTrackerPicoImpl::update(XrTime time) XrResult result = pfn_locate_body_joints_(body_tracker_, &locate_info, &locations); if (XR_FAILED(result)) { - tracked_.data->is_active = false; + body_pose_.is_active = false; return false; } // allJointPosesTracked indicates if all joint poses are valid - tracked_.data->is_active = locations.allJointPosesTracked; + body_pose_.is_active = locations.allJointPosesTracked; - auto now_ns = - std::chrono::duration_cast(std::chrono::steady_clock::now().time_since_epoch()).count(); - *tracked_.timestamp = DeviceOutputTimestamp(now_ns, time, 0); - last_record_timestamp_ = DeviceDataTimestamp(time, time, 0); + last_timestamp_ = DeviceDataTimestamp(time, time, 0); // Ensure joints struct is allocated - if (!tracked_.data->joints) + if (!body_pose_.joints) { - tracked_.data->joints = std::make_unique(); + body_pose_.joints = std::make_unique(); } for (uint32_t i = 0; i < XR_BODY_JOINT_COUNT_BD; ++i) @@ -171,28 +164,28 @@ bool FullBodyTrackerPicoImpl::update(XrTime time) // Create BodyJointPose and set it in the array BodyJointPose joint_pose(pose, is_valid); - tracked_.data->joints->mutable_joints()->Mutate(i, joint_pose); + body_pose_.joints->mutable_joints()->Mutate(i, joint_pose); } return true; } -const FullBodyPosePicoTrackedT& FullBodyTrackerPicoImpl::get_body_pose() const +const FullBodyPosePicoT& FullBodyTrackerPicoImpl::get_body_pose() const { - return tracked_; + return body_pose_; } DeviceDataTimestamp FullBodyTrackerPicoImpl::serialize(flatbuffers::FlatBufferBuilder& builder, size_t /* channel_index */) const { - auto data_offset = FullBodyPosePico::Pack(builder, tracked_.data.get()); + auto data_offset = FullBodyPosePico::Pack(builder, &body_pose_); FullBodyPosePicoRecordBuilder record_builder(builder); record_builder.add_data(data_offset); - record_builder.add_timestamp(&last_record_timestamp_); + record_builder.add_timestamp(&last_timestamp_); builder.Finish(record_builder.Finish()); - return last_record_timestamp_; + return last_timestamp_; } // ============================================================================ @@ -204,7 +197,7 @@ std::vector FullBodyTrackerPico::get_required_extensions() const return { XR_BD_BODY_TRACKING_EXTENSION_NAME }; } -const FullBodyPosePicoTrackedT& FullBodyTrackerPico::get_body_pose(const DeviceIOSession& session) const +const FullBodyPosePicoT& FullBodyTrackerPico::get_body_pose(const DeviceIOSession& session) const { return static_cast(session.get_tracker_impl(*this)).get_body_pose(); } diff --git a/src/core/deviceio/cpp/generic_3axis_pedal_tracker.cpp b/src/core/deviceio/cpp/generic_3axis_pedal_tracker.cpp index f121fefb5..fb8aed351 100644 --- a/src/core/deviceio/cpp/generic_3axis_pedal_tracker.cpp +++ b/src/core/deviceio/cpp/generic_3axis_pedal_tracker.cpp @@ -7,7 +7,6 @@ #include -#include #include namespace core @@ -22,8 +21,6 @@ class Generic3AxisPedalTracker::Impl : public ITrackerImpl public: Impl(const OpenXRSessionHandles& handles, SchemaTrackerConfig config) : m_schema_reader(handles, std::move(config)) { - m_tracked.data = std::make_shared(); - m_tracked.timestamp = std::make_shared(); } bool update(XrTime /* time */) override @@ -46,16 +43,12 @@ class Generic3AxisPedalTracker::Impl : public ITrackerImpl if (!m_pending_records.empty()) { - *m_tracked.data = m_pending_records.back().data; - auto now_ns = - std::chrono::duration_cast(std::chrono::steady_clock::now().time_since_epoch()) - .count(); - *m_tracked.timestamp = DeviceOutputTimestamp(now_ns, 0, 0); - m_last_record_timestamp = m_pending_records.back().timestamp; + m_data = m_pending_records.back().data; + m_last_timestamp = m_pending_records.back().timestamp; } else { - m_tracked.data->is_valid = false; + m_data.is_valid = false; } return true; @@ -63,14 +56,14 @@ class Generic3AxisPedalTracker::Impl : public ITrackerImpl DeviceDataTimestamp serialize(flatbuffers::FlatBufferBuilder& builder, size_t /* channel_index */) const override { - auto data_offset = Generic3AxisPedalOutput::Pack(builder, m_tracked.data.get()); + auto data_offset = Generic3AxisPedalOutput::Pack(builder, &m_data); Generic3AxisPedalOutputRecordBuilder record_builder(builder); record_builder.add_data(data_offset); - record_builder.add_timestamp(&m_last_record_timestamp); + record_builder.add_timestamp(&m_last_timestamp); builder.Finish(record_builder.Finish()); - return m_last_record_timestamp; + return m_last_timestamp; } void serialize_all(size_t /* channel_index */, const RecordCallback& callback) const override @@ -89,9 +82,9 @@ class Generic3AxisPedalTracker::Impl : public ITrackerImpl } } - const Generic3AxisPedalOutputTrackedT& get_data() const + const Generic3AxisPedalOutputT& get_data() const { - return m_tracked; + return m_data; } private: @@ -102,8 +95,8 @@ class Generic3AxisPedalTracker::Impl : public ITrackerImpl }; SchemaTracker m_schema_reader; - Generic3AxisPedalOutputTrackedT m_tracked; - DeviceDataTimestamp m_last_record_timestamp{}; + Generic3AxisPedalOutputT m_data; + DeviceDataTimestamp m_last_timestamp{}; std::vector m_pending_records; }; @@ -150,7 +143,7 @@ const SchemaTrackerConfig& Generic3AxisPedalTracker::get_config() const return m_config; } -const Generic3AxisPedalOutputTrackedT& Generic3AxisPedalTracker::get_data(const DeviceIOSession& session) const +const Generic3AxisPedalOutputT& Generic3AxisPedalTracker::get_data(const DeviceIOSession& session) const { return static_cast(session.get_tracker_impl(*this)).get_data(); } diff --git a/src/core/deviceio/cpp/hand_tracker.cpp b/src/core/deviceio/cpp/hand_tracker.cpp index 62aea74be..13e24d4c3 100644 --- a/src/core/deviceio/cpp/hand_tracker.cpp +++ b/src/core/deviceio/cpp/hand_tracker.cpp @@ -6,7 +6,6 @@ #include "inc/deviceio/deviceio_session.hpp" #include -#include #include #include @@ -81,29 +80,24 @@ HandTracker::Impl::Impl(const OpenXRSessionHandles& handles) throw std::runtime_error("Failed to create right hand tracker: " + std::to_string(result)); } - left_tracked_.data = std::make_shared(); - left_tracked_.timestamp = std::make_shared(); - right_tracked_.data = std::make_shared(); - right_tracked_.timestamp = std::make_shared(); - - left_tracked_.data->is_active = false; - right_tracked_.data->is_active = false; + left_hand_.is_active = false; + right_hand_.is_active = false; std::cout << "HandTracker initialized (left + right)" << std::endl; } DeviceDataTimestamp HandTracker::Impl::serialize(flatbuffers::FlatBufferBuilder& builder, size_t channel_index) const { - const HandPoseT& hand = (channel_index == 0) ? *left_tracked_.data : *right_tracked_.data; + const HandPoseT& hand = (channel_index == 0) ? left_hand_ : right_hand_; auto data_offset = HandPose::Pack(builder, &hand); HandPoseRecordBuilder record_builder(builder); record_builder.add_data(data_offset); - record_builder.add_timestamp(&last_record_timestamp_); + record_builder.add_timestamp(&last_timestamp_); builder.Finish(record_builder.Finish()); - return last_record_timestamp_; + return last_timestamp_; } HandTracker::Impl::~Impl() @@ -126,27 +120,23 @@ HandTracker::Impl::~Impl() // Override from ITrackerImpl bool HandTracker::Impl::update(XrTime time) { - bool left_ok = update_hand(left_hand_tracker_, time, *left_tracked_.data); - bool right_ok = update_hand(right_hand_tracker_, time, *right_tracked_.data); + bool left_ok = update_hand(left_hand_tracker_, time, left_hand_); + bool right_ok = update_hand(right_hand_tracker_, time, right_hand_); - auto now_ns = - std::chrono::duration_cast(std::chrono::steady_clock::now().time_since_epoch()).count(); - *left_tracked_.timestamp = DeviceOutputTimestamp(now_ns, time, 0); - *right_tracked_.timestamp = DeviceOutputTimestamp(now_ns, time, 0); - last_record_timestamp_ = DeviceDataTimestamp(time, time, 0); + last_timestamp_ = DeviceDataTimestamp(time, time, 0); // Return true if at least one hand updated successfully return left_ok || right_ok; } -const HandPoseTrackedT& HandTracker::Impl::get_left_hand() const +const HandPoseT& HandTracker::Impl::get_left_hand() const { - return left_tracked_; + return left_hand_; } -const HandPoseTrackedT& HandTracker::Impl::get_right_hand() const +const HandPoseT& HandTracker::Impl::get_right_hand() const { - return right_tracked_; + return right_hand_; } bool HandTracker::Impl::update_hand(XrHandTrackerEXT tracker, XrTime time, HandPoseT& out_data) @@ -207,12 +197,12 @@ std::vector HandTracker::get_required_extensions() const return { XR_EXT_HAND_TRACKING_EXTENSION_NAME }; } -const HandPoseTrackedT& HandTracker::get_left_hand(const DeviceIOSession& session) const +const HandPoseT& HandTracker::get_left_hand(const DeviceIOSession& session) const { return static_cast(session.get_tracker_impl(*this)).get_left_hand(); } -const HandPoseTrackedT& HandTracker::get_right_hand(const DeviceIOSession& session) const +const HandPoseT& HandTracker::get_right_hand(const DeviceIOSession& session) const { return static_cast(session.get_tracker_impl(*this)).get_right_hand(); } diff --git a/src/core/deviceio/cpp/head_tracker.cpp b/src/core/deviceio/cpp/head_tracker.cpp index 055b35cbe..821d9c0e5 100644 --- a/src/core/deviceio/cpp/head_tracker.cpp +++ b/src/core/deviceio/cpp/head_tracker.cpp @@ -5,7 +5,6 @@ #include "inc/deviceio/deviceio_session.hpp" -#include #include #include @@ -25,10 +24,9 @@ HeadTracker::Impl::Impl(const OpenXRSessionHandles& handles) handles.session, { .type = XR_TYPE_REFERENCE_SPACE_CREATE_INFO, .referenceSpaceType = XR_REFERENCE_SPACE_TYPE_VIEW, - .poseInReferenceSpace = { .orientation = { 0, 0, 0, 1 } } })) + .poseInReferenceSpace = { .orientation = { 0, 0, 0, 1 } } })), + head_{} { - tracked_.data = std::make_shared(); - tracked_.timestamp = std::make_shared(); } // Override from ITrackerImpl @@ -40,7 +38,7 @@ bool HeadTracker::Impl::update(XrTime time) if (XR_FAILED(result)) { - tracked_.data->is_valid = false; + head_.is_valid = false; return false; } @@ -48,45 +46,42 @@ bool HeadTracker::Impl::update(XrTime time) bool position_valid = (location.locationFlags & XR_SPACE_LOCATION_POSITION_VALID_BIT) != 0; bool orientation_valid = (location.locationFlags & XR_SPACE_LOCATION_ORIENTATION_VALID_BIT) != 0; - tracked_.data->is_valid = position_valid && orientation_valid; + head_.is_valid = position_valid && orientation_valid; - auto now_ns = - std::chrono::duration_cast(std::chrono::steady_clock::now().time_since_epoch()).count(); - *tracked_.timestamp = DeviceOutputTimestamp(now_ns, time, 0); - last_record_timestamp_ = DeviceDataTimestamp(time, time, 0); + last_timestamp_ = DeviceDataTimestamp(time, time, 0); - if (tracked_.data->is_valid) + if (head_.is_valid) { // Create pose from position and orientation using FlatBuffers structs Point position(location.pose.position.x, location.pose.position.y, location.pose.position.z); Quaternion orientation(location.pose.orientation.x, location.pose.orientation.y, location.pose.orientation.z, location.pose.orientation.w); - tracked_.data->pose = std::make_shared(position, orientation); + head_.pose = std::make_shared(position, orientation); } else { // Invalid - reset pose - tracked_.data->pose.reset(); + head_.pose.reset(); } return true; } -const HeadPoseTrackedT& HeadTracker::Impl::get_head() const +const HeadPoseT& HeadTracker::Impl::get_head() const { - return tracked_; + return head_; } DeviceDataTimestamp HeadTracker::Impl::serialize(flatbuffers::FlatBufferBuilder& builder, size_t /* channel_index */) const { - auto data_offset = HeadPose::Pack(builder, tracked_.data.get()); + auto data_offset = HeadPose::Pack(builder, &head_); HeadPoseRecordBuilder record_builder(builder); record_builder.add_data(data_offset); - record_builder.add_timestamp(&last_record_timestamp_); + record_builder.add_timestamp(&last_timestamp_); builder.Finish(record_builder.Finish()); - return last_record_timestamp_; + return last_timestamp_; } // ============================================================================ @@ -99,7 +94,7 @@ std::vector HeadTracker::get_required_extensions() const return {}; } -const HeadPoseTrackedT& HeadTracker::get_head(const DeviceIOSession& session) const +const HeadPoseT& HeadTracker::get_head(const DeviceIOSession& session) const { return static_cast(session.get_tracker_impl(*this)).get_head(); } diff --git a/src/core/deviceio/cpp/inc/deviceio/controller_tracker.hpp b/src/core/deviceio/cpp/inc/deviceio/controller_tracker.hpp index 29d926d7e..5b50dd71f 100644 --- a/src/core/deviceio/cpp/inc/deviceio/controller_tracker.hpp +++ b/src/core/deviceio/cpp/inc/deviceio/controller_tracker.hpp @@ -43,9 +43,12 @@ class ControllerTracker : public ITracker return { "left_controller", "right_controller" }; } - // Query methods - public API for getting individual controller data (returns tracked output with timestamp) - const ControllerSnapshotTrackedT& get_left_controller(const DeviceIOSession& session) const; - const ControllerSnapshotTrackedT& get_right_controller(const DeviceIOSession& session) const; + // 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"; @@ -63,8 +66,9 @@ class ControllerTracker : public ITracker DeviceDataTimestamp serialize(flatbuffers::FlatBufferBuilder& builder, size_t channel_index) const override; - const ControllerSnapshotTrackedT& get_left_controller() const; - const ControllerSnapshotTrackedT& get_right_controller() const; + const ControllerSnapshot& get_left_controller() const; + const ControllerSnapshot& get_right_controller() const; + XrTime get_last_update_time() const; private: const OpenXRCoreFunctions core_funcs_; @@ -93,12 +97,12 @@ class ControllerTracker : public ITracker XrSpacePtr left_aim_space_; XrSpacePtr right_aim_space_; - // Tracked output (data + DeviceOutputTimestamp) - ControllerSnapshotTrackedT left_tracked_; - ControllerSnapshotTrackedT right_tracked_; + // Controller snapshots stored separately + ControllerSnapshot left_controller_{}; + ControllerSnapshot right_controller_{}; - // Record timestamp for MCAP - DeviceDataTimestamp last_record_timestamp_{}; + // 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 ab8a6bed7..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 @@ -51,9 +51,9 @@ class FrameMetadataTrackerOak : public ITracker std::vector get_record_channels() const override; /*! - * @brief Get the current frame metadata (returns tracked output with timestamp). + * @brief Get the current frame metadata. */ - const FrameMetadataTrackedT& get_data(const DeviceIOSession& session) const; + const FrameMetadataT& get_data(const DeviceIOSession& session) const; protected: const SchemaTrackerConfig& get_config() const; 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 69f375278..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 @@ -46,8 +46,8 @@ class FullBodyTrackerPico : public ITracker return { "full_body" }; } - // Query method - public API for getting body pose data (returns tracked output with timestamp) - const FullBodyPosePicoTrackedT& get_body_pose(const DeviceIOSession& session) const; + // Query method - public API for getting body pose data + const FullBodyPosePicoT& get_body_pose(const DeviceIOSession& session) const; private: std::shared_ptr create_tracker(const OpenXRSessionHandles& handles) const override; 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 1256dc5ca..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 @@ -51,9 +51,9 @@ class Generic3AxisPedalTracker : public ITracker std::vector get_record_channels() const override; /*! - * @brief Get the current foot pedal data (returns tracked output with timestamp). + * @brief Get the current foot pedal data. */ - const Generic3AxisPedalOutputTrackedT& get_data(const DeviceIOSession& session) const; + const Generic3AxisPedalOutputT& get_data(const DeviceIOSession& session) const; protected: /*! diff --git a/src/core/deviceio/cpp/inc/deviceio/hand_tracker.hpp b/src/core/deviceio/cpp/inc/deviceio/hand_tracker.hpp index e5cd004c8..561f2f055 100644 --- a/src/core/deviceio/cpp/inc/deviceio/hand_tracker.hpp +++ b/src/core/deviceio/cpp/inc/deviceio/hand_tracker.hpp @@ -41,9 +41,9 @@ class HandTracker : public ITracker return { "left_hand", "right_hand" }; } - // Query methods - public API for getting hand data (returns tracked output with timestamp) - const HandPoseTrackedT& get_left_hand(const DeviceIOSession& session) const; - const HandPoseTrackedT& get_right_hand(const DeviceIOSession& session) const; + // Query methods - public API for getting hand data + const HandPoseT& get_left_hand(const DeviceIOSession& session) const; + const HandPoseT& get_right_hand(const DeviceIOSession& session) const; // Get joint name for debugging static std::string get_joint_name(uint32_t joint_index); @@ -67,8 +67,8 @@ class HandTracker : public ITracker DeviceDataTimestamp serialize(flatbuffers::FlatBufferBuilder& builder, size_t channel_index) const override; - const HandPoseTrackedT& get_left_hand() const; - const HandPoseTrackedT& get_right_hand() const; + const HandPoseT& get_left_hand() const; + const HandPoseT& get_right_hand() const; private: bool update_hand(XrHandTrackerEXT tracker, XrTime time, HandPoseT& out_data); @@ -79,12 +79,12 @@ class HandTracker : public ITracker XrHandTrackerEXT left_hand_tracker_; XrHandTrackerEXT right_hand_tracker_; - // Tracked output (data + DeviceOutputTimestamp) - HandPoseTrackedT left_tracked_; - HandPoseTrackedT right_tracked_; + // Hand data + HandPoseT left_hand_; + HandPoseT right_hand_; - // Record timestamp for MCAP - DeviceDataTimestamp last_record_timestamp_{}; + // Timestamp from last update + DeviceDataTimestamp last_timestamp_{}; // Extension function pointers PFN_xrCreateHandTrackerEXT pfn_create_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 c80f3856b..833276e57 100644 --- a/src/core/deviceio/cpp/inc/deviceio/head_tracker.hpp +++ b/src/core/deviceio/cpp/inc/deviceio/head_tracker.hpp @@ -42,8 +42,8 @@ class HeadTracker : public ITracker return { "head" }; } - // Query methods - public API for getting head data (returns tracked output with timestamp) - const HeadPoseTrackedT& get_head(const DeviceIOSession& session) const; + // Query methods - public API for getting head data + const HeadPoseT& get_head(const DeviceIOSession& session) const; private: static constexpr const char* TRACKER_NAME = "HeadTracker"; @@ -61,14 +61,14 @@ class HeadTracker : public ITracker DeviceDataTimestamp serialize(flatbuffers::FlatBufferBuilder& builder, size_t channel_index) const override; - const HeadPoseTrackedT& get_head() const; + const HeadPoseT& get_head() const; private: const OpenXRCoreFunctions core_funcs_; XrSpace base_space_; XrSpacePtr view_space_; - HeadPoseTrackedT tracked_; - DeviceDataTimestamp last_record_timestamp_{}; + HeadPoseT head_; + DeviceDataTimestamp last_timestamp_{}; }; }; diff --git a/src/core/deviceio/python/deviceio_bindings.cpp b/src/core/deviceio/python/deviceio_bindings.cpp index d8f74b30b..998374b23 100644 --- a/src/core/deviceio/python/deviceio_bindings.cpp +++ b/src/core/deviceio/python/deviceio_bindings.cpp @@ -15,72 +15,6 @@ PYBIND11_MODULE(_deviceio, m) { m.doc() = "Isaac Teleop DeviceIO - Device I/O Module"; - // DeviceOutputTimestamp (FlatBuffer struct for tracker output timestamps) - py::class_(m, "DeviceOutputTimestamp") - .def_property_readonly("query_time_common_clock", &core::DeviceOutputTimestamp::query_time_common_clock) - .def_property_readonly("target_time_common_clock", &core::DeviceOutputTimestamp::target_time_common_clock) - .def_property_readonly("target_time_consumer_clock", &core::DeviceOutputTimestamp::target_time_consumer_clock) - .def("__repr__", - [](const core::DeviceOutputTimestamp& self) - { - return "DeviceOutputTimestamp(query_time=" + std::to_string(self.query_time_common_clock()) + - ", target_time=" + std::to_string(self.target_time_common_clock()) + - ", target_time_consumer=" + std::to_string(self.target_time_consumer_clock()) + ")"; - }); - - // TrackedT wrapper types (pair data + DeviceOutputTimestamp from each tracker) - py::class_(m, "HandPoseTrackedT") - .def_property_readonly( - "data", [](const core::HandPoseTrackedT& self) -> const core::HandPoseT* { return self.data.get(); }, - py::return_value_policy::reference_internal) - .def_property_readonly( - "timestamp", - [](const core::HandPoseTrackedT& self) -> const core::DeviceOutputTimestamp* { return self.timestamp.get(); }, - py::return_value_policy::reference_internal); - - py::class_(m, "HeadPoseTrackedT") - .def_property_readonly( - "data", [](const core::HeadPoseTrackedT& self) -> const core::HeadPoseT* { return self.data.get(); }, - py::return_value_policy::reference_internal) - .def_property_readonly( - "timestamp", - [](const core::HeadPoseTrackedT& self) -> const core::DeviceOutputTimestamp* { return self.timestamp.get(); }, - py::return_value_policy::reference_internal); - - py::class_(m, "ControllerSnapshotTrackedT") - .def_property_readonly( - "data", - [](const core::ControllerSnapshotTrackedT& self) -> const core::ControllerSnapshot* - { return self.data.get(); }, - py::return_value_policy::reference_internal) - .def_property_readonly( - "timestamp", - [](const core::ControllerSnapshotTrackedT& self) -> const core::DeviceOutputTimestamp* - { return self.timestamp.get(); }, - py::return_value_policy::reference_internal); - - py::class_(m, "FrameMetadataTrackedT") - .def_property_readonly( - "data", - [](const core::FrameMetadataTrackedT& self) -> const core::FrameMetadataT* { return self.data.get(); }, - py::return_value_policy::reference_internal) - .def_property_readonly( - "timestamp", - [](const core::FrameMetadataTrackedT& self) -> const core::DeviceOutputTimestamp* - { return self.timestamp.get(); }, - py::return_value_policy::reference_internal); - - py::class_(m, "FullBodyPosePicoTrackedT") - .def_property_readonly( - "data", - [](const core::FullBodyPosePicoTrackedT& self) -> const core::FullBodyPosePicoT* { return self.data.get(); }, - py::return_value_policy::reference_internal) - .def_property_readonly( - "timestamp", - [](const core::FullBodyPosePicoTrackedT& self) -> const core::DeviceOutputTimestamp* - { return self.timestamp.get(); }, - py::return_value_policy::reference_internal); - // ITracker interface (base class) py::class_>(m, "ITracker").def("get_name", &core::ITracker::get_name); @@ -89,12 +23,12 @@ PYBIND11_MODULE(_deviceio, m) .def(py::init<>()) .def( "get_left_hand", - [](core::HandTracker& self, PyDeviceIOSession& session) -> const core::HandPoseTrackedT& + [](core::HandTracker& self, PyDeviceIOSession& session) -> const core::HandPoseT& { return self.get_left_hand(session.native()); }, py::arg("session"), py::return_value_policy::reference_internal) .def( "get_right_hand", - [](core::HandTracker& self, PyDeviceIOSession& session) -> const core::HandPoseTrackedT& + [](core::HandTracker& self, PyDeviceIOSession& session) -> const core::HandPoseT& { return self.get_right_hand(session.native()); }, py::arg("session"), py::return_value_policy::reference_internal) .def_static("get_joint_name", &core::HandTracker::get_joint_name); @@ -104,7 +38,7 @@ PYBIND11_MODULE(_deviceio, m) .def(py::init<>()) .def( "get_head", - [](core::HeadTracker& self, PyDeviceIOSession& session) -> const core::HeadPoseTrackedT& + [](core::HeadTracker& self, PyDeviceIOSession& session) -> const core::HeadPoseT& { return self.get_head(session.native()); }, py::arg("session"), py::return_value_policy::reference_internal); @@ -113,14 +47,19 @@ PYBIND11_MODULE(_deviceio, m) .def(py::init<>()) .def( "get_left_controller", - [](core::ControllerTracker& self, PyDeviceIOSession& session) -> const core::ControllerSnapshotTrackedT& + [](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 tracked output") + 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::ControllerSnapshotTrackedT& + [](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 tracked output"); + 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_>( @@ -130,10 +69,10 @@ PYBIND11_MODULE(_deviceio, m) "Construct a FrameMetadataTrackerOak for the given tensor collection ID") .def( "get_data", - [](core::FrameMetadataTrackerOak& self, PyDeviceIOSession& session) -> const core::FrameMetadataTrackedT& + [](core::FrameMetadataTrackerOak& self, PyDeviceIOSession& session) -> const core::FrameMetadataT& { return self.get_data(session.native()); }, py::arg("session"), py::return_value_policy::reference_internal, - "Get the current frame metadata tracked output (data + timestamp)"); + "Get the current frame metadata (timestamp and sequence_number)"); // FullBodyTrackerPico class (PICO XR_BD_body_tracking extension) py::class_>( @@ -141,10 +80,10 @@ PYBIND11_MODULE(_deviceio, m) .def(py::init<>()) .def( "get_body_pose", - [](core::FullBodyTrackerPico& self, PyDeviceIOSession& session) -> const core::FullBodyPosePicoTrackedT& + [](core::FullBodyTrackerPico& self, PyDeviceIOSession& session) -> const core::FullBodyPosePicoT& { return self.get_body_pose(session.native()); }, py::arg("session"), py::return_value_policy::reference_internal, - "Get full body pose tracked output (24 joints from pelvis to hands)"); + "Get full body pose data (24 joints from pelvis to hands)"); // DeviceIOSession class (bound via wrapper for context management) // Other C++ modules (like mcap) should include and accept diff --git a/src/core/deviceio/python/deviceio_init.py b/src/core/deviceio/python/deviceio_init.py index c5d19670a..585ee7f42 100644 --- a/src/core/deviceio/python/deviceio_init.py +++ b/src/core/deviceio/python/deviceio_init.py @@ -5,15 +5,11 @@ This module provides trackers and teleop session functionality. -Tracker getters return TrackedT wrapper types containing `.data` (the raw data) -and `.timestamp` (DeviceOutputTimestamp with query/target times): - HandTracker.get_left_hand(session) / get_right_hand(session) -> HandPoseTrackedT - HeadTracker.get_head(session) -> HeadPoseTrackedT - ControllerTracker.get_left_controller(session) / get_right_controller(session) -> ControllerSnapshotTrackedT - FrameMetadataTrackerOak.get_data(session) -> FrameMetadataTrackedT - FullBodyTrackerPico.get_body_pose(session) -> FullBodyPosePicoTrackedT - -Import raw data types from isaacteleop.schema if you need to work with pose types. +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_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. """ from ._deviceio import ( @@ -24,12 +20,6 @@ FrameMetadataTrackerOak, FullBodyTrackerPico, DeviceIOSession, - DeviceOutputTimestamp, - HandPoseTrackedT, - HeadPoseTrackedT, - ControllerSnapshotTrackedT, - FrameMetadataTrackedT, - FullBodyPosePicoTrackedT, NUM_JOINTS, JOINT_PALM, JOINT_WRIST, @@ -54,13 +44,7 @@ "ControllerPose", "ControllerSnapshot", "DeviceDataTimestamp", - "DeviceOutputTimestamp", "FrameMetadata", - "HandPoseTrackedT", - "HeadPoseTrackedT", - "ControllerSnapshotTrackedT", - "FrameMetadataTrackedT", - "FullBodyPosePicoTrackedT", "ITracker", "HandTracker", "HeadTracker", diff --git a/src/core/schema/fbs/controller.fbs b/src/core/schema/fbs/controller.fbs index a9f8eaab1..24ceaa4b0 100644 --- a/src/core/schema/fbs/controller.fbs +++ b/src/core/schema/fbs/controller.fbs @@ -50,10 +50,4 @@ table ControllerSnapshotRecord { timestamp: DeviceDataTimestamp (id: 1); } -// Tracker output wrapper: pairs a ControllerSnapshot with a DeviceOutputTimestamp. -table ControllerSnapshotTracked { - data: ControllerSnapshot (id: 0); - timestamp: DeviceOutputTimestamp (id: 1); -} - root_type ControllerSnapshotRecord; diff --git a/src/core/schema/fbs/full_body.fbs b/src/core/schema/fbs/full_body.fbs index 594b5c21d..82f7c0491 100644 --- a/src/core/schema/fbs/full_body.fbs +++ b/src/core/schema/fbs/full_body.fbs @@ -68,10 +68,4 @@ table FullBodyPosePicoRecord { timestamp: DeviceDataTimestamp (id: 1); } -// Tracker output wrapper: pairs a FullBodyPosePico with a DeviceOutputTimestamp. -table FullBodyPosePicoTracked { - data: FullBodyPosePico (id: 0); - timestamp: DeviceOutputTimestamp (id: 1); -} - root_type FullBodyPosePicoRecord; diff --git a/src/core/schema/fbs/hand.fbs b/src/core/schema/fbs/hand.fbs index b1744443e..16c5eaa3f 100644 --- a/src/core/schema/fbs/hand.fbs +++ b/src/core/schema/fbs/hand.fbs @@ -39,10 +39,4 @@ table HandPoseRecord { timestamp: DeviceDataTimestamp (id: 1); } -// Tracker output wrapper: pairs a HandPose with a DeviceOutputTimestamp. -table HandPoseTracked { - data: HandPose (id: 0); - timestamp: DeviceOutputTimestamp (id: 1); -} - root_type HandPoseRecord; diff --git a/src/core/schema/fbs/head.fbs b/src/core/schema/fbs/head.fbs index f6fbbffb4..2efa3e631 100644 --- a/src/core/schema/fbs/head.fbs +++ b/src/core/schema/fbs/head.fbs @@ -21,10 +21,4 @@ table HeadPoseRecord { timestamp: DeviceDataTimestamp (id: 1); } -// Tracker output wrapper: pairs a HeadPose with a DeviceOutputTimestamp. -table HeadPoseTracked { - data: HeadPose (id: 0); - timestamp: DeviceOutputTimestamp (id: 1); -} - root_type HeadPoseRecord; diff --git a/src/core/schema/fbs/locomotion.fbs b/src/core/schema/fbs/locomotion.fbs index 4cc798ade..9e905cfc3 100644 --- a/src/core/schema/fbs/locomotion.fbs +++ b/src/core/schema/fbs/locomotion.fbs @@ -22,10 +22,4 @@ table LocomotionCommandRecord { timestamp: DeviceDataTimestamp (id: 1); } -// Tracker output wrapper: pairs a LocomotionCommand with a DeviceOutputTimestamp. -table LocomotionCommandTracked { - data: LocomotionCommand (id: 0); - timestamp: DeviceOutputTimestamp (id: 1); -} - root_type LocomotionCommandRecord; diff --git a/src/core/schema/fbs/oak.fbs b/src/core/schema/fbs/oak.fbs index 8cba5400b..14fd7add2 100644 --- a/src/core/schema/fbs/oak.fbs +++ b/src/core/schema/fbs/oak.fbs @@ -16,10 +16,4 @@ table FrameMetadataRecord { timestamp: DeviceDataTimestamp (id: 1); } -// Tracker output wrapper: pairs a FrameMetadata with a DeviceOutputTimestamp. -table FrameMetadataTracked { - data: FrameMetadata (id: 0); - timestamp: DeviceOutputTimestamp (id: 1); -} - root_type FrameMetadataRecord; diff --git a/src/core/schema/fbs/pedals.fbs b/src/core/schema/fbs/pedals.fbs index 5028b9516..9e79c8410 100644 --- a/src/core/schema/fbs/pedals.fbs +++ b/src/core/schema/fbs/pedals.fbs @@ -27,10 +27,4 @@ table Generic3AxisPedalOutputRecord { timestamp: DeviceDataTimestamp (id: 1); } -// Tracker output wrapper: pairs a Generic3AxisPedalOutput with a DeviceOutputTimestamp. -table Generic3AxisPedalOutputTracked { - data: Generic3AxisPedalOutput (id: 0); - timestamp: DeviceOutputTimestamp (id: 1); -} - root_type Generic3AxisPedalOutputRecord; diff --git a/src/core/schema/fbs/timestamp.fbs b/src/core/schema/fbs/timestamp.fbs index d10681b80..68ceaf69b 100644 --- a/src/core/schema/fbs/timestamp.fbs +++ b/src/core/schema/fbs/timestamp.fbs @@ -24,22 +24,3 @@ struct DeviceDataTimestamp { // Useful for measuring pipeline latency (available - sample). available_time_common_clock: int64; } - -// Timestamp attached to tracker output (what the consumer receives). -// -// All timestamps are in nanoseconds. The "common clock" is the system monotonic -// clock (CLOCK_MONOTONIC on Linux, QueryPerformanceCounter on Windows). -// The "consumer clock" is the application-domain clock, which may differ from -// real time (e.g., a simulation running slower than real time). -struct DeviceOutputTimestamp { - // When the tracker query was executed, in the common clock domain. - query_time_common_clock: int64; - - // The target prediction time requested, in the common clock domain. - // Zero if not applicable (e.g., for historical tensor data). - target_time_common_clock: int64; - - // The target time in the consumer's application-domain clock. - // TODO: Not yet wired — will be passed into update() in a future change. - target_time_consumer_clock: int64; -} diff --git a/src/plugins/controller_synthetic_hands/synthetic_hands_plugin.cpp b/src/plugins/controller_synthetic_hands/synthetic_hands_plugin.cpp index bf460cc0a..785ccf731 100644 --- a/src/plugins/controller_synthetic_hands/synthetic_hands_plugin.cpp +++ b/src/plugins/controller_synthetic_hands/synthetic_hands_plugin.cpp @@ -73,11 +73,9 @@ void SyntheticHandsPlugin::worker_thread() continue; } - const auto& left_tracked = m_controller_tracker->get_left_controller(*m_deviceio_session); - const auto& right_tracked = m_controller_tracker->get_right_controller(*m_deviceio_session); - const auto& left_ctrl = *left_tracked.data; - const auto& right_ctrl = *right_tracked.data; - XrTime time = left_tracked.timestamp->target_time_common_clock(); + 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); float left_target = left_ctrl.inputs().trigger_value(); float right_target = right_ctrl.inputs().trigger_value(); diff --git a/src/plugins/manus/core/manus_hand_tracking_plugin.cpp b/src/plugins/manus/core/manus_hand_tracking_plugin.cpp index ca2d48811..da04442c8 100644 --- a/src/plugins/manus/core/manus_hand_tracking_plugin.cpp +++ b/src/plugins/manus/core/manus_hand_tracking_plugin.cpp @@ -342,9 +342,9 @@ void ManusTracker::inject_hand_data() right_nodes = m_right_hand_nodes; } - const auto& left_tracked = m_controller_tracker->get_left_controller(*m_deviceio_session); - const auto& right_tracked = m_controller_tracker->get_right_controller(*m_deviceio_session); - XrTime time = left_tracked.timestamp->target_time_common_clock(); + 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) { @@ -357,7 +357,7 @@ 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; - const core::ControllerSnapshot& snapshot = is_left ? *left_tracked.data : *right_tracked.data; + const core::ControllerSnapshot& snapshot = is_left ? left_ctrl : right_ctrl; if (snapshot.is_active()) { From 60aaabd80d59af1e0a1ff4b50a72612f3a6127d2 Mon Sep 17 00:00:00 2001 From: Devdeep Ray Date: Fri, 20 Feb 2026 11:55:17 -0800 Subject: [PATCH 5/6] ruff format --- .../python/test_sources.py | 4 +--- .../python/test_transforms.py | 4 +--- .../schema_tests/python/test_controller.py | 22 ++++++++++++++----- 3 files changed, 19 insertions(+), 11 deletions(-) diff --git a/src/core/retargeting_engine_tests/python/test_sources.py b/src/core/retargeting_engine_tests/python/test_sources.py index 5006e0d3d..d14f32625 100644 --- a/src/core/retargeting_engine_tests/python/test_sources.py +++ b/src/core/retargeting_engine_tests/python/test_sources.py @@ -60,9 +60,7 @@ def create_controller_snapshot(grip_pos, aim_pos, trigger_val): ) # Create snapshot - return ControllerSnapshot( - grip_controller_pose, aim_controller_pose, inputs, True - ) + 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 242a4871d..0b5838707 100644 --- a/src/core/retargeting_engine_tests/python/test_transforms.py +++ b/src/core/retargeting_engine_tests/python/test_transforms.py @@ -325,9 +325,7 @@ 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 - ) + 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"] diff --git a/src/core/schema_tests/python/test_controller.py b/src/core/schema_tests/python/test_controller.py index 8ca7d3320..289caed11 100644 --- a/src/core/schema_tests/python/test_controller.py +++ b/src/core/schema_tests/python/test_controller.py @@ -102,7 +102,10 @@ def test_default_construction(self): def test_set_timestamp_values(self): """Test constructing with timestamp values.""" - timestamp = DeviceDataTimestamp(sample_time_device_clock=1234567890123456789, sample_time_common_clock=9876543210) + timestamp = DeviceDataTimestamp( + sample_time_device_clock=1234567890123456789, + sample_time_common_clock=9876543210, + ) assert timestamp.sample_time_device_clock == 1234567890123456789 assert timestamp.sample_time_common_clock == 9876543210 @@ -110,14 +113,19 @@ def test_set_timestamp_values(self): def test_large_timestamp_values(self): """Test with large int64 timestamp values.""" max_int64 = 9223372036854775807 - timestamp = DeviceDataTimestamp(sample_time_device_clock=max_int64, sample_time_common_clock=max_int64 - 1000) + timestamp = DeviceDataTimestamp( + sample_time_device_clock=max_int64, + sample_time_common_clock=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 = DeviceDataTimestamp(sample_time_device_clock=1000, sample_time_common_clock=2000) + timestamp = DeviceDataTimestamp( + sample_time_device_clock=1000, sample_time_common_clock=2000 + ) repr_str = repr(timestamp) assert "DeviceDataTimestamp" in repr_str @@ -355,14 +363,18 @@ def test_invalid_pose(self): def test_zero_timestamp(self): """Test with zero timestamp values.""" - timestamp = DeviceDataTimestamp(sample_time_device_clock=0, sample_time_common_clock=0) + timestamp = DeviceDataTimestamp( + sample_time_device_clock=0, sample_time_common_clock=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 = DeviceDataTimestamp(sample_time_device_clock=-1000, sample_time_common_clock=-2000) + timestamp = DeviceDataTimestamp( + sample_time_device_clock=-1000, sample_time_common_clock=-2000 + ) assert timestamp.sample_time_device_clock == -1000 assert timestamp.sample_time_common_clock == -2000 From 88acb316012bc6f06be6037c794c3eec46f45148 Mon Sep 17 00:00:00 2001 From: Devdeep Ray Date: Tue, 17 Feb 2026 17:01:55 -0800 Subject: [PATCH 6/6] Add clock sync server and client, and clock sync client library --- src/core/CMakeLists.txt | 3 + src/core/synchronization/CMakeLists.txt | 6 + .../synchronization/clock_sync/CMakeLists.txt | 102 ++++++++++ .../clock_sync/clock_client.cpp | 152 ++++++++++++++ .../clock_sync/clock_client_lib.cpp | 191 ++++++++++++++++++ .../clock_sync/clock_client_xr.cpp | 165 +++++++++++++++ .../clock_sync/clock_server.cpp | 107 ++++++++++ .../inc/clock_sync/clock_client.hpp | 126 ++++++++++++ .../clock_sync/inc/clock_sync/clock_types.hpp | 170 ++++++++++++++++ .../clock_sync/inc/clock_sync/platform.hpp | 132 ++++++++++++ .../inc/clock_sync/xr_clock_translator.hpp | 47 +++++ .../clock_sync/xr_clock_translator.cpp | 67 ++++++ 12 files changed, 1268 insertions(+) create mode 100644 src/core/synchronization/CMakeLists.txt create mode 100644 src/core/synchronization/clock_sync/CMakeLists.txt create mode 100644 src/core/synchronization/clock_sync/clock_client.cpp create mode 100644 src/core/synchronization/clock_sync/clock_client_lib.cpp create mode 100644 src/core/synchronization/clock_sync/clock_client_xr.cpp create mode 100644 src/core/synchronization/clock_sync/clock_server.cpp create mode 100644 src/core/synchronization/clock_sync/inc/clock_sync/clock_client.hpp create mode 100644 src/core/synchronization/clock_sync/inc/clock_sync/clock_types.hpp create mode 100644 src/core/synchronization/clock_sync/inc/clock_sync/platform.hpp create mode 100644 src/core/synchronization/clock_sync/inc/clock_sync/xr_clock_translator.hpp create mode 100644 src/core/synchronization/clock_sync/xr_clock_translator.cpp 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/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