diff --git a/examples/oxr/python/modular_example_with_mcap.py b/examples/oxr/python/modular_example_with_mcap.py index b5771aa8..65ce8b8a 100644 --- a/examples/oxr/python/modular_example_with_mcap.py +++ b/examples/oxr/python/modular_example_with_mcap.py @@ -8,14 +8,13 @@ - Create independent trackers - Add only the trackers you need - Record all tracker data to an MCAP file for playback/analysis -- McapRecorder.create() is similar to DeviceIOSession.run() +- Pass mcap_filename and mcap_channels to DeviceIOSession.run() to enable recording """ import sys import time from datetime import datetime import isaacteleop.deviceio as deviceio -import isaacteleop.mcap as mcap import isaacteleop.oxr as oxr @@ -50,55 +49,42 @@ def main(): handles = oxr_session.get_handles() print("✓ OpenXR session created") - # Run deviceio session with trackers - print("\nRunning deviceio session...") - with deviceio.DeviceIOSession.run(trackers, handles) as session: + # Run deviceio session with MCAP recording enabled. + print("\nRunning deviceio session with MCAP recording...") + recording_config = deviceio.McapRecordingConfig( + mcap_filename, [(hand_tracker, "hands"), (head_tracker, "head")] + ) + with deviceio.DeviceIOSession.run( + trackers, handles, recording_config + ) as session: print("✓ DeviceIO session initialized with all trackers!") + print(f"✓ MCAP recording active → {mcap_filename}") + print() - with mcap.McapRecorder.create( - mcap_filename, - [ - (hand_tracker, "hands"), - (head_tracker, "head"), - ], - ) as recorder: - print("✓ MCAP recording started!") - print() - - # Main tracking loop - print("=" * 60) - print("Tracking (60 seconds)...") - print("=" * 60) - print() - - frame_count = 0 - start_time = time.time() - - while time.time() - start_time < 30.0: - # Update session and all trackers - if not session.update(): - print("Update failed") - break - - # Record all registered trackers - recorder.record(session) - - # Print every 60 frames (~1 second) - if frame_count % 60 == 0: - elapsed = time.time() - start_time - print(f"[{elapsed:4.1f}s] Frame {frame_count} (recording...)") - print() - - frame_count += 1 - time.sleep(0.016) # ~60 FPS - - # Cleanup - print(f"\nProcessed {frame_count} frames") - print("Cleaning up (RAII)...") - - print("✓ Recording stopped") - - print("✓ DeviceIO session cleaned up") + # Main tracking loop + print("=" * 60) + print("Tracking (30 seconds)...") + print("=" * 60) + print() + + frame_count = 0 + start_time = time.time() + + while time.time() - start_time < 30.0: + session.update() + + # Print every 60 frames (~1 second) + if frame_count % 60 == 0: + elapsed = time.time() - start_time + print(f"[{elapsed:4.1f}s] Frame {frame_count} (recording...)") + print() + + frame_count += 1 + time.sleep(0.016) # ~60 FPS + + print(f"\nProcessed {frame_count} frames") + + print("✓ Recording stopped (MCAP file closed by session destructor)") print() print("=" * 60) diff --git a/examples/oxr/python/test_oak_camera.py b/examples/oxr/python/test_oak_camera.py index abc4258b..a4812995 100755 --- a/examples/oxr/python/test_oak_camera.py +++ b/examples/oxr/python/test_oak_camera.py @@ -24,7 +24,6 @@ import isaacteleop.plugin_manager as pm import isaacteleop.deviceio as deviceio -import isaacteleop.mcap as mcap import isaacteleop.oxr as oxr PLUGIN_ROOT_DIR = Path(__file__).resolve().parent.parent.parent.parent / "plugins" @@ -66,62 +65,53 @@ def _run_schema_pusher( handles = oxr_session.get_handles() print(" ✓ OpenXR session created") - # Create DeviceIOSession with all trackers - with deviceio.DeviceIOSession.run([tracker], handles) as session: - print(" ✓ DeviceIO session initialized") - - # Create MCAP recorder with per-stream FrameMetadataOak channels - mcap_entries = [(tracker, "oak_metadata")] - with mcap.McapRecorder.create( - mcap_filename, - mcap_entries, - ) as recorder: - print(" ✓ MCAP recording started") - print() - - # 6. Main tracking loop - print(f"[Step 6] Recording video and metadata ({duration} seconds)...") - print("-" * 80) - start_time = time.time() - frame_count = 0 - last_print_time = 0 - last_seq = dict.fromkeys(stream_names, -1) - metadata_samples = dict.fromkeys(stream_names, 0) - - while time.time() - start_time < duration: - plugin.check_health() - if not session.update(): - print(" Warning: Session update failed") - continue - recorder.record(session) - frame_count += 1 - - elapsed = time.time() - start_time - for idx, name in enumerate(stream_names): - tracked = tracker.get_stream_data(session, idx) - if ( - tracked.data is not None - and tracked.data.sequence_number != last_seq.get(name, -1) - ): - metadata_samples[name] = metadata_samples.get(name, 0) + 1 - last_seq[name] = tracked.data.sequence_number - - if int(elapsed) > last_print_time: - last_print_time = int(elapsed) - parts = [] - for name in stream_names: - parts.append(f"{name}={metadata_samples.get(name, 0)}") - print(f" [{last_print_time:3d}s] samples: {', '.join(parts)}") - time.sleep(0.016) - - print("-" * 80) - print() - print(f" ✓ Recording completed ({duration:.1f} seconds)") - print(f" ✓ Processed {frame_count} update cycles") - for name in stream_names: - print( - f" ✓ {name}: {metadata_samples.get(name, 0)} metadata samples" - ) + recording_config = deviceio.McapRecordingConfig( + mcap_filename, [(tracker, "oak_metadata")] + ) + with deviceio.DeviceIOSession.run( + [tracker], handles, recording_config + ) as session: + print(" ✓ DeviceIO session initialized (recording active during update())") + print() + + print(f"[Step 6] Recording video and metadata ({duration} seconds)...") + print("-" * 80) + start_time = time.time() + frame_count = 0 + last_print_time = 0 + last_seq = dict.fromkeys(stream_names, -1) + metadata_samples = dict.fromkeys(stream_names, 0) + + while time.time() - start_time < duration: + plugin.check_health() + session.update() + frame_count += 1 + + elapsed = time.time() - start_time + for idx, name in enumerate(stream_names): + tracked = tracker.get_stream_data(session, idx) + if ( + tracked.data is not None + and tracked.data.sequence_number != last_seq.get(name, -1) + ): + metadata_samples[name] = metadata_samples.get(name, 0) + 1 + last_seq[name] = tracked.data.sequence_number + + if int(elapsed) > last_print_time: + last_print_time = int(elapsed) + parts = [ + f"{name}={metadata_samples.get(name, 0)}" + for name in stream_names + ] + print(f" [{last_print_time:3d}s] samples: {', '.join(parts)}") + time.sleep(0.016) + + print("-" * 80) + print() + print(f" ✓ Recording completed ({duration:.1f} seconds)") + print(f" ✓ Processed {frame_count} update cycles") + for name in stream_names: + print(f" ✓ {name}: {metadata_samples.get(name, 0)} metadata samples") def run_test(duration: float = 10.0, mode: str = MODE_NO_METADATA): diff --git a/src/core/deviceio_base/cpp/inc/deviceio_base/tracker.hpp b/src/core/deviceio_base/cpp/inc/deviceio_base/tracker.hpp index 74c8936c..63ff05b2 100644 --- a/src/core/deviceio_base/cpp/inc/deviceio_base/tracker.hpp +++ b/src/core/deviceio_base/cpp/inc/deviceio_base/tracker.hpp @@ -4,9 +4,7 @@ #pragma once #include -#include -#include #include #include #include @@ -19,62 +17,14 @@ namespace core struct OpenXRSessionHandles; class ITrackerFactory; -// Base interface for tracker implementations -// These are the actual worker objects that get updated by the session +// Base interface for tracker implementations. +// The actual worker objects updated each frame by DeviceIOSession. class ITrackerImpl { public: virtual ~ITrackerImpl() = default; - // Update the tracker with the current time virtual bool update(XrTime time) = 0; - - /** - * @brief Callback type for serialize_all. - * - * Receives (log_time_ns, data_ptr, data_size) for each serialized record. - * - * @param log_time_ns Monotonic nanoseconds used as the MCAP logTime/publishTime - * for this record. This is the time at which the recording - * system processed the record (update-tick time), not the - * sample capture time. The full per-sample DeviceDataTimestamp - * (including sample_time and raw_device_time) is embedded - * inside the serialized FlatBuffer payload. - * - * @warning The data_ptr and data_size are only valid for the duration of the - * callback invocation. The buffer is owned by a FlatBufferBuilder - * local to the tracker's serialize_all implementation and will be - * destroyed on return. If you need the bytes after the callback - * returns, copy them into your own storage before returning. - */ - using RecordCallback = std::function; - - /** - * @brief Serialize all records accumulated since the last update() call. - * - * Each call to update() clears the previous batch and accumulates a fresh - * set of records (one for OpenXR-direct trackers; potentially many for - * SchemaTracker-based tensor-device trackers). serialize_all emits every - * record in that batch via the callback. - * - * @note For multi-channel trackers the recorder calls serialize_all once per - * channel index (channel_index = 0, 1, … N-1) after each update(). - * All serialize_all calls for a given update() are guaranteed to - * complete before the next update() is issued. Implementations may - * therefore maintain a single shared pending batch and clear it at the - * start of the next update(); there is no need to track per-channel - * drain state. - * - * For read access without MCAP recording, use the tracker's typed get_*() - * accessors, which always reflect the last record in the current batch. - * - * @note The buffer pointer passed to the callback is only valid for the - * duration of that callback call. Copy if you need it beyond return. - * - * @param channel_index Which record channel to serialize (0-based). - * @param callback Invoked once per record with (timestamp, data_ptr, data_size). - */ - virtual void serialize_all(size_t channel_index, const RecordCallback& callback) const = 0; }; /** @@ -100,8 +50,8 @@ class ITrackerSession virtual const ITrackerImpl& get_tracker_impl(const class ITracker& tracker) const = 0; }; -// Base interface for all trackers -// PUBLIC API: Only exposes methods that external users should call +// Base interface for all trackers. +// Public API: configuration, extension requirements, and typed data accessors. class ITracker { public: @@ -110,48 +60,15 @@ class ITracker virtual std::vector get_required_extensions() const = 0; virtual std::string_view get_name() const = 0; - /** - * @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. - */ - virtual std::string_view get_schema_name() const = 0; - - /** - * @brief Get the binary FlatBuffer schema text for MCAP recording. - */ - virtual std::string_view get_schema_text() const = 0; - - /** - * @brief Get the channel names for MCAP recording. - * - * Every tracker must return at least one non-empty channel name. The returned - * vector size determines how many times serialize_all() is called per update, - * with the vector index used as the channel_index argument. - * - * Single-channel trackers return one name (e.g. {"head"}). - * Multi-channel trackers return multiple (e.g. {"left_hand", "right_hand"}). - * - * The MCAP recorder combines each channel name with the base channel name - * provided at registration as "base_name/channel_name". For example, a - * single-channel head tracker registered with base name "tracking" produces - * the MCAP channel "tracking/head". A multi-channel hand tracker registered - * with base name "hands" produces "hands/left_hand" and "hands/right_hand". - * - * @return Non-empty vector of non-empty channel name strings. - */ - virtual std::vector get_record_channels() const = 0; - /** * @brief Create the tracker's implementation via the provided factory. * * Uses double dispatch: the tracker calls the factory method specific to its - * type (e.g., factory.create_head_tracker_impl()), so the factory controls - * which concrete impl is constructed without the tracker needing to know the - * session type. + * concrete type. The factory (e.g. LiveDeviceIOFactory) owns the optional + * MCAP writer and creates typed McapTrackerChannels for each impl that + * should record. * - * @param factory Session-provided factory (e.g., LiveDeviceIOFactory). + * @param factory Session-provided factory. * @return Owning pointer to the newly created impl. */ virtual std::unique_ptr create_tracker_impl(ITrackerFactory& factory) const = 0; diff --git a/src/core/deviceio_session/cpp/CMakeLists.txt b/src/core/deviceio_session/cpp/CMakeLists.txt index bdc2fc0e..31ecf2a0 100644 --- a/src/core/deviceio_session/cpp/CMakeLists.txt +++ b/src/core/deviceio_session/cpp/CMakeLists.txt @@ -24,6 +24,7 @@ target_link_libraries(deviceio_session PRIVATE deviceio::deviceio_trackers deviceio::live_trackers + mcap::mcap ) add_library(deviceio::deviceio_session ALIAS deviceio_session) diff --git a/src/core/deviceio_session/cpp/deviceio_session.cpp b/src/core/deviceio_session/cpp/deviceio_session.cpp index 8a313276..44b3c22d 100644 --- a/src/core/deviceio_session/cpp/deviceio_session.cpp +++ b/src/core/deviceio_session/cpp/deviceio_session.cpp @@ -1,10 +1,15 @@ // SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. // SPDX-License-Identifier: Apache-2.0 +// MCAP_IMPLEMENTATION must be defined in exactly one translation unit that +// includes . All other TUs get declarations only. +#define MCAP_IMPLEMENTATION + #include "inc/deviceio_session/deviceio_session.hpp" #include #include +#include #include #include @@ -19,10 +24,48 @@ namespace core // ============================================================================ DeviceIOSession::DeviceIOSession(const std::vector>& trackers, - const OpenXRSessionHandles& handles) + const OpenXRSessionHandles& handles, + std::optional recording_config) : handles_(handles), time_converter_(handles) { - LiveDeviceIOFactory factory(handles_); + std::vector> tracker_names; + + if (recording_config) + { + for (const auto& [tracker_ptr, name] : recording_config->tracker_names) + { + bool found = false; + for (const auto& t : trackers) + { + if (t.get() == tracker_ptr) + { + found = true; + break; + } + } + if (!found) + { + throw std::invalid_argument("DeviceIOSession: McapRecordingConfig references tracker '" + name + + "' that is not in the session's tracker list"); + } + } + + mcap_writer_ = std::make_unique(); + mcap::McapWriterOptions options("teleop"); + options.compression = mcap::Compression::None; + + auto status = mcap_writer_->open(recording_config->filename, options); + if (!status.ok()) + { + throw std::runtime_error("DeviceIOSession: failed to open MCAP file '" + recording_config->filename + + "': " + status.message); + } + std::cout << "DeviceIOSession: recording to " << recording_config->filename << std::endl; + + tracker_names = std::move(recording_config->tracker_names); + } + + LiveDeviceIOFactory factory(handles_, mcap_writer_.get(), tracker_names); for (const auto& tracker : trackers) { @@ -40,18 +83,17 @@ DeviceIOSession::DeviceIOSession(const std::vector>& t } } -// Static helper - Get all required OpenXR extensions from a list of trackers +DeviceIOSession::~DeviceIOSession() = default; + std::vector DeviceIOSession::get_required_extensions(const std::vector>& trackers) { std::set all_extensions; - // Extensions required for XrTime conversion for (const auto& ext : XrTimeConverter::get_required_extensions()) { all_extensions.insert(ext); } - // Add extensions from each tracker for (const auto& tracker : trackers) { if (!tracker) @@ -65,23 +107,20 @@ std::vector DeviceIOSession::get_required_extensions(const std::vec } } - // Convert set to vector return std::vector(all_extensions.begin(), all_extensions.end()); } -// Static factory - Create and initialize a session with trackers std::unique_ptr DeviceIOSession::run(const std::vector>& trackers, - const OpenXRSessionHandles& handles) + const OpenXRSessionHandles& handles, + std::optional recording_config) { - // These should never be null - this is improper API usage assert(handles.instance != XR_NULL_HANDLE && "OpenXR instance handle cannot be null"); assert(handles.session != XR_NULL_HANDLE && "OpenXR session handle cannot be null"); assert(handles.space != XR_NULL_HANDLE && "OpenXR space handle cannot be null"); std::cout << "DeviceIOSession: Creating session with " << trackers.size() << " trackers" << std::endl; - // Constructor will throw on failure - return std::unique_ptr(new DeviceIOSession(trackers, handles)); + return std::unique_ptr(new DeviceIOSession(trackers, handles, std::move(recording_config))); } bool DeviceIOSession::update() diff --git a/src/core/deviceio_session/cpp/inc/deviceio_session/deviceio_session.hpp b/src/core/deviceio_session/cpp/inc/deviceio_session/deviceio_session.hpp index d21946f2..94b1dc18 100644 --- a/src/core/deviceio_session/cpp/inc/deviceio_session/deviceio_session.hpp +++ b/src/core/deviceio_session/cpp/inc/deviceio_session/deviceio_session.hpp @@ -9,28 +9,58 @@ #include #include +#include #include #include #include +#include #include +// Forward declaration -- mcap::McapWriter is an implementation detail of DeviceIOSession. +// Consumers of deviceio_core do not need to link against mcap::mcap. +namespace mcap +{ +class McapWriter; +} // namespace mcap + namespace core { -// OpenXR DeviceIO Session - Main user-facing class for OpenXR tracking -// Manages trackers and session lifetime +/** + * @brief MCAP recording configuration for DeviceIOSession. + * + * tracker_names maps each ITracker pointer to its MCAP channel base name. + * Trackers not in the map receive no channel writer and skip recording. + * Pass as std::optional to DeviceIOSession::run(); + * std::nullopt disables recording. + */ +struct McapRecordingConfig +{ + std::string filename; + std::vector> tracker_names; +}; + +// OpenXR DeviceIO Session - manages trackers and optional MCAP recording. +// When a McapRecordingConfig is provided, the session owns and drives a +// mcap::McapWriter; each tracker impl registers its own channels and writes +// directly during update(). class DeviceIOSession : public ITrackerSession { public: // Static helper - Get all required OpenXR extensions from a list of trackers static std::vector get_required_extensions(const std::vector>& trackers); - // Static factory - Create and initialize a session with trackers - // Returns fully initialized session ready to use (throws on failure) + // Static factory - Create and initialize a session with trackers. + // Optionally pass a McapRecordingConfig to enable automatic MCAP recording. static std::unique_ptr run(const std::vector>& trackers, - const OpenXRSessionHandles& handles); + const OpenXRSessionHandles& handles, + std::optional recording_config = std::nullopt); + + // Destructor defined in .cpp where mcap::McapWriter is fully defined + ~DeviceIOSession(); - // Update session and all trackers + // Update session and all trackers. If recording is active, tracker impls + // write their data to the MCAP file directly during this call. bool update(); const ITrackerImpl& get_tracker_impl(const ITracker& tracker) const override @@ -44,15 +74,17 @@ class DeviceIOSession : public ITrackerSession } private: - // Private constructor - use run() instead (throws std::runtime_error on failure) - DeviceIOSession(const std::vector>& trackers, const OpenXRSessionHandles& handles); + DeviceIOSession(const std::vector>& trackers, + const OpenXRSessionHandles& handles, + std::optional recording_config); const OpenXRSessionHandles handles_; std::unordered_map> tracker_impls_; std::unordered_map tracker_update_failure_counts_; - - // For time conversion XrTimeConverter time_converter_; + + // Owned MCAP writer; null when recording is not configured. + std::unique_ptr mcap_writer_; }; } // namespace core diff --git a/src/core/deviceio_session/python/deviceio_session_init.py b/src/core/deviceio_session/python/deviceio_session_init.py index 178966bc..76f13d4a 100644 --- a/src/core/deviceio_session/python/deviceio_session_init.py +++ b/src/core/deviceio_session/python/deviceio_session_init.py @@ -3,8 +3,9 @@ """Isaac Teleop DeviceIO Session — session management for device I/O.""" -from ._deviceio_session import DeviceIOSession +from ._deviceio_session import DeviceIOSession, McapRecordingConfig __all__ = [ "DeviceIOSession", + "McapRecordingConfig", ] diff --git a/src/core/deviceio_session/python/session_bindings.cpp b/src/core/deviceio_session/python/session_bindings.cpp index d31963ee..12123731 100644 --- a/src/core/deviceio_session/python/session_bindings.cpp +++ b/src/core/deviceio_session/python/session_bindings.cpp @@ -2,10 +2,15 @@ // SPDX-License-Identifier: Apache-2.0 #include +#include #include #include +#include #include +#include +#include +#include namespace py = pybind11; @@ -15,6 +20,25 @@ PYBIND11_MODULE(_deviceio_session, m) py::module_::import("isaacteleop.deviceio_trackers._deviceio_trackers"); + py::class_(m, "McapRecordingConfig", + "Configuration for MCAP recording. " + "Pass to DeviceIOSession.run() to enable recording, " + "or omit / pass None to disable.") + .def(py::init( + [](const std::string& filename, + const std::vector, std::string>>& tracker_names) + { + core::McapRecordingConfig config; + config.filename = filename; + for (const auto& [tracker, name] : tracker_names) + { + config.tracker_names.emplace_back(tracker.get(), name); + } + return config; + }), + py::arg("filename"), py::arg("tracker_names")) + .def_readwrite("filename", &core::McapRecordingConfig::filename); + py::class_>( m, "DeviceIOSession") .def("update", &core::PyDeviceIOSession::update, "Update session and all trackers") @@ -26,7 +50,8 @@ PYBIND11_MODULE(_deviceio_session, m) "Get list of OpenXR extensions required by a list of trackers") .def_static( "run", - [](const std::vector>& trackers, const core::OpenXRSessionHandles& handles) + [](const std::vector>& trackers, const core::OpenXRSessionHandles& handles, + std::optional recording_config) { if (handles.instance == XR_NULL_HANDLE || handles.session == XR_NULL_HANDLE || handles.space == XR_NULL_HANDLE || handles.xrGetInstanceProcAddr == nullptr) @@ -35,9 +60,10 @@ PYBIND11_MODULE(_deviceio_session, m) "DeviceIOSession.run: invalid OpenXRSessionHandles (instance, session, space must be non-null " "handles and xrGetInstanceProcAddr must be set)"); } - auto session = core::DeviceIOSession::run(trackers, handles); + auto session = core::DeviceIOSession::run(trackers, handles, std::move(recording_config)); return std::make_unique(std::move(session)); }, - py::arg("trackers"), py::arg("handles"), - "Create and initialize a session with trackers (returns context-managed session, throws on failure)."); + py::arg("trackers"), py::arg("handles"), py::arg("recording_config") = py::none(), + "Create and initialize a session with trackers. " + "Pass a McapRecordingConfig to enable MCAP recording."); } diff --git a/src/core/deviceio_trackers/cpp/controller_tracker.cpp b/src/core/deviceio_trackers/cpp/controller_tracker.cpp index 7027486a..84e77df7 100644 --- a/src/core/deviceio_trackers/cpp/controller_tracker.cpp +++ b/src/core/deviceio_trackers/cpp/controller_tracker.cpp @@ -4,7 +4,6 @@ #include "inc/deviceio_trackers/controller_tracker.hpp" #include -#include #include @@ -20,12 +19,6 @@ std::vector ControllerTracker::get_required_extensions() const return { XR_NVX1_ACTION_CONTEXT_EXTENSION_NAME }; } -std::string_view ControllerTracker::get_schema_text() const -{ - return std::string_view(reinterpret_cast(ControllerSnapshotRecordBinarySchema::data()), - ControllerSnapshotRecordBinarySchema::size()); -} - const ControllerSnapshotTrackedT& ControllerTracker::get_left_controller(const ITrackerSession& session) const { return static_cast(session.get_tracker_impl(*this)).get_left_controller(); diff --git a/src/core/deviceio_trackers/cpp/frame_metadata_tracker_oak.cpp b/src/core/deviceio_trackers/cpp/frame_metadata_tracker_oak.cpp index 2460e923..b769c09b 100644 --- a/src/core/deviceio_trackers/cpp/frame_metadata_tracker_oak.cpp +++ b/src/core/deviceio_trackers/cpp/frame_metadata_tracker_oak.cpp @@ -4,7 +4,6 @@ #include "inc/deviceio_trackers/frame_metadata_tracker_oak.hpp" #include -#include #include #include @@ -34,7 +33,7 @@ FrameMetadataTrackerOak::FrameMetadataTrackerOak(const std::string& collection_p throw std::invalid_argument("FrameMetadataTrackerOak: invalid StreamType value " + std::to_string(static_cast(type))); } - m_channel_names.emplace_back(name); + m_stream_names.emplace_back(name); } } @@ -45,12 +44,6 @@ std::vector FrameMetadataTrackerOak::get_required_extensions() cons return { "XR_NVX1_tensor_data" }; } -std::string_view FrameMetadataTrackerOak::get_schema_text() const -{ - return std::string_view(reinterpret_cast(FrameMetadataOakRecordBinarySchema::data()), - FrameMetadataOakRecordBinarySchema::size()); -} - const FrameMetadataOakTrackedT& FrameMetadataTrackerOak::get_stream_data(const ITrackerSession& session, size_t stream_index) const { diff --git a/src/core/deviceio_trackers/cpp/full_body_tracker_pico.cpp b/src/core/deviceio_trackers/cpp/full_body_tracker_pico.cpp index 5f3accc3..25821239 100644 --- a/src/core/deviceio_trackers/cpp/full_body_tracker_pico.cpp +++ b/src/core/deviceio_trackers/cpp/full_body_tracker_pico.cpp @@ -4,7 +4,6 @@ #include "inc/deviceio_trackers/full_body_tracker_pico.hpp" #include -#include namespace core { @@ -18,12 +17,6 @@ std::vector FullBodyTrackerPico::get_required_extensions() const return { XR_BD_BODY_TRACKING_EXTENSION_NAME }; } -std::string_view FullBodyTrackerPico::get_schema_text() const -{ - return std::string_view(reinterpret_cast(FullBodyPosePicoRecordBinarySchema::data()), - FullBodyPosePicoRecordBinarySchema::size()); -} - const FullBodyPosePicoTrackedT& FullBodyTrackerPico::get_body_pose(const ITrackerSession& session) const { return static_cast(session.get_tracker_impl(*this)).get_body_pose(); diff --git a/src/core/deviceio_trackers/cpp/generic_3axis_pedal_tracker.cpp b/src/core/deviceio_trackers/cpp/generic_3axis_pedal_tracker.cpp index f69f4d87..5bbdc77d 100644 --- a/src/core/deviceio_trackers/cpp/generic_3axis_pedal_tracker.cpp +++ b/src/core/deviceio_trackers/cpp/generic_3axis_pedal_tracker.cpp @@ -4,7 +4,6 @@ #include "inc/deviceio_trackers/generic_3axis_pedal_tracker.hpp" #include -#include namespace core { @@ -25,12 +24,6 @@ std::vector Generic3AxisPedalTracker::get_required_extensions() con return { "XR_NVX1_tensor_data" }; } -std::string_view Generic3AxisPedalTracker::get_schema_text() const -{ - return std::string_view(reinterpret_cast(Generic3AxisPedalOutputRecordBinarySchema::data()), - Generic3AxisPedalOutputRecordBinarySchema::size()); -} - const Generic3AxisPedalOutputTrackedT& Generic3AxisPedalTracker::get_data(const ITrackerSession& session) const { return static_cast(session.get_tracker_impl(*this)).get_data(); diff --git a/src/core/deviceio_trackers/cpp/hand_tracker.cpp b/src/core/deviceio_trackers/cpp/hand_tracker.cpp index e2c909fd..6376be26 100644 --- a/src/core/deviceio_trackers/cpp/hand_tracker.cpp +++ b/src/core/deviceio_trackers/cpp/hand_tracker.cpp @@ -4,7 +4,6 @@ #include "inc/deviceio_trackers/hand_tracker.hpp" #include -#include #include @@ -20,12 +19,6 @@ std::vector HandTracker::get_required_extensions() const return { XR_EXT_HAND_TRACKING_EXTENSION_NAME }; } -std::string_view HandTracker::get_schema_text() const -{ - return std::string_view( - reinterpret_cast(HandPoseRecordBinarySchema::data()), HandPoseRecordBinarySchema::size()); -} - std::unique_ptr HandTracker::create_tracker_impl(ITrackerFactory& factory) const { return factory.create_hand_tracker_impl(this); diff --git a/src/core/deviceio_trackers/cpp/head_tracker.cpp b/src/core/deviceio_trackers/cpp/head_tracker.cpp index 813be4e9..35b7da0e 100644 --- a/src/core/deviceio_trackers/cpp/head_tracker.cpp +++ b/src/core/deviceio_trackers/cpp/head_tracker.cpp @@ -4,7 +4,6 @@ #include "inc/deviceio_trackers/head_tracker.hpp" #include -#include namespace core { @@ -18,12 +17,6 @@ std::vector HeadTracker::get_required_extensions() const return {}; } -std::string_view HeadTracker::get_schema_text() const -{ - return std::string_view( - reinterpret_cast(HeadPoseRecordBinarySchema::data()), HeadPoseRecordBinarySchema::size()); -} - std::unique_ptr HeadTracker::create_tracker_impl(ITrackerFactory& factory) const { return factory.create_head_tracker_impl(this); diff --git a/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/controller_tracker.hpp b/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/controller_tracker.hpp index 2db3dd11..1610a91a 100644 --- a/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/controller_tracker.hpp +++ b/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/controller_tracker.hpp @@ -11,12 +11,9 @@ namespace core { -// Controller tracker - tracks both left and right controllers. -// Updates all controller state (poses + inputs) each frame. -// -// Each instance creates its own XR_NVX1_action_context so that multiple -// ControllerTracker instances can coexist on the same XrSession without -// conflicting action-set names or interaction-profile bindings. +// Tracks both left and right controllers via XR_NVX1_action_context. +// Each instance creates its own action context, so multiple ControllerTracker +// instances can coexist on the same XrSession. class ControllerTracker : public ITracker { public: @@ -25,15 +22,6 @@ class ControllerTracker : public ITracker { return TRACKER_NAME; } - std::string_view get_schema_name() const override - { - return SCHEMA_NAME; - } - std::string_view get_schema_text() const override; - std::vector get_record_channels() const override - { - return { "left_controller", "right_controller" }; - } // Double-dispatch: calls factory.create_controller_tracker_impl() std::unique_ptr create_tracker_impl(ITrackerFactory& factory) const override; @@ -44,7 +32,6 @@ class ControllerTracker : public ITracker private: static constexpr const char* TRACKER_NAME = "ControllerTracker"; - static constexpr const char* SCHEMA_NAME = "core.ControllerSnapshotRecord"; }; } // namespace core diff --git a/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/frame_metadata_tracker_oak.hpp b/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/frame_metadata_tracker_oak.hpp index 62df9c76..60e9cac3 100644 --- a/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/frame_metadata_tracker_oak.hpp +++ b/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/frame_metadata_tracker_oak.hpp @@ -15,10 +15,10 @@ namespace core { /*! - * @brief Multi-channel tracker for reading OAK FrameMetadataOak from multiple streams. + * @brief Multi-stream tracker for OAK FrameMetadataOak. * - * Maintains one tensor reader per stream and records each as a separate MCAP - * channel using FrameMetadataOak as the root type. + * Maintains one SchemaTracker per stream. Each stream is identified by its + * StreamType enum name (e.g., "Color", "MonoLeft"). * * Usage: * @code @@ -53,15 +53,6 @@ class FrameMetadataTrackerOak : public ITracker { return TRACKER_NAME; } - std::string_view get_schema_name() const override - { - return SCHEMA_NAME; - } - std::string_view get_schema_text() const override; - std::vector get_record_channels() const override - { - return m_channel_names; - } // Double-dispatch: calls factory.create_frame_metadata_tracker_oak_impl() std::unique_ptr create_tracker_impl(ITrackerFactory& factory) const override; @@ -78,7 +69,7 @@ class FrameMetadataTrackerOak : public ITracker //! Number of streams this tracker is configured for. size_t get_stream_count() const { - return m_channel_names.size(); + return m_stream_names.size(); } const std::string& collection_prefix() const @@ -96,14 +87,18 @@ class FrameMetadataTrackerOak : public ITracker return max_flatbuffer_size_; } + const std::vector& get_stream_names() const + { + return m_stream_names; + } + private: static constexpr const char* TRACKER_NAME = "FrameMetadataTrackerOak"; - static constexpr const char* SCHEMA_NAME = "core.FrameMetadataOakRecord"; std::string collection_prefix_; std::vector streams_; size_t max_flatbuffer_size_{ DEFAULT_MAX_FLATBUFFER_SIZE }; - std::vector m_channel_names; + std::vector m_stream_names; }; } // namespace core diff --git a/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/full_body_tracker_pico.hpp b/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/full_body_tracker_pico.hpp index 775cfdf6..8f76905f 100644 --- a/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/full_body_tracker_pico.hpp +++ b/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/full_body_tracker_pico.hpp @@ -12,12 +12,12 @@ namespace core { -// Full body tracker for PICO devices using XR_BD_body_tracking extension. -// Tracks 24 body joints from pelvis to hands. +// Full body tracker for PICO devices using XR_BD_body_tracking. +// Tracks 24 body joints (indices 0-23) from pelvis to hands. class FullBodyTrackerPico : public ITracker { public: - //! Number of joints in XR_BD_body_tracking (0-23) + //! Number of joints in XR_BD_body_tracking (0-23). static constexpr uint32_t JOINT_COUNT = 24; std::vector get_required_extensions() const override; @@ -25,15 +25,6 @@ class FullBodyTrackerPico : public ITracker { return TRACKER_NAME; } - std::string_view get_schema_name() const override - { - return SCHEMA_NAME; - } - std::string_view get_schema_text() const override; - std::vector get_record_channels() const override - { - return { "full_body" }; - } // Double-dispatch: calls factory.create_full_body_tracker_pico_impl() std::unique_ptr create_tracker_impl(ITrackerFactory& factory) const override; @@ -43,7 +34,6 @@ class FullBodyTrackerPico : public ITracker private: static constexpr const char* TRACKER_NAME = "FullBodyTrackerPico"; - static constexpr const char* SCHEMA_NAME = "core.FullBodyPosePicoRecord"; }; } // namespace core diff --git a/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/generic_3axis_pedal_tracker.hpp b/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/generic_3axis_pedal_tracker.hpp index 9eea6582..06f5277c 100644 --- a/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/generic_3axis_pedal_tracker.hpp +++ b/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/generic_3axis_pedal_tracker.hpp @@ -58,16 +58,6 @@ class Generic3AxisPedalTracker : public ITracker { return TRACKER_NAME; } - std::string_view get_schema_name() const override - { - return SCHEMA_NAME; - } - std::string_view get_schema_text() const override; - std::vector get_record_channels() const override - { - return { "pedals" }; - } - // Double-dispatch: calls factory.create_generic_3axis_pedal_tracker_impl(this) std::unique_ptr create_tracker_impl(ITrackerFactory& factory) const override; @@ -93,7 +83,6 @@ class Generic3AxisPedalTracker : public ITracker private: static constexpr const char* TRACKER_NAME = "Generic3AxisPedalTracker"; - static constexpr const char* SCHEMA_NAME = "core.Generic3AxisPedalOutputRecord"; std::string collection_id_; size_t max_flatbuffer_size_; diff --git a/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/hand_tracker.hpp b/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/hand_tracker.hpp index a1d9416b..0b1060e9 100644 --- a/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/hand_tracker.hpp +++ b/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/hand_tracker.hpp @@ -12,7 +12,7 @@ namespace core { -// Hand tracker - tracks both left and right hands via XR_EXT_hand_tracking +// Tracks both left and right hands via XR_EXT_hand_tracking. class HandTracker : public ITracker { public: @@ -21,15 +21,6 @@ class HandTracker : public ITracker { return TRACKER_NAME; } - std::string_view get_schema_name() const override - { - return SCHEMA_NAME; - } - std::string_view get_schema_text() const override; - std::vector get_record_channels() const override - { - return { "left_hand", "right_hand" }; - } // Double-dispatch: calls factory.create_hand_tracker_impl() std::unique_ptr create_tracker_impl(ITrackerFactory& factory) const override; @@ -43,7 +34,6 @@ class HandTracker : public ITracker private: static constexpr const char* TRACKER_NAME = "HandTracker"; - static constexpr const char* SCHEMA_NAME = "core.HandPoseRecord"; }; } // namespace core diff --git a/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/head_tracker.hpp b/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/head_tracker.hpp index 25f41c19..515844f7 100644 --- a/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/head_tracker.hpp +++ b/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/head_tracker.hpp @@ -11,7 +11,7 @@ namespace core { -// Head tracker - tracks HMD pose (returns HeadPoseTrackedT from FlatBuffer schema) +// Tracks HMD pose via XR_REFERENCE_SPACE_TYPE_VIEW. class HeadTracker : public ITracker { public: @@ -20,15 +20,6 @@ class HeadTracker : public ITracker { return TRACKER_NAME; } - std::string_view get_schema_name() const override - { - return SCHEMA_NAME; - } - std::string_view get_schema_text() const override; - std::vector get_record_channels() const override - { - return { "head" }; - } // Double-dispatch: calls factory.create_head_tracker_impl() std::unique_ptr create_tracker_impl(ITrackerFactory& factory) const override; @@ -38,7 +29,6 @@ class HeadTracker : public ITracker private: static constexpr const char* TRACKER_NAME = "HeadTracker"; - static constexpr const char* SCHEMA_NAME = "core.HeadPoseRecord"; }; } // namespace core diff --git a/src/core/live_trackers/cpp/CMakeLists.txt b/src/core/live_trackers/cpp/CMakeLists.txt index eaa8b604..f562f718 100644 --- a/src/core/live_trackers/cpp/CMakeLists.txt +++ b/src/core/live_trackers/cpp/CMakeLists.txt @@ -4,7 +4,7 @@ cmake_minimum_required(VERSION 3.20) add_library(live_trackers STATIC - schema_tracker.cpp + schema_tracker_base.cpp live_deviceio_factory.cpp live_head_tracker_impl.cpp live_hand_tracker_impl.cpp @@ -12,6 +12,7 @@ add_library(live_trackers STATIC live_full_body_tracker_pico_impl.cpp live_generic_3axis_pedal_tracker_impl.cpp live_frame_metadata_tracker_oak_impl.cpp + inc/live_trackers/schema_tracker_base.hpp inc/live_trackers/schema_tracker.hpp inc/live_trackers/live_deviceio_factory.hpp live_head_tracker_impl.hpp @@ -31,6 +32,7 @@ target_link_libraries(live_trackers PUBLIC deviceio::deviceio_base deviceio::deviceio_trackers + mcap::mcap_core Teleop::openxr_extensions ) diff --git a/src/core/live_trackers/cpp/inc/live_trackers/live_deviceio_factory.hpp b/src/core/live_trackers/cpp/inc/live_trackers/live_deviceio_factory.hpp index b8cccc70..8c2862bb 100644 --- a/src/core/live_trackers/cpp/inc/live_trackers/live_deviceio_factory.hpp +++ b/src/core/live_trackers/cpp/inc/live_trackers/live_deviceio_factory.hpp @@ -6,21 +6,36 @@ #include #include +#include +#include +#include +#include +#include + +namespace mcap +{ +class McapWriter; +} // namespace mcap namespace core { +class ITracker; struct OpenXRSessionHandles; /** * @brief ITrackerFactory implementation for live OpenXR sessions. * * Used by DeviceIOSession to construct OpenXR-backed tracker implementations. + * When writer is non-null, each simple impl receives a typed McapTrackerChannels + * for MCAP recording. */ class LiveDeviceIOFactory : public ITrackerFactory { public: - explicit LiveDeviceIOFactory(const OpenXRSessionHandles& handles); + LiveDeviceIOFactory(const OpenXRSessionHandles& handles, + mcap::McapWriter* writer, + const std::vector>& tracker_names); std::unique_ptr create_head_tracker_impl(const HeadTracker* tracker) override; std::unique_ptr create_hand_tracker_impl(const HandTracker* tracker) override; @@ -32,7 +47,12 @@ class LiveDeviceIOFactory : public ITrackerFactory const FrameMetadataTrackerOak* tracker) override; private: + bool should_record(const ITracker* tracker) const; + std::string_view get_name(const ITracker* tracker) const; + const OpenXRSessionHandles& handles_; + mcap::McapWriter* writer_; + std::unordered_map name_map_; }; } // namespace core diff --git a/src/core/live_trackers/cpp/inc/live_trackers/schema_tracker.hpp b/src/core/live_trackers/cpp/inc/live_trackers/schema_tracker.hpp index 5dad793c..31454654 100644 --- a/src/core/live_trackers/cpp/inc/live_trackers/schema_tracker.hpp +++ b/src/core/live_trackers/cpp/inc/live_trackers/schema_tracker.hpp @@ -3,149 +3,102 @@ #pragma once -#include -#include -#include +#include "schema_tracker_base.hpp" -#include -#include -#include -#include -#include -#include +#include +#include -namespace core -{ +#include -/*! - * @brief Configuration for OpenXR tensor-backed FlatBuffer readers (used by SchemaTracker). - */ -struct SchemaTrackerConfig +namespace core { - //! Tensor collection identifier for discovery (e.g., "head_data"). - std::string collection_id; - - //! Maximum serialized FlatBuffer message size in bytes. - size_t max_flatbuffer_size; - - //! Tensor name within the collection (e.g., "head_pose"). - std::string tensor_identifier; - - //! Human-readable description for debugging and runtime display. - std::string localized_name; -}; -/*! - * @brief Utility class for reading FlatBuffer schema data via OpenXR tensor extensions. - * - * This class handles all the OpenXR tensor extension calls for reading data. - * Use it via composition in live ITrackerImpl implementations. +/** + * @brief Typed SchemaTracker that optionally records to MCAP. * - * The caller is responsible for creating the OpenXR session with the required extensions - * (XR_NVX1_TENSOR_DATA_EXTENSION_NAME). + * Wraps SchemaTrackerBase with FlatBuffer type knowledge so that each sample + * read from the tensor can be automatically written to an MCAP channel. * - * See LiveGeneric3AxisPedalTrackerImpl for a concrete usage example. + * @tparam RecordT FlatBuffer record wrapper (e.g. Generic3AxisPedalOutputRecord). + * @tparam DataTableT FlatBuffer data table (e.g. Generic3AxisPedalOutput). */ -class SchemaTracker +template +class SchemaTracker : public SchemaTrackerBase { public: - /*! - * @brief Constructs the tracker and initializes the OpenXR tensor list. - * @param handles OpenXR session handles. - * @param config Configuration for the tensor collection. - * @throws std::runtime_error if initialization fails. - */ - SchemaTracker(const OpenXRSessionHandles& handles, SchemaTrackerConfig config); - - /*! - * @brief Destroys the tracker and cleans up OpenXR resources. - */ - ~SchemaTracker(); - - // Non-copyable, non-movable - SchemaTracker(const SchemaTracker&) = delete; - SchemaTracker& operator=(const SchemaTracker&) = delete; - SchemaTracker(SchemaTracker&&) = delete; - SchemaTracker& operator=(SchemaTracker&&) = delete; - - /*! - * @brief Get required OpenXR extensions for tensor data reading and time conversion. - * @return Vector of required extension name strings. + using NativeDataT = typename DataTableT::NativeTableType; + using Channels = McapTrackerChannels; + + /** + * @param mcap_channels Non-owning pointer to the MCAP channel writer. Must outlive + * this SchemaTracker. Owned by the live tracker impl that also owns this + * SchemaTracker instance. Null when recording is disabled. + * @param mcap_channel_index 0-based sub-channel index within mcap_channels. */ - static std::vector get_required_extensions(); - - /*! - * @brief A single tensor sample with its data buffer and timestamps. - * - * The DeviceDataTimestamp fields are populated as follows: - * - available_time_local_common_clock: system monotonic nanoseconds when the runtime - * received the sample (converted from XrTime via xrConvertTimeToTimespecTimeKHR). - * - sample_time_local_common_clock: system monotonic nanoseconds when the sample was - * captured on the push side (converted from XrTime symmetrically with push_buffer). - * - sample_time_raw_device_clock: raw device clock nanoseconds, unchanged from what - * the pusher provided. - */ - struct SampleResult + SchemaTracker(const OpenXRSessionHandles& handles, + SchemaTrackerConfig config, + Channels* mcap_channels = nullptr, + size_t mcap_channel_index = 0) + : SchemaTrackerBase(handles, std::move(config)), + mcap_channels_(mcap_channels), + mcap_channel_index_(mcap_channel_index) { - std::vector buffer; - DeviceDataTimestamp timestamp; - }; + } - /*! - * @brief Read ALL pending samples from the tensor collection. - * - * Drains every available sample since the last read, appending each to the - * output vector with timestamps converted from XrTensorSampleMetadataNV: - * - available_time_local_common_clock = arrivalTimestamp → local monotonic nanoseconds - * - sample_time_local_common_clock = timestamp → local monotonic nanoseconds - * - sample_time_raw_device_clock = rawDeviceTimestamp (raw device clock, not converted) + /** + * @brief Read all pending samples; write each to MCAP if channels are set. * - * A @c false return does not imply that no samples were appended — use - * @c samples.size() to determine how many were read. The return value only - * indicates whether the target collection is currently reachable, which lets - * callers distinguish between "tracker disappeared" (false) and "update called - * before any new samples arrived" (true, zero appended). + * Each sample is unpacked, repacked into a Record with its timestamp, + * and written to the MCAP channel. The last sample's unpacked data is + * returned via out_latest (if non-null and samples were read). * - * @param samples Output vector; new samples are appended (not cleared). - * @return @c true if the target collection is present; @c false if it has not - * been discovered yet or has disappeared. + * @param out_latest If non-null and samples were read, receives the unpacked + * data from the last sample. + * @return true if the tensor collection is present. */ - bool read_all_samples(std::vector& samples); - - /*! - * @brief Access the configuration. - */ - const SchemaTrackerConfig& config() const; + bool update(std::shared_ptr& out_latest) + { + samples_.clear(); + bool present = read_all_samples(samples_); + + if (samples_.empty()) + { + if (!present) + { + out_latest.reset(); + } + return present; + } + + for (const auto& sample : samples_) + { + auto fb = flatbuffers::GetRoot(sample.buffer.data()); + if (!fb) + { + continue; + } + + if (!out_latest) + { + out_latest = std::make_shared(); + } + fb->UnPackTo(out_latest.get()); + + // write() serializes synchronously and does not retain the shared_ptr, + // so reusing out_latest across loop iterations is safe. + if (mcap_channels_) + { + mcap_channels_->write(mcap_channel_index_, sample.timestamp, out_latest); + } + } + + return present; + } 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(SampleResult& out); - - OpenXRSessionHandles m_handles; - SchemaTrackerConfig m_config; - XrTimeConverter m_time_converter; - - XrTensorListNV m_tensor_list{ XR_NULL_HANDLE }; - - PFN_xrGetTensorListLatestGenerationNV m_get_latest_gen_fn{ nullptr }; - PFN_xrCreateTensorListNV m_create_list_fn{ nullptr }; - PFN_xrGetTensorListPropertiesNV m_get_list_props_fn{ nullptr }; - PFN_xrGetTensorCollectionPropertiesNV m_get_coll_props_fn{ nullptr }; - PFN_xrGetTensorDataNV m_get_data_fn{ nullptr }; - PFN_xrUpdateTensorListNV m_update_list_fn{ nullptr }; - PFN_xrDestroyTensorListNV m_destroy_list_fn{ nullptr }; - - std::optional m_target_collection_index; - uint32_t m_sample_batch_stride{ 0 }; - uint32_t m_sample_size{ 0 }; - - uint64_t m_cached_generation{ 0 }; - - std::optional m_last_sample_index; + Channels* mcap_channels_; + size_t mcap_channel_index_; + std::vector samples_; }; } // namespace core diff --git a/src/core/live_trackers/cpp/inc/live_trackers/schema_tracker_base.hpp b/src/core/live_trackers/cpp/inc/live_trackers/schema_tracker_base.hpp new file mode 100644 index 00000000..8adc5c1e --- /dev/null +++ b/src/core/live_trackers/cpp/inc/live_trackers/schema_tracker_base.hpp @@ -0,0 +1,145 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace core +{ + +/*! + * @brief Configuration for OpenXR tensor-backed FlatBuffer readers (used by SchemaTracker). + */ +struct SchemaTrackerConfig +{ + //! Tensor collection identifier for discovery (e.g., "head_data"). + std::string collection_id; + + //! Maximum serialized FlatBuffer message size in bytes. + size_t max_flatbuffer_size; + + //! Tensor name within the collection (e.g., "head_pose"). + std::string tensor_identifier; + + //! Human-readable description for debugging and runtime display. + std::string localized_name; +}; + +/*! + * @brief Utility class for reading FlatBuffer schema data via OpenXR tensor extensions. + * + * This class handles all the OpenXR tensor extension calls for reading data. + * Use it via composition in live ITrackerImpl implementations. + * + * The caller is responsible for creating the OpenXR session with the required extensions + * (XR_NVX1_TENSOR_DATA_EXTENSION_NAME). + * + * See LiveGeneric3AxisPedalTrackerImpl for a concrete usage example. + */ +class SchemaTrackerBase +{ +public: + /*! + * @brief Constructs the tracker and initializes the OpenXR tensor list. + * @param handles OpenXR session handles. + * @param config Configuration for the tensor collection. + * @throws std::runtime_error if initialization fails. + */ + SchemaTrackerBase(const OpenXRSessionHandles& handles, SchemaTrackerConfig config); + + ~SchemaTrackerBase(); + + SchemaTrackerBase(const SchemaTrackerBase&) = delete; + SchemaTrackerBase& operator=(const SchemaTrackerBase&) = delete; + SchemaTrackerBase(SchemaTrackerBase&&) = delete; + SchemaTrackerBase& operator=(SchemaTrackerBase&&) = delete; + + /*! + * @brief Get required OpenXR extensions for tensor data reading and time conversion. + * @return Vector of required extension name strings. + */ + static std::vector get_required_extensions(); + + /*! + * @brief A single tensor sample with its data buffer and timestamps. + * + * The DeviceDataTimestamp fields are populated as follows: + * - available_time_local_common_clock: system monotonic nanoseconds when the runtime + * received the sample (converted from XrTime via xrConvertTimeToTimespecTimeKHR). + * - sample_time_local_common_clock: system monotonic nanoseconds when the sample was + * captured on the push side (converted from XrTime symmetrically with push_buffer). + * - sample_time_raw_device_clock: raw device clock nanoseconds, unchanged from what + * the pusher provided. + */ + struct SampleResult + { + std::vector buffer; + DeviceDataTimestamp timestamp; + }; + + /*! + * @brief Read ALL pending samples from the tensor collection. + * + * Drains every available sample since the last read, appending each to the + * output vector with timestamps converted from XrTensorSampleMetadataNV: + * - available_time_local_common_clock = arrivalTimestamp → local monotonic nanoseconds + * - sample_time_local_common_clock = timestamp → local monotonic nanoseconds + * - sample_time_raw_device_clock = rawDeviceTimestamp (raw device clock, not converted) + * + * A @c false return does not imply that no samples were appended — use + * @c samples.size() to determine how many were read. The return value only + * indicates whether the target collection is currently reachable, which lets + * callers distinguish between "tracker disappeared" (false) and "update called + * before any new samples arrived" (true, zero appended). + * + * @param samples Output vector; new samples are appended (not cleared). + * @return @c true if the target collection is present; @c false if it has not + * been discovered yet or has disappeared. + */ + bool read_all_samples(std::vector& samples); + + /*! + * @brief Access the configuration. + */ + const SchemaTrackerConfig& config() const; + +private: + void initialize_tensor_data_functions(); + void create_tensor_list(); + bool ensure_collection(); + bool read_next_sample(SampleResult& out); + + OpenXRSessionHandles m_handles; + SchemaTrackerConfig m_config; + XrTimeConverter m_time_converter; + + XrTensorListNV m_tensor_list{ XR_NULL_HANDLE }; + + PFN_xrGetTensorListLatestGenerationNV m_get_latest_gen_fn{ nullptr }; + PFN_xrCreateTensorListNV m_create_list_fn{ nullptr }; + PFN_xrGetTensorListPropertiesNV m_get_list_props_fn{ nullptr }; + PFN_xrGetTensorCollectionPropertiesNV m_get_coll_props_fn{ nullptr }; + PFN_xrGetTensorDataNV m_get_data_fn{ nullptr }; + PFN_xrUpdateTensorListNV m_update_list_fn{ nullptr }; + PFN_xrDestroyTensorListNV m_destroy_list_fn{ nullptr }; + + std::optional m_target_collection_index; + uint32_t m_sample_batch_stride{ 0 }; + uint32_t m_sample_size{ 0 }; + + uint64_t m_cached_generation{ 0 }; + + std::optional m_last_sample_index; +}; + +} // namespace core diff --git a/src/core/live_trackers/cpp/live_controller_tracker_impl.cpp b/src/core/live_trackers/cpp/live_controller_tracker_impl.cpp index 98851ae4..4866f76a 100644 --- a/src/core/live_trackers/cpp/live_controller_tracker_impl.cpp +++ b/src/core/live_trackers/cpp/live_controller_tracker_impl.cpp @@ -3,8 +3,10 @@ #include "live_controller_tracker_impl.hpp" +#include #include #include +#include #include #include @@ -157,7 +159,16 @@ XrAction create_action(const OpenXRCoreFunctions& funcs, // LiveControllerTrackerImpl // ============================================================================ -LiveControllerTrackerImpl::LiveControllerTrackerImpl(const OpenXRSessionHandles& handles) +std::unique_ptr LiveControllerTrackerImpl::create_mcap_channels(mcap::McapWriter& writer, + std::string_view base_name) +{ + return std::make_unique( + writer, base_name, ControllerRecordingTraits::schema_name, + std::vector(ControllerRecordingTraits::channels.begin(), ControllerRecordingTraits::channels.end())); +} + +LiveControllerTrackerImpl::LiveControllerTrackerImpl(const OpenXRSessionHandles& handles, + std::unique_ptr mcap_channels) : core_funcs_(OpenXRCoreFunctions::load(handles.instance, handles.xrGetInstanceProcAddr)), time_converter_(handles), session_(handles.session), @@ -222,7 +233,8 @@ LiveControllerTrackerImpl::LiveControllerTrackerImpl(const OpenXRSessionHandles& left_grip_space_(create_space(core_funcs_, session_, grip_pose_action_, left_hand_path_)), right_grip_space_(create_space(core_funcs_, session_, grip_pose_action_, right_hand_path_)), left_aim_space_(create_space(core_funcs_, session_, aim_pose_action_, left_hand_path_)), - right_aim_space_(create_space(core_funcs_, session_, aim_pose_action_, right_hand_path_)) + right_aim_space_(create_space(core_funcs_, session_, aim_pose_action_, right_hand_path_)), + mcap_channels_(std::move(mcap_channels)) { // Suggest interaction profile bindings (chained to this action context) std::vector bindings; @@ -308,8 +320,6 @@ bool LiveControllerTrackerImpl::update(XrTime time) right_tracked_.data.reset(); return false; } - // sync_state.interactionProfileChanged is intentionally ignored: this - // tracker uses a fixed Oculus Touch binding and has no rebinding logic. auto update_controller = [&](XrPath hand_path, const XrSpacePtr& grip_space, const XrSpacePtr& aim_space, ControllerSnapshotTrackedT& tracked) @@ -379,8 +389,14 @@ bool LiveControllerTrackerImpl::update(XrTime time) update_controller(left_hand_path_, left_grip_space_, left_aim_space_, left_tracked_); update_controller(right_hand_path_, right_grip_space_, right_aim_space_, right_tracked_); - // Sync succeeded; return true even if neither controller is equipped. - // Returns false only on xrSyncActions2NV failure (early return above). + if (mcap_channels_) + { + int64_t monotonic_ns = time_converter_.convert_xrtime_to_monotonic_ns(last_update_time_); + DeviceDataTimestamp timestamp(monotonic_ns, monotonic_ns, last_update_time_); + mcap_channels_->write(0, timestamp, left_tracked_.data); + mcap_channels_->write(1, timestamp, right_tracked_.data); + } + return true; } @@ -394,29 +410,4 @@ const ControllerSnapshotTrackedT& LiveControllerTrackerImpl::get_right_controlle return right_tracked_; } -void LiveControllerTrackerImpl::serialize_all(size_t channel_index, const RecordCallback& callback) const -{ - if (channel_index > 1) - { - throw std::runtime_error("ControllerTracker::serialize_all: invalid channel_index " + - std::to_string(channel_index) + " (must be 0 or 1)"); - } - flatbuffers::FlatBufferBuilder builder(256); - - const auto& tracked = (channel_index == 0) ? left_tracked_ : right_tracked_; - int64_t monotonic_ns = time_converter_.convert_xrtime_to_monotonic_ns(last_update_time_); - DeviceDataTimestamp timestamp(monotonic_ns, monotonic_ns, last_update_time_); - - ControllerSnapshotRecordBuilder record_builder(builder); - if (tracked.data) - { - auto data_offset = ControllerSnapshot::Pack(builder, tracked.data.get()); - record_builder.add_data(data_offset); - } - record_builder.add_timestamp(×tamp); - builder.Finish(record_builder.Finish()); - - callback(monotonic_ns, builder.GetBufferPointer(), builder.GetSize()); -} - } // namespace core diff --git a/src/core/live_trackers/cpp/live_controller_tracker_impl.hpp b/src/core/live_trackers/cpp/live_controller_tracker_impl.hpp index ac61302b..3067a6b8 100644 --- a/src/core/live_trackers/cpp/live_controller_tracker_impl.hpp +++ b/src/core/live_trackers/cpp/live_controller_tracker_impl.hpp @@ -4,20 +4,28 @@ #pragma once #include +#include #include #include #include #include #include +#include +#include + namespace core { -// OpenXR-backed implementation of ControllerTrackerImpl. +using ControllerMcapChannels = McapTrackerChannels; + class LiveControllerTrackerImpl : public ControllerTrackerImpl { public: - explicit LiveControllerTrackerImpl(const OpenXRSessionHandles& handles); + static std::unique_ptr create_mcap_channels(mcap::McapWriter& writer, + std::string_view base_name); + + LiveControllerTrackerImpl(const OpenXRSessionHandles& handles, std::unique_ptr mcap_channels); ~LiveControllerTrackerImpl() = default; LiveControllerTrackerImpl(const LiveControllerTrackerImpl&) = delete; @@ -26,7 +34,6 @@ class LiveControllerTrackerImpl : public ControllerTrackerImpl LiveControllerTrackerImpl& operator=(LiveControllerTrackerImpl&&) = delete; bool update(XrTime time) override; - void serialize_all(size_t channel_index, const RecordCallback& callback) const override; const ControllerSnapshotTrackedT& get_left_controller() const override; const ControllerSnapshotTrackedT& get_right_controller() const override; @@ -40,7 +47,6 @@ class LiveControllerTrackerImpl : public ControllerTrackerImpl XrPath left_hand_path_; XrPath right_hand_path_; - // Action context -- declared before action_set_ so it outlives it. ActionContextFunctions action_ctx_funcs_; XrInstanceActionContextPtr instance_action_context_; XrSessionActionContextPtr session_action_context_; @@ -63,6 +69,8 @@ class LiveControllerTrackerImpl : public ControllerTrackerImpl ControllerSnapshotTrackedT left_tracked_; ControllerSnapshotTrackedT right_tracked_; XrTime last_update_time_ = 0; + + std::unique_ptr mcap_channels_; }; } // namespace core diff --git a/src/core/live_trackers/cpp/live_deviceio_factory.cpp b/src/core/live_trackers/cpp/live_deviceio_factory.cpp index e2d94b82..3d9a2e8d 100644 --- a/src/core/live_trackers/cpp/live_deviceio_factory.cpp +++ b/src/core/live_trackers/cpp/live_deviceio_factory.cpp @@ -17,44 +17,100 @@ #include #include +#include + namespace core { -LiveDeviceIOFactory::LiveDeviceIOFactory(const OpenXRSessionHandles& handles) : handles_(handles) +LiveDeviceIOFactory::LiveDeviceIOFactory(const OpenXRSessionHandles& handles, + mcap::McapWriter* writer, + const std::vector>& tracker_names) + : handles_(handles), writer_(writer) +{ + for (const auto& [tracker, name] : tracker_names) + { + auto [it, inserted] = name_map_.emplace(tracker, name); + if (!inserted) + { + throw std::invalid_argument("LiveDeviceIOFactory: duplicate tracker pointer for channel name '" + name + + "' (already mapped as '" + it->second + "')"); + } + } +} + +bool LiveDeviceIOFactory::should_record(const ITracker* tracker) const +{ + return writer_ && name_map_.count(tracker); +} + +std::string_view LiveDeviceIOFactory::get_name(const ITracker* tracker) const { + auto it = name_map_.find(tracker); + assert(it != name_map_.end() && "get_name called for tracker not in name_map_ (call should_record first)"); + return it->second; } -std::unique_ptr LiveDeviceIOFactory::create_head_tracker_impl(const HeadTracker* /*tracker*/) +std::unique_ptr LiveDeviceIOFactory::create_head_tracker_impl(const HeadTracker* tracker) { - return std::make_unique(handles_); + std::unique_ptr channels; + if (should_record(tracker)) + { + channels = LiveHeadTrackerImpl::create_mcap_channels(*writer_, get_name(tracker)); + } + return std::make_unique(handles_, std::move(channels)); } -std::unique_ptr LiveDeviceIOFactory::create_hand_tracker_impl(const HandTracker* /*tracker*/) +std::unique_ptr LiveDeviceIOFactory::create_hand_tracker_impl(const HandTracker* tracker) { - return std::make_unique(handles_); + std::unique_ptr channels; + if (should_record(tracker)) + { + channels = LiveHandTrackerImpl::create_mcap_channels(*writer_, get_name(tracker)); + } + return std::make_unique(handles_, std::move(channels)); } -std::unique_ptr LiveDeviceIOFactory::create_controller_tracker_impl(const ControllerTracker* /*tracker*/) +std::unique_ptr LiveDeviceIOFactory::create_controller_tracker_impl(const ControllerTracker* tracker) { - return std::make_unique(handles_); + std::unique_ptr channels; + if (should_record(tracker)) + { + channels = LiveControllerTrackerImpl::create_mcap_channels(*writer_, get_name(tracker)); + } + return std::make_unique(handles_, std::move(channels)); } std::unique_ptr LiveDeviceIOFactory::create_full_body_tracker_pico_impl( - const FullBodyTrackerPico* /*tracker*/) + const FullBodyTrackerPico* tracker) { - return std::make_unique(handles_); + std::unique_ptr channels; + if (should_record(tracker)) + { + channels = LiveFullBodyTrackerPicoImpl::create_mcap_channels(*writer_, get_name(tracker)); + } + return std::make_unique(handles_, std::move(channels)); } std::unique_ptr LiveDeviceIOFactory::create_generic_3axis_pedal_tracker_impl( const Generic3AxisPedalTracker* tracker) { - return std::make_unique(handles_, tracker); + std::unique_ptr channels; + if (should_record(tracker)) + { + channels = LiveGeneric3AxisPedalTrackerImpl::create_mcap_channels(*writer_, get_name(tracker)); + } + return std::make_unique(handles_, tracker, std::move(channels)); } std::unique_ptr LiveDeviceIOFactory::create_frame_metadata_tracker_oak_impl( const FrameMetadataTrackerOak* tracker) { - return std::make_unique(handles_, tracker); + std::unique_ptr channels; + if (should_record(tracker)) + { + channels = LiveFrameMetadataTrackerOakImpl::create_mcap_channels(*writer_, get_name(tracker), tracker); + } + return std::make_unique(handles_, tracker, std::move(channels)); } } // namespace core diff --git a/src/core/live_trackers/cpp/live_frame_metadata_tracker_oak_impl.cpp b/src/core/live_trackers/cpp/live_frame_metadata_tracker_oak_impl.cpp index 6012f24c..df842eb5 100644 --- a/src/core/live_trackers/cpp/live_frame_metadata_tracker_oak_impl.cpp +++ b/src/core/live_trackers/cpp/live_frame_metadata_tracker_oak_impl.cpp @@ -3,13 +3,11 @@ #include "live_frame_metadata_tracker_oak_impl.hpp" -#include -#include -#include +#include +#include #include #include -#include namespace core { @@ -40,97 +38,35 @@ std::vector make_oak_tensor_configs(const FrameMetadataTrac // LiveFrameMetadataTrackerOakImpl // ============================================================================ +std::unique_ptr LiveFrameMetadataTrackerOakImpl::create_mcap_channels( + mcap::McapWriter& writer, std::string_view base_name, const FrameMetadataTrackerOak* tracker) +{ + return std::make_unique( + writer, base_name, OakRecordingTraits::schema_name, tracker->get_stream_names()); +} + LiveFrameMetadataTrackerOakImpl::LiveFrameMetadataTrackerOakImpl(const OpenXRSessionHandles& handles, - const FrameMetadataTrackerOak* tracker) - : m_time_converter_(handles) + const FrameMetadataTrackerOak* tracker, + std::unique_ptr mcap_channels) + : mcap_channels_(std::move(mcap_channels)) { auto configs = make_oak_tensor_configs(tracker); - for (auto& config : configs) + for (size_t i = 0; i < configs.size(); ++i) { StreamState state; - state.reader = std::make_unique(handles, std::move(config)); + state.reader = std::make_unique(handles, std::move(configs[i]), mcap_channels_.get(), i); m_streams.push_back(std::move(state)); } } -bool LiveFrameMetadataTrackerOakImpl::update(XrTime time) +bool LiveFrameMetadataTrackerOakImpl::update(XrTime /*time*/) { - m_last_update_time_ = time; + bool any_present = false; for (auto& stream : m_streams) { - stream.pending_records.clear(); - stream.collection_present = stream.reader->read_all_samples(stream.pending_records); - - if (!stream.collection_present) - { - stream.tracked.data.reset(); - continue; - } - - // When present but empty, intentionally retain the last sample in stream.tracked.data - if (!stream.pending_records.empty()) - { - auto fb = flatbuffers::GetRoot(stream.pending_records.back().buffer.data()); - if (fb) - { - if (!stream.tracked.data) - { - stream.tracked.data = std::make_shared(); - } - fb->UnPackTo(stream.tracked.data.get()); - } - } - } - - return true; -} - -void LiveFrameMetadataTrackerOakImpl::serialize_all(size_t channel_index, const RecordCallback& callback) const -{ - if (channel_index >= m_streams.size()) - { - throw std::runtime_error("FrameMetadataTrackerOak::serialize_all: invalid channel_index " + - std::to_string(channel_index) + " (have " + std::to_string(m_streams.size()) + - " streams)"); - } - - int64_t update_ns = m_time_converter_.convert_xrtime_to_monotonic_ns(m_last_update_time_); - - const auto& stream = m_streams[channel_index]; - if (stream.pending_records.empty()) - { - if (!stream.collection_present) - { - DeviceDataTimestamp update_timestamp(update_ns, update_ns, 0); - flatbuffers::FlatBufferBuilder builder(64); - FrameMetadataOakRecordBuilder record_builder(builder); - record_builder.add_timestamp(&update_timestamp); - builder.Finish(record_builder.Finish()); - callback(update_ns, builder.GetBufferPointer(), builder.GetSize()); - } - return; - } - const auto& pending = stream.pending_records; - - for (const auto& sample : pending) - { - auto fb = flatbuffers::GetRoot(sample.buffer.data()); - if (!fb) - { - continue; - } - - FrameMetadataOakT parsed; - fb->UnPackTo(&parsed); - - flatbuffers::FlatBufferBuilder builder(256); - auto data_offset = FrameMetadataOak::Pack(builder, &parsed); - FrameMetadataOakRecordBuilder record_builder(builder); - record_builder.add_data(data_offset); - record_builder.add_timestamp(&sample.timestamp); - builder.Finish(record_builder.Finish()); - callback(update_ns, builder.GetBufferPointer(), builder.GetSize()); + any_present |= stream.reader->update(stream.tracked.data); } + return any_present; } const FrameMetadataOakTrackedT& LiveFrameMetadataTrackerOakImpl::get_stream_data(size_t stream_index) const diff --git a/src/core/live_trackers/cpp/live_frame_metadata_tracker_oak_impl.hpp b/src/core/live_trackers/cpp/live_frame_metadata_tracker_oak_impl.hpp index a2ad046a..fc9651c3 100644 --- a/src/core/live_trackers/cpp/live_frame_metadata_tracker_oak_impl.hpp +++ b/src/core/live_trackers/cpp/live_frame_metadata_tracker_oak_impl.hpp @@ -7,19 +7,28 @@ #include #include -#include +#include #include +#include #include namespace core { -// OpenXR-backed implementation of FrameMetadataTrackerOakImpl. +using OakMcapChannels = McapTrackerChannels; +using OakSchemaTracker = SchemaTracker; + class LiveFrameMetadataTrackerOakImpl : public FrameMetadataTrackerOakImpl { public: - LiveFrameMetadataTrackerOakImpl(const OpenXRSessionHandles& handles, const FrameMetadataTrackerOak* tracker); + static std::unique_ptr create_mcap_channels(mcap::McapWriter& writer, + std::string_view base_name, + const FrameMetadataTrackerOak* tracker); + + LiveFrameMetadataTrackerOakImpl(const OpenXRSessionHandles& handles, + const FrameMetadataTrackerOak* tracker, + std::unique_ptr mcap_channels); LiveFrameMetadataTrackerOakImpl(const LiveFrameMetadataTrackerOakImpl&) = delete; LiveFrameMetadataTrackerOakImpl& operator=(const LiveFrameMetadataTrackerOakImpl&) = delete; @@ -27,20 +36,16 @@ class LiveFrameMetadataTrackerOakImpl : public FrameMetadataTrackerOakImpl LiveFrameMetadataTrackerOakImpl& operator=(LiveFrameMetadataTrackerOakImpl&&) = delete; bool update(XrTime time) override; - void serialize_all(size_t channel_index, const RecordCallback& callback) const override; const FrameMetadataOakTrackedT& get_stream_data(size_t stream_index) const override; private: struct StreamState { - std::unique_ptr reader; + std::unique_ptr reader; FrameMetadataOakTrackedT tracked; - bool collection_present = false; - std::vector pending_records; }; - XrTimeConverter m_time_converter_; - XrTime m_last_update_time_ = 0; + std::unique_ptr mcap_channels_; std::vector m_streams; }; diff --git a/src/core/live_trackers/cpp/live_full_body_tracker_pico_impl.cpp b/src/core/live_trackers/cpp/live_full_body_tracker_pico_impl.cpp index 28f21a13..340fd926 100644 --- a/src/core/live_trackers/cpp/live_full_body_tracker_pico_impl.cpp +++ b/src/core/live_trackers/cpp/live_full_body_tracker_pico_impl.cpp @@ -3,8 +3,10 @@ #include "live_full_body_tracker_pico_impl.hpp" +#include #include #include +#include #include #include @@ -17,13 +19,23 @@ namespace core // LiveFullBodyTrackerPicoImpl // ============================================================================ -LiveFullBodyTrackerPicoImpl::LiveFullBodyTrackerPicoImpl(const OpenXRSessionHandles& handles) +std::unique_ptr LiveFullBodyTrackerPicoImpl::create_mcap_channels(mcap::McapWriter& writer, + std::string_view base_name) +{ + return std::make_unique(writer, base_name, FullBodyPicoRecordingTraits::schema_name, + std::vector(FullBodyPicoRecordingTraits::channels.begin(), + FullBodyPicoRecordingTraits::channels.end())); +} + +LiveFullBodyTrackerPicoImpl::LiveFullBodyTrackerPicoImpl(const OpenXRSessionHandles& handles, + std::unique_ptr mcap_channels) : time_converter_(handles), base_space_(handles.space), body_tracker_(XR_NULL_HANDLE), pfn_create_body_tracker_(nullptr), pfn_destroy_body_tracker_(nullptr), - pfn_locate_body_joints_(nullptr) + pfn_locate_body_joints_(nullptr), + mcap_channels_(std::move(mcap_channels)) { auto core_funcs = OpenXRCoreFunctions::load(handles.instance, handles.xrGetInstanceProcAddr); @@ -62,11 +74,6 @@ LiveFullBodyTrackerPicoImpl::LiveFullBodyTrackerPicoImpl(const OpenXRSessionHand loadExtensionFunction(handles.instance, handles.xrGetInstanceProcAddr, "xrLocateBodyJointsBD", reinterpret_cast(&pfn_locate_body_joints_)); - if (!pfn_create_body_tracker_ || !pfn_destroy_body_tracker_ || !pfn_locate_body_joints_) - { - throw std::runtime_error("Failed to get body tracking function pointers"); - } - XrBodyTrackerCreateInfoBD create_info{ XR_TYPE_BODY_TRACKER_CREATE_INFO_BD }; create_info.next = nullptr; create_info.jointSet = XR_BODY_JOINT_SET_FULL_BODY_JOINTS_BD; @@ -135,22 +142,25 @@ bool LiveFullBodyTrackerPicoImpl::update(XrTime time) { const auto& joint_loc = joint_locations[i]; + Point position(joint_loc.pose.position.x, joint_loc.pose.position.y, joint_loc.pose.position.z); + Quaternion orientation(joint_loc.pose.orientation.x, joint_loc.pose.orientation.y, joint_loc.pose.orientation.z, + joint_loc.pose.orientation.w); + Pose pose(position, orientation); + bool is_valid = (joint_loc.locationFlags & XR_SPACE_LOCATION_POSITION_VALID_BIT) && (joint_loc.locationFlags & XR_SPACE_LOCATION_ORIENTATION_VALID_BIT); - Pose pose(Point(0.0f, 0.0f, 0.0f), Quaternion(0.0f, 0.0f, 0.0f, 1.0f)); - if (is_valid) - { - Point position(joint_loc.pose.position.x, joint_loc.pose.position.y, joint_loc.pose.position.z); - Quaternion orientation(joint_loc.pose.orientation.x, joint_loc.pose.orientation.y, - joint_loc.pose.orientation.z, joint_loc.pose.orientation.w); - pose = Pose(position, orientation); - } - BodyJointPose joint_pose(pose, is_valid); tracked_.data->joints->mutable_joints()->Mutate(i, joint_pose); } + if (mcap_channels_) + { + int64_t monotonic_ns = time_converter_.convert_xrtime_to_monotonic_ns(last_update_time_); + DeviceDataTimestamp timestamp(monotonic_ns, monotonic_ns, last_update_time_); + mcap_channels_->write(0, timestamp, tracked_.data); + } + return true; } @@ -159,29 +169,4 @@ const FullBodyPosePicoTrackedT& LiveFullBodyTrackerPicoImpl::get_body_pose() con return tracked_; } -void LiveFullBodyTrackerPicoImpl::serialize_all(size_t channel_index, const RecordCallback& callback) const -{ - if (channel_index != 0) - { - throw std::runtime_error("FullBodyTrackerPico::serialize_all: invalid channel_index " + - std::to_string(channel_index) + " (only channel 0 exists)"); - } - - flatbuffers::FlatBufferBuilder builder(256); - - int64_t monotonic_ns = time_converter_.convert_xrtime_to_monotonic_ns(last_update_time_); - DeviceDataTimestamp timestamp(monotonic_ns, monotonic_ns, last_update_time_); - - FullBodyPosePicoRecordBuilder record_builder(builder); - if (tracked_.data) - { - auto data_offset = FullBodyPosePico::Pack(builder, tracked_.data.get()); - record_builder.add_data(data_offset); - } - record_builder.add_timestamp(×tamp); - builder.Finish(record_builder.Finish()); - - callback(monotonic_ns, builder.GetBufferPointer(), builder.GetSize()); -} - } // namespace core diff --git a/src/core/live_trackers/cpp/live_full_body_tracker_pico_impl.hpp b/src/core/live_trackers/cpp/live_full_body_tracker_pico_impl.hpp index b4721b50..b62b9ceb 100644 --- a/src/core/live_trackers/cpp/live_full_body_tracker_pico_impl.hpp +++ b/src/core/live_trackers/cpp/live_full_body_tracker_pico_impl.hpp @@ -4,21 +4,29 @@ #pragma once #include +#include #include #include #include #include +#include +#include + namespace core { -// OpenXR-backed implementation of FullBodyTrackerPicoImpl. +using FullBodyMcapChannels = McapTrackerChannels; + // Supports limp-mode: if body tracking hardware is unavailable, the constructor // succeeds but body_tracker_ remains XR_NULL_HANDLE and update() returns empty data. class LiveFullBodyTrackerPicoImpl : public FullBodyTrackerPicoImpl { public: - explicit LiveFullBodyTrackerPicoImpl(const OpenXRSessionHandles& handles); + static std::unique_ptr create_mcap_channels(mcap::McapWriter& writer, + std::string_view base_name); + + LiveFullBodyTrackerPicoImpl(const OpenXRSessionHandles& handles, std::unique_ptr mcap_channels); ~LiveFullBodyTrackerPicoImpl(); LiveFullBodyTrackerPicoImpl(const LiveFullBodyTrackerPicoImpl&) = delete; @@ -27,7 +35,6 @@ class LiveFullBodyTrackerPicoImpl : public FullBodyTrackerPicoImpl LiveFullBodyTrackerPicoImpl& operator=(LiveFullBodyTrackerPicoImpl&&) = delete; bool update(XrTime time) override; - void serialize_all(size_t channel_index, const RecordCallback& callback) const override; const FullBodyPosePicoTrackedT& get_body_pose() const override; private: @@ -40,6 +47,8 @@ class LiveFullBodyTrackerPicoImpl : public FullBodyTrackerPicoImpl PFN_xrCreateBodyTrackerBD pfn_create_body_tracker_; PFN_xrDestroyBodyTrackerBD pfn_destroy_body_tracker_; PFN_xrLocateBodyJointsBD pfn_locate_body_joints_; + + std::unique_ptr mcap_channels_; }; } // namespace core diff --git a/src/core/live_trackers/cpp/live_generic_3axis_pedal_tracker_impl.cpp b/src/core/live_trackers/cpp/live_generic_3axis_pedal_tracker_impl.cpp index e1f1b1c2..bfcf2d19 100644 --- a/src/core/live_trackers/cpp/live_generic_3axis_pedal_tracker_impl.cpp +++ b/src/core/live_trackers/cpp/live_generic_3axis_pedal_tracker_impl.cpp @@ -3,12 +3,8 @@ #include "live_generic_3axis_pedal_tracker_impl.hpp" -#include -#include - -#include -#include -#include +#include +#include namespace core { @@ -32,87 +28,25 @@ SchemaTrackerConfig make_pedal_tensor_config(const Generic3AxisPedalTracker* tra // LiveGeneric3AxisPedalTrackerImpl // ============================================================================ -LiveGeneric3AxisPedalTrackerImpl::LiveGeneric3AxisPedalTrackerImpl(const OpenXRSessionHandles& handles, - const Generic3AxisPedalTracker* tracker) - : m_schema_reader(handles, make_pedal_tensor_config(tracker)), m_time_converter_(handles) +std::unique_ptr LiveGeneric3AxisPedalTrackerImpl::create_mcap_channels(mcap::McapWriter& writer, + std::string_view base_name) { + return std::make_unique( + writer, base_name, PedalRecordingTraits::schema_name, + std::vector(PedalRecordingTraits::channels.begin(), PedalRecordingTraits::channels.end())); } -bool LiveGeneric3AxisPedalTrackerImpl::update(XrTime time) +LiveGeneric3AxisPedalTrackerImpl::LiveGeneric3AxisPedalTrackerImpl(const OpenXRSessionHandles& handles, + const Generic3AxisPedalTracker* tracker, + std::unique_ptr mcap_channels) + : mcap_channels_(std::move(mcap_channels)), + m_schema_reader(handles, make_pedal_tensor_config(tracker), mcap_channels_.get(), 0) { - m_last_update_time_ = time; - m_pending_records.clear(); - m_collection_present = m_schema_reader.read_all_samples(m_pending_records); - - // Apply any samples returned by read_all_samples before treating the collection as absent. - if (!m_pending_records.empty()) - { - auto fb = flatbuffers::GetRoot(m_pending_records.back().buffer.data()); - if (fb) - { - if (!m_tracked.data) - { - m_tracked.data = std::make_shared(); - } - fb->UnPackTo(m_tracked.data.get()); - } - } - else if (!m_collection_present) - { - // No new samples and collection unavailable — clear last-known state. - m_tracked.data.reset(); - } - // When the collection exists but read_all_samples drained zero new samples, retain m_tracked.data. - - return true; } -void LiveGeneric3AxisPedalTrackerImpl::serialize_all(size_t channel_index, const RecordCallback& callback) const +bool LiveGeneric3AxisPedalTrackerImpl::update(XrTime /*time*/) { - if (channel_index != 0) - { - throw std::runtime_error("Generic3AxisPedalTracker::serialize_all: invalid channel_index " + - std::to_string(channel_index) + " (only channel 0 exists)"); - } - - int64_t update_ns = m_time_converter_.convert_xrtime_to_monotonic_ns(m_last_update_time_); - const size_t builder_capacity = m_schema_reader.config().max_flatbuffer_size; - - if (m_pending_records.empty()) - { - if (!m_collection_present) - { - DeviceDataTimestamp update_timestamp(update_ns, update_ns, 0); - flatbuffers::FlatBufferBuilder builder(builder_capacity); - Generic3AxisPedalOutputRecordBuilder record_builder(builder); - record_builder.add_timestamp(&update_timestamp); - builder.Finish(record_builder.Finish()); - callback(update_ns, builder.GetBufferPointer(), builder.GetSize()); - } - return; - } - - // MCAP logTime is update_ns for all records in one update() batch; - // per-sample timing is in the embedded DeviceDataTimestamp. - for (const auto& sample : m_pending_records) - { - auto fb = flatbuffers::GetRoot(sample.buffer.data()); - if (!fb) - { - continue; - } - - Generic3AxisPedalOutputT parsed; - fb->UnPackTo(&parsed); - - flatbuffers::FlatBufferBuilder builder(builder_capacity); - auto data_offset = Generic3AxisPedalOutput::Pack(builder, &parsed); - Generic3AxisPedalOutputRecordBuilder record_builder(builder); - record_builder.add_data(data_offset); - record_builder.add_timestamp(&sample.timestamp); - builder.Finish(record_builder.Finish()); - callback(update_ns, builder.GetBufferPointer(), builder.GetSize()); - } + return m_schema_reader.update(m_tracked.data); } const Generic3AxisPedalOutputTrackedT& LiveGeneric3AxisPedalTrackerImpl::get_data() const diff --git a/src/core/live_trackers/cpp/live_generic_3axis_pedal_tracker_impl.hpp b/src/core/live_trackers/cpp/live_generic_3axis_pedal_tracker_impl.hpp index 51fdd352..62ae27e9 100644 --- a/src/core/live_trackers/cpp/live_generic_3axis_pedal_tracker_impl.hpp +++ b/src/core/live_trackers/cpp/live_generic_3axis_pedal_tracker_impl.hpp @@ -7,18 +7,25 @@ #include #include -#include +#include -#include +#include +#include namespace core { -// OpenXR-backed implementation of Generic3AxisPedalTrackerImpl. +using PedalMcapChannels = McapTrackerChannels; +using PedalSchemaTracker = SchemaTracker; + class LiveGeneric3AxisPedalTrackerImpl : public Generic3AxisPedalTrackerImpl { public: - LiveGeneric3AxisPedalTrackerImpl(const OpenXRSessionHandles& handles, const Generic3AxisPedalTracker* tracker); + static std::unique_ptr create_mcap_channels(mcap::McapWriter& writer, std::string_view base_name); + + LiveGeneric3AxisPedalTrackerImpl(const OpenXRSessionHandles& handles, + const Generic3AxisPedalTracker* tracker, + std::unique_ptr mcap_channels); LiveGeneric3AxisPedalTrackerImpl(const LiveGeneric3AxisPedalTrackerImpl&) = delete; LiveGeneric3AxisPedalTrackerImpl& operator=(const LiveGeneric3AxisPedalTrackerImpl&) = delete; @@ -26,16 +33,12 @@ class LiveGeneric3AxisPedalTrackerImpl : public Generic3AxisPedalTrackerImpl LiveGeneric3AxisPedalTrackerImpl& operator=(LiveGeneric3AxisPedalTrackerImpl&&) = delete; bool update(XrTime time) override; - void serialize_all(size_t channel_index, const RecordCallback& callback) const override; const Generic3AxisPedalOutputTrackedT& get_data() const override; private: - SchemaTracker m_schema_reader; - XrTimeConverter m_time_converter_; - XrTime m_last_update_time_ = 0; - bool m_collection_present = false; + std::unique_ptr mcap_channels_; + PedalSchemaTracker m_schema_reader; Generic3AxisPedalOutputTrackedT m_tracked; - std::vector m_pending_records; }; } // namespace core diff --git a/src/core/live_trackers/cpp/live_hand_tracker_impl.cpp b/src/core/live_trackers/cpp/live_hand_tracker_impl.cpp index 59982eae..3848669a 100644 --- a/src/core/live_trackers/cpp/live_hand_tracker_impl.cpp +++ b/src/core/live_trackers/cpp/live_hand_tracker_impl.cpp @@ -3,8 +3,10 @@ #include "live_hand_tracker_impl.hpp" +#include #include #include +#include #include #include @@ -18,14 +20,24 @@ namespace core // LiveHandTrackerImpl // ============================================================================ -LiveHandTrackerImpl::LiveHandTrackerImpl(const OpenXRSessionHandles& handles) +std::unique_ptr LiveHandTrackerImpl::create_mcap_channels(mcap::McapWriter& writer, + std::string_view base_name) +{ + return std::make_unique( + writer, base_name, HandRecordingTraits::schema_name, + std::vector(HandRecordingTraits::channels.begin(), HandRecordingTraits::channels.end())); +} + +LiveHandTrackerImpl::LiveHandTrackerImpl(const OpenXRSessionHandles& handles, + std::unique_ptr mcap_channels) : time_converter_(handles), base_space_(handles.space), left_hand_tracker_(XR_NULL_HANDLE), right_hand_tracker_(XR_NULL_HANDLE), pfn_create_hand_tracker_(nullptr), pfn_destroy_hand_tracker_(nullptr), - pfn_locate_hand_joints_(nullptr) + pfn_locate_hand_joints_(nullptr), + mcap_channels_(std::move(mcap_channels)) { auto core_funcs = OpenXRCoreFunctions::load(handles.instance, handles.xrGetInstanceProcAddr); @@ -36,7 +48,7 @@ LiveHandTrackerImpl::LiveHandTrackerImpl(const OpenXRSessionHandles& handles) XrResult result = core_funcs.xrGetSystem(handles.instance, &system_info, &system_id); if (XR_FAILED(result)) { - throw std::runtime_error("xrGetSystem failed: " + std::to_string(result)); + throw std::runtime_error("Failed to get OpenXR system: " + std::to_string(result)); } XrSystemHandTrackingPropertiesEXT hand_tracking_props{ XR_TYPE_SYSTEM_HAND_TRACKING_PROPERTIES_EXT }; @@ -46,7 +58,7 @@ LiveHandTrackerImpl::LiveHandTrackerImpl(const OpenXRSessionHandles& handles) result = core_funcs.xrGetSystemProperties(handles.instance, system_id, &system_props); if (XR_FAILED(result)) { - throw std::runtime_error("xrGetSystemProperties failed: " + std::to_string(result)); + throw std::runtime_error("Failed to get system properties: " + std::to_string(result)); } if (!hand_tracking_props.supportsHandTracking) { @@ -110,7 +122,16 @@ bool LiveHandTrackerImpl::update(XrTime time) last_update_time_ = time; bool left_ok = update_hand(left_hand_tracker_, time, left_tracked_); bool right_ok = update_hand(right_hand_tracker_, time, right_tracked_); - return left_ok && right_ok; + + if (mcap_channels_) + { + int64_t monotonic_ns = time_converter_.convert_xrtime_to_monotonic_ns(last_update_time_); + DeviceDataTimestamp timestamp(monotonic_ns, monotonic_ns, last_update_time_); + mcap_channels_->write(0, timestamp, left_tracked_.data); + mcap_channels_->write(1, timestamp, right_tracked_.data); + } + + return left_ok || right_ok; } const HandPoseTrackedT& LiveHandTrackerImpl::get_left_hand() const @@ -123,31 +144,6 @@ const HandPoseTrackedT& LiveHandTrackerImpl::get_right_hand() const return right_tracked_; } -void LiveHandTrackerImpl::serialize_all(size_t channel_index, const RecordCallback& callback) const -{ - if (channel_index > 1) - { - throw std::runtime_error("HandTracker::serialize_all: invalid channel_index " + std::to_string(channel_index) + - " (must be 0 or 1)"); - } - flatbuffers::FlatBufferBuilder builder(256); - - const auto& tracked = (channel_index == 0) ? left_tracked_ : right_tracked_; - int64_t monotonic_ns = time_converter_.convert_xrtime_to_monotonic_ns(last_update_time_); - DeviceDataTimestamp timestamp(monotonic_ns, monotonic_ns, last_update_time_); - - HandPoseRecordBuilder record_builder(builder); - if (tracked.data) - { - auto data_offset = HandPose::Pack(builder, tracked.data.get()); - record_builder.add_data(data_offset); - } - record_builder.add_timestamp(×tamp); - builder.Finish(record_builder.Finish()); - - callback(monotonic_ns, builder.GetBufferPointer(), builder.GetSize()); -} - bool LiveHandTrackerImpl::update_hand(XrHandTrackerEXT tracker, XrTime time, HandPoseTrackedT& tracked) { XrHandJointsLocateInfoEXT locate_info{ XR_TYPE_HAND_JOINTS_LOCATE_INFO_EXT }; @@ -188,19 +184,15 @@ bool LiveHandTrackerImpl::update_hand(XrHandTrackerEXT tracker, XrTime time, Han { const auto& joint_loc = joint_locations[i]; + Point position(joint_loc.pose.position.x, joint_loc.pose.position.y, joint_loc.pose.position.z); + Quaternion orientation(joint_loc.pose.orientation.x, joint_loc.pose.orientation.y, joint_loc.pose.orientation.z, + joint_loc.pose.orientation.w); + Pose pose(position, orientation); + bool is_valid = (joint_loc.locationFlags & XR_SPACE_LOCATION_POSITION_VALID_BIT) && (joint_loc.locationFlags & XR_SPACE_LOCATION_ORIENTATION_VALID_BIT); - Pose pose; - if (is_valid) - { - Point position(joint_loc.pose.position.x, joint_loc.pose.position.y, joint_loc.pose.position.z); - Quaternion orientation(joint_loc.pose.orientation.x, joint_loc.pose.orientation.y, - joint_loc.pose.orientation.z, joint_loc.pose.orientation.w); - pose = Pose(position, orientation); - } - - HandJointPose joint_pose(pose, is_valid, is_valid ? joint_loc.radius : 0.0f); + HandJointPose joint_pose(pose, is_valid, joint_loc.radius); tracked.data->joints->mutable_poses()->Mutate(i, joint_pose); } diff --git a/src/core/live_trackers/cpp/live_hand_tracker_impl.hpp b/src/core/live_trackers/cpp/live_hand_tracker_impl.hpp index c34619ee..47489536 100644 --- a/src/core/live_trackers/cpp/live_hand_tracker_impl.hpp +++ b/src/core/live_trackers/cpp/live_hand_tracker_impl.hpp @@ -4,20 +4,27 @@ #pragma once #include +#include #include #include #include #include #include +#include +#include + namespace core { -// OpenXR-backed implementation of HandTrackerImpl. +using HandMcapChannels = McapTrackerChannels; + class LiveHandTrackerImpl : public HandTrackerImpl { public: - explicit LiveHandTrackerImpl(const OpenXRSessionHandles& handles); + static std::unique_ptr create_mcap_channels(mcap::McapWriter& writer, std::string_view base_name); + + LiveHandTrackerImpl(const OpenXRSessionHandles& handles, std::unique_ptr mcap_channels); ~LiveHandTrackerImpl(); LiveHandTrackerImpl(const LiveHandTrackerImpl&) = delete; @@ -26,7 +33,6 @@ class LiveHandTrackerImpl : public HandTrackerImpl LiveHandTrackerImpl& operator=(LiveHandTrackerImpl&&) = delete; bool update(XrTime time) override; - void serialize_all(size_t channel_index, const RecordCallback& callback) const override; const HandPoseTrackedT& get_left_hand() const override; const HandPoseTrackedT& get_right_hand() const override; @@ -46,6 +52,8 @@ class LiveHandTrackerImpl : public HandTrackerImpl PFN_xrCreateHandTrackerEXT pfn_create_hand_tracker_; PFN_xrDestroyHandTrackerEXT pfn_destroy_hand_tracker_; PFN_xrLocateHandJointsEXT pfn_locate_hand_joints_; + + std::unique_ptr mcap_channels_; }; } // namespace core diff --git a/src/core/live_trackers/cpp/live_head_tracker_impl.cpp b/src/core/live_trackers/cpp/live_head_tracker_impl.cpp index 06392e01..22e45acc 100644 --- a/src/core/live_trackers/cpp/live_head_tracker_impl.cpp +++ b/src/core/live_trackers/cpp/live_head_tracker_impl.cpp @@ -3,6 +3,9 @@ #include "live_head_tracker_impl.hpp" +#include +#include + #include #include @@ -13,7 +16,16 @@ namespace core // LiveHeadTrackerImpl // ============================================================================ -LiveHeadTrackerImpl::LiveHeadTrackerImpl(const OpenXRSessionHandles& handles) +std::unique_ptr LiveHeadTrackerImpl::create_mcap_channels(mcap::McapWriter& writer, + std::string_view base_name) +{ + return std::make_unique( + writer, base_name, HeadRecordingTraits::schema_name, + std::vector(HeadRecordingTraits::channels.begin(), HeadRecordingTraits::channels.end())); +} + +LiveHeadTrackerImpl::LiveHeadTrackerImpl(const OpenXRSessionHandles& handles, + std::unique_ptr mcap_channels) : core_funcs_(OpenXRCoreFunctions::load(handles.instance, handles.xrGetInstanceProcAddr)), time_converter_(handles), base_space_(handles.space), @@ -22,7 +34,8 @@ LiveHeadTrackerImpl::LiveHeadTrackerImpl(const OpenXRSessionHandles& handles) { .type = XR_TYPE_REFERENCE_SPACE_CREATE_INFO, .referenceSpaceType = XR_REFERENCE_SPACE_TYPE_VIEW, .poseInReferenceSpace = { .orientation = { 0, 0, 0, 1 } } })), - tracked_{} + tracked_{}, + mcap_channels_(std::move(mcap_channels)) { } @@ -61,6 +74,13 @@ bool LiveHeadTrackerImpl::update(XrTime time) tracked_.data->pose.reset(); } + if (mcap_channels_) + { + int64_t monotonic_ns = time_converter_.convert_xrtime_to_monotonic_ns(last_update_time_); + DeviceDataTimestamp timestamp(monotonic_ns, monotonic_ns, last_update_time_); + mcap_channels_->write(0, timestamp, tracked_.data); + } + return true; } @@ -69,29 +89,4 @@ const HeadPoseTrackedT& LiveHeadTrackerImpl::get_head() const return tracked_; } -void LiveHeadTrackerImpl::serialize_all(size_t channel_index, const RecordCallback& callback) const -{ - if (channel_index != 0) - { - throw std::runtime_error("LiveHeadTrackerImpl::serialize_all: invalid channel_index " + - std::to_string(channel_index) + " (only channel 0 exists)"); - } - - flatbuffers::FlatBufferBuilder builder(256); - - int64_t monotonic_ns = time_converter_.convert_xrtime_to_monotonic_ns(last_update_time_); - DeviceDataTimestamp timestamp(monotonic_ns, monotonic_ns, last_update_time_); - - HeadPoseRecordBuilder record_builder(builder); - if (tracked_.data) - { - auto data_offset = HeadPose::Pack(builder, tracked_.data.get()); - record_builder.add_data(data_offset); - } - record_builder.add_timestamp(×tamp); - builder.Finish(record_builder.Finish()); - - callback(monotonic_ns, builder.GetBufferPointer(), builder.GetSize()); -} - } // namespace core diff --git a/src/core/live_trackers/cpp/live_head_tracker_impl.hpp b/src/core/live_trackers/cpp/live_head_tracker_impl.hpp index a934a8c7..f4b033c8 100644 --- a/src/core/live_trackers/cpp/live_head_tracker_impl.hpp +++ b/src/core/live_trackers/cpp/live_head_tracker_impl.hpp @@ -4,19 +4,26 @@ #pragma once #include +#include #include #include #include #include +#include +#include + namespace core { -// OpenXR-backed implementation of HeadTrackerImpl. +using HeadMcapChannels = McapTrackerChannels; + class LiveHeadTrackerImpl : public HeadTrackerImpl { public: - explicit LiveHeadTrackerImpl(const OpenXRSessionHandles& handles); + static std::unique_ptr create_mcap_channels(mcap::McapWriter& writer, std::string_view base_name); + + LiveHeadTrackerImpl(const OpenXRSessionHandles& handles, std::unique_ptr mcap_channels); LiveHeadTrackerImpl(const LiveHeadTrackerImpl&) = delete; LiveHeadTrackerImpl& operator=(const LiveHeadTrackerImpl&) = delete; @@ -24,17 +31,16 @@ class LiveHeadTrackerImpl : public HeadTrackerImpl LiveHeadTrackerImpl& operator=(LiveHeadTrackerImpl&&) = delete; bool update(XrTime time) override; - void serialize_all(size_t channel_index, const RecordCallback& callback) const override; const HeadPoseTrackedT& get_head() const override; private: const OpenXRCoreFunctions core_funcs_; XrTimeConverter time_converter_; - // base_space_ borrows handles.space; view_space_ owns the created view-space handle via RAII. XrSpace base_space_; XrSpacePtr view_space_; HeadPoseTrackedT tracked_; XrTime last_update_time_ = 0; + std::unique_ptr mcap_channels_; }; } // namespace core diff --git a/src/core/live_trackers/cpp/schema_tracker.cpp b/src/core/live_trackers/cpp/schema_tracker_base.cpp similarity index 78% rename from src/core/live_trackers/cpp/schema_tracker.cpp rename to src/core/live_trackers/cpp/schema_tracker_base.cpp index 04feb194..2a1676a4 100644 --- a/src/core/live_trackers/cpp/schema_tracker.cpp +++ b/src/core/live_trackers/cpp/schema_tracker_base.cpp @@ -1,7 +1,7 @@ // SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. // SPDX-License-Identifier: Apache-2.0 -#include "inc/live_trackers/schema_tracker.hpp" +#include "inc/live_trackers/schema_tracker_base.hpp" #include #include @@ -19,7 +19,7 @@ namespace core // SchemaTracker Implementation // ============================================================================= -SchemaTracker::SchemaTracker(const OpenXRSessionHandles& handles, SchemaTrackerConfig config) +SchemaTrackerBase::SchemaTrackerBase(const OpenXRSessionHandles& handles, SchemaTrackerConfig config) : m_handles(handles), m_config(std::move(config)), m_time_converter(handles) { // Validate handles @@ -36,7 +36,7 @@ SchemaTracker::SchemaTracker(const OpenXRSessionHandles& handles, SchemaTrackerC std::cout << "SchemaTracker initialized, looking for collection: " << m_config.collection_id << std::endl; } -SchemaTracker::~SchemaTracker() +SchemaTrackerBase::~SchemaTrackerBase() { // m_tensor_list is guaranteed to be non-null by create_tensor_list(), or the constructor would have thrown. assert(m_tensor_list != XR_NULL_HANDLE && m_destroy_list_fn != nullptr); @@ -48,7 +48,7 @@ SchemaTracker::~SchemaTracker() } } -std::vector SchemaTracker::get_required_extensions() +std::vector SchemaTrackerBase::get_required_extensions() { std::vector exts = { "XR_NVX1_tensor_data" }; for (const auto& ext : XrTimeConverter::get_required_extensions()) @@ -58,25 +58,76 @@ std::vector SchemaTracker::get_required_extensions() return exts; } -bool SchemaTracker::ensure_collection() +bool SchemaTrackerBase::ensure_collection() { - poll_for_updates(); + uint64_t latest_generation = 0; + XrResult result = m_get_latest_gen_fn(m_handles.session, &latest_generation); + if (result != XR_SUCCESS) + { + throw std::runtime_error("Failed to get latest generation, result=" + std::to_string(result)); + } - auto new_index = find_target_collection(); - if (!new_index) + if (latest_generation == m_cached_generation && m_target_collection_index.has_value()) { - return false; + return true; } - if (new_index != m_target_collection_index) + + if (latest_generation != m_cached_generation) { - m_target_collection_index = new_index; - m_last_sample_index.reset(); - std::cout << "Found target collection at index " << *m_target_collection_index << std::endl; + result = m_update_list_fn(m_tensor_list); + if (result != XR_SUCCESS) + { + throw std::runtime_error("Failed to update tensor list, result=" + std::to_string(result)); + } + m_cached_generation = latest_generation; } - return true; + + XrSystemTensorListPropertiesNV list_props{ XR_TYPE_SYSTEM_TENSOR_LIST_PROPERTIES_NV }; + result = m_get_list_props_fn(m_tensor_list, &list_props); + if (result != XR_SUCCESS) + { + throw std::runtime_error("Failed to get list properties, result=" + std::to_string(result)); + } + + for (uint32_t i = 0; i < list_props.tensorCollectionCount; ++i) + { + XrTensorCollectionPropertiesNV coll_props{ XR_TYPE_TENSOR_COLLECTION_PROPERTIES_NV }; + result = m_get_coll_props_fn(m_tensor_list, i, &coll_props); + if (result != XR_SUCCESS) + { + throw std::runtime_error("Failed to get collection properties, result=" + std::to_string(result)); + } + + if (std::strncmp(coll_props.data.identifier, m_config.collection_id.c_str(), XR_MAX_TENSOR_IDENTIFIER_SIZE) != 0) + { + continue; + } + + if (coll_props.data.totalSampleSize > m_config.max_flatbuffer_size) + { + throw std::runtime_error( + "SchemaTracker: collection '" + m_config.collection_id + + "' reports totalSampleSize=" + std::to_string(coll_props.data.totalSampleSize) + + " which exceeds configured max_flatbuffer_size=" + std::to_string(m_config.max_flatbuffer_size)); + } + + m_sample_batch_stride = coll_props.sampleBatchStride; + m_sample_size = coll_props.data.totalSampleSize; + + if (i != m_target_collection_index) + { + m_last_sample_index.reset(); + std::cout << "Found target collection at index " << i << std::endl; + } + m_target_collection_index = i; + return true; + } + + m_target_collection_index.reset(); + return false; } -bool SchemaTracker::read_all_samples(std::vector& samples) +bool SchemaTrackerBase::read_all_samples(std::vector& samples) { if (!ensure_collection()) { @@ -99,12 +150,12 @@ bool SchemaTracker::read_all_samples(std::vector& samples) return true; } -const SchemaTrackerConfig& SchemaTracker::config() const +const SchemaTrackerConfig& SchemaTrackerBase::config() const { return m_config; } -void SchemaTracker::initialize_tensor_data_functions() +void SchemaTrackerBase::initialize_tensor_data_functions() { loadExtensionFunction(m_handles.instance, m_handles.xrGetInstanceProcAddr, "xrGetTensorListLatestGenerationNV", reinterpret_cast(&m_get_latest_gen_fn)); @@ -122,7 +173,7 @@ void SchemaTracker::initialize_tensor_data_functions() reinterpret_cast(&m_destroy_list_fn)); } -void SchemaTracker::create_tensor_list() +void SchemaTrackerBase::create_tensor_list() { XrCreateTensorListInfoNV createInfo{ XR_TYPE_CREATE_TENSOR_LIST_INFO_NV }; createInfo.next = nullptr; @@ -134,70 +185,7 @@ void SchemaTracker::create_tensor_list() } } -void SchemaTracker::poll_for_updates() -{ - // Check if tensor list needs update - uint64_t latest_generation = 0; - XrResult result = m_get_latest_gen_fn(m_handles.session, &latest_generation); - if (result != XR_SUCCESS) - { - throw std::runtime_error("Failed to get latest generation, result=" + std::to_string(result)); - } - - if (latest_generation != m_cached_generation) - { - result = m_update_list_fn(m_tensor_list); - if (result != XR_SUCCESS) - { - throw std::runtime_error("Failed to update tensor list, result=" + std::to_string(result)); - } - m_cached_generation = latest_generation; - // Invalidate the cached collection index so ensure_collection() re-discovers - // it on the next call, which also resets m_last_sample_index for the new collection. - m_target_collection_index = std::nullopt; - } -} - -std::optional SchemaTracker::find_target_collection() -{ - // Get list properties - XrSystemTensorListPropertiesNV listProps{ XR_TYPE_SYSTEM_TENSOR_LIST_PROPERTIES_NV }; - - XrResult result = m_get_list_props_fn(m_tensor_list, &listProps); - if (result != XR_SUCCESS) - { - throw std::runtime_error("Failed to get list properties, result=" + std::to_string(result)); - } - - if (listProps.tensorCollectionCount == 0) - { - return std::nullopt; // No collections available yet - } - - // Search for matching collection - for (uint32_t i = 0; i < listProps.tensorCollectionCount; ++i) - { - XrTensorCollectionPropertiesNV collProps{ XR_TYPE_TENSOR_COLLECTION_PROPERTIES_NV }; - - result = m_get_coll_props_fn(m_tensor_list, i, &collProps); - if (result != XR_SUCCESS) - { - throw std::runtime_error("Failed to get collection properties, result=" + std::to_string(result)); - } - - if (std::strncmp(collProps.data.identifier, m_config.collection_id.c_str(), XR_MAX_TENSOR_IDENTIFIER_SIZE) == 0) - { - // Found matching collection - m_sample_batch_stride = collProps.sampleBatchStride; - m_sample_size = collProps.data.totalSampleSize; - return i; - } - } - - return std::nullopt; -} - -bool SchemaTracker::read_next_sample(SampleResult& out) +bool SchemaTrackerBase::read_next_sample(SampleResult& out) { if (!m_target_collection_index.has_value()) { @@ -246,12 +234,6 @@ bool SchemaTracker::read_next_sample(SampleResult& out) return false; // No new samples } - // Update last sample index - if (!m_last_sample_index.has_value() || metadata.sampleIndex > m_last_sample_index.value()) - { - m_last_sample_index = metadata.sampleIndex; - } - // Guard against invalid runtime state: batch stride must cover at least one full sample. if (dataBuffer.size() < m_sample_size) { @@ -264,6 +246,12 @@ bool SchemaTracker::read_next_sample(SampleResult& out) out.buffer.resize(m_sample_size); std::memcpy(out.buffer.data(), dataBuffer.data(), m_sample_size); + // Advance past this sample only after successful copy + if (!m_last_sample_index.has_value() || metadata.sampleIndex > m_last_sample_index.value()) + { + m_last_sample_index = metadata.sampleIndex; + } + // Convert XrTime values from tensor metadata back to monotonic nanoseconds. int64_t available_ns = m_time_converter.convert_xrtime_to_monotonic_ns(static_cast(metadata.arrivalTimestamp)); diff --git a/src/core/mcap/CMakeLists.txt b/src/core/mcap/CMakeLists.txt index 1b9eef32..d9c7d22c 100644 --- a/src/core/mcap/CMakeLists.txt +++ b/src/core/mcap/CMakeLists.txt @@ -3,12 +3,4 @@ cmake_minimum_required(VERSION 3.20) -# Build C++ library add_subdirectory(cpp) - -# Python bindings (optional, controlled by parent or option) -option(BUILD_PYTHON_BINDINGS "Build Python bindings" ON) - -if(BUILD_PYTHON_BINDINGS) - add_subdirectory(python) -endif() diff --git a/src/core/mcap/cpp/CMakeLists.txt b/src/core/mcap/cpp/CMakeLists.txt index ea669433..2b8e62d3 100644 --- a/src/core/mcap/cpp/CMakeLists.txt +++ b/src/core/mcap/cpp/CMakeLists.txt @@ -3,38 +3,23 @@ cmake_minimum_required(VERSION 3.20) -# MCAP Recorder library (static for distribution) -add_library(mcap_core STATIC - mcap_recorder.cpp - inc/mcap/recorder.hpp -) +# mcap_core is a header-only interface library. +# It provides McapTrackerChannels (mcap/tracker_channels.hpp) -- a thin helper +# that reduces boilerplate in Live*TrackerImpl constructors and update() methods. +# The actual MCAP implementation (MCAP_IMPLEMENTATION) is compiled once inside +# deviceio_core (deviceio_session.cpp). +add_library(mcap_core INTERFACE) -# Public headers for consumers of this library target_include_directories(mcap_core INTERFACE $ ) -# mcap_core depends on deviceio_session and deviceio_trackers for DeviceIOSession and tracker interfaces +# Transitively expose mcap::mcap so consumers get target_link_libraries(mcap_core - PUBLIC - # deviceio_session + deviceio_trackers for DeviceIOSession and ITracker interfaces - deviceio::deviceio_session - deviceio::deviceio_trackers - PRIVATE - # MCAP header-only library (PRIVATE - implementation detail) + INTERFACE mcap::mcap + isaacteleop_schema ) -# Create alias for consistent naming add_library(mcap::mcap_core ALIAS mcap_core) - -# Optional: Install for distribution -install(TARGETS mcap_core - ARCHIVE DESTINATION lib - INCLUDES DESTINATION include -) - -install(DIRECTORY inc/mcap - DESTINATION include -) diff --git a/src/core/mcap/cpp/inc/mcap/recorder.hpp b/src/core/mcap/cpp/inc/mcap/recorder.hpp deleted file mode 100644 index 3f1794f3..00000000 --- a/src/core/mcap/cpp/inc/mcap/recorder.hpp +++ /dev/null @@ -1,86 +0,0 @@ -// SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// SPDX-License-Identifier: Apache-2.0 - -#pragma once - -#include - -#include -#include -#include -#include - -namespace core -{ - -/** - * @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. - * - * Usage: - * auto recorder = McapRecorder::create("output.mcap", { - * {hand_tracker, "hands"}, - * {head_tracker, "head"}, - * }); - * // In your loop: - * recorder->record(session); - * // When done, let the recorder go out of scope or reset it - */ -class McapRecorder -{ -public: - /// Tracker configuration: pair of (tracker, base_channel_name). - /// The base_channel_name must be non-empty. It is combined with each tracker's - /// record channel names as "base_channel_name/channel_name" to form the final - /// MCAP topic names. For example, registering a hand tracker with base name - /// "hands" that returns channels {"left_hand", "right_hand"} produces MCAP - /// topics "hands/left_hand" and "hands/right_hand". - 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. - * - * MCAP logTime and publishTime are set to os_monotonic_now_ns() at the - * moment each record is written, not from the tracker's own timestamps. - * The tracker's DeviceDataTimestamp fields (available_time, sample times) - * are embedded in the FlatBuffer payload and remain available for downstream - * latency analysis. - * - * @param filename Path to the output MCAP file. - * @param trackers List of (tracker, base_channel_name) pairs to record. - * Both base_channel_name and the tracker's channel names must be non-empty. - * @return A unique_ptr to the McapRecorder. - * @throws std::runtime_error if the recorder cannot be created, or if any - * base_channel_name or tracker channel name is empty. - */ - static std::unique_ptr create(const std::string& filename, - const std::vector& trackers); - - /** - * @brief Destructor - closes the MCAP file. - */ - ~McapRecorder(); - - /** - * @brief Record the current state of all registered trackers. - * - * This should be called after session.update() in your main loop. - * - * @param session Session that can resolve tracker implementations (e.g. DeviceIOSession). - */ - void record(const ITrackerSession& session); - -private: - // Private constructor - use create() factory method - McapRecorder(const std::string& filename, const std::vector& trackers); - - class Impl; - std::unique_ptr impl_; -}; - -} // namespace core diff --git a/src/core/mcap/cpp/inc/mcap/recording_traits.hpp b/src/core/mcap/cpp/inc/mcap/recording_traits.hpp new file mode 100644 index 00000000..2e0d89db --- /dev/null +++ b/src/core/mcap/cpp/inc/mcap/recording_traits.hpp @@ -0,0 +1,55 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include + +namespace core +{ + +/** + * @brief Compile-time MCAP recording metadata per tracker type. + * + * Centralizes schema names and default channel names used for MCAP recording + * and replay. Each tracker impl's create_mcap_channels references these + * instead of embedding string literals. + */ + +struct HeadRecordingTraits +{ + static constexpr std::string_view schema_name = "core.HeadPoseRecord"; + static constexpr std::array channels = { "head" }; +}; + +struct HandRecordingTraits +{ + static constexpr std::string_view schema_name = "core.HandPoseRecord"; + static constexpr std::array channels = { "left_hand", "right_hand" }; +}; + +struct ControllerRecordingTraits +{ + static constexpr std::string_view schema_name = "core.ControllerSnapshotRecord"; + static constexpr std::array channels = { "left_controller", "right_controller" }; +}; + +struct FullBodyPicoRecordingTraits +{ + static constexpr std::string_view schema_name = "core.FullBodyPosePicoRecord"; + static constexpr std::array channels = { "full_body" }; +}; + +struct PedalRecordingTraits +{ + static constexpr std::string_view schema_name = "core.Generic3AxisPedalOutputRecord"; + static constexpr std::array channels = { "pedals" }; +}; + +struct OakRecordingTraits +{ + static constexpr std::string_view schema_name = "core.FrameMetadataOakRecord"; +}; + +} // namespace core diff --git a/src/core/mcap/cpp/inc/mcap/tracker_channels.hpp b/src/core/mcap/cpp/inc/mcap/tracker_channels.hpp new file mode 100644 index 00000000..1cbe3c55 --- /dev/null +++ b/src/core/mcap/cpp/inc/mcap/tracker_channels.hpp @@ -0,0 +1,106 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace core +{ + +/** + * @brief Type-safe MCAP channel writer for FlatBuffer record types. + * + * @tparam RecordT The FlatBuffer record wrapper type (e.g. HeadPoseRecord). + * Must expose Builder, BinarySchema, and VT_DATA/VT_TIMESTAMP. + * @tparam DataTableT The FlatBuffer data table type (e.g. HeadPose). + * Must expose Pack() and NativeTableType. + * + * The factory creates a unique_ptr> only when recording + * is active and passes it to the impl. Impls null-check before calling write(). + */ +template +class McapTrackerChannels +{ +public: + using NativeDataT = typename DataTableT::NativeTableType; + + McapTrackerChannels(mcap::McapWriter& writer, + std::string_view base_name, + std::string_view schema_name, + const std::vector& sub_channels) + : writer_(&writer) + { + std::string_view schema_text( + reinterpret_cast(RecordT::BinarySchema::data()), RecordT::BinarySchema::size()); + + mcap::Schema schema(std::string(schema_name), "flatbuffer", std::string(schema_text)); + writer_->addSchema(schema); + + channel_ids_.reserve(sub_channels.size()); + for (const auto& sub : sub_channels) + { + mcap::Channel ch(std::string(base_name) + "/" + sub, "flatbuffer", schema.id); + writer_->addChannel(ch); + channel_ids_.push_back(ch.id); + } + } + + void write(size_t channel_index, const DeviceDataTimestamp& timestamp, const std::shared_ptr& data) + { + if (channel_index >= channel_ids_.size()) + { + throw std::out_of_range( + "McapTrackerChannels: write called with channel_index=" + std::to_string(channel_index) + " but only " + + std::to_string(channel_ids_.size()) + " channels registered"); + } + + flatbuffers::FlatBufferBuilder builder(256); + + flatbuffers::Offset data_offset; + if (data) + { + data_offset = DataTableT::Pack(builder, data.get()); + } + + DeviceDataTimestamp ts = timestamp; + typename RecordT::Builder record_builder(builder); + if (data) + { + record_builder.add_data(data_offset); + } + record_builder.add_timestamp(&ts); + builder.Finish(record_builder.Finish()); + + mcap::Message msg; + msg.channelId = channel_ids_[channel_index]; + msg.logTime = static_cast(timestamp.available_time_local_common_clock()); + msg.publishTime = msg.logTime; + msg.sequence = sequence_++; + msg.data = reinterpret_cast(builder.GetBufferPointer()); + msg.dataSize = builder.GetSize(); + auto status = writer_->write(msg); + if (!status.ok()) + { + std::cerr << "McapTrackerChannels: write failed: " << status.message << std::endl; + } + } + +private: + mcap::McapWriter* writer_; + std::vector channel_ids_; + uint32_t sequence_ = 0; +}; + +} // namespace core diff --git a/src/core/mcap/cpp/mcap_recorder.cpp b/src/core/mcap/cpp/mcap_recorder.cpp deleted file mode 100644 index 39821d93..00000000 --- a/src/core/mcap/cpp/mcap_recorder.cpp +++ /dev/null @@ -1,208 +0,0 @@ -// SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// SPDX-License-Identifier: Apache-2.0 - -#define MCAP_IMPLEMENTATION -#include "inc/mcap/recorder.hpp" - -#include -#include - -#include -#include - -namespace core -{ - -class McapRecorder::Impl -{ -public: - explicit Impl(const std::string& filename, const std::vector& trackers) - : filename_(filename), tracker_configs_(trackers) - { - if (tracker_configs_.empty()) - { - throw std::runtime_error("McapRecorder: No trackers provided"); - } - - mcap::McapWriterOptions options("teleop"); - options.compression = mcap::Compression::None; // No compression to avoid deps - - auto status = writer_.open(filename_, options); - if (!status.ok()) - { - throw std::runtime_error("McapRecorder: Failed to open file " + filename_ + ": " + status.message); - } - - for (const auto& config : tracker_configs_) - { - register_tracker(config.first.get(), config.second); - } - - std::cout << "McapRecorder: Started recording to " << filename_ << std::endl; - } - - ~Impl() - { - writer_.close(); - std::cout << "McapRecorder: Closed " << filename_ << " with " << message_count_ << " messages" << std::endl; - } - - void record(const ITrackerSession& session) - { - for (const auto& config : tracker_configs_) - { - try - { - const auto& tracker_impl = session.get_tracker_impl(*config.first); - auto it = tracker_channel_ids_.find(config.first.get()); - if (it == tracker_channel_ids_.end()) - { - std::cerr << "McapRecorder: Tracker " << config.second << " not registered" << std::endl; - continue; - } - for (size_t i = 0; i < it->second.size(); ++i) - { - if (!record_tracker(config.first.get(), tracker_impl, i)) - { - std::cerr << "McapRecorder: Failed to record tracker " << config.second << std::endl; - } - } - } - catch (const std::exception& e) - { - std::cerr << "McapRecorder: Failed to record tracker " << config.second << ": " << e.what() << std::endl; - } - } - } - -private: - void register_tracker(const ITracker* tracker, const std::string& base_channel_name) - { - if (base_channel_name.empty()) - { - throw std::runtime_error("McapRecorder: Empty base channel name for tracker '" + - std::string(tracker->get_name()) + "'"); - } - - auto record_channels = tracker->get_record_channels(); - if (record_channels.empty()) - { - throw std::runtime_error("McapRecorder: Tracker '" + std::string(tracker->get_name()) + - "' returned no record channels"); - } - for (const auto& ch : record_channels) - { - if (ch.empty()) - { - throw std::runtime_error("McapRecorder: Tracker '" + std::string(tracker->get_name()) + - "' returned an empty channel name"); - } - } - - std::string schema_name(tracker->get_schema_name()); - - if (schema_ids_.find(schema_name) == schema_ids_.end()) - { - mcap::Schema schema(schema_name, "flatbuffer", std::string(tracker->get_schema_text())); - writer_.addSchema(schema); - schema_ids_[schema_name] = schema.id; - } - - std::vector channel_ids; - - for (const auto& sub_channel : record_channels) - { - std::string full_name = base_channel_name + "/" + sub_channel; - mcap::Channel channel(full_name, "flatbuffer", schema_ids_[schema_name]); - writer_.addChannel(channel); - channel_ids.push_back(channel.id); - } - - tracker_channel_ids_[tracker] = std::move(channel_ids); - } - - bool record_tracker(const ITracker* tracker, const ITrackerImpl& tracker_impl, size_t channel_index) - { - auto it = tracker_channel_ids_.find(tracker); - if (it == tracker_channel_ids_.end() || channel_index >= it->second.size()) - { - std::cerr << "McapRecorder: Tracker not registered or invalid channel index" << std::endl; - return false; - } - - bool success = true; - mcap::ChannelId mcap_channel_id = it->second[channel_index]; - - tracker_impl.serialize_all( - channel_index, - [&](int64_t log_time_ns, const uint8_t* data, size_t size) - { - if (log_time_ns <= 0) - { - std::cerr << "McapRecorder: Skipping record with non-positive timestamp: " << log_time_ns - << std::endl; - success = false; - return; - } - const mcap::Timestamp log_time = static_cast(log_time_ns); - - if (size > 0 && data == nullptr) - { - std::cerr << "McapRecorder: Null data pointer with non-zero size (" << size - << " bytes), skipping record" << std::endl; - success = false; - return; - } - - mcap::Message msg; - msg.channelId = 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; - success = false; - return; - } - - ++message_count_; - }); - - return success; - } - - std::string filename_; - std::vector tracker_configs_; - mcap::McapWriter writer_; - uint64_t message_count_ = 0; - - std::unordered_map schema_ids_; - std::unordered_map> tracker_channel_ids_; -}; - -// McapRecorder public interface implementation - -std::unique_ptr McapRecorder::create(const std::string& filename, - const std::vector& trackers) -{ - return std::unique_ptr(new McapRecorder(filename, trackers)); -} - -McapRecorder::McapRecorder(const std::string& filename, const std::vector& trackers) - : impl_(std::make_unique(filename, trackers)) -{ -} - -McapRecorder::~McapRecorder() = default; - -void McapRecorder::record(const ITrackerSession& session) -{ - impl_->record(session); -} - -} // namespace core diff --git a/src/core/mcap/python/CMakeLists.txt b/src/core/mcap/python/CMakeLists.txt deleted file mode 100644 index 527e6499..00000000 --- a/src/core/mcap/python/CMakeLists.txt +++ /dev/null @@ -1,19 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 - -pybind11_add_module(mcap_py - mcap_bindings.cpp -) - -target_link_libraries(mcap_py - PRIVATE - mcap::mcap_core - deviceio::deviceio_session - deviceio::deviceio_trackers -) - -set_target_properties(mcap_py PROPERTIES - OUTPUT_NAME "_mcap" - # Use genex in output directory - CMake won't add another config subdirectory - LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/python_package/$/isaacteleop/mcap" -) diff --git a/src/core/mcap/python/mcap_bindings.cpp b/src/core/mcap/python/mcap_bindings.cpp deleted file mode 100644 index 13d3e273..00000000 --- a/src/core/mcap/python/mcap_bindings.cpp +++ /dev/null @@ -1,62 +0,0 @@ -// SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// SPDX-License-Identifier: Apache-2.0 - -#include -#include -#include -#include - -namespace py = pybind11; - -// Wrapper class to manage McapRecorder lifetime with Python context manager -class PyMcapRecorder -{ -public: - PyMcapRecorder(std::unique_ptr recorder) : recorder_(std::move(recorder)) - { - } - - void record(const core::ITrackerSession& session) - { - if (!recorder_) - { - throw std::runtime_error("Recorder has been closed"); - } - recorder_->record(session); - } - - PyMcapRecorder& enter() - { - return *this; - } - - void exit(py::object, py::object, py::object) - { - recorder_.reset(); - } - -private: - std::unique_ptr recorder_; -}; - -PYBIND11_MODULE(_mcap, m) -{ - m.doc() = "Isaac Teleop MCAP - MCAP Recording Module"; - - py::module_::import("isaacteleop.deviceio_trackers._deviceio_trackers"); - py::module_::import("isaacteleop.deviceio_session._deviceio_session"); - - py::class_(m, "McapRecorder") - .def_static( - "create", - [](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. " - "Returns a context-managed recorder.") - .def( - "record", [](PyMcapRecorder& self, const core::ITrackerSession& session) { self.record(session); }, - py::arg("session"), "Record the current state of all registered trackers") - .def("__enter__", &PyMcapRecorder::enter) - .def("__exit__", &PyMcapRecorder::exit); -} diff --git a/src/core/mcap/python/mcap_init.py b/src/core/mcap/python/mcap_init.py index 9763991c..65f0115e 100644 --- a/src/core/mcap/python/mcap_init.py +++ b/src/core/mcap/python/mcap_init.py @@ -1,29 +1,21 @@ # SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # SPDX-License-Identifier: Apache-2.0 -"""Isaac Teleop MCAP - MCAP Recording Module +"""Isaac Teleop MCAP module. -This module provides MCAP file recording functionality for tracker data. +MCAP recording is handled by DeviceIOSession. Pass a McapRecordingConfig +to DeviceIOSession.run() to enable automatic recording; omit it (or pass +None) to disable recording: -Usage: - from isaacteleop.mcap import McapRecorder - from isaacteleop.deviceio import DeviceIOSession, HandTracker, HeadTracker + from isaacteleop.deviceio_session import DeviceIOSession, McapRecordingConfig - hand_tracker = HandTracker() - head_tracker = HeadTracker() - - # Create recorder with context manager (similar to DeviceIOSession.run) - with McapRecorder.create("output.mcap", [ + config = McapRecordingConfig("output.mcap", [ (hand_tracker, "hands"), (head_tracker, "head"), - ]) as recorder: + ]) + with DeviceIOSession.run(trackers, handles, config) as session: while running: - session.update() - recorder.record(session) + session.update() # writes to MCAP automatically """ -from ._mcap import McapRecorder - -__all__ = [ - "McapRecorder", -] +__all__ = [] diff --git a/src/core/mcap_tests/cpp/CMakeLists.txt b/src/core/mcap_tests/cpp/CMakeLists.txt index 50449559..65205fdd 100644 --- a/src/core/mcap_tests/cpp/CMakeLists.txt +++ b/src/core/mcap_tests/cpp/CMakeLists.txt @@ -4,13 +4,13 @@ cmake_minimum_required(VERSION 3.20) add_executable(mcap_tests - test_mcap_recorder.cpp + test_mcap_tracker_channels.cpp ) target_link_libraries(mcap_tests PRIVATE - mcap_core - mcap::mcap - Catch2::Catch2WithMain + deviceio::deviceio_session + mcap::mcap_core + Catch2::Catch2WithMain ) message(STATUS "mcap_tests target enabled with Catch2") diff --git a/src/core/mcap_tests/cpp/test_mcap_recorder.cpp b/src/core/mcap_tests/cpp/test_mcap_recorder.cpp deleted file mode 100644 index 7d035031..00000000 --- a/src/core/mcap_tests/cpp/test_mcap_recorder.cpp +++ /dev/null @@ -1,599 +0,0 @@ -// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// SPDX-License-Identifier: Apache-2.0 - -// Unit tests for McapRecorder - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#ifdef _WIN32 -# include -# define GET_PID() _getpid() -#else -# include -# define GET_PID() ::getpid() -#endif - -namespace fs = std::filesystem; - -namespace -{ - -// ============================================================================= -// Mock TrackerImpl for testing -// ============================================================================= -class MockTrackerImpl : public core::ITrackerImpl -{ -public: - static constexpr const char* TRACKER_NAME = "MockTracker"; - - MockTrackerImpl() = default; - - bool update(XrTime time) override - { - timestamp_ = time; - update_count_++; - return true; - } - - void serialize_all(size_t /*channel_index*/, const RecordCallback& callback) const override - { - flatbuffers::FlatBufferBuilder builder(64); - std::vector data = { 0x01, 0x02, 0x03, 0x04 }; - auto vec = builder.CreateVector(data); - builder.Finish(vec); - serialize_count_++; - callback(timestamp_, builder.GetBufferPointer(), builder.GetSize()); - } - - // Test helpers - int get_update_count() const - { - return update_count_; - } - int get_serialize_count() const - { - return serialize_count_; - } - int64_t get_timestamp() const - { - return timestamp_; - } - -private: - int64_t timestamp_ = 0; - mutable int update_count_ = 0; - mutable int serialize_count_ = 0; -}; - -// Forward declaration — used by TestITrackerFactory::create_mock_tracker_impl -class MockTracker; - -// ============================================================================= -// Test-only ITrackerFactory (mirrors production double-dispatch entry point) -// ============================================================================= -class TestITrackerFactory : public core::ITrackerFactory -{ -public: - mutable bool mock_create_invoked = false; - const MockTracker* last_tracker_arg = nullptr; - - std::unique_ptr create_mock_tracker_impl(const MockTracker* tracker) - { - mock_create_invoked = true; - last_tracker_arg = tracker; - return std::make_unique(); - } -}; - -// ============================================================================= -// Mock Tracker for testing (implements ITracker interface) -// ============================================================================= -class MockTracker : public core::ITracker -{ -public: - static constexpr const char* SCHEMA_NAME = "core.MockPose"; - static constexpr const char* SCHEMA_TEXT = "mock_schema_binary_data"; - - MockTracker() : impl_(std::make_shared()) - { - } - - std::vector get_required_extensions() const override - { - return {}; // No extensions required for mock - } - - std::string_view get_name() const override - { - return MockTrackerImpl::TRACKER_NAME; - } - - std::string_view get_schema_name() const override - { - return SCHEMA_NAME; - } - - std::string_view get_schema_text() const override - { - return SCHEMA_TEXT; - } - - std::vector get_record_channels() const override - { - return { "mock" }; - } - - std::shared_ptr get_impl() const - { - return impl_; - } - - std::unique_ptr create_tracker_impl(core::ITrackerFactory& factory) const override; - -private: - std::shared_ptr impl_; -}; - -inline std::unique_ptr MockTracker::create_tracker_impl(core::ITrackerFactory& factory) const -{ - if (auto* test_factory = dynamic_cast(&factory)) - { - return test_factory->create_mock_tracker_impl(this); - } - return std::make_unique(); -} - -// ============================================================================= -// Multi-channel mock tracker for testing -// ============================================================================= -class MockMultiChannelTracker : public core::ITracker -{ -public: - MockMultiChannelTracker() : impl_(std::make_shared()) - { - } - - std::vector get_required_extensions() const override - { - return {}; - } - - std::string_view get_name() const override - { - return "MockMultiChannelTracker"; - } - - std::string_view get_schema_name() const override - { - return MockTracker::SCHEMA_NAME; - } - - std::string_view get_schema_text() const override - { - return MockTracker::SCHEMA_TEXT; - } - - std::vector get_record_channels() const override - { - return { "left", "right" }; - } - - std::shared_ptr get_impl() const - { - return impl_; - } - - // Not called in these tests (no DeviceIOSession used) - std::unique_ptr create_tracker_impl(core::ITrackerFactory& /*factory*/) const override - { - return std::make_unique(); - } - -private: - std::shared_ptr impl_; -}; - -// ============================================================================= -// Mock tracker returning an empty channel name (for validation testing) -// ============================================================================= -class MockEmptyChannelTracker : public core::ITracker -{ -public: - std::vector get_required_extensions() const override - { - return {}; - } - - std::string_view get_name() const override - { - return "MockEmptyChannelTracker"; - } - - std::string_view get_schema_name() const override - { - return MockTracker::SCHEMA_NAME; - } - - std::string_view get_schema_text() const override - { - return MockTracker::SCHEMA_TEXT; - } - - std::vector get_record_channels() const override - { - return { "" }; - } - - std::unique_ptr create_tracker_impl(core::ITrackerFactory& /*factory*/) const override - { - return std::make_unique(); - } -}; - -// Helper to create a temporary file path unique across parallel CTest processes. -// Each CTest invocation runs in a separate process (due to catch_discover_tests), -// so std::rand() with the default seed would produce identical filenames. -std::string get_temp_mcap_path() -{ - static std::atomic counter{ 0 }; - auto temp_dir = fs::temp_directory_path(); - auto filename = "test_mcap_" + std::to_string(GET_PID()) + "_" + std::to_string(counter++) + ".mcap"; - auto path = (temp_dir / filename).string(); - std::cout << "Test temp file: " << path << std::endl; - return path; -} - -// RAII cleanup helper -class TempFileCleanup -{ -public: - explicit TempFileCleanup(const std::string& path) : path_(path) - { - } - ~TempFileCleanup() - { - if (fs::exists(path_)) - { - fs::remove(path_); - } - } - TempFileCleanup(const TempFileCleanup&) = delete; - TempFileCleanup& operator=(const TempFileCleanup&) = delete; - -private: - std::string path_; -}; - -} // anonymous namespace - -// ============================================================================= -// ITrackerFactory / create_tracker_impl (double dispatch) -// ============================================================================= - -TEST_CASE("MockTracker create_tracker_impl double-dispatches through TestITrackerFactory", - "[mcap_recorder][tracker_factory]") -{ - TestITrackerFactory factory; - auto tracker = std::make_shared(); - - auto impl = tracker->create_tracker_impl(factory); - - REQUIRE(impl != nullptr); - CHECK(factory.mock_create_invoked); - REQUIRE(factory.last_tracker_arg == tracker.get()); - - auto* mock_impl = dynamic_cast(impl.get()); - REQUIRE(mock_impl != nullptr); - - constexpr XrTime test_time = 42; - REQUIRE(mock_impl->update(test_time)); - CHECK(mock_impl->get_update_count() == 1); -} - -// ============================================================================= -// McapRecorder Basic Tests -// ============================================================================= - -TEST_CASE("McapRecorder create static factory", "[mcap_recorder]") -{ - auto path = get_temp_mcap_path(); - TempFileCleanup cleanup(path); - - auto tracker = std::make_shared(); - - SECTION("create creates file and returns recorder") - { - auto recorder = core::McapRecorder::create(path, { { tracker, "test_channel" } }); - REQUIRE(recorder != nullptr); - - recorder.reset(); // Close via destructor - - // File should exist after close - CHECK(fs::exists(path)); - } - - SECTION("create with empty trackers throws") - { - CHECK_THROWS_AS(core::McapRecorder::create(path, {}), std::runtime_error); - } -} - -TEST_CASE("McapRecorder with multiple trackers", "[mcap_recorder]") -{ - auto path = get_temp_mcap_path(); - TempFileCleanup cleanup(path); - - auto tracker1 = std::make_shared(); - auto tracker2 = std::make_shared(); - auto tracker3 = std::make_shared(); - - auto recorder = core::McapRecorder::create( - path, { { tracker1, "channel1" }, { tracker2, "channel2" }, { tracker3, "channel3" } }); - REQUIRE(recorder != nullptr); - - recorder.reset(); // Close via destructor - CHECK(fs::exists(path)); -} - -TEST_CASE("McapRecorder destructor closes file", "[mcap_recorder]") -{ - auto path = get_temp_mcap_path(); - TempFileCleanup cleanup(path); - - auto tracker = std::make_shared(); - - { - auto recorder = core::McapRecorder::create(path, { { tracker, "test_channel" } }); - REQUIRE(recorder != nullptr); - // Destructor closes the file - } - - // File should exist after recorder is destroyed - CHECK(fs::exists(path)); -} - -// ============================================================================= -// McapRecorder creates valid MCAP file -// ============================================================================= - -TEST_CASE("McapRecorder creates valid MCAP file", "[mcap_recorder][file]") -{ - auto path = get_temp_mcap_path(); - TempFileCleanup cleanup(path); - - auto tracker = std::make_shared(); - - { - auto recorder = core::McapRecorder::create(path, { { tracker, "test_channel" } }); - REQUIRE(recorder != nullptr); - - // Note: We can't test record() without a real DeviceIOSession, - // but we can verify the file structure is created correctly - // Destructor will close the file - } - - // Verify file exists and has content - CHECK(fs::exists(path)); - CHECK(fs::file_size(path) > 0); - - // Verify MCAP magic bytes (first 8 bytes should be MCAP magic) - std::ifstream file(path, std::ios::binary); - REQUIRE(file.is_open()); - - char magic[8]; - file.read(magic, 8); - CHECK(file.gcount() == 8); - - // MCAP files start with magic bytes: 0x89 M C A P 0x30 \r \n - CHECK(static_cast(magic[0]) == 0x89); - CHECK(magic[1] == 'M'); - CHECK(magic[2] == 'C'); - CHECK(magic[3] == 'A'); - CHECK(magic[4] == 'P'); -} - -// ============================================================================= -// Multi-channel tracker tests -// ============================================================================= - -TEST_CASE("McapRecorder with multi-channel tracker", "[mcap_recorder]") -{ - auto path = get_temp_mcap_path(); - TempFileCleanup cleanup(path); - - auto tracker = std::make_shared(); - - auto recorder = core::McapRecorder::create(path, { { tracker, "controllers" } }); - REQUIRE(recorder != nullptr); - - recorder.reset(); - - // Verify file was created with content (channels "controllers/left" and "controllers/right") - CHECK(fs::exists(path)); - CHECK(fs::file_size(path) > 0); -} - -TEST_CASE("McapRecorder with mixed single and multi-channel trackers", "[mcap_recorder]") -{ - auto path = get_temp_mcap_path(); - TempFileCleanup cleanup(path); - - auto single_tracker = std::make_shared(); - auto multi_tracker = std::make_shared(); - - auto recorder = core::McapRecorder::create(path, { { single_tracker, "head" }, { multi_tracker, "controllers" } }); - REQUIRE(recorder != nullptr); - - recorder.reset(); - CHECK(fs::exists(path)); - CHECK(fs::file_size(path) > 0); -} - -// ============================================================================= -// Channel name validation tests -// ============================================================================= - -TEST_CASE("McapRecorder rejects empty base channel name", "[mcap_recorder]") -{ - auto path = get_temp_mcap_path(); - TempFileCleanup cleanup(path); - - auto tracker = std::make_shared(); - - CHECK_THROWS_AS(core::McapRecorder::create(path, { { tracker, "" } }), std::runtime_error); -} - -TEST_CASE("McapRecorder rejects tracker with empty channel name", "[mcap_recorder]") -{ - auto path = get_temp_mcap_path(); - TempFileCleanup cleanup(path); - - auto tracker = std::make_shared(); - - CHECK_THROWS_AS(core::McapRecorder::create(path, { { tracker, "base" } }), std::runtime_error); -} - -// ============================================================================= -// Note: Full recording tests require a real DeviceIOSession -// ============================================================================= -// The record(session) function requires a DeviceIOSession, which in turn -// requires OpenXR handles. Full integration testing of the recording -// functionality should be done with actual hardware or a mock OpenXR runtime. - -// ============================================================================= -// No-drops: mock that queues N independent records and overrides serialize_all -// ============================================================================= -namespace -{ - -// Queues independent samples and overrides serialize_all to emit each as a -// separate callback, mirroring what SchemaTracker-based impls do. -class MockMultiSampleTrackerImpl : public core::ITrackerImpl -{ -public: - void add_pending(int64_t sample_time_ns) - { - pending_.push_back(sample_time_ns); - } - - size_t pending_count() const - { - return pending_.size(); - } - - bool update(XrTime) override - { - return true; - } - - void serialize_all(size_t, const RecordCallback& callback) const override - { - for (int64_t ts : pending_) - { - flatbuffers::FlatBufferBuilder builder(64); - auto vec = builder.CreateVector(std::vector{ 0xAA }); - builder.Finish(vec); - callback(ts, builder.GetBufferPointer(), builder.GetSize()); - } - } - -private: - std::vector pending_; -}; - -} // anonymous namespace - -// ============================================================================= -// No-drops: serialize_all contract tests (no MCAP I/O or OpenXR required) -// ============================================================================= - -TEST_CASE("MockTrackerImpl serialize_all invokes callback exactly once per update", "[no_drops]") -{ - // MockTrackerImpl::serialize_all emits one record per call (single-state tracker). - MockTrackerImpl impl; - impl.update(1'000'000'000LL); - - int count = 0; - impl.serialize_all(0, [&](int64_t, const uint8_t*, size_t) { ++count; }); - - CHECK(count == 1); -} - -TEST_CASE("serialize_all emits every pending record without dropping any", "[no_drops]") -{ - constexpr int N = 7; - MockMultiSampleTrackerImpl impl; - for (int i = 0; i < N; ++i) - { - impl.add_pending(static_cast(i + 1) * 1'000'000'000LL); - } - - int callback_count = 0; - std::vector seen_timestamps; - - impl.serialize_all(0, - [&](int64_t ts, const uint8_t*, size_t) - { - ++callback_count; - seen_timestamps.push_back(ts); - }); - - // All N records must reach the recorder — none dropped. - REQUIRE(callback_count == N); - for (int i = 0; i < N; ++i) - { - // Each record carries its own distinct monotonic timestamp. - CHECK(seen_timestamps[i] == static_cast(i + 1) * 1'000'000'000LL); - } -} - -TEST_CASE("MockMultiSampleTrackerImpl serialize_all with zero pending records invokes no callbacks", "[no_drops]") -{ - // This mock does not emit a heartbeat when pending_ is empty. - // Heartbeat emission is implementation-specific and not required by ITrackerImpl. - // Some trackers (e.g. Generic3AxisPedalTracker, FrameMetadataTrackerOak) do emit - // an empty record each tick; that behaviour is verified in their own unit tests. - MockMultiSampleTrackerImpl impl; - - int count = 0; - impl.serialize_all(0, [&](int64_t, const uint8_t*, size_t) { ++count; }); - - CHECK(count == 0); -} - -TEST_CASE("serialize_all pending count matches callback invocations (single accumulated batch)", "[no_drops]") -{ - MockMultiSampleTrackerImpl impl; - - // Simulate three update ticks with different burst sizes. - const std::vector burst_sizes = { 3, 1, 5 }; - int64_t ts = 1'000'000'000LL; - for (int burst : burst_sizes) - { - for (int j = 0; j < burst; ++j) - { - impl.add_pending(ts); - ts += 1'000'000'000LL; - } - } - - const int total = static_cast(impl.pending_count()); - int count = 0; - impl.serialize_all(0, [&](int64_t, const uint8_t*, size_t) { ++count; }); - - CHECK(count == total); -} diff --git a/src/core/mcap_tests/cpp/test_mcap_tracker_channels.cpp b/src/core/mcap_tests/cpp/test_mcap_tracker_channels.cpp new file mode 100644 index 00000000..2af9101e --- /dev/null +++ b/src/core/mcap_tests/cpp/test_mcap_tracker_channels.cpp @@ -0,0 +1,261 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Unit tests for McapTrackerChannels. + +#define MCAP_IMPLEMENTATION + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#ifdef _WIN32 +# include +# define GET_PID() _getpid() +#else +# include +# define GET_PID() ::getpid() +#endif + +namespace fs = std::filesystem; + +namespace +{ + +std::string get_temp_mcap_path() +{ + static std::atomic cnt{ 0 }; + auto fn = "test_mcap_" + std::to_string(GET_PID()) + "_" + std::to_string(cnt++) + ".mcap"; + return (fs::temp_directory_path() / fn).string(); +} + +struct TempFileCleanup +{ + std::string path; + explicit TempFileCleanup(const std::string& p) : path(p) + { + } + ~TempFileCleanup() noexcept + { + std::error_code ec; + fs::remove(path, ec); + } + TempFileCleanup(const TempFileCleanup&) = delete; + TempFileCleanup& operator=(const TempFileCleanup&) = delete; +}; + +std::unique_ptr open_writer(const std::string& path) +{ + auto writer = std::make_unique(); + mcap::McapWriterOptions options("teleop-test"); + options.compression = mcap::Compression::None; + auto status = writer->open(path, options); + REQUIRE(status.ok()); + return writer; +} + +using HeadChannels = core::McapTrackerChannels; + +} // namespace + +// ============================================================================= +// McapTrackerChannels - typed write + readback +// ============================================================================= + +TEST_CASE("McapTrackerChannels: typed write produces readable MCAP with correct record content", + "[mcap][tracker_channels]") +{ + auto path = get_temp_mcap_path(); + TempFileCleanup cleanup(path); + + auto head_data = std::make_shared(); + head_data->is_valid = true; + head_data->pose = + std::make_shared(core::Point(1.0f, 2.0f, 3.0f), core::Quaternion(0.0f, 0.0f, 0.707f, 0.707f)); + + { + auto writer = open_writer(path); + HeadChannels ch(*writer, "tracking", core::HeadRecordingTraits::schema_name, { "head" }); + ch.write(0, core::DeviceDataTimestamp(1000000, 1000000, 42), head_data); + writer->close(); + } + + mcap::McapReader reader; + REQUIRE(reader.open(path).ok()); + + size_t msg_count = 0; + for (const auto& view : reader.readMessages()) + { + CHECK(view.channel->topic == "tracking/head"); + CHECK(view.schema->name == core::HeadRecordingTraits::schema_name); + CHECK(view.message.logTime == 1000000); + + auto record = flatbuffers::GetRoot(view.message.data); + REQUIRE(record != nullptr); + REQUIRE(record->timestamp() != nullptr); + CHECK(record->timestamp()->sample_time_raw_device_clock() == 42); + REQUIRE(record->data() != nullptr); + CHECK(record->data()->is_valid() == true); + + REQUIRE(record->data()->pose() != nullptr); + CHECK(record->data()->pose()->position().x() == 1.0f); + CHECK(record->data()->pose()->position().y() == 2.0f); + CHECK(record->data()->pose()->position().z() == 3.0f); + CHECK(record->data()->pose()->orientation().x() == 0.0f); + CHECK(record->data()->pose()->orientation().y() == 0.0f); + CHECK(record->data()->pose()->orientation().z() == 0.707f); + CHECK(record->data()->pose()->orientation().w() == 0.707f); + + msg_count++; + } + CHECK(msg_count == 1); + reader.close(); +} + +TEST_CASE("McapTrackerChannels: null data writes record with timestamp only", "[mcap][tracker_channels]") +{ + auto path = get_temp_mcap_path(); + TempFileCleanup cleanup(path); + + { + auto writer = open_writer(path); + HeadChannels ch(*writer, "tracking", core::HeadRecordingTraits::schema_name, { "head" }); + ch.write(0, core::DeviceDataTimestamp(500, 500, 10), std::shared_ptr{ nullptr }); + writer->close(); + } + + mcap::McapReader reader; + REQUIRE(reader.open(path).ok()); + + size_t msg_count = 0; + for (const auto& view : reader.readMessages()) + { + auto record = flatbuffers::GetRoot(view.message.data); + REQUIRE(record != nullptr); + REQUIRE(record->timestamp() != nullptr); + CHECK(record->timestamp()->sample_time_raw_device_clock() == 10); + CHECK(record->data() == nullptr); + + msg_count++; + } + CHECK(msg_count == 1); + reader.close(); +} + +TEST_CASE("McapTrackerChannels: multi-channel write routes to correct topics", "[mcap][tracker_channels]") +{ + auto path = get_temp_mcap_path(); + TempFileCleanup cleanup(path); + + auto data = std::make_shared(); + + { + auto writer = open_writer(path); + HeadChannels ch(*writer, "hands", core::HeadRecordingTraits::schema_name, { "left", "right" }); + ch.write(0, core::DeviceDataTimestamp(100, 100, 1), data); + ch.write(1, core::DeviceDataTimestamp(200, 200, 2), data); + writer->close(); + } + + mcap::McapReader reader; + REQUIRE(reader.open(path).ok()); + + std::vector topics; + for (const auto& view : reader.readMessages()) + { + topics.push_back(view.channel->topic); + } + + REQUIRE(topics.size() == 2); + CHECK(topics[0] == "hands/left"); + CHECK(topics[1] == "hands/right"); + reader.close(); +} + +TEST_CASE("McapTrackerChannels: out-of-range channel_index throws", "[mcap][tracker_channels]") +{ + auto path = get_temp_mcap_path(); + TempFileCleanup cleanup(path); + + auto data = std::make_shared(); + + auto writer = open_writer(path); + HeadChannels ch(*writer, "test", core::HeadRecordingTraits::schema_name, { "only" }); + CHECK_THROWS_AS(ch.write(99, core::DeviceDataTimestamp(100, 100, 1), data), std::out_of_range); + writer->close(); +} + +TEST_CASE("McapTrackerChannels: sequence numbers increment across writes", "[mcap][tracker_channels]") +{ + auto path = get_temp_mcap_path(); + TempFileCleanup cleanup(path); + + auto data = std::make_shared(); + + { + auto writer = open_writer(path); + HeadChannels ch(*writer, "seq", core::HeadRecordingTraits::schema_name, { "ch" }); + ch.write(0, core::DeviceDataTimestamp(100, 100, 1), data); + ch.write(0, core::DeviceDataTimestamp(200, 200, 2), data); + ch.write(0, core::DeviceDataTimestamp(300, 300, 3), data); + writer->close(); + } + + mcap::McapReader reader; + REQUIRE(reader.open(path).ok()); + + std::vector sequences; + for (const auto& view : reader.readMessages()) + { + sequences.push_back(view.message.sequence); + } + + REQUIRE(sequences.size() == 3); + CHECK(sequences[0] == 0); + CHECK(sequences[1] == 1); + CHECK(sequences[2] == 2); + reader.close(); +} + +TEST_CASE("McapTrackerChannels: multiple same-type channel instances share one writer", "[mcap][tracker_channels]") +{ + auto path = get_temp_mcap_path(); + TempFileCleanup cleanup(path); + + auto data = std::make_shared(); + + { + auto writer = open_writer(path); + HeadChannels head_ch(*writer, "head", core::HeadRecordingTraits::schema_name, { "pose" }); + HeadChannels ctrl_ch(*writer, "ctrl", core::HeadRecordingTraits::schema_name, { "left", "right" }); + + head_ch.write(0, core::DeviceDataTimestamp(100, 100, 1), data); + ctrl_ch.write(0, core::DeviceDataTimestamp(200, 200, 2), data); + ctrl_ch.write(1, core::DeviceDataTimestamp(300, 300, 3), data); + writer->close(); + } + + mcap::McapReader reader; + REQUIRE(reader.open(path).ok()); + + std::vector topics; + for (const auto& view : reader.readMessages()) + { + topics.push_back(view.channel->topic); + } + + REQUIRE(topics.size() == 3); + CHECK(topics[0] == "head/pose"); + CHECK(topics[1] == "ctrl/left"); + CHECK(topics[2] == "ctrl/right"); + reader.close(); +} diff --git a/src/core/python/CMakeLists.txt b/src/core/python/CMakeLists.txt index df6c4461..55238515 100644 --- a/src/core/python/CMakeLists.txt +++ b/src/core/python/CMakeLists.txt @@ -40,7 +40,7 @@ add_custom_target(python_package ALL COMMAND ${CMAKE_COMMAND} -E copy "${CMAKE_CURRENT_SOURCE_DIR}/requirements-cloudxr.txt" "${CMAKE_BINARY_DIR}/python_package/$/" COMMAND ${CMAKE_COMMAND} -E copy "${CMAKE_CURRENT_SOURCE_DIR}/requirements-retargeters.txt" "${CMAKE_BINARY_DIR}/python_package/$/" COMMAND ${CMAKE_COMMAND} -E copy "${CMAKE_CURRENT_SOURCE_DIR}/requirements-retargeters-lite.txt" "${CMAKE_BINARY_DIR}/python_package/$/" - DEPENDS deviceio_trackers_py deviceio_session_py mcap_py oxr_py plugin_manager_py schema_py retargeting_engine_python retargeters_python retargeting_engine_ui_python teleop_session_manager_python cloudxr_python + DEPENDS deviceio_trackers_py deviceio_session_py oxr_py plugin_manager_py schema_py retargeting_engine_python retargeters_python retargeting_engine_ui_python teleop_session_manager_python cloudxr_python COMMENT "Preparing Python package structure" ) @@ -65,8 +65,6 @@ add_custom_command( python "${STUBGEN_SCRIPT}" isaacteleop.deviceio_trackers._deviceio_trackers "${STUBGEN_PACKAGE_DIR}" COMMAND uv run --project "${CMAKE_BINARY_DIR}/stubgen" --python ${ISAAC_TELEOP_PYTHON_VERSION} python "${STUBGEN_SCRIPT}" isaacteleop.deviceio_session._deviceio_session "${STUBGEN_PACKAGE_DIR}" - COMMAND uv run --project "${CMAKE_BINARY_DIR}/stubgen" --python ${ISAAC_TELEOP_PYTHON_VERSION} - python "${STUBGEN_SCRIPT}" isaacteleop.mcap._mcap "${STUBGEN_PACKAGE_DIR}" COMMAND uv run --project "${CMAKE_BINARY_DIR}/stubgen" --python ${ISAAC_TELEOP_PYTHON_VERSION} python "${STUBGEN_SCRIPT}" isaacteleop.oxr._oxr "${STUBGEN_PACKAGE_DIR}" COMMAND uv run --project "${CMAKE_BINARY_DIR}/stubgen" --python ${ISAAC_TELEOP_PYTHON_VERSION} @@ -79,7 +77,6 @@ add_custom_command( DEPENDS python_package "${STUBGEN_SCRIPT}" "${STUBGEN_PYPROJECT}" $ $ - $ $ $ $ diff --git a/src/core/python/deviceio_init.py b/src/core/python/deviceio_init.py index c856f3d3..f3ab4686 100644 --- a/src/core/python/deviceio_init.py +++ b/src/core/python/deviceio_init.py @@ -5,7 +5,7 @@ Prefer importing directly: from isaacteleop.deviceio_trackers import HeadTracker, HandTracker - from isaacteleop.deviceio_session import DeviceIOSession + from isaacteleop.deviceio_session import DeviceIOSession, McapRecordingConfig """ from isaacteleop.deviceio_trackers import ( @@ -23,7 +23,7 @@ JOINT_INDEX_TIP, ) -from isaacteleop.deviceio_session import DeviceIOSession +from isaacteleop.deviceio_session import DeviceIOSession, McapRecordingConfig from ..oxr import OpenXRSessionHandles @@ -54,6 +54,7 @@ "FullBodyTrackerPico", "OpenXRSessionHandles", "DeviceIOSession", + "McapRecordingConfig", "NUM_JOINTS", "JOINT_PALM", "JOINT_WRIST",