diff --git a/docs/source/device/body_tracking.rst b/docs/source/device/body_tracking.rst index a95bbe65b..26de8d0a0 100644 --- a/docs/source/device/body_tracking.rst +++ b/docs/source/device/body_tracking.rst @@ -6,7 +6,7 @@ Body Tracking Isaac Teleop supports streaming full-body tracking data from an XR headset through the CloudXR WebXR client to the teleop server. The server exposes the -body skeleton to applications through the ``FullBodyTrackerPico`` tracker and +body skeleton to applications through the ``FullBodyTracker`` tracker and the OpenXR ``XR_BD_body_tracking`` extension. Body tracking support currently targets the **PICO 4 Ultra Enterprise** with @@ -183,10 +183,10 @@ Server-side access ~~~~~~~~~~~~~~~~~~ On the server, body tracking data is consumed through the -``FullBodyTrackerPico`` tracker (see :doc:`trackers` for the full tracker +``FullBodyTracker`` tracker (see :doc:`trackers` for the full tracker reference). The tracker exposes a ``get_body_pose()`` method that returns the 24-joint skeleton each frame (or null when body tracking is not available). -Joint data follows the ``FullBodyPosePico`` FlatBuffers schema defined in +Joint data follows the ``FullBodyPose`` FlatBuffers schema defined in ``src/core/schema/fbs/full_body.fbs``. The ``all_joint_poses_tracked`` quality flag indicates whether every joint was diff --git a/docs/source/device/trackers.rst b/docs/source/device/trackers.rst index 699275b15..7bcac1bef 100644 --- a/docs/source/device/trackers.rst +++ b/docs/source/device/trackers.rst @@ -14,7 +14,7 @@ APIs (``xrLocateSpace``, ``xrSyncActions``, etc.): - :code-file:`HeadTracker ` -- HMD head pose - :code-file:`HandTracker ` -- articulated hand joints (left and right) - :code-file:`ControllerTracker ` -- controller poses and button/axis inputs (left and right) -- :code-file:`FullBodyTrackerPico ` -- 24-joint full body pose (PICO ``XR_BD_body_tracking``) +- :code-file:`FullBodyTracker ` -- vendor-agnostic 24-joint full body pose; default vendor reads the PICO ``XR_BD_body_tracking`` extension (see `Vendor Selection`_) **SchemaTracker-based trackers** -- create new device type by defining a FlatBuffer schema and reading it from OpenXR tensor collections via the @@ -185,22 +185,31 @@ axis inputs. Uses standard OpenXR action bindings. - :code-file:`examples/teleop/python/locomotion_retargeting_example.py` - :code-file:`examples/teleop/python/gripper_retargeting_example_simple.py` -FullBodyTrackerPico -~~~~~~~~~~~~~~~~~~~ +FullBodyTracker +~~~~~~~~~~~~~~~ -Tracks 24 body joints on PICO devices using the ``XR_BD_body_tracking`` -extension. +Tracks 24 body joints through a vendor-selected backend. The tracker itself is +a vendor-agnostic marker and carries no vendor or live/replay state: a live +session picks the backend via ``VendorConfig`` (see `Vendor Selection`_), and +replay reads the recorded ``full_body`` channel regardless of which vendor +produced it. When no vendor is selected, the default vendor ``body.pico-xr`` +reads the PICO ``XR_BD_body_tracking`` extension directly. - Schema: :code-file:`src/core/schema/fbs/full_body.fbs` -- C++ header: ``#include `` -- Python import: ``from isaacteleop.deviceio import FullBodyTrackerPico`` -- Record channels: ``full_body`` | MCAP schema: ``core.FullBodyPosePicoRecord`` +- C++ header: ``#include `` +- Python import: ``from isaacteleop.deviceio import FullBodyTracker`` +- Record channels: ``full_body`` | MCAP schema: ``core.FullBodyPoseRecord`` - Tests: - :code-file:`src/core/schema_tests/cpp/test_full_body.cpp` - :code-file:`src/core/schema_tests/python/test_full_body.py` - :code-file:`examples/oxr/python/test_full_body_tracker.py` +.. note:: + + ``FullBodyTrackerPico`` remains available as a deprecated alias for + ``FullBodyTracker`` so existing scripts run unchanged. + FrameMetadataTrackerOak ~~~~~~~~~~~~~~~~~~~~~~~ @@ -248,6 +257,57 @@ utility internally. The Python method is named ``get_pedal_data()`` (instead of the C++ ``get_data()``). +.. _vendor-selection: + +Vendor Selection +---------------- + +Some trackers are **vendor-agnostic markers**: the tracker declares *what* +device data it represents, while a live session chooses *which* backend +("vendor") produces that data. This mirrors how live-vs-replay is chosen at the +session level -- the same tracker instance works across vendors and across live +and replay. ``FullBodyTracker`` is currently the only vendored tracker; its +default vendor ``body.pico-xr`` reads the PICO ``XR_BD_body_tracking`` +extension. + +Select a vendor by passing a ``VendorConfig`` to both +``DeviceIOSession.get_required_extensions()`` and ``DeviceIOSession.run()``. A +``VendorConfig`` maps tracker instances to a ``TrackerVendor(id, params)``, +where ``id`` selects the backend from the live factory's vendor registry and +``params`` carries free-form string key/value options for it. Trackers left out +of the config use their default vendor. Vendor selections on non-vendored +trackers, and unknown vendor ids, are rejected at session construction. + +.. code-block:: python + + import isaacteleop.deviceio as deviceio + + body = deviceio.FullBodyTracker() + + # Select the backend for the vendored tracker (default shown explicitly). + vendor_config = deviceio.VendorConfig([ + (body, deviceio.TrackerVendor("body.pico-xr")), + ]) + + required_extensions = deviceio.DeviceIOSession.get_required_extensions( + [body], vendor_config + ) + with deviceio.DeviceIOSession.run( + [body], handles, None, vendor_config + ) as session: + ... + +Replay is always vendor-neutral: the replay full-body impl reads the recorded +``full_body`` channel regardless of which live vendor produced it, so +``VendorConfig`` applies to live sessions only. The vendor registry is open for +additional pre-built plugin vendors without changing the tracker marker. + +When driving devices through the higher-level teleop session manager, vendor +selection is carried on the DeviceIO source itself via its ``vendor`` argument +(e.g. ``FullBodySource(name="full_body", vendor=deviceio.TrackerVendor("body.pico-xr"))``), +so it travels with the pipeline into both extension discovery and session +construction; see :doc:`../getting_started/teleop_session`. + .. _tracker-usage-example: Usage Examples diff --git a/docs/source/getting_started/teleop_session.rst b/docs/source/getting_started/teleop_session.rst index 2e80d4177..ff9daefff 100644 --- a/docs/source/getting_started/teleop_session.rst +++ b/docs/source/getting_started/teleop_session.rst @@ -115,6 +115,30 @@ argument is a ``uint64`` handle value. oxr_handles=handles, # Skip internal OpenXR session creation ) +Tracker vendor selection +"""""""""""""""""""""""" + +In live mode, a vendored source selects the backend ("vendor") for its tracker +-- for example which backend drives a ``FullBodySource``. The selection is +carried **on the source** via its ``vendor`` argument, a +``deviceio.TrackerVendor(id, params)``; sources left at the default use their +tracker's default vendor. Because the vendor travels with the source, it is part +of the pipeline: ``TeleopSession`` picks it up automatically, and +``get_required_oxr_extensions_from_pipeline(pipeline)`` reports the matching +OpenXR extensions with no extra argument (important for the external-``oxr_handles`` +flow). See :ref:`vendor-selection` for the underlying DeviceIO mechanism and the +available vendor ids. + +.. code-block:: python + + import isaacteleop.deviceio as deviceio + from isaacteleop.retargeting_engine.deviceio_source_nodes import FullBodySource + + # Select the backend on the source; it flows through the pipeline into the session. + full_body = FullBodySource( + name="full_body", vendor=deviceio.TrackerVendor("body.pico-xr") + ) + Retargeting execution """"""""""""""""""""" @@ -382,7 +406,12 @@ The module also exports two utility functions: - ``get_required_oxr_extensions_from_pipeline(pipeline) -> List[str]`` -- Discover the OpenXR extensions needed by a retargeting pipeline by traversing its DeviceIO source leaf nodes. Returns a sorted, deduplicated - list of extension name strings. + list of extension name strings. Extensions are vendor-dependent, but each + source carries its own vendor selection, so the result already reflects them + -- when you create the OpenXR session yourself and feed the handles in via + ``oxr_handles``, the enabled extensions match the session that gets built + with no extra argument. Select a vendor on the source (e.g. + ``FullBodySource(name="full_body", vendor=deviceio.TrackerVendor("body.pico-xr"))``). - ``create_standard_inputs(trackers) -> Dict[str, IDeviceIOSource]`` -- Convenience function that creates ``HandsSource``, ``ControllersSource``, diff --git a/examples/deviceio_live_view/python/deviceio_viser.py b/examples/deviceio_live_view/python/deviceio_viser.py index 9b1912194..7f612a840 100644 --- a/examples/deviceio_live_view/python/deviceio_viser.py +++ b/examples/deviceio_live_view/python/deviceio_viser.py @@ -23,14 +23,14 @@ from isaacteleop.retargeting_engine.interface import OutputCombiner from isaacteleop.retargeting_engine.tensor_types import HandInputIndex from isaacteleop.retargeting_engine.tensor_types.indices import ( - BodyJointPicoIndex, + BodyJointIndex, ControllerInputIndex, FullBodyInputIndex, HeadPoseIndex, ) HANDS_CHANNEL = "hands" -BODY_JOINT_NAMES = [joint.name for joint in BodyJointPicoIndex] +BODY_JOINT_NAMES = [joint.name for joint in BodyJointIndex] # --------------------------------------------------------------------------- # Color palette shared across all viz scripts @@ -61,7 +61,7 @@ def build_all_human_pipeline(): # PICO body-joint connectivity (parent → child) for skeleton rendering. -# Indices follow BodyJointPicoIndex: 0=PELVIS, 1/2=LEFT/RIGHT_HIP, 3/6/9=SPINE1/2/3, +# Indices follow BodyJointIndex: 0=PELVIS, 1/2=LEFT/RIGHT_HIP, 3/6/9=SPINE1/2/3, # 4/5=LEFT/RIGHT_KNEE, 7/8=LEFT/RIGHT_ANKLE, 10/11=LEFT/RIGHT_FOOT, 12=NECK, # 13/14=LEFT/RIGHT_COLLAR, 15=HEAD, 16/17=LEFT/RIGHT_SHOULDER, # 18/19=LEFT/RIGHT_ELBOW, 20/21=LEFT/RIGHT_WRIST, 22/23=LEFT/RIGHT_HAND — 24 total. diff --git a/examples/mcap_record_replay/python/common.py b/examples/mcap_record_replay/python/common.py index f0d042362..835db44bd 100644 --- a/examples/mcap_record_replay/python/common.py +++ b/examples/mcap_record_replay/python/common.py @@ -39,7 +39,7 @@ HandInputIndex, ) from isaacteleop.retargeting_engine.tensor_types.indices import ( - BodyJointPicoIndex, + BodyJointIndex, ControllerInputIndex, ) from isaacteleop.retargeting_engine.tensor_types.ndarray_types import ( @@ -52,7 +52,7 @@ HANDS_CHANNEL = "hands" -BODY_JOINT_NAMES = [joint.name for joint in BodyJointPicoIndex] +BODY_JOINT_NAMES = [joint.name for joint in BodyJointIndex] # --------------------------------------------------------------------------- # Color palette shared across all viz scripts @@ -149,7 +149,7 @@ def build_full_body_pipeline(): # PICO body-joint connectivity (parent → child) for skeleton rendering. -# Indices follow BodyJointPicoIndex: 0=PELVIS, 1/2=LEFT/RIGHT_HIP, 3/6/9=SPINE1/2/3, +# Indices follow BodyJointIndex: 0=PELVIS, 1/2=LEFT/RIGHT_HIP, 3/6/9=SPINE1/2/3, # 4/5=LEFT/RIGHT_KNEE, 7/8=LEFT/RIGHT_ANKLE, 10/11=LEFT/RIGHT_FOOT, 12=NECK, # 13/14=LEFT/RIGHT_COLLAR, 15=HEAD, 16/17=LEFT/RIGHT_SHOULDER, # 18/19=LEFT/RIGHT_ELBOW, 20/21=LEFT/RIGHT_WRIST, 22/23=LEFT/RIGHT_HAND — 24 total. diff --git a/examples/oxr/python/test_full_body_tracker.py b/examples/oxr/python/test_full_body_tracker.py index 06a8b2073..18fda3241 100644 --- a/examples/oxr/python/test_full_body_tracker.py +++ b/examples/oxr/python/test_full_body_tracker.py @@ -1,12 +1,12 @@ -# SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # SPDX-License-Identifier: Apache-2.0 """ -Test script for FullBodyTrackerPico with XR_BD_body_tracking extension. +Test script for FullBodyTracker with XR_BD_body_tracking extension. Demonstrates: - Getting full body pose data (24 joints from pelvis to hands) -- Requires PICO device with body tracking support +- Requires PICO device with body tracking support (default "body.pico-xr" vendor) """ import time @@ -22,7 +22,7 @@ # Test 1: Create full body tracker print("[Test 1] Creating full body tracker...") -body_tracker = deviceio.FullBodyTrackerPico() +body_tracker = deviceio.FullBodyTracker() print(f"✓ {body_tracker.get_name()} created") print() @@ -33,10 +33,10 @@ print(f"Required extensions: {required_extensions}") print() -# Test 3: Show joint names from schema (BodyJointPico enum) -print(f"[Test 3] Body joint names ({schema.BodyJointPico.NUM_JOINTS} joints):") -for i in range(schema.BodyJointPico.NUM_JOINTS): - print(f" [{i:2d}] {schema.BodyJointPico(i).name}") +# Test 3: Show joint names from schema (BodyJoint enum) +print(f"[Test 3] Body joint names ({schema.BodyJoint.NUM_JOINTS} joints):") +for i in range(schema.BodyJoint.NUM_JOINTS): + print(f" [{i:2d}] {schema.BodyJoint(i).name}") print() # Test 4: Initialize @@ -70,10 +70,10 @@ if body_tracked.data is not None: valid_count = sum( 1 - for i in range(schema.BodyJointPico.NUM_JOINTS) + for i in range(schema.BodyJoint.NUM_JOINTS) if body_tracked.data.joints.joints(i).is_valid ) - print(f" Valid joints: {valid_count}/{schema.BodyJointPico.NUM_JOINTS}") + print(f" Valid joints: {valid_count}/{schema.BodyJoint.NUM_JOINTS}") print() # Test 7: Run tracking loop @@ -96,10 +96,10 @@ if body_tracked.data is not None: pelvis_pos = body_tracked.data.joints.joints( - int(schema.BodyJointPico.PELVIS) + int(schema.BodyJoint.PELVIS) ).pose.position head_pos = body_tracked.data.joints.joints( - int(schema.BodyJointPico.HEAD) + int(schema.BodyJoint.HEAD) ).pose.position print( f" [{elapsed:5.2f}s] Frame {frame_count:4d}" @@ -128,9 +128,9 @@ if body_tracked.data is not None: print() print(" Joint positions:") - for i in range(schema.BodyJointPico.NUM_JOINTS): + for i in range(schema.BodyJoint.NUM_JOINTS): joint = body_tracked.data.joints.joints(i) - name = schema.BodyJointPico(i).name + name = schema.BodyJoint(i).name pos = joint.pose.position rot = joint.pose.orientation print( diff --git a/examples/teleop_ros2/cpp/integration_tests/mcap_generator.cpp b/examples/teleop_ros2/cpp/integration_tests/mcap_generator.cpp index 6cb85fc77..f391c1f72 100644 --- a/examples/teleop_ros2/cpp/integration_tests/mcap_generator.cpp +++ b/examples/teleop_ros2/cpp/integration_tests/mcap_generator.cpp @@ -27,7 +27,7 @@ using ControllerChannels = core::McapTrackerChannels; using HeadChannels = core::McapTrackerChannels; using PedalChannels = core::McapTrackerChannels; -using FullBodyChannels = core::McapTrackerChannels; +using FullBodyChannels = core::McapTrackerChannels; constexpr int kDefaultFrameCount = 1800; constexpr int64_t kFramePeriodNs = 16'666'667; @@ -113,12 +113,12 @@ std::shared_ptr make_pedal_sample(int frame) return sample; } -std::shared_ptr make_full_body_sample(int frame) +std::shared_ptr make_full_body_sample(int frame) { const float delta = kDriftRatePerFrameM * static_cast(frame); - auto sample = std::make_shared(); - sample->joints = std::make_unique(); - for (int joint = 0; joint < core::BodyJointPico_NUM_JOINTS; ++joint) + auto sample = std::make_shared(); + sample->joints = std::make_unique(); + for (int joint = 0; joint < core::BodyJoint_NUM_JOINTS; ++joint) { // Per-joint offsets spread the joints into a plausible-looking body layout. const float joint_f = static_cast(joint); @@ -157,7 +157,7 @@ void write_fixture(const std::filesystem::path& output_path, int frame_count) const auto hand_names = to_strings(core::HandRecordingTraits::recording_channels); const auto head_names = to_strings(core::HeadRecordingTraits::recording_channels); const auto pedal_names = to_strings(core::PedalRecordingTraits::recording_channels); - const auto full_body_names = to_strings(core::FullBodyPicoRecordingTraits::recording_channels); + const auto full_body_names = to_strings(core::FullBodyRecordingTraits::recording_channels); ControllerChannels controller_channels( *writer, "controllers", core::ControllerRecordingTraits::schema_name, controller_names); @@ -165,7 +165,7 @@ void write_fixture(const std::filesystem::path& output_path, int frame_count) HeadChannels head_channels(*writer, "head", core::HeadRecordingTraits::schema_name, head_names); PedalChannels pedal_channels(*writer, "pedals", core::PedalRecordingTraits::schema_name, pedal_names); FullBodyChannels full_body_channels( - *writer, "full_body", core::FullBodyPicoRecordingTraits::schema_name, full_body_names); + *writer, "full_body", core::FullBodyRecordingTraits::schema_name, full_body_names); for (int frame = 0; frame < frame_count; ++frame) { diff --git a/examples/teleop_ros2/python/constants.py b/examples/teleop_ros2/python/constants.py index ed1fbaf20..4e2182af0 100644 --- a/examples/teleop_ros2/python/constants.py +++ b/examples/teleop_ros2/python/constants.py @@ -7,7 +7,7 @@ from enum import Enum from isaacteleop.retargeting_engine.tensor_types.indices import ( - BodyJointPicoIndex, + BodyJointIndex, HandJointIndex, ) @@ -34,7 +34,7 @@ class TeleopMode(StrEnum): FULL_BODY = "full_body" -BODY_JOINT_NAMES = [e.name for e in BodyJointPicoIndex] +BODY_JOINT_NAMES = [e.name for e in BodyJointIndex] HAND_POSE_JOINT_INDICES = tuple( HandJointIndex(i) for i in range(HandJointIndex.WRIST, HandJointIndex.LITTLE_TIP + 1) diff --git a/examples/teleop_ros2/python/tests/test_messages.py b/examples/teleop_ros2/python/tests/test_messages.py index 169265ee0..86f666a33 100644 --- a/examples/teleop_ros2/python/tests/test_messages.py +++ b/examples/teleop_ros2/python/tests/test_messages.py @@ -17,7 +17,7 @@ from isaacteleop.retargeting_engine.tensor_types import ( DLDataType, NDArrayType, - NUM_BODY_JOINTS_PICO, + NUM_BODY_JOINTS, NUM_HAND_JOINTS, ControllerInput, ControllerInputIndex, @@ -73,11 +73,11 @@ def _active_controller() -> TensorGroup: def _active_full_body() -> TensorGroup: full_body = TensorGroup(FullBodyInput()) - positions = np.zeros((NUM_BODY_JOINTS_PICO, 3), dtype=np.float32) + positions = np.zeros((NUM_BODY_JOINTS, 3), dtype=np.float32) positions[0] = [1.0, 2.0, 3.0] - orientations = np.zeros((NUM_BODY_JOINTS_PICO, 4), dtype=np.float32) + orientations = np.zeros((NUM_BODY_JOINTS, 4), dtype=np.float32) orientations[:, 3] = 1.0 - valid = np.zeros(NUM_BODY_JOINTS_PICO, dtype=np.uint8) + valid = np.zeros(NUM_BODY_JOINTS, dtype=np.uint8) valid[0] = 1 full_body[FullBodyInputIndex.JOINT_POSITIONS] = positions diff --git a/src/core/CMakeLists.txt b/src/core/CMakeLists.txt index ad044a70a..da04ec10d 100644 --- a/src/core/CMakeLists.txt +++ b/src/core/CMakeLists.txt @@ -84,4 +84,7 @@ if(BUILD_TESTING) # Replay DeviceIO session tests (C++) add_subdirectory(replay_deviceio_session_tests/cpp) + + # Live trackers tests (C++) + add_subdirectory(live_trackers_tests/cpp) endif() diff --git a/src/core/deviceio_base/cpp/inc/deviceio_base/full_body_tracker_base.hpp b/src/core/deviceio_base/cpp/inc/deviceio_base/full_body_tracker_base.hpp new file mode 100644 index 000000000..c7648538a --- /dev/null +++ b/src/core/deviceio_base/cpp/inc/deviceio_base/full_body_tracker_base.hpp @@ -0,0 +1,22 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "tracker.hpp" + +namespace core +{ + +struct FullBodyPoseTrackedT; + +// Abstract base interface for full body tracker implementations. +// Vendor-agnostic: every live/replay backend (native XR, pushed tensor, ...) +// implements this and produces the same FullBodyPoseTrackedT payload. +class IFullBodyTrackerImpl : public ITrackerImpl +{ +public: + virtual const FullBodyPoseTrackedT& get_body_pose() const = 0; +}; + +} // namespace core diff --git a/src/core/deviceio_base/cpp/inc/deviceio_base/full_body_tracker_pico_base.hpp b/src/core/deviceio_base/cpp/inc/deviceio_base/full_body_tracker_pico_base.hpp deleted file mode 100644 index bd60e92ec..000000000 --- a/src/core/deviceio_base/cpp/inc/deviceio_base/full_body_tracker_pico_base.hpp +++ /dev/null @@ -1,20 +0,0 @@ -// SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// SPDX-License-Identifier: Apache-2.0 - -#pragma once - -#include "tracker.hpp" - -namespace core -{ - -struct FullBodyPosePicoTrackedT; - -// Abstract base interface for full body tracker (PICO) implementations. -class IFullBodyTrackerPicoImpl : public ITrackerImpl -{ -public: - virtual const FullBodyPosePicoTrackedT& get_body_pose() const = 0; -}; - -} // namespace core diff --git a/src/core/deviceio_base/cpp/inc/deviceio_base/tracker_vendor.hpp b/src/core/deviceio_base/cpp/inc/deviceio_base/tracker_vendor.hpp new file mode 100644 index 000000000..d44125a49 --- /dev/null +++ b/src/core/deviceio_base/cpp/inc/deviceio_base/tracker_vendor.hpp @@ -0,0 +1,32 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include + +namespace core +{ + +// Per-tracker vendor selection for a live session. +// +// A vendored tracker (e.g. full body) can be sourced from more than one backend +// (native XR hardware, an external pushed-tensor plugin, ...). The vendor is +// chosen at live-session construction, not baked into the tracker marker, so a +// single tracker type stays vendor-agnostic. +// +// `id` is a string (rather than an enum baked into the tracker marker) so vendor +// routing stays decoupled from the tracker type. The selectable vendors are the +// live factory's compile-time dispatch table (e.g. "body.pico-xr"), so adding a +// vendor still means extending that table and rebuilding core. `params` is +// reserved for vendor-specific settings as free-form strings (mirroring plugin +// CLI arguments, e.g. {"max_flatbuffer_size": "16384"}); no vendor consumes it +// yet, so a non-empty map is rejected at validation until the first consumer lands. +struct TrackerVendor +{ + std::string id; + std::map params; +}; + +} // namespace core diff --git a/src/core/deviceio_session/cpp/deviceio_session.cpp b/src/core/deviceio_session/cpp/deviceio_session.cpp index 629ed9391..f1707a6a7 100644 --- a/src/core/deviceio_session/cpp/deviceio_session.cpp +++ b/src/core/deviceio_session/cpp/deviceio_session.cpp @@ -19,30 +19,66 @@ namespace core // DeviceIOSession Implementation // ============================================================================ +namespace +{ + +// Identify a tracker in an error message by its name, tolerating a null pointer. +std::string tracker_name_for_error(const ITracker* tracker) +{ + return tracker ? std::string(tracker->get_name()) : std::string(""); +} + +bool tracker_in_list(const std::vector>& trackers, const ITracker* tracker_ptr) +{ + for (const auto& t : trackers) + { + if (t.get() == tracker_ptr) + return true; + } + return false; +} + +// Fully validate a vendor config against the session's tracker list before anything consumes it. +// DeviceIOSession is the single owner of vendor validation: the live factory assumes a validated +// config and treats an invalid one as undefined behavior. Two parts: the tracker-list presence +// check (done here since the session holds the list) and the dispatch-driven vendor-validity check +// (delegated to validate_vendor_selections(), which owns the vendor dispatch table). +void validate_vendor_config(const std::vector>& trackers, + const std::vector>& tracker_vendors) +{ + for (const auto& [tracker_ptr, vendor] : tracker_vendors) + { + if (!tracker_in_list(trackers, tracker_ptr)) + { + throw std::invalid_argument("DeviceIOSession: vendor selection '" + vendor.id + "' references tracker '" + + tracker_name_for_error(tracker_ptr) + "' that is not in the trackers list"); + } + } + validate_vendor_selections(tracker_vendors); +} + +} // namespace + DeviceIOSession::DeviceIOSession(const std::vector>& trackers, const OpenXRSessionHandles& handles, - std::optional recording_config) + std::optional recording_config, + VendorConfig vendor_config) : handles_(handles) { std::vector> tracker_names; + // Validate up front, before the MCAP writer opens below, so an invalid config leaves no + // stray recording file on disk. + validate_vendor_config(trackers, vendor_config.tracker_vendors); + 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) + if (!tracker_in_list(trackers, tracker_ptr)) { throw std::invalid_argument("DeviceIOSession: McapRecordingConfig references tracker '" + name + - "' that is not in the session's tracker list"); + "' that is not in the trackers list"); } } @@ -61,7 +97,7 @@ DeviceIOSession::DeviceIOSession(const std::vector>& t tracker_names = std::move(recording_config->tracker_names); } - LiveDeviceIOFactory factory(handles_, mcap_writer_.get(), tracker_names); + LiveDeviceIOFactory factory(handles_, mcap_writer_.get(), tracker_names, vendor_config.tracker_vendors); for (const auto& tracker : trackers) { @@ -75,14 +111,18 @@ DeviceIOSession::DeviceIOSession(const std::vector>& t DeviceIOSession::~DeviceIOSession() = default; -std::vector DeviceIOSession::get_required_extensions(const std::vector>& trackers) +std::vector DeviceIOSession::get_required_extensions(const std::vector>& trackers, + const VendorConfig& vendor_config) { - return LiveDeviceIOFactory::get_required_extensions(trackers); + // Validate here too, so an extension query rejects a bad vendor config just like construction. + validate_vendor_config(trackers, vendor_config.tracker_vendors); + return LiveDeviceIOFactory::get_required_extensions(trackers, vendor_config.tracker_vendors); } std::unique_ptr DeviceIOSession::run(const std::vector>& trackers, const OpenXRSessionHandles& handles, - std::optional recording_config) + std::optional recording_config, + VendorConfig vendor_config) { assert(handles.instance != XR_NULL_HANDLE && "OpenXR instance handle cannot be null"); assert(handles.session != XR_NULL_HANDLE && "OpenXR session handle cannot be null"); @@ -90,7 +130,8 @@ std::unique_ptr DeviceIOSession::run(const std::vector(new DeviceIOSession(trackers, handles, std::move(recording_config))); + return std::unique_ptr( + new DeviceIOSession(trackers, handles, std::move(recording_config), std::move(vendor_config))); } void 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 67aee3100..2fda2e656 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 @@ -1,9 +1,10 @@ -// SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. // SPDX-License-Identifier: Apache-2.0 #pragma once #include +#include #include #include @@ -39,6 +40,21 @@ struct McapRecordingConfig std::vector> tracker_names; }; +/** + * @brief Per-session vendor selection for vendored trackers (live sessions only). + * + * tracker_vendors maps each ITracker pointer to the vendor (id + params) used to + * source it. Trackers not listed fall back to their default vendor id. Vendor is a + * live-session concern (native XR hardware vs. an external pushed-tensor plugin); + * replay reads the recorded channel regardless of vendor. Pass to + * DeviceIOSession::run() and get_required_extensions(); an empty config selects + * every tracker's default vendor. + */ +struct VendorConfig +{ + std::vector> tracker_vendors; +}; + // 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 @@ -47,13 +63,17 @@ class DeviceIOSession : public ITrackerSession { public: // Static helper — required OpenXR extensions for the given trackers (live factory; not per-tracker API). - static std::vector get_required_extensions(const std::vector>& trackers); + // Vendored trackers resolve their extensions through the vendor id selected in vendor_config. + static std::vector get_required_extensions(const std::vector>& trackers, + const VendorConfig& vendor_config = {}); // Static factory - Create and initialize a session with trackers. - // Optionally pass a McapRecordingConfig to enable automatic MCAP recording. + // Optionally pass a McapRecordingConfig to enable automatic MCAP recording, and a + // VendorConfig to select the vendor for any vendored trackers. static std::unique_ptr run(const std::vector>& trackers, const OpenXRSessionHandles& handles, - std::optional recording_config = std::nullopt); + std::optional recording_config = std::nullopt, + VendorConfig vendor_config = {}); // Destructor defined in .cpp where mcap::McapWriter is fully defined ~DeviceIOSession(); @@ -83,7 +103,8 @@ class DeviceIOSession : public ITrackerSession private: DeviceIOSession(const std::vector>& trackers, const OpenXRSessionHandles& handles, - std::optional recording_config); + std::optional recording_config, + VendorConfig vendor_config); const OpenXRSessionHandles handles_; std::unordered_map> tracker_impls_; diff --git a/src/core/deviceio_session/python/deviceio_session_init.py b/src/core/deviceio_session/python/deviceio_session_init.py index 86e627114..bcd02ce70 100644 --- a/src/core/deviceio_session/python/deviceio_session_init.py +++ b/src/core/deviceio_session/python/deviceio_session_init.py @@ -8,6 +8,8 @@ McapRecordingConfig, McapReplayConfig, ReplaySession, + TrackerVendor, + VendorConfig, ) __all__ = [ @@ -15,4 +17,6 @@ "McapRecordingConfig", "McapReplayConfig", "ReplaySession", + "TrackerVendor", + "VendorConfig", ] diff --git a/src/core/deviceio_session/python/session_bindings.cpp b/src/core/deviceio_session/python/session_bindings.cpp index a198c6b3f..0c5c55873 100644 --- a/src/core/deviceio_session/python/session_bindings.cpp +++ b/src/core/deviceio_session/python/session_bindings.cpp @@ -1,12 +1,14 @@ // SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. // SPDX-License-Identifier: Apache-2.0 +#include #include #include #include #include #include +#include #include #include #include @@ -86,6 +88,49 @@ PYBIND11_MODULE(_deviceio_session, m) }, "Return the list of (tracker, channel_name) pairs."); + // ---- TrackerVendor / VendorConfig (live vendor selection) ---- + py::class_(m, "TrackerVendor", + "Per-tracker vendor selection: a string id (e.g. \"body.pico-xr\") plus " + "free-form vendor params. Pass inside VendorConfig to DeviceIOSession.run().") + .def(py::init( + [](std::string id, std::map params) + { + core::TrackerVendor vendor; + vendor.id = std::move(id); + vendor.params = std::move(params); + return vendor; + }), + py::arg("id"), py::arg("params") = std::map{}) + .def_readwrite("id", &core::TrackerVendor::id) + .def_readwrite("params", &core::TrackerVendor::params); + + py::class_(m, "VendorConfig", + "Per-session vendor selection for vendored trackers (live sessions only). " + "Trackers not listed use their default vendor id.") + .def(py::init( + [](const std::vector, core::TrackerVendor>>& tracker_vendors) + { + core::VendorConfig config; + for (const auto& [tracker, vendor] : tracker_vendors) + { + config.tracker_vendors.emplace_back(tracker.get(), vendor); + } + return config; + }), + py::arg("tracker_vendors") = std::vector, core::TrackerVendor>>{}) + .def( + "get_tracker_vendors", + [](const core::VendorConfig& c) + { + py::list result; + for (const auto& [tracker, vendor] : c.tracker_vendors) + { + result.append(py::make_tuple(py::cast(tracker), vendor)); + } + return result; + }, + "Return the list of (tracker, TrackerVendor) pairs."); + // ---- DeviceIOSession (live) ---- py::class_>( m, "DeviceIOSession") @@ -95,12 +140,13 @@ PYBIND11_MODULE(_deviceio_session, m) .def("__enter__", &core::PyDeviceIOSession::enter) .def("__exit__", &core::PyDeviceIOSession::exit) .def_static("get_required_extensions", &core::DeviceIOSession::get_required_extensions, py::arg("trackers"), + py::arg("vendor_config") = core::VendorConfig{}, "Aggregate OpenXR extensions required for a live session with these tracker types " - "(not a per-tracker instance method)") + "(not a per-tracker instance method). Pass a VendorConfig to resolve vendored trackers.") .def_static( "run", [](const std::vector>& trackers, const core::OpenXRSessionHandles& handles, - std::optional recording_config) + std::optional recording_config, core::VendorConfig vendor_config) { if (handles.instance == XR_NULL_HANDLE || handles.session == XR_NULL_HANDLE || handles.space == XR_NULL_HANDLE || handles.xrGetInstanceProcAddr == nullptr) @@ -109,12 +155,15 @@ 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, std::move(recording_config)); + auto session = + core::DeviceIOSession::run(trackers, handles, std::move(recording_config), std::move(vendor_config)); return std::make_unique(std::move(session)); }, py::arg("trackers"), py::arg("handles"), py::arg("recording_config") = py::none(), + py::arg("vendor_config") = core::VendorConfig{}, "Create and initialize a session with trackers. " - "Pass a McapRecordingConfig to enable MCAP recording."); + "Pass a McapRecordingConfig to enable MCAP recording, and a VendorConfig to select " + "vendors for any vendored trackers."); // ---- ReplaySession ---- py::class_>(m, "ReplaySession") diff --git a/src/core/deviceio_trackers/cpp/CMakeLists.txt b/src/core/deviceio_trackers/cpp/CMakeLists.txt index 629b9efd0..1b432d7e9 100644 --- a/src/core/deviceio_trackers/cpp/CMakeLists.txt +++ b/src/core/deviceio_trackers/cpp/CMakeLists.txt @@ -16,12 +16,12 @@ add_library(deviceio_trackers STATIC joint_state_tracker.cpp se3_tracker.cpp frame_metadata_tracker_oak.cpp - full_body_tracker_pico.cpp + full_body_tracker.cpp inc/deviceio_trackers/head_tracker.hpp inc/deviceio_trackers/hand_tracker.hpp inc/deviceio_trackers/controller_tracker.hpp inc/deviceio_trackers/message_channel_tracker.hpp - inc/deviceio_trackers/full_body_tracker_pico.hpp + inc/deviceio_trackers/full_body_tracker.hpp inc/deviceio_trackers/generic_3axis_pedal_tracker.hpp inc/deviceio_trackers/oglo_tactile_tracker.hpp inc/deviceio_trackers/tensor_push_tracker.hpp diff --git a/src/core/deviceio_trackers/cpp/full_body_tracker.cpp b/src/core/deviceio_trackers/cpp/full_body_tracker.cpp new file mode 100644 index 000000000..30c59108f --- /dev/null +++ b/src/core/deviceio_trackers/cpp/full_body_tracker.cpp @@ -0,0 +1,14 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/deviceio_trackers/full_body_tracker.hpp" + +namespace core +{ + +const FullBodyPoseTrackedT& FullBodyTracker::get_body_pose(const ITrackerSession& session) const +{ + return static_cast(session.get_tracker_impl(*this)).get_body_pose(); +} + +} // namespace core diff --git a/src/core/deviceio_trackers/cpp/full_body_tracker_pico.cpp b/src/core/deviceio_trackers/cpp/full_body_tracker_pico.cpp deleted file mode 100644 index aed4029d4..000000000 --- a/src/core/deviceio_trackers/cpp/full_body_tracker_pico.cpp +++ /dev/null @@ -1,18 +0,0 @@ -// SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// SPDX-License-Identifier: Apache-2.0 - -#include "inc/deviceio_trackers/full_body_tracker_pico.hpp" - -namespace core -{ - -// ============================================================================ -// FullBodyTrackerPico Public Interface -// ============================================================================ - -const FullBodyPosePicoTrackedT& FullBodyTrackerPico::get_body_pose(const ITrackerSession& session) const -{ - return static_cast(session.get_tracker_impl(*this)).get_body_pose(); -} - -} // namespace core diff --git a/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/full_body_tracker.hpp b/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/full_body_tracker.hpp new file mode 100644 index 000000000..f1d06e746 --- /dev/null +++ b/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/full_body_tracker.hpp @@ -0,0 +1,52 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include + +#include +#include + +namespace core +{ + +// Vendor-agnostic full body tracker marker. Tracks 24 body joints (indices +// 0-23) from pelvis to hands (XR_BD_body_tracking layout). +// +// The tracker carries no vendor or backend state: a live session selects the +// vendor (native XR hardware, an external pushed-tensor plugin, ...) via +// VendorConfig, and replay reads the recorded channel regardless of vendor. +// When no vendor is specified for this tracker, the live factory uses its +// default vendor. +class FullBodyTracker : public ITracker +{ +public: + //! Number of joints in XR_BD_body_tracking (0-23). + static constexpr uint32_t JOINT_COUNT = 24; + + std::string_view get_name() const override + { + return TRACKER_NAME; + } + + // Query method: + // - tracked.data is null when the body tracker is inactive. + // - when tracked.data is non-null, nested fields in FullBodyPoseT are safe to read. + const FullBodyPoseTrackedT& get_body_pose(const ITrackerSession& session) const; + +private: + static constexpr const char* TRACKER_NAME = "FullBodyTracker"; +}; + +// Deprecated alias for the renamed FullBodyTracker (was FullBodyTrackerPico before the +// vendor-agnostic rename). Retained so source referencing the old type name keeps +// compiling (with a deprecation warning); prefer FullBodyTracker. Note the old +// full_body_tracker_pico.hpp / full_body_tracker_pico_base.hpp headers were removed and +// there is no IFullBodyTrackerPicoImpl alias, so include-path and impl-interface users +// must update. Mirrors the Python alias and the ReplayFullBodyTrackerPicoImpl / +// FullBodyPicoRecordingTraits C++ aliases. +using FullBodyTrackerPico [[deprecated("renamed to core::FullBodyTracker")]] = FullBodyTracker; + +} // 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 deleted file mode 100644 index cad5ed95f..000000000 --- a/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/full_body_tracker_pico.hpp +++ /dev/null @@ -1,35 +0,0 @@ -// SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// SPDX-License-Identifier: Apache-2.0 - -#pragma once - -#include -#include - -#include -namespace core -{ - -// 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). - static constexpr uint32_t JOINT_COUNT = 24; - - std::string_view get_name() const override - { - return TRACKER_NAME; - } - - // Query method: - // - tracked.data is null when the body tracker is inactive. - // - when tracked.data is non-null, nested fields in FullBodyPosePicoT are safe to read. - const FullBodyPosePicoTrackedT& get_body_pose(const ITrackerSession& session) const; - -private: - static constexpr const char* TRACKER_NAME = "FullBodyTrackerPico"; -}; - -} // namespace core diff --git a/src/core/deviceio_trackers/python/deviceio_trackers_init.py b/src/core/deviceio_trackers/python/deviceio_trackers_init.py index ce5930733..2cdafa3e1 100644 --- a/src/core/deviceio_trackers/python/deviceio_trackers_init.py +++ b/src/core/deviceio_trackers/python/deviceio_trackers_init.py @@ -3,6 +3,8 @@ """Isaac Teleop DeviceIO Trackers — tracker classes for device I/O.""" +import warnings + from ._deviceio_trackers import ( ITracker, HandTracker, @@ -16,7 +18,7 @@ TensorPushTracker, JointStateTracker, Se3Tracker, - FullBodyTrackerPico, + FullBodyTracker, ITrackerSession, NUM_JOINTS, JOINT_PALM, @@ -25,12 +27,30 @@ JOINT_INDEX_TIP, ) +# Deprecated aliases resolved lazily via __getattr__ so that accessing them emits a +# DeprecationWarning. Intentionally omitted from __all__ so `import *` no longer pulls +# the old names. +_DEPRECATED_ALIASES = {"FullBodyTrackerPico": "FullBodyTracker"} + + +def __getattr__(name: str): + new_name = _DEPRECATED_ALIASES.get(name) + if new_name is not None: + warnings.warn( + f"{name} is deprecated; use {new_name} instead.", + DeprecationWarning, + stacklevel=2, + ) + return globals()[new_name] + raise AttributeError(f"module {__name__!r} has no attribute {name!r}") + + __all__ = [ "ControllerTracker", "MessageChannelStatus", "MessageChannelTracker", "FrameMetadataTrackerOak", - "FullBodyTrackerPico", + "FullBodyTracker", "Generic3AxisPedalTracker", "OgloTactileTracker", "TensorPushTracker", diff --git a/src/core/deviceio_trackers/python/tracker_bindings.cpp b/src/core/deviceio_trackers/python/tracker_bindings.cpp index 792cc76d2..7d4f422fc 100644 --- a/src/core/deviceio_trackers/python/tracker_bindings.cpp +++ b/src/core/deviceio_trackers/python/tracker_bindings.cpp @@ -3,7 +3,7 @@ #include #include -#include +#include #include #include #include @@ -209,12 +209,13 @@ PYBIND11_MODULE(_deviceio_trackers, m) "Get the current SE3 tracked snapshot (data is None when no data available; gate on " "data.is_valid before consuming the pose)"); - py::class_>( - m, "FullBodyTrackerPico") - .def(py::init<>()) + py::class_>(m, "FullBodyTracker") + .def(py::init<>(), + "Construct a vendor-agnostic full body tracker marker. The live session selects the " + "vendor via VendorConfig (default: native PICO XR_BD_body_tracking); replay is vendor-neutral.") .def( "get_body_pose", - [](const core::FullBodyTrackerPico& self, const core::ITrackerSession& session) -> core::FullBodyPosePicoTrackedT + [](const core::FullBodyTracker& self, const core::ITrackerSession& session) -> core::FullBodyPoseTrackedT { return self.get_body_pose(session); }, py::arg("session"), "Get full body pose tracked state (data is None if inactive)"); 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 f1d835876..ce8e557b4 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 @@ -3,7 +3,10 @@ #pragma once +#include + #include +#include #include #include #include @@ -26,8 +29,8 @@ class FrameMetadataTrackerOak; class IFrameMetadataTrackerOakImpl; class MessageChannelTracker; class IMessageChannelTrackerImpl; -class FullBodyTrackerPico; -class IFullBodyTrackerPicoImpl; +class FullBodyTracker; +class IFullBodyTrackerImpl; class Generic3AxisPedalTracker; class IGeneric3AxisPedalTrackerImpl; class OgloTactileTracker; @@ -56,20 +59,36 @@ struct OpenXRSessionHandles; class LiveDeviceIOFactory { public: - /** Aggregate OpenXR extensions required by the given trackers for a live session. */ - static std::vector get_required_extensions(const std::vector>& trackers); - /** Create tracker impl from a tracker instance using the same dispatch table as extension discovery. */ + /** + * @brief Aggregate OpenXR extensions required by the given trackers for a live session. + * + * Each tracker resolves its required extensions through the dispatch table using the vendor + * id selected in @p tracker_vendors (or its default vendor when unlisted). + * + * @pre @p tracker_vendors is a validated vendor config (see validate_vendor_selections()). + * Passing an invalid config is undefined behavior; DeviceIOSession validates before + * calling this. + */ + static std::vector get_required_extensions( + const std::vector>& trackers, + const std::vector>& tracker_vendors = {}); + + /** Create tracker impl from a tracker instance using the same dispatch as extension discovery. */ std::unique_ptr create_tracker_impl(const ITracker& tracker); + // @pre @p tracker_vendors is a validated vendor config (see validate_vendor_selections()). + // The factory assumes validity; passing an invalid config is undefined behavior. + // DeviceIOSession validates before constructing the factory. LiveDeviceIOFactory(const OpenXRSessionHandles& handles, mcap::McapWriter* writer, - const std::vector>& tracker_names); + const std::vector>& tracker_names, + const std::vector>& tracker_vendors = {}); std::unique_ptr create_head_tracker_impl(const HeadTracker* tracker); std::unique_ptr create_hand_tracker_impl(const HandTracker* tracker); std::unique_ptr create_controller_tracker_impl(const ControllerTracker* tracker); std::unique_ptr create_message_channel_tracker_impl(const MessageChannelTracker* tracker); - std::unique_ptr create_full_body_tracker_pico_impl(const FullBodyTrackerPico* tracker); + std::unique_ptr create_full_body_tracker_pico_impl(const FullBodyTracker* tracker); std::unique_ptr create_generic_3axis_pedal_tracker_impl( const Generic3AxisPedalTracker* tracker); std::unique_ptr create_oglo_tactile_tracker_impl(const OgloTactileTracker* tracker); @@ -82,12 +101,35 @@ class LiveDeviceIOFactory const FrameMetadataTrackerOak* tracker); private: + // Per-tracker data resolved from the session config: MCAP channel base name (recording) and + // vendor selection. A tracker appears only when it has one or the other. + struct TrackerData + { + std::optional name; // MCAP channel base name; absent -> not recorded. + std::optional vendor; // vendor selection; absent -> default vendor id. + }; + bool should_record(const ITracker* tracker) const; std::string_view get_name(const ITracker* tracker) const; + const TrackerVendor* find_vendor(const ITracker* tracker) const; const OpenXRSessionHandles& handles_; mcap::McapWriter* writer_; - std::unordered_map name_map_; + std::unordered_map tracker_data_; }; +/** + * @brief Validate per-tracker vendor selections against the live vendor dispatch table. + * + * Rejects selections on tracker types that do not support vendors, unknown vendor ids, vendor + * ids that belong to a different tracker type, non-empty vendor params, and duplicate entries. + * Throws std::invalid_argument on the first violation. List-independent: the caller checks that + * each selection references a tracker it owns. + * + * This owns the dispatch-driven vendor rules; the factory assumes a config that has passed here. + * DeviceIOSession runs this (with its own tracker-list presence check) before opening any + * recording output and before constructing the factory. + */ +void validate_vendor_selections(const std::vector>& tracker_vendors); + } // 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 9e2df8671..028934ef1 100644 --- a/src/core/live_trackers/cpp/live_deviceio_factory.cpp +++ b/src/core/live_trackers/cpp/live_deviceio_factory.cpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include #include @@ -31,9 +31,13 @@ #include #include +#include #include #include #include +#include +#include +#include namespace core { @@ -41,16 +45,23 @@ namespace core namespace { +// Pure type probe for a dispatch row: true when `tracker` is dynamically a TrackerT. +// Kept separate from try_add_extensions so callers that only need the type test (e.g. +// tracker_supports_vendors) do not run -- and then discard -- the extension collection. +template +bool is_tracker_type(const ITracker& tracker) +{ + return dynamic_cast(&tracker) != nullptr; +} + template bool try_add_extensions(const ITracker& tracker, std::set& out) { - if (dynamic_cast(&tracker)) - { - for (const auto& ext : ImplT::required_extensions()) - out.insert(ext); - return true; - } - return false; + if (!is_tracker_type(tracker)) + return false; + for (const auto& ext : ImplT::required_extensions()) + out.insert(ext); + return true; } std::unique_ptr try_create_head_impl(LiveDeviceIOFactory& factory, const ITracker& tracker) @@ -79,7 +90,7 @@ std::unique_ptr try_create_message_channel_impl(LiveDeviceIOFactor std::unique_ptr try_create_full_body_pico_impl(LiveDeviceIOFactory& factory, const ITracker& tracker) { - auto* typed = dynamic_cast(&tracker); + auto* typed = dynamic_cast(&tracker); return typed ? factory.create_full_body_tracker_pico_impl(typed) : nullptr; } @@ -126,37 +137,197 @@ std::unique_ptr try_create_oglo_impl(LiveDeviceIOFactory& factory, } using CollectExtensionsFn = bool (*)(const ITracker&, std::set&); +using TypeMatchesFn = bool (*)(const ITracker&); using TryCreateFn = std::unique_ptr (*)(LiveDeviceIOFactory&, const ITracker&); struct TrackerDispatchEntry { CollectExtensionsFn collect_extensions; + // Pure type probe for this row's tracker type (see is_tracker_type); no side effects. + TypeMatchesFn matches; TryCreateFn try_create; + // Vendor routing (last so single-vendor rows can omit it): a default-initialized row is a + // type's sole vendor; multi-vendor types set vendor_id per row and list the default vendor + // first (see k_tracker_dispatch). + std::string_view vendor_id = {}; }; -// Shared tracker dispatch table for both extension collection and impl creation. +// Build a dispatch row for a (tracker type, impl) pair, wiring up its extension +// collector, type probe, and impl builder. vendor_id defaults to the non-vendored +// sentinel; pass it to declare a vendored row. +template +constexpr TrackerDispatchEntry make_dispatch_entry(TryCreateFn try_create, std::string_view vendor_id = {}) +{ + return TrackerDispatchEntry{ &try_add_extensions, &is_tracker_type, try_create, vendor_id }; +} + +// One row per (tracker type, vendor). A tracker type may have several vendor rows; the first row +// for a type is the one chosen when no vendor is selected. Extension discovery and impl creation +// both scan this table in order: keep rows whose vendor id matches the selection (or, when +// unselected, every row and let the type-checked thunk pick the first match), then the row's +// type-checked thunk builds the concrete impl. inline const TrackerDispatchEntry k_tracker_dispatch[] = { - { &try_add_extensions, &try_create_head_impl }, - { &try_add_extensions, &try_create_hand_impl }, - { &try_add_extensions, &try_create_controller_impl }, - { &try_add_extensions, &try_create_message_channel_impl }, - { &try_add_extensions, &try_create_full_body_pico_impl }, - { &try_add_extensions, &try_create_generic_pedal_impl }, - { &try_add_extensions, &try_create_tensor_push_impl }, - { &try_add_extensions, - &try_create_haptic_command_reader_impl }, - { &try_add_extensions, &try_create_joint_state_impl }, - { &try_add_extensions, &try_create_se3_tracker_impl }, - { &try_add_extensions, &try_create_oak_impl }, - { &try_add_extensions, &try_create_oglo_impl }, + make_dispatch_entry(&try_create_head_impl), + make_dispatch_entry(&try_create_hand_impl), + make_dispatch_entry(&try_create_controller_impl), + make_dispatch_entry(&try_create_message_channel_impl), + make_dispatch_entry(&try_create_full_body_pico_impl, "body.pico-xr"), + make_dispatch_entry(&try_create_generic_pedal_impl), + make_dispatch_entry(&try_create_tensor_push_impl), + make_dispatch_entry( + &try_create_haptic_command_reader_impl), + make_dispatch_entry(&try_create_joint_state_impl), + make_dispatch_entry(&try_create_se3_tracker_impl), + make_dispatch_entry(&try_create_oak_impl), + make_dispatch_entry(&try_create_oglo_impl), }; +// Find a tracker's vendor selection in the config, or nullptr when unlisted. +const TrackerVendor* find_tracker_vendor(const std::vector>& tracker_vendors, + const ITracker* tracker) +{ + for (const auto& [ptr, vendor] : tracker_vendors) + { + if (ptr == tracker) + return &vendor; + } + return nullptr; +} + +// True when a dispatch row may be selected for a tracker. With a vendor selected, only its +// row matches. With none, every row is a candidate: the scanning loop breaks on the first +// type match, so the first row for the tracker's type -- its default vendor -- wins. +bool row_selected(const TrackerDispatchEntry& row, const TrackerVendor* selected) +{ + return selected ? (row.vendor_id == selected->id) : true; +} + +// No dispatch row produced an impl for a tracker (and its selected vendor); report why. +[[noreturn]] void throw_unresolved_tracker(const char* context, const ITracker& tracker, const TrackerVendor* selected) +{ + if (selected) + { + throw std::invalid_argument(std::string(context) + ": no live vendor '" + selected->id + "' for tracker '" + + std::string(tracker.get_name()) + "'"); + } + throw std::invalid_argument(std::string(context) + ": unsupported tracker type '" + + std::string(tracker.get_name()) + "'"); +} + +// True when a dispatch row offers this vendor id, i.e. it names a live vendor a tracker can select. +bool dispatch_has_vendor(std::string_view vendor_id) +{ + // An empty id is the non-vendored-row sentinel (vendor_id = {}), never a selectable + // vendor. Reject it here so an empty TrackerVendor id is reported up front as an + // unknown vendor id instead of matching those sentinel rows and failing later. + if (vendor_id.empty()) + return false; + for (const auto& row : k_tracker_dispatch) + { + if (row.vendor_id == vendor_id) + return true; + } + return false; +} + +// True when a tracker's type has at least one vendored dispatch row (a row with a +// non-empty vendor id). Derived from the table, not a hardcoded type: adding a +// vendored row for a new tracker type makes that type vendor-selectable here with +// no other change. Each row's `matches` predicate is the type probe (a pure +// dynamic_cast, no side effects). Null is treated as unsupported. +bool tracker_supports_vendors(const ITracker* tracker) +{ + if (!tracker) + return false; + for (const auto& row : k_tracker_dispatch) + { + if (!row.vendor_id.empty() && row.matches(*tracker)) + return true; + } + return false; +} + +// True when a dispatch row offers `vendor_id` for this tracker's own type, i.e. the +// selection is valid for this specific tracker rather than merely for some other +// vendored type. Scoping the id to the tracker's type is what makes a cross-type +// pairing (a valid id belonging to a different type) fail here, with a precise +// error, instead of later during impl creation. Null tracker or empty id are never +// accepted. +bool tracker_accepts_vendor(const ITracker* tracker, std::string_view vendor_id) +{ + if (!tracker || vendor_id.empty()) + return false; + for (const auto& row : k_tracker_dispatch) + { + if (row.vendor_id == vendor_id && row.matches(*tracker)) + return true; + } + return false; +} + +// Identify a tracker in an error message by its name, tolerating a null pointer +// (validation can run before the tracker list is known to hold no nulls). +std::string tracker_name_for_error(const ITracker* tracker) +{ + return tracker ? std::string(tracker->get_name()) : std::string(""); +} + } // namespace -std::vector LiveDeviceIOFactory::get_required_extensions(const std::vector>& trackers) +// Defined in this translation unit because it consults the private vendor dispatch table +// (k_tracker_dispatch above); the anonymous-namespace helpers it calls stay visible here. See the +// header for the full contract. +void validate_vendor_selections(const std::vector>& tracker_vendors) +{ + std::unordered_set seen; + for (const auto& [tracker, vendor] : tracker_vendors) + { + // Only vendored tracker types accept a vendor selection; reject any other so a + // misassigned (silently-ignored) selection surfaces as an error. + if (!tracker_supports_vendors(tracker)) + { + throw std::invalid_argument("LiveDeviceIOFactory: vendor selection '" + vendor.id + + "' provided for tracker '" + tracker_name_for_error(tracker) + + "', whose type does not support vendors"); + } + // Reject unknown vendor ids up front rather than when the impl is built. + if (!dispatch_has_vendor(vendor.id)) + { + throw std::invalid_argument("LiveDeviceIOFactory: unknown vendor id '" + vendor.id + "' for tracker '" + + tracker_name_for_error(tracker) + "'"); + } + // The id names a real vendor, but reject it unless it belongs to this + // tracker's own type so a cross-type pairing fails here instead of later. + if (!tracker_accepts_vendor(tracker, vendor.id)) + { + throw std::invalid_argument("LiveDeviceIOFactory: vendor id '" + vendor.id + + "' is not available for tracker '" + tracker_name_for_error(tracker) + "'"); + } + // No impl consumes TrackerVendor::params yet, so a non-empty map would be + // silently dropped. Reject it to keep the contract strict; accepting params + // once a vendor reads them is an additive, backward-compatible change. + if (!vendor.params.empty()) + { + throw std::invalid_argument("LiveDeviceIOFactory: vendor params are not supported yet for tracker '" + + tracker_name_for_error(tracker) + "' (vendor id '" + vendor.id + "')"); + } + + if (!seen.insert(tracker).second) + { + throw std::invalid_argument("LiveDeviceIOFactory: duplicate vendor selection for tracker '" + + tracker_name_for_error(tracker) + "' (vendor id '" + vendor.id + "')"); + } + } +} + +std::vector LiveDeviceIOFactory::get_required_extensions( + const std::vector>& trackers, + const std::vector>& tracker_vendors) { std::set all; + // Precondition: DeviceIOSession has validated tracker_vendors (see @pre); this only resolves. + // DeviceIOSession always owns an XrTimeConverter; match session requirements even with zero trackers. for (const auto& ext : XrTimeConverter::get_required_extensions()) all.insert(ext); @@ -166,9 +337,13 @@ std::vector LiveDeviceIOFactory::get_required_extensions(const std: if (!tracker) throw std::invalid_argument("LiveDeviceIOFactory: null tracker in trackers list"); + const TrackerVendor* selected = find_tracker_vendor(tracker_vendors, tracker.get()); + bool matched = false; for (const auto& dispatch : k_tracker_dispatch) { + if (!row_selected(dispatch, selected)) + continue; if (dispatch.collect_extensions(*tracker, all)) { matched = true; @@ -177,10 +352,7 @@ std::vector LiveDeviceIOFactory::get_required_extensions(const std: } if (!matched) - { - throw std::invalid_argument("LiveDeviceIOFactory::get_required_extensions: unsupported tracker type '" + - std::string(tracker->get_name()) + "'"); - } + throw_unresolved_tracker("LiveDeviceIOFactory::get_required_extensions", *tracker, selected); } return { all.begin(), all.end() }; @@ -188,43 +360,63 @@ std::vector LiveDeviceIOFactory::get_required_extensions(const std: LiveDeviceIOFactory::LiveDeviceIOFactory(const OpenXRSessionHandles& handles, mcap::McapWriter* writer, - const std::vector>& tracker_names) + const std::vector>& tracker_names, + const std::vector>& tracker_vendors) : handles_(handles), writer_(writer) { + // Precondition: DeviceIOSession has validated tracker_vendors (see the ctor @pre); assume valid. + for (const auto& [tracker, name] : tracker_names) { - auto [it, inserted] = name_map_.emplace(tracker, name); - if (!inserted) + TrackerData& data = tracker_data_[tracker]; + if (data.name.has_value()) { throw std::invalid_argument("LiveDeviceIOFactory: duplicate tracker pointer for channel name '" + name + - "' (already mapped as '" + it->second + "')"); + "' (already mapped as '" + *data.name + "')"); } + data.name = name; + } + + for (const auto& [tracker, vendor] : tracker_vendors) + { + tracker_data_[tracker].vendor = vendor; } } std::unique_ptr LiveDeviceIOFactory::create_tracker_impl(const ITracker& tracker) { + const TrackerVendor* selected = find_vendor(&tracker); + for (const auto& dispatch : k_tracker_dispatch) { + if (!row_selected(dispatch, selected)) + continue; if (std::unique_ptr impl = dispatch.try_create(*this, tracker)) { return impl; } } - throw std::invalid_argument("LiveDeviceIOFactory::create_tracker_impl: unsupported tracker type '" + - std::string(tracker.get_name()) + "'"); + throw_unresolved_tracker("LiveDeviceIOFactory::create_tracker_impl", tracker, selected); } bool LiveDeviceIOFactory::should_record(const ITracker* tracker) const { - return writer_ && name_map_.count(tracker); + auto it = tracker_data_.find(tracker); + return writer_ && it != tracker_data_.end() && it->second.name.has_value(); } 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; + auto it = tracker_data_.find(tracker); + assert(it != tracker_data_.end() && it->second.name.has_value() && + "get_name called for tracker without a channel name (call should_record first)"); + return *it->second.name; +} + +const TrackerVendor* LiveDeviceIOFactory::find_vendor(const ITracker* tracker) const +{ + auto it = tracker_data_.find(tracker); + return (it != tracker_data_.end() && it->second.vendor) ? &*it->second.vendor : nullptr; } std::unique_ptr LiveDeviceIOFactory::create_head_tracker_impl(const HeadTracker* tracker) @@ -268,8 +460,7 @@ std::unique_ptr LiveDeviceIOFactory::create_message_ return std::make_unique(handles_, tracker, std::move(channels)); } -std::unique_ptr LiveDeviceIOFactory::create_full_body_tracker_pico_impl( - const FullBodyTrackerPico* tracker) +std::unique_ptr LiveDeviceIOFactory::create_full_body_tracker_pico_impl(const FullBodyTracker* tracker) { std::unique_ptr channels; if (should_record(tracker)) 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 722f94d6b..3e28b7a55 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 @@ -23,9 +23,9 @@ std::unique_ptr LiveFullBodyTrackerPicoImpl::create_mcap_c std::string_view base_name) { return std::make_unique( - writer, base_name, FullBodyPicoRecordingTraits::schema_name, - std::vector(FullBodyPicoRecordingTraits::recording_channels.begin(), - FullBodyPicoRecordingTraits::recording_channels.end())); + writer, base_name, FullBodyRecordingTraits::schema_name, + std::vector( + FullBodyRecordingTraits::recording_channels.begin(), FullBodyRecordingTraits::recording_channels.end())); } LiveFullBodyTrackerPicoImpl::LiveFullBodyTrackerPicoImpl(const OpenXRSessionHandles& handles, @@ -58,7 +58,7 @@ LiveFullBodyTrackerPicoImpl::LiveFullBodyTrackerPicoImpl(const OpenXRSessionHand } if (!body_tracking_props.supportsBodyTracking) { - std::cerr << "[FullBodyTrackerPico] Body tracking not supported by this system, running in limp mode" + std::cerr << "[FullBodyTracker] Body tracking not supported by this system, running in limp mode" << std::endl; return; } @@ -85,7 +85,7 @@ LiveFullBodyTrackerPicoImpl::LiveFullBodyTrackerPicoImpl(const OpenXRSessionHand throw std::runtime_error("Failed to create body tracker: " + std::to_string(result)); } - std::cout << "FullBodyTrackerPico initialized (24 joints)" << std::endl; + std::cout << "FullBodyTracker initialized (24 joints)" << std::endl; } LiveFullBodyTrackerPicoImpl::~LiveFullBodyTrackerPicoImpl() @@ -127,19 +127,19 @@ void LiveFullBodyTrackerPicoImpl::update(int64_t monotonic_time_ns) if (XR_FAILED(result)) { tracked_.data.reset(); - throw std::runtime_error("[FullBodyTrackerPico] xrLocateBodyJointsBD failed: " + std::to_string(result)); + throw std::runtime_error("[FullBodyTracker] xrLocateBodyJointsBD failed: " + std::to_string(result)); } if (!tracked_.data) { - tracked_.data = std::make_shared(); + tracked_.data = std::make_shared(); } tracked_.data->all_joint_poses_tracked = locations.allJointPosesTracked; if (!tracked_.data->joints) { - tracked_.data->joints = std::make_shared(); + tracked_.data->joints = std::make_shared(); } for (uint32_t i = 0; i < XR_BODY_JOINT_COUNT_BD; ++i) @@ -165,7 +165,7 @@ void LiveFullBodyTrackerPicoImpl::update(int64_t monotonic_time_ns) } } -const FullBodyPosePicoTrackedT& LiveFullBodyTrackerPicoImpl::get_body_pose() const +const FullBodyPoseTrackedT& LiveFullBodyTrackerPicoImpl::get_body_pose() const { return tracked_; } 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 181d6b803..7c11b5d72 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 @@ -1,9 +1,9 @@ -// SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. // SPDX-License-Identifier: Apache-2.0 #pragma once -#include +#include #include #include #include @@ -19,11 +19,14 @@ namespace core { -using FullBodyMcapChannels = McapTrackerChannels; +using FullBodyMcapChannels = McapTrackerChannels; +// Live full-body impl for the "body.pico-xr" vendor: sources joints directly from +// the native PICO XR_BD_body_tracking extension. +// // 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 IFullBodyTrackerPicoImpl +class LiveFullBodyTrackerPicoImpl : public IFullBodyTrackerImpl { public: static std::vector required_extensions() @@ -42,13 +45,13 @@ class LiveFullBodyTrackerPicoImpl : public IFullBodyTrackerPicoImpl LiveFullBodyTrackerPicoImpl& operator=(LiveFullBodyTrackerPicoImpl&&) = delete; void update(int64_t monotonic_time_ns) override; - const FullBodyPosePicoTrackedT& get_body_pose() const override; + const FullBodyPoseTrackedT& get_body_pose() const override; private: XrTimeConverter time_converter_; XrSpace base_space_; XrBodyTrackerBD body_tracker_; - FullBodyPosePicoTrackedT tracked_; + FullBodyPoseTrackedT tracked_; int64_t last_update_time_ = 0; PFN_xrCreateBodyTrackerBD pfn_create_body_tracker_; diff --git a/src/core/live_trackers_tests/cpp/CMakeLists.txt b/src/core/live_trackers_tests/cpp/CMakeLists.txt new file mode 100644 index 000000000..d005b22fd --- /dev/null +++ b/src/core/live_trackers_tests/cpp/CMakeLists.txt @@ -0,0 +1,21 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.20) + +add_executable(live_trackers_tests + test_vendor_validation.cpp +) + +target_link_libraries(live_trackers_tests PRIVATE + deviceio::live_trackers + deviceio::deviceio_trackers + # Provides the MCAP_IMPLEMENTATION symbols (mcap::McapWriter::*) that + # live_trackers' McapTrackerChannels instantiations reference; MCAP is + # compiled once inside deviceio_session (see src/core/mcap/cpp/CMakeLists.txt). + deviceio::deviceio_session + Catch2::Catch2WithMain +) + +message(STATUS "live_trackers_tests target enabled with Catch2") +catch_discover_tests(live_trackers_tests) diff --git a/src/core/live_trackers_tests/cpp/test_vendor_validation.cpp b/src/core/live_trackers_tests/cpp/test_vendor_validation.cpp new file mode 100644 index 000000000..c843dc56b --- /dev/null +++ b/src/core/live_trackers_tests/cpp/test_vendor_validation.cpp @@ -0,0 +1,163 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Unit tests for vendor-selection validation. Validation is owned by DeviceIOSession +// (the single gatekeeper): it runs the tracker-list presence check itself and delegates +// the dispatch-driven vendor-validity checks to core::validate_vendor_selections(). The +// live factory assumes a validated config. +// +// Most outcomes are exercised through DeviceIOSession::get_required_extensions(), the +// public entry point plugins call; it validates the vendor config before resolving +// extensions and needs no OpenXR handles. +// +// Outcomes covered: +// 1. accepted - a vendored tracker with a known vendor id (and the default, +// no-selection, path) resolves and returns extensions. +// 2. rejected - a vendor selection on a non-vendored tracker type. +// 3. rejected - an unknown vendor id. +// 4. rejected - non-empty vendor params (no consumer reads them yet). +// 5. rejected - a duplicate selection for the same tracker. +// 6. rejected - a selection referencing a tracker absent from the list. +// Plus a direct unit test of the list-independent primitive +// core::validate_vendor_selections(). + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +using Catch::Matchers::ContainsSubstring; + +namespace +{ + +using TrackerList = std::vector>; +using VendorList = std::vector>; + +// Runs DeviceIOSession::get_required_extensions and returns the thrown +// std::invalid_argument message, or an empty string when it does not throw. +std::string vendor_validation_error(const TrackerList& trackers, const VendorList& vendors) +{ + try + { + core::DeviceIOSession::get_required_extensions(trackers, core::VendorConfig{ vendors }); + return {}; + } + catch (const std::invalid_argument& e) + { + return e.what(); + } +} + +bool contains(const std::vector& haystack, const std::string& needle) +{ + return std::find(haystack.begin(), haystack.end(), needle) != haystack.end(); +} + +} // namespace + +TEST_CASE("vendor validation: accepted configurations resolve extensions", "[live_trackers][vendor]") +{ + auto body = std::make_shared(); + TrackerList trackers{ body }; + + SECTION("no vendor config falls back to the default vendor") + { + // Empty vendor config -> default vendor (body.pico-xr) is selected. + const auto extensions = core::DeviceIOSession::get_required_extensions(trackers); + REQUIRE(contains(extensions, "XR_BD_body_tracking")); + } + + SECTION("explicitly naming the default vendor is accepted and resolves the same way") + { + VendorList vendors{ { body.get(), core::TrackerVendor{ "body.pico-xr" } } }; + std::vector extensions; + REQUIRE_NOTHROW(extensions = + core::DeviceIOSession::get_required_extensions(trackers, core::VendorConfig{ vendors })); + REQUIRE(contains(extensions, "XR_BD_body_tracking")); + } +} + +TEST_CASE("vendor validation: invalid configurations are rejected", "[live_trackers][vendor]") +{ + auto body = std::make_shared(); + auto head = std::make_shared(); + TrackerList trackers{ body, head }; + + SECTION("a vendor selection on a non-vendored tracker type is rejected") + { + VendorList vendors{ { head.get(), core::TrackerVendor{ "body.pico-xr" } } }; + REQUIRE_THAT(vendor_validation_error(trackers, vendors), ContainsSubstring("does not support vendors")); + } + + SECTION("an unknown vendor id is rejected") + { + VendorList vendors{ { body.get(), core::TrackerVendor{ "body.does-not-exist" } } }; + REQUIRE_THAT(vendor_validation_error(trackers, vendors), ContainsSubstring("unknown vendor id")); + } + + SECTION("an empty vendor id is rejected as unknown (not silently matched to the sentinel rows)") + { + // A default-constructed / empty TrackerVendor id must not match the empty + // vendor_id sentinel carried by non-vendored dispatch rows. + VendorList vendors{ { body.get(), core::TrackerVendor{ "" } } }; + REQUIRE_THAT(vendor_validation_error(trackers, vendors), ContainsSubstring("unknown vendor id")); + } + + SECTION("a non-empty vendor params map is rejected while no vendor consumes it") + { + // params is bound and reserved, but no impl reads it yet; a non-empty map + // must be rejected rather than silently dropped. + VendorList vendors{ { body.get(), + core::TrackerVendor{ "body.pico-xr", { { "max_flatbuffer_size", "16384" } } } } }; + REQUIRE_THAT(vendor_validation_error(trackers, vendors), ContainsSubstring("params are not supported")); + } + + SECTION("a duplicate selection for the same tracker is rejected") + { + VendorList vendors{ + { body.get(), core::TrackerVendor{ "body.pico-xr" } }, + { body.get(), core::TrackerVendor{ "body.pico-xr" } }, + }; + REQUIRE_THAT(vendor_validation_error(trackers, vendors), ContainsSubstring("duplicate vendor selection")); + } + + SECTION("a selection referencing a tracker absent from the list is rejected") + { + // Presence check owned by DeviceIOSession: the stray tracker is validated + // but never added to the session's tracker list. + auto stray = std::make_shared(); + VendorList vendors{ { stray.get(), core::TrackerVendor{ "body.pico-xr" } } }; + REQUIRE_THAT(vendor_validation_error(trackers, vendors), ContainsSubstring("not in the trackers list")); + } +} + +// This primitive is list-independent: it runs the dispatch-driven checks with no tracker +// list (presence is the caller's responsibility) and is what the session delegates to. +TEST_CASE("vendor validation: validate_vendor_selections rejects an invalid selection directly", "[live_trackers][vendor]") +{ + auto body = std::make_shared(); + + SECTION("a valid selection is accepted") + { + VendorList vendors{ { body.get(), core::TrackerVendor{ "body.pico-xr" } } }; + REQUIRE_NOTHROW(core::validate_vendor_selections(vendors)); + } + + SECTION("an invalid selection is rejected") + { + VendorList vendors{ { body.get(), core::TrackerVendor{ "body.does-not-exist" } } }; + REQUIRE_THROWS_AS(core::validate_vendor_selections(vendors), std::invalid_argument); + } +} diff --git a/src/core/mcap/cpp/inc/mcap/recording_traits.hpp b/src/core/mcap/cpp/inc/mcap/recording_traits.hpp index 937dcd7b3..4e5b61b4a 100644 --- a/src/core/mcap/cpp/inc/mcap/recording_traits.hpp +++ b/src/core/mcap/cpp/inc/mcap/recording_traits.hpp @@ -38,13 +38,19 @@ struct ControllerRecordingTraits static constexpr std::array replay_channels = { "left_controller", "right_controller" }; }; -struct FullBodyPicoRecordingTraits +struct FullBodyRecordingTraits { - static constexpr std::string_view schema_name = "core.FullBodyPosePicoRecord"; + static constexpr std::string_view schema_name = "core.FullBodyPoseRecord"; static constexpr std::array recording_channels = { "full_body" }; static constexpr std::array replay_channels = { "full_body" }; }; +// Deprecated alias for the renamed FullBodyRecordingTraits (was +// FullBodyPicoRecordingTraits before the vendor-agnostic rename). Retained so source +// referencing the old type name keeps compiling (with a deprecation warning); prefer +// FullBodyRecordingTraits. +using FullBodyPicoRecordingTraits [[deprecated("renamed to core::FullBodyRecordingTraits")]] = FullBodyRecordingTraits; + struct PedalRecordingTraits { static constexpr std::string_view schema_name = "core.Generic3AxisPedalOutputRecord"; diff --git a/src/core/python/deviceio_init.py b/src/core/python/deviceio_init.py index ce2381cd1..0f9cc8c71 100644 --- a/src/core/python/deviceio_init.py +++ b/src/core/python/deviceio_init.py @@ -8,6 +8,8 @@ from isaacteleop.deviceio_session import DeviceIOSession, McapRecordingConfig """ +import warnings + from isaacteleop.deviceio_trackers import ( ITracker, HandTracker, @@ -20,7 +22,7 @@ OgloTactileTracker, TensorPushTracker, JointStateTracker, - FullBodyTrackerPico, + FullBodyTracker, NUM_JOINTS, JOINT_PALM, JOINT_WRIST, @@ -33,6 +35,8 @@ McapRecordingConfig, McapReplayConfig, ReplaySession, + TrackerVendor, + VendorConfig, ) from ..oxr import OpenXRSessionHandles @@ -68,15 +72,33 @@ "OgloTactileTracker", "TensorPushTracker", "JointStateTracker", - "FullBodyTrackerPico", + "FullBodyTracker", "OpenXRSessionHandles", "DeviceIOSession", "McapRecordingConfig", "McapReplayConfig", "ReplaySession", + "TrackerVendor", + "VendorConfig", "NUM_JOINTS", "JOINT_PALM", "JOINT_WRIST", "JOINT_THUMB_TIP", "JOINT_INDEX_TIP", ] + +# Deprecated re-exports resolved lazily so access emits a DeprecationWarning; kept out +# of __all__ and out of the eager imports above so importing this module stays quiet. +_DEPRECATED_ALIASES = {"FullBodyTrackerPico": "FullBodyTracker"} + + +def __getattr__(name: str): + new_name = _DEPRECATED_ALIASES.get(name) + if new_name is not None: + warnings.warn( + f"{name} is deprecated; use {new_name} instead.", + DeprecationWarning, + stacklevel=2, + ) + return globals()[new_name] + raise AttributeError(f"module {__name__!r} has no attribute {name!r}") diff --git a/src/core/replay_trackers/cpp/CMakeLists.txt b/src/core/replay_trackers/cpp/CMakeLists.txt index f72601a21..f87dc13d8 100644 --- a/src/core/replay_trackers/cpp/CMakeLists.txt +++ b/src/core/replay_trackers/cpp/CMakeLists.txt @@ -8,7 +8,7 @@ add_library(replay_trackers STATIC replay_hand_tracker_impl.cpp replay_head_tracker_impl.cpp replay_controller_tracker_impl.cpp - replay_full_body_tracker_pico_impl.cpp + replay_full_body_tracker_impl.cpp replay_generic_3axis_pedal_tracker_impl.cpp replay_oglo_tactile_tracker_impl.cpp replay_joint_state_tracker_impl.cpp @@ -20,7 +20,7 @@ add_library(replay_trackers STATIC replay_hand_tracker_impl.hpp replay_head_tracker_impl.hpp replay_controller_tracker_impl.hpp - replay_full_body_tracker_pico_impl.hpp + replay_full_body_tracker_impl.hpp replay_generic_3axis_pedal_tracker_impl.hpp replay_oglo_tactile_tracker_impl.hpp replay_joint_state_tracker_impl.hpp diff --git a/src/core/replay_trackers/cpp/inc/replay_trackers/replay_deviceio_factory.hpp b/src/core/replay_trackers/cpp/inc/replay_trackers/replay_deviceio_factory.hpp index 293aadf19..e231f0c8a 100644 --- a/src/core/replay_trackers/cpp/inc/replay_trackers/replay_deviceio_factory.hpp +++ b/src/core/replay_trackers/cpp/inc/replay_trackers/replay_deviceio_factory.hpp @@ -17,8 +17,8 @@ class ITracker; class ITrackerImpl; class ControllerTracker; class IControllerTrackerImpl; -class FullBodyTrackerPico; -class IFullBodyTrackerPicoImpl; +class FullBodyTracker; +class IFullBodyTrackerImpl; class Generic3AxisPedalTracker; class IGeneric3AxisPedalTrackerImpl; class OgloTactileTracker; @@ -57,7 +57,7 @@ class ReplayDeviceIOFactory std::unique_ptr create_head_tracker_impl(const HeadTracker* tracker); std::unique_ptr create_hand_tracker_impl(const HandTracker* tracker); std::unique_ptr create_controller_tracker_impl(const ControllerTracker* tracker); - std::unique_ptr create_full_body_tracker_pico_impl(const FullBodyTrackerPico* tracker); + std::unique_ptr create_full_body_tracker_impl(const FullBodyTracker* tracker); std::unique_ptr create_generic_3axis_pedal_tracker_impl( const Generic3AxisPedalTracker* tracker); std::unique_ptr create_oglo_tactile_tracker_impl(const OgloTactileTracker* tracker); diff --git a/src/core/replay_trackers/cpp/replay_deviceio_factory.cpp b/src/core/replay_trackers/cpp/replay_deviceio_factory.cpp index cbfea5bef..7a6981126 100644 --- a/src/core/replay_trackers/cpp/replay_deviceio_factory.cpp +++ b/src/core/replay_trackers/cpp/replay_deviceio_factory.cpp @@ -4,7 +4,7 @@ #include "inc/replay_trackers/replay_deviceio_factory.hpp" #include "replay_controller_tracker_impl.hpp" -#include "replay_full_body_tracker_pico_impl.hpp" +#include "replay_full_body_tracker_impl.hpp" #include "replay_generic_3axis_pedal_tracker_impl.hpp" #include "replay_hand_tracker_impl.hpp" #include "replay_haptic_command_reader_tracker_impl.hpp" @@ -16,7 +16,7 @@ #include "replay_tensor_push_tracker_impl.hpp" #include -#include +#include #include #include #include @@ -69,10 +69,10 @@ std::unique_ptr try_create_controller_impl(ReplayDeviceIOFactory& return typed ? factory.create_controller_tracker_impl(typed) : nullptr; } -std::unique_ptr try_create_full_body_pico_impl(ReplayDeviceIOFactory& factory, const ITracker& tracker) +std::unique_ptr try_create_full_body_impl(ReplayDeviceIOFactory& factory, const ITracker& tracker) { - auto* typed = dynamic_cast(&tracker); - return typed ? factory.create_full_body_tracker_pico_impl(typed) : nullptr; + auto* typed = dynamic_cast(&tracker); + return typed ? factory.create_full_body_tracker_impl(typed) : nullptr; } std::unique_ptr try_create_generic_pedal_impl(ReplayDeviceIOFactory& factory, const ITracker& tracker) @@ -124,7 +124,7 @@ inline const TryCreateFn k_tracker_dispatch[] = { &try_create_head_impl, &try_create_hand_impl, &try_create_controller_impl, - &try_create_full_body_pico_impl, + &try_create_full_body_impl, &try_create_generic_pedal_impl, &try_create_oglo_impl, &try_create_tensor_push_impl, @@ -186,10 +186,9 @@ std::unique_ptr ReplayDeviceIOFactory::create_controller return std::make_unique(open_reader(filename_), get_name(tracker)); } -std::unique_ptr ReplayDeviceIOFactory::create_full_body_tracker_pico_impl( - const FullBodyTrackerPico* tracker) +std::unique_ptr ReplayDeviceIOFactory::create_full_body_tracker_impl(const FullBodyTracker* tracker) { - return std::make_unique(open_reader(filename_), get_name(tracker)); + return std::make_unique(open_reader(filename_), get_name(tracker)); } std::unique_ptr ReplayDeviceIOFactory::create_generic_3axis_pedal_tracker_impl( diff --git a/src/core/replay_trackers/cpp/replay_full_body_tracker_pico_impl.cpp b/src/core/replay_trackers/cpp/replay_full_body_tracker_impl.cpp similarity index 54% rename from src/core/replay_trackers/cpp/replay_full_body_tracker_pico_impl.cpp rename to src/core/replay_trackers/cpp/replay_full_body_tracker_impl.cpp index aeab31a1b..5b3f7f86a 100644 --- a/src/core/replay_trackers/cpp/replay_full_body_tracker_pico_impl.cpp +++ b/src/core/replay_trackers/cpp/replay_full_body_tracker_impl.cpp @@ -1,7 +1,7 @@ // SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. // SPDX-License-Identifier: Apache-2.0 -#include "replay_full_body_tracker_pico_impl.hpp" +#include "replay_full_body_tracker_impl.hpp" #include #include @@ -15,25 +15,24 @@ namespace core { // ============================================================================ -// ReplayFullBodyTrackerPicoImpl +// ReplayFullBodyTrackerImpl // ============================================================================ -ReplayFullBodyTrackerPicoImpl::ReplayFullBodyTrackerPicoImpl(std::unique_ptr reader, - std::string_view base_name) +ReplayFullBodyTrackerImpl::ReplayFullBodyTrackerImpl(std::unique_ptr reader, std::string_view base_name) : mcap_viewers_(std::make_unique( std::move(reader), base_name, - std::vector(FullBodyPicoRecordingTraits::replay_channels.begin(), - FullBodyPicoRecordingTraits::replay_channels.end()))) + std::vector( + FullBodyRecordingTraits::replay_channels.begin(), FullBodyRecordingTraits::replay_channels.end()))) { } -const FullBodyPosePicoTrackedT& ReplayFullBodyTrackerPicoImpl::get_body_pose() const +const FullBodyPoseTrackedT& ReplayFullBodyTrackerImpl::get_body_pose() const { return tracked_; } -void ReplayFullBodyTrackerPicoImpl::update(int64_t /*monotonic_time_ns*/) +void ReplayFullBodyTrackerImpl::update(int64_t /*monotonic_time_ns*/) { auto record = mcap_viewers_->read(0); if (record) @@ -42,7 +41,7 @@ void ReplayFullBodyTrackerPicoImpl::update(int64_t /*monotonic_time_ns*/) } else { - std::cerr << "ReplayFullBodyTrackerPicoImpl: body data not found" << std::endl; + std::cerr << "ReplayFullBodyTrackerImpl: body data not found" << std::endl; tracked_.data.reset(); } } diff --git a/src/core/replay_trackers/cpp/replay_full_body_tracker_impl.hpp b/src/core/replay_trackers/cpp/replay_full_body_tracker_impl.hpp new file mode 100644 index 000000000..38cdc8251 --- /dev/null +++ b/src/core/replay_trackers/cpp/replay_full_body_tracker_impl.hpp @@ -0,0 +1,46 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include + +#include +#include +#include + +namespace core +{ + +using FullBodyMcapViewers = McapTrackerViewers; + +// Vendor-neutral replay impl: reads the recorded full-body channel regardless of +// which live vendor produced it (all vendors record core.FullBodyPoseRecord). +class ReplayFullBodyTrackerImpl : public IFullBodyTrackerImpl +{ +public: + ReplayFullBodyTrackerImpl(std::unique_ptr reader, std::string_view base_name); + + ReplayFullBodyTrackerImpl(const ReplayFullBodyTrackerImpl&) = delete; + ReplayFullBodyTrackerImpl& operator=(const ReplayFullBodyTrackerImpl&) = delete; + ReplayFullBodyTrackerImpl(ReplayFullBodyTrackerImpl&&) = delete; + ReplayFullBodyTrackerImpl& operator=(ReplayFullBodyTrackerImpl&&) = delete; + + void update(int64_t monotonic_time_ns) override; + const FullBodyPoseTrackedT& get_body_pose() const override; + +private: + FullBodyPoseTrackedT tracked_; + std::unique_ptr mcap_viewers_; +}; + +// Deprecated alias for the renamed ReplayFullBodyTrackerImpl (was +// ReplayFullBodyTrackerPicoImpl before the vendor-neutral rename). Retained so source +// referencing the old type name keeps compiling (with a deprecation warning); prefer +// ReplayFullBodyTrackerImpl. +using ReplayFullBodyTrackerPicoImpl [[deprecated("renamed to core::ReplayFullBodyTrackerImpl")]] = + ReplayFullBodyTrackerImpl; + +} // namespace core diff --git a/src/core/replay_trackers/cpp/replay_full_body_tracker_pico_impl.hpp b/src/core/replay_trackers/cpp/replay_full_body_tracker_pico_impl.hpp deleted file mode 100644 index 8dd4716f1..000000000 --- a/src/core/replay_trackers/cpp/replay_full_body_tracker_pico_impl.hpp +++ /dev/null @@ -1,37 +0,0 @@ -// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// SPDX-License-Identifier: Apache-2.0 - -#pragma once - -#include -#include -#include - -#include -#include -#include - -namespace core -{ - -using FullBodyMcapViewers = McapTrackerViewers; - -class ReplayFullBodyTrackerPicoImpl : public IFullBodyTrackerPicoImpl -{ -public: - ReplayFullBodyTrackerPicoImpl(std::unique_ptr reader, std::string_view base_name); - - ReplayFullBodyTrackerPicoImpl(const ReplayFullBodyTrackerPicoImpl&) = delete; - ReplayFullBodyTrackerPicoImpl& operator=(const ReplayFullBodyTrackerPicoImpl&) = delete; - ReplayFullBodyTrackerPicoImpl(ReplayFullBodyTrackerPicoImpl&&) = delete; - ReplayFullBodyTrackerPicoImpl& operator=(ReplayFullBodyTrackerPicoImpl&&) = delete; - - void update(int64_t monotonic_time_ns) override; - const FullBodyPosePicoTrackedT& get_body_pose() const override; - -private: - FullBodyPosePicoTrackedT tracked_; - std::unique_ptr mcap_viewers_; -}; - -} // namespace core diff --git a/src/core/retargeting_engine/python/deviceio_source_nodes/__init__.py b/src/core/retargeting_engine/python/deviceio_source_nodes/__init__.py index 7b0a0efee..b1b583e57 100644 --- a/src/core/retargeting_engine/python/deviceio_source_nodes/__init__.py +++ b/src/core/retargeting_engine/python/deviceio_source_nodes/__init__.py @@ -3,6 +3,8 @@ """DeviceIO Source Nodes - Stateless converters from DeviceIO to retargeting engine formats.""" +import warnings + from .interface import IDeviceIOSource from .sink_interface import IDeviceIOSink from .head_source import HeadSource @@ -25,13 +27,13 @@ ControllerSnapshotTrackedType, Generic3AxisPedalOutputTrackedType, JointStateOutputTrackedType, - FullBodyPosePicoTrackedType, + FullBodyPoseTrackedType, DeviceIOHeadPoseTracked, DeviceIOHandPoseTracked, DeviceIOControllerSnapshotTracked, DeviceIOGeneric3AxisPedalOutputTracked, DeviceIOJointStateOutputTracked, - DeviceIOFullBodyPosePicoTracked, + DeviceIOFullBodyPoseTracked, MessageChannelMessagesTrackedType, MessageChannelConnectionStatus, MessageChannelStatusType, @@ -60,7 +62,7 @@ "ControllerSnapshotTrackedType", "Generic3AxisPedalOutputTrackedType", "JointStateOutputTrackedType", - "FullBodyPosePicoTrackedType", + "FullBodyPoseTrackedType", "MessageChannelMessagesTrackedType", "MessageChannelConnectionStatus", "MessageChannelStatusType", @@ -69,8 +71,27 @@ "DeviceIOControllerSnapshotTracked", "DeviceIOGeneric3AxisPedalOutputTracked", "DeviceIOJointStateOutputTracked", - "DeviceIOFullBodyPosePicoTracked", + "DeviceIOFullBodyPoseTracked", "DeviceIOMessageChannelMessagesTracked", "MessageChannelMessagesTrackedGroup", "MessageChannelStatusGroup", ] + +# Deprecated re-exports resolved lazily so access emits a DeprecationWarning; kept out +# of __all__ and the eager imports above so importing this module stays quiet. +_DEPRECATED_ALIASES = { + "FullBodyPosePicoTrackedType": "FullBodyPoseTrackedType", + "DeviceIOFullBodyPosePicoTracked": "DeviceIOFullBodyPoseTracked", +} + + +def __getattr__(name: str): + new_name = _DEPRECATED_ALIASES.get(name) + if new_name is not None: + warnings.warn( + f"{name} is deprecated; use {new_name} instead.", + DeprecationWarning, + stacklevel=2, + ) + return globals()[new_name] + raise AttributeError(f"module {__name__!r} has no attribute {name!r}") diff --git a/src/core/retargeting_engine/python/deviceio_source_nodes/deviceio_tensor_types.py b/src/core/retargeting_engine/python/deviceio_source_nodes/deviceio_tensor_types.py index 89f92b5db..cb86f3bed 100644 --- a/src/core/retargeting_engine/python/deviceio_source_nodes/deviceio_tensor_types.py +++ b/src/core/retargeting_engine/python/deviceio_source_nodes/deviceio_tensor_types.py @@ -9,6 +9,7 @@ the raw flatbuffer object (or None when the tracker is inactive). """ +import warnings from enum import IntEnum from typing import Any from ..interface.tensor_type import TensorType @@ -19,7 +20,7 @@ ControllerSnapshotTrackedT, Generic3AxisPedalOutputTrackedT, JointStateOutputTrackedT, - FullBodyPosePicoTrackedT, + FullBodyPoseTrackedT, MessageChannelMessagesTrackedT, ) @@ -120,23 +121,27 @@ def validate_value(self, value: Any) -> None: ) -class FullBodyPosePicoTrackedType(TensorType): - """FullBodyPosePicoTrackedT wrapper type from DeviceIO FullBodyTrackerPico.""" +class FullBodyPoseTrackedType(TensorType): + """FullBodyPoseTrackedT wrapper type from DeviceIO FullBodyTracker. + + Vendor-agnostic: the full-body tracker produces the same FullBodyPoseTrackedT + payload regardless of the live vendor (native XR, pushed tensor, ...). + """ def __init__(self, name: str) -> None: super().__init__(name) def _check_instance_compatibility(self, other: TensorType) -> bool: - if not isinstance(other, FullBodyPosePicoTrackedType): + if not isinstance(other, FullBodyPoseTrackedType): raise TypeError( - f"Expected FullBodyPosePicoTrackedType, got {type(other).__name__}" + f"Expected FullBodyPoseTrackedType, got {type(other).__name__}" ) return True def validate_value(self, value: Any) -> None: - if not isinstance(value, FullBodyPosePicoTrackedT): + if not isinstance(value, FullBodyPoseTrackedT): raise TypeError( - f"Expected FullBodyPosePicoTrackedT for '{self.name}', got {type(value).__name__}" + f"Expected FullBodyPoseTrackedT for '{self.name}', got {type(value).__name__}" ) @@ -244,15 +249,15 @@ def DeviceIOJointStateOutputTracked() -> TensorGroupType: ) -def DeviceIOFullBodyPosePicoTracked() -> TensorGroupType: - """Tracked full body pose data from DeviceIO FullBodyTrackerPico. +def DeviceIOFullBodyPoseTracked() -> TensorGroupType: + """Tracked full body pose data from DeviceIO FullBodyTracker. Contains: - full_body_tracked: FullBodyPosePicoTrackedT wrapper (always set; .data is None when inactive) + full_body_tracked: FullBodyPoseTrackedT wrapper (always set; .data is None when inactive) """ return TensorGroupType( - "deviceio_full_body_pose_pico", - [FullBodyPosePicoTrackedType("full_body_tracked")], + "deviceio_full_body_pose", + [FullBodyPoseTrackedType("full_body_tracked")], ) @@ -278,3 +283,23 @@ def MessageChannelStatusGroup() -> TensorGroupType: "message_channel_status", [MessageChannelStatusType("status")], ) + + +# Deprecated aliases resolved lazily via __getattr__ so accessing them emits a +# DeprecationWarning. +_DEPRECATED_ALIASES = { + "FullBodyPosePicoTrackedType": "FullBodyPoseTrackedType", + "DeviceIOFullBodyPosePicoTracked": "DeviceIOFullBodyPoseTracked", +} + + +def __getattr__(name: str): + new_name = _DEPRECATED_ALIASES.get(name) + if new_name is not None: + warnings.warn( + f"{name} is deprecated; use {new_name} instead.", + DeprecationWarning, + stacklevel=2, + ) + return globals()[new_name] + raise AttributeError(f"module {__name__!r} has no attribute {name!r}") diff --git a/src/core/retargeting_engine/python/deviceio_source_nodes/full_body_source.py b/src/core/retargeting_engine/python/deviceio_source_nodes/full_body_source.py index 3a13c363a..b2d61e241 100644 --- a/src/core/retargeting_engine/python/deviceio_source_nodes/full_body_source.py +++ b/src/core/retargeting_engine/python/deviceio_source_nodes/full_body_source.py @@ -1,14 +1,14 @@ -# SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # SPDX-License-Identifier: Apache-2.0 """ Full Body Source Node - DeviceIO to Retargeting Engine converter. -Converts raw FullBodyPosePicoT flatbuffer data to standard FullBodyInput tensor format. +Converts raw FullBodyPoseT flatbuffer data to standard FullBodyInput tensor format. """ import numpy as np -from typing import Any, TYPE_CHECKING +from typing import Any, Optional, TYPE_CHECKING from .interface import IDeviceIOSource from ..interface.retargeter_core_types import ( RetargeterIO, @@ -17,20 +17,20 @@ from ..interface.tensor_group import TensorGroup from ..tensor_types import FullBodyInput, FullBodyInputIndex from ..interface.tensor_group_type import OptionalType -from ..tensor_types.standard_types import NUM_BODY_JOINTS_PICO -from .deviceio_tensor_types import DeviceIOFullBodyPosePicoTracked +from ..tensor_types.standard_types import NUM_BODY_JOINTS +from .deviceio_tensor_types import DeviceIOFullBodyPoseTracked if TYPE_CHECKING: - from isaacteleop.deviceio import ITracker - from isaacteleop.schema import FullBodyPosePicoT, FullBodyPosePicoTrackedT + from isaacteleop.deviceio import ITracker, TrackerVendor + from isaacteleop.schema import FullBodyPoseT, FullBodyPoseTrackedT class FullBodySource(IDeviceIOSource): """ - Stateless converter: DeviceIO FullBodyPosePicoT -> FullBodyInput tensors. + Stateless converter: DeviceIO FullBodyPoseT -> FullBodyInput tensors. Inputs: - - "deviceio_full_body": Raw FullBodyPosePicoT flatbuffer + - "deviceio_full_body": Raw FullBodyPoseT flatbuffer Outputs (Optional — absent when body tracking is inactive): - "full_body": OptionalTensorGroup (check ``.is_none`` before access) @@ -44,24 +44,29 @@ class FullBodySource(IDeviceIOSource): FULL_BODY = "full_body" - def __init__(self, name: str) -> None: + def __init__(self, name: str, vendor: "Optional[TrackerVendor]" = None) -> None: """Initialize stateless full body source node. - Creates a FullBodyTrackerPico instance for TeleopSession to discover and use. + Creates a FullBodyTracker instance for TeleopSession to discover and use. Args: name: Unique name for this source node + vendor: Optional ``deviceio.TrackerVendor`` selecting the backend that + sources body poses (e.g. ``TrackerVendor("body.pico-xr")``). Leave + ``None`` for the tracker's default vendor. Carried on the source so + the required OpenXR extensions and the live session both resolve it + from the pipeline directly. """ import isaacteleop.deviceio as deviceio - self._body_tracker = deviceio.FullBodyTrackerPico() - super().__init__(name) + self._body_tracker = deviceio.FullBodyTracker() + super().__init__(name, vendor=vendor) def get_tracker(self) -> "ITracker": - """Get the FullBodyTrackerPico instance. + """Get the FullBodyTracker instance. Returns: - The FullBodyTrackerPico instance for TeleopSession to initialize + The FullBodyTracker instance for TeleopSession to initialize """ return self._body_tracker @@ -73,7 +78,7 @@ def poll_tracker(self, deviceio_session: Any) -> RetargeterIO: Returns: Dict with "deviceio_full_body" TensorGroup containing raw - FullBodyPosePicoT data. + FullBodyPoseT data. """ body_pose = self._body_tracker.get_body_pose(deviceio_session) source_inputs = self.input_spec() @@ -87,7 +92,7 @@ def poll_tracker(self, deviceio_session: Any) -> RetargeterIO: def input_spec(self) -> RetargeterIOType: """Declare DeviceIO full body input.""" return { - "deviceio_full_body": DeviceIOFullBodyPosePicoTracked(), + "deviceio_full_body": DeviceIOFullBodyPoseTracked(), } def output_spec(self) -> RetargeterIOType: @@ -98,17 +103,17 @@ def output_spec(self) -> RetargeterIOType: def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: """ - Convert DeviceIO FullBodyPosePicoT to standard FullBodyInput tensors. + Convert DeviceIO FullBodyPoseT to standard FullBodyInput tensors. Calls ``set_none()`` on the output when body tracking is inactive. Args: - inputs: Dict with "deviceio_full_body" containing FullBodyPosePicoTrackedT wrapper + inputs: Dict with "deviceio_full_body" containing FullBodyPoseTrackedT wrapper outputs: Dict with "full_body" OptionalTensorGroup context: Shared ComputeContext for the current step (carries GraphTime). """ - tracked: "FullBodyPosePicoTrackedT" = inputs["deviceio_full_body"][0] - body_pose: "FullBodyPosePicoT | None" = tracked.data + tracked: "FullBodyPoseTrackedT" = inputs["deviceio_full_body"][0] + body_pose: "FullBodyPoseT | None" = tracked.data if body_pose is None: outputs["full_body"].set_none() @@ -116,12 +121,12 @@ def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> N group = outputs["full_body"] - positions = np.zeros((NUM_BODY_JOINTS_PICO, 3), dtype=np.float32) - orientations = np.zeros((NUM_BODY_JOINTS_PICO, 4), dtype=np.float32) - valid = np.zeros(NUM_BODY_JOINTS_PICO, dtype=np.uint8) + positions = np.zeros((NUM_BODY_JOINTS, 3), dtype=np.float32) + orientations = np.zeros((NUM_BODY_JOINTS, 4), dtype=np.float32) + valid = np.zeros(NUM_BODY_JOINTS, dtype=np.uint8) if body_pose.joints is not None: - for i in range(NUM_BODY_JOINTS_PICO): + for i in range(NUM_BODY_JOINTS): joint = body_pose.joints.joints(i) positions[i] = [ joint.pose.position.x, diff --git a/src/core/retargeting_engine/python/deviceio_source_nodes/interface.py b/src/core/retargeting_engine/python/deviceio_source_nodes/interface.py index a7bad5ef1..5608409d3 100644 --- a/src/core/retargeting_engine/python/deviceio_source_nodes/interface.py +++ b/src/core/retargeting_engine/python/deviceio_source_nodes/interface.py @@ -9,13 +9,13 @@ """ from abc import abstractmethod -from typing import Any, TYPE_CHECKING +from typing import Any, Optional, TYPE_CHECKING from ..interface.base_retargeter import BaseRetargeter from ..interface.retargeter_core_types import RetargeterIO if TYPE_CHECKING: - from isaacteleop.deviceio import ITracker + from isaacteleop.deviceio import ITracker, TrackerVendor class IDeviceIOSource(BaseRetargeter): @@ -40,6 +40,28 @@ class IDeviceIOSource(BaseRetargeter): 6. Keep all session management in one place """ + def __init__(self, name: str, vendor: "Optional[TrackerVendor]" = None) -> None: + """Initialize a DeviceIO source node. + + Args: + name: Name identifier for this source node. + vendor: Optional vendor selection for a vendored tracker (live mode + only). Leave ``None`` for the tracker's default vendor. The vendor + travels with the source so pipeline introspection (required OpenXR + extensions) and session construction both resolve it directly from + the pipeline, without a separate source-name-keyed config map. + """ + super().__init__(name) + self._vendor = vendor + + def get_vendor(self) -> "Optional[TrackerVendor]": + """Get this source's vendor selection, or ``None`` for the tracker default. + + Used by TeleopSession and the extension-discovery helpers to build the + DeviceIO ``VendorConfig`` for vendored trackers. + """ + return self._vendor + @abstractmethod def get_tracker(self) -> "ITracker": """Get the DeviceIO tracker associated with this source node. diff --git a/src/core/retargeting_engine/python/tensor_types/__init__.py b/src/core/retargeting_engine/python/tensor_types/__init__.py index 70b5c21ff..22989ada2 100644 --- a/src/core/retargeting_engine/python/tensor_types/__init__.py +++ b/src/core/retargeting_engine/python/tensor_types/__init__.py @@ -3,6 +3,8 @@ """Basic tensor types for the retargeting engine.""" +import warnings + from .scalar_types import FloatType, IntType, BoolType from .ndarray_types import NDArrayType, DLDeviceType, DLDataType from .standard_types import ( @@ -13,7 +15,7 @@ TransformMatrix, Generic3AxisPedalInput, NUM_HAND_JOINTS, - NUM_BODY_JOINTS_PICO, + NUM_BODY_JOINTS, RobotHandJoints, ) from .tactile_types import ( @@ -33,7 +35,7 @@ Generic3AxisPedalInputIndex, FullBodyInputIndex, HandJointIndex, - BodyJointPicoIndex, + BodyJointIndex, FingerIndex, ControllerHapticPulseField, EndEffectorForceAxis, @@ -54,7 +56,7 @@ "TransformMatrix", "Generic3AxisPedalInput", "NUM_HAND_JOINTS", - "NUM_BODY_JOINTS_PICO", + "NUM_BODY_JOINTS", "RobotHandJoints", # Tactile / haptic types "TactileVector", @@ -72,8 +74,27 @@ "Generic3AxisPedalInputIndex", "FullBodyInputIndex", "HandJointIndex", - "BodyJointPicoIndex", + "BodyJointIndex", "FingerIndex", "ControllerHapticPulseField", "EndEffectorForceAxis", ] + +# Deprecated re-exports resolved lazily so access emits a DeprecationWarning; kept out +# of __all__ and the eager imports above so importing this module stays quiet. +_DEPRECATED_ALIASES = { + "BodyJointPicoIndex": "BodyJointIndex", + "NUM_BODY_JOINTS_PICO": "NUM_BODY_JOINTS", +} + + +def __getattr__(name: str): + new_name = _DEPRECATED_ALIASES.get(name) + if new_name is not None: + warnings.warn( + f"{name} is deprecated; use {new_name} instead.", + DeprecationWarning, + stacklevel=2, + ) + return globals()[new_name] + raise AttributeError(f"module {__name__!r} has no attribute {name!r}") diff --git a/src/core/retargeting_engine/python/tensor_types/indices.py b/src/core/retargeting_engine/python/tensor_types/indices.py index 61ab7a375..513c33b49 100644 --- a/src/core/retargeting_engine/python/tensor_types/indices.py +++ b/src/core/retargeting_engine/python/tensor_types/indices.py @@ -6,12 +6,13 @@ This module provides IntEnum classes for indexing into standard tensor groups (HandInput, HeadPose, ControllerInput, Generic3AxisPedalInput, FullBodyInput) and standard joint arrays -(HandJointIndex, BodyJointPicoIndex). +(HandJointIndex, BodyJointIndex). The indices for TensorGroupTypes are generated automatically from the type definitions to ensure they always match the schema. """ +import warnings from typing import Any from enum import IntEnum from .standard_types import ( @@ -79,8 +80,8 @@ class HandJointIndex(IntEnum): LITTLE_TIP = 25 -class BodyJointPicoIndex(IntEnum): - """Indices for PICO body joints (XR_BD_body_tracking, 24 joints).""" +class BodyJointIndex(IntEnum): + """Indices for body joints (XR_BD_body_tracking layout, 24 joints).""" PELVIS = 0 LEFT_HIP = 1 @@ -108,6 +109,23 @@ class BodyJointPicoIndex(IntEnum): RIGHT_HAND = 23 +# Deprecated alias for BodyJointIndex, resolved lazily via __getattr__ so accessing +# it emits a DeprecationWarning. +_DEPRECATED_ALIASES = {"BodyJointPicoIndex": "BodyJointIndex"} + + +def __getattr__(name: str): + new_name = _DEPRECATED_ALIASES.get(name) + if new_name is not None: + warnings.warn( + f"{name} is deprecated; use {new_name} instead.", + DeprecationWarning, + stacklevel=2, + ) + return globals()[new_name] + raise AttributeError(f"module {__name__!r} has no attribute {name!r}") + + class FingerIndex(IntEnum): """Channel indices into a :func:`FingerPowerVector`, standard glove order.""" diff --git a/src/core/retargeting_engine/python/tensor_types/standard_types.py b/src/core/retargeting_engine/python/tensor_types/standard_types.py index 74d04ecf2..382dcc82b 100644 --- a/src/core/retargeting_engine/python/tensor_types/standard_types.py +++ b/src/core/retargeting_engine/python/tensor_types/standard_types.py @@ -15,7 +15,7 @@ # Constants NUM_HAND_JOINTS = 26 # XR_HAND_JOINT_COUNT_EXT from OpenXR -NUM_BODY_JOINTS_PICO = 24 # XR_BODY_JOINT_COUNT_BD from XR_BD_body_tracking +NUM_BODY_JOINTS = 24 # XR_BODY_JOINT_COUNT_BD from XR_BD_body_tracking # ============================================================================ # Hand Tracking Types @@ -211,8 +211,8 @@ def FullBodyInput() -> TensorGroupType: """ Standard TensorGroupType for full body tracking data. - Matches the FullBodyPosePico schema from full_body.fbs with 24 joints - (XR_BD_body_tracking extension for PICO devices). + Matches the FullBodyPose schema from full_body.fbs with 24 joints + (XR_BD_body_tracking extension layout). Fields: - joint_positions: (24, 3) float32 array - XYZ positions for each joint @@ -229,19 +229,19 @@ def FullBodyInput() -> TensorGroupType: [ NDArrayType( "body_joint_positions", - shape=(NUM_BODY_JOINTS_PICO, 3), + shape=(NUM_BODY_JOINTS, 3), dtype=DLDataType.FLOAT, dtype_bits=32, ), NDArrayType( "body_joint_orientations", - shape=(NUM_BODY_JOINTS_PICO, 4), + shape=(NUM_BODY_JOINTS, 4), dtype=DLDataType.FLOAT, dtype_bits=32, ), NDArrayType( "body_joint_valid", - shape=(NUM_BODY_JOINTS_PICO,), + shape=(NUM_BODY_JOINTS,), dtype=DLDataType.UINT, dtype_bits=8, ), diff --git a/src/core/schema/cpp/CMakeLists.txt b/src/core/schema/cpp/CMakeLists.txt index 1234240c5..f3fd2f308 100644 --- a/src/core/schema/cpp/CMakeLists.txt +++ b/src/core/schema/cpp/CMakeLists.txt @@ -26,6 +26,8 @@ add_dependencies(isaacteleop_schema schema_header_generation) target_include_directories(isaacteleop_schema INTERFACE $ + # Hand-written companion headers (e.g. deprecated back-compat aliases), reached as . + $ ) target_link_libraries(isaacteleop_schema INTERFACE diff --git a/src/core/schema/cpp/inc/schema/full_body_compat.hpp b/src/core/schema/cpp/inc/schema/full_body_compat.hpp new file mode 100644 index 000000000..549426055 --- /dev/null +++ b/src/core/schema/cpp/inc/schema/full_body_compat.hpp @@ -0,0 +1,61 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Deprecated back-compat aliases for the full-body schema types. +// +// The full-body FlatBuffers schema was renamed from the vendor-specific "...Pico" +// names to vendor-neutral names (FullBodyPosePico -> FullBodyPose, BodyJointPico -> +// BodyJoint, ...). Include this header to keep pre-rename identifiers compiling; new +// code should use the generic names in directly. +// +// The wire format is unchanged: the aliases resolve to the same generated types, and +// the FlatBuffer field IDs / enum values are identical, so recordings round-trip. + +#pragma once + +#include + +namespace core +{ + +// ---- Table / struct / record type aliases ------------------------------------- +using FullBodyPosePico [[deprecated("renamed to core::FullBodyPose")]] = FullBodyPose; +using FullBodyPosePicoT [[deprecated("renamed to core::FullBodyPoseT")]] = FullBodyPoseT; +using FullBodyPosePicoTracked [[deprecated("renamed to core::FullBodyPoseTracked")]] = FullBodyPoseTracked; +using FullBodyPosePicoTrackedT [[deprecated("renamed to core::FullBodyPoseTrackedT")]] = FullBodyPoseTrackedT; +using FullBodyPosePicoRecord [[deprecated("renamed to core::FullBodyPoseRecord")]] = FullBodyPoseRecord; +using FullBodyPosePicoRecordT [[deprecated("renamed to core::FullBodyPoseRecordT")]] = FullBodyPoseRecordT; +using BodyJointsPico [[deprecated("renamed to core::BodyJoints")]] = BodyJoints; + +// ---- Enum type + enumerator aliases ------------------------------------------- +// BodyJointPico -> BodyJoint; the BodyJointPico_* enumerators below keep identical +// numeric values. Each is [[deprecated]] in favor of the matching BodyJoint_*. +using BodyJointPico [[deprecated("renamed to core::BodyJoint")]] = BodyJoint; + +[[deprecated]] inline constexpr BodyJoint BodyJointPico_PELVIS = BodyJoint_PELVIS; +[[deprecated]] inline constexpr BodyJoint BodyJointPico_LEFT_HIP = BodyJoint_LEFT_HIP; +[[deprecated]] inline constexpr BodyJoint BodyJointPico_RIGHT_HIP = BodyJoint_RIGHT_HIP; +[[deprecated]] inline constexpr BodyJoint BodyJointPico_SPINE1 = BodyJoint_SPINE1; +[[deprecated]] inline constexpr BodyJoint BodyJointPico_LEFT_KNEE = BodyJoint_LEFT_KNEE; +[[deprecated]] inline constexpr BodyJoint BodyJointPico_RIGHT_KNEE = BodyJoint_RIGHT_KNEE; +[[deprecated]] inline constexpr BodyJoint BodyJointPico_SPINE2 = BodyJoint_SPINE2; +[[deprecated]] inline constexpr BodyJoint BodyJointPico_LEFT_ANKLE = BodyJoint_LEFT_ANKLE; +[[deprecated]] inline constexpr BodyJoint BodyJointPico_RIGHT_ANKLE = BodyJoint_RIGHT_ANKLE; +[[deprecated]] inline constexpr BodyJoint BodyJointPico_SPINE3 = BodyJoint_SPINE3; +[[deprecated]] inline constexpr BodyJoint BodyJointPico_LEFT_FOOT = BodyJoint_LEFT_FOOT; +[[deprecated]] inline constexpr BodyJoint BodyJointPico_RIGHT_FOOT = BodyJoint_RIGHT_FOOT; +[[deprecated]] inline constexpr BodyJoint BodyJointPico_NECK = BodyJoint_NECK; +[[deprecated]] inline constexpr BodyJoint BodyJointPico_LEFT_COLLAR = BodyJoint_LEFT_COLLAR; +[[deprecated]] inline constexpr BodyJoint BodyJointPico_RIGHT_COLLAR = BodyJoint_RIGHT_COLLAR; +[[deprecated]] inline constexpr BodyJoint BodyJointPico_HEAD = BodyJoint_HEAD; +[[deprecated]] inline constexpr BodyJoint BodyJointPico_LEFT_SHOULDER = BodyJoint_LEFT_SHOULDER; +[[deprecated]] inline constexpr BodyJoint BodyJointPico_RIGHT_SHOULDER = BodyJoint_RIGHT_SHOULDER; +[[deprecated]] inline constexpr BodyJoint BodyJointPico_LEFT_ELBOW = BodyJoint_LEFT_ELBOW; +[[deprecated]] inline constexpr BodyJoint BodyJointPico_RIGHT_ELBOW = BodyJoint_RIGHT_ELBOW; +[[deprecated]] inline constexpr BodyJoint BodyJointPico_LEFT_WRIST = BodyJoint_LEFT_WRIST; +[[deprecated]] inline constexpr BodyJoint BodyJointPico_RIGHT_WRIST = BodyJoint_RIGHT_WRIST; +[[deprecated]] inline constexpr BodyJoint BodyJointPico_LEFT_HAND = BodyJoint_LEFT_HAND; +[[deprecated]] inline constexpr BodyJoint BodyJointPico_RIGHT_HAND = BodyJoint_RIGHT_HAND; +[[deprecated]] inline constexpr BodyJoint BodyJointPico_NUM_JOINTS = BodyJoint_NUM_JOINTS; + +} // namespace core diff --git a/src/core/schema/fbs/full_body.fbs b/src/core/schema/fbs/full_body.fbs index 8d9b0bc5c..a8abcc5be 100644 --- a/src/core/schema/fbs/full_body.fbs +++ b/src/core/schema/fbs/full_body.fbs @@ -6,9 +6,10 @@ include "timestamp.fbs"; namespace core; -// Body joint indices for PICO body tracking (XR_BD_body_tracking extension). +// Body joint indices (XR_BD_body_tracking extension layout). +// Vendor-neutral: every full-body vendor produces joints in this order. // See: https://registry.khronos.org/OpenXR/specs/1.1/html/xrspec.html#XrBodyJointBD -enum BodyJointPico : uint8 { +enum BodyJoint : uint8 { PELVIS = 0, LEFT_HIP = 1, RIGHT_HIP = 2, @@ -48,16 +49,16 @@ struct BodyJointPose { // Fixed-size array of 24 body joint locations. This follows the joint format defined in: // See: https://registry.khronos.org/OpenXR/specs/1.1/html/xrspec.html#XR_BD_body_tracking -struct BodyJointsPico { +struct BodyJoints { joints: [BodyJointPose:24]; } -// Full body pose data for PICO body tracking (XR_BD_body_tracking extension). +// Full body pose data (XR_BD_body_tracking joint layout, vendor-neutral). // All fields are always present when the parent Tracked/Record wrapper's data is non-null. -table FullBodyPosePico { +table FullBodyPose { // Vector of BodyJointPose. // For XR_BD_body_tracking, this is 24 joints. - joints: BodyJointsPico (id: 0); + joints: BodyJoints (id: 0); // allJointPosesTracked from the OpenXR extension — quality flag only. // When false, individual joint is_valid flags should be consulted. @@ -66,18 +67,18 @@ table FullBodyPosePico { } // Tracked wrapper for the in-memory tracker API (data is null when body tracking is inactive). -table FullBodyPosePicoTracked { - data: FullBodyPosePico (id: 0); +table FullBodyPoseTracked { + data: FullBodyPose (id: 0); } -// MCAP recording wrapper for FullBodyPosePico. +// MCAP recording wrapper for FullBodyPose. // // Record types are the root types written to MCAP channels by the McapRecorder. // Trackers serialize into Record types via their serialize() method, but the // public query API returns the inner data type directly. -table FullBodyPosePicoRecord { - data: FullBodyPosePico (id: 0); +table FullBodyPoseRecord { + data: FullBodyPose (id: 0); timestamp: DeviceDataTimestamp (id: 1); } -root_type FullBodyPosePicoRecord; +root_type FullBodyPoseRecord; diff --git a/src/core/schema/python/full_body_bindings.h b/src/core/schema/python/full_body_bindings.h index 320c8fcd3..f735955e7 100644 --- a/src/core/schema/python/full_body_bindings.h +++ b/src/core/schema/python/full_body_bindings.h @@ -1,8 +1,8 @@ // SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. // SPDX-License-Identifier: Apache-2.0 -// Python bindings for the FullBodyPosePico FlatBuffer schema. -// Includes BodyJointPose struct, BodyJointsPico struct, and FullBodyPosePicoT table. +// Python bindings for the FullBodyPose FlatBuffer schema. +// Includes BodyJointPose struct, BodyJoints struct, and FullBodyPoseT table. #pragma once @@ -23,33 +23,33 @@ namespace core inline void bind_full_body(py::module& m) { - // Bind BodyJointPico enum (joint indices for XR_BD_body_tracking). - py::enum_(m, "BodyJointPico") - .value("PELVIS", BodyJointPico_PELVIS) - .value("LEFT_HIP", BodyJointPico_LEFT_HIP) - .value("RIGHT_HIP", BodyJointPico_RIGHT_HIP) - .value("SPINE1", BodyJointPico_SPINE1) - .value("LEFT_KNEE", BodyJointPico_LEFT_KNEE) - .value("RIGHT_KNEE", BodyJointPico_RIGHT_KNEE) - .value("SPINE2", BodyJointPico_SPINE2) - .value("LEFT_ANKLE", BodyJointPico_LEFT_ANKLE) - .value("RIGHT_ANKLE", BodyJointPico_RIGHT_ANKLE) - .value("SPINE3", BodyJointPico_SPINE3) - .value("LEFT_FOOT", BodyJointPico_LEFT_FOOT) - .value("RIGHT_FOOT", BodyJointPico_RIGHT_FOOT) - .value("NECK", BodyJointPico_NECK) - .value("LEFT_COLLAR", BodyJointPico_LEFT_COLLAR) - .value("RIGHT_COLLAR", BodyJointPico_RIGHT_COLLAR) - .value("HEAD", BodyJointPico_HEAD) - .value("LEFT_SHOULDER", BodyJointPico_LEFT_SHOULDER) - .value("RIGHT_SHOULDER", BodyJointPico_RIGHT_SHOULDER) - .value("LEFT_ELBOW", BodyJointPico_LEFT_ELBOW) - .value("RIGHT_ELBOW", BodyJointPico_RIGHT_ELBOW) - .value("LEFT_WRIST", BodyJointPico_LEFT_WRIST) - .value("RIGHT_WRIST", BodyJointPico_RIGHT_WRIST) - .value("LEFT_HAND", BodyJointPico_LEFT_HAND) - .value("RIGHT_HAND", BodyJointPico_RIGHT_HAND) - .value("NUM_JOINTS", BodyJointPico_NUM_JOINTS); + // Bind BodyJoint enum (joint indices for XR_BD_body_tracking). + py::enum_(m, "BodyJoint") + .value("PELVIS", BodyJoint_PELVIS) + .value("LEFT_HIP", BodyJoint_LEFT_HIP) + .value("RIGHT_HIP", BodyJoint_RIGHT_HIP) + .value("SPINE1", BodyJoint_SPINE1) + .value("LEFT_KNEE", BodyJoint_LEFT_KNEE) + .value("RIGHT_KNEE", BodyJoint_RIGHT_KNEE) + .value("SPINE2", BodyJoint_SPINE2) + .value("LEFT_ANKLE", BodyJoint_LEFT_ANKLE) + .value("RIGHT_ANKLE", BodyJoint_RIGHT_ANKLE) + .value("SPINE3", BodyJoint_SPINE3) + .value("LEFT_FOOT", BodyJoint_LEFT_FOOT) + .value("RIGHT_FOOT", BodyJoint_RIGHT_FOOT) + .value("NECK", BodyJoint_NECK) + .value("LEFT_COLLAR", BodyJoint_LEFT_COLLAR) + .value("RIGHT_COLLAR", BodyJoint_RIGHT_COLLAR) + .value("HEAD", BodyJoint_HEAD) + .value("LEFT_SHOULDER", BodyJoint_LEFT_SHOULDER) + .value("RIGHT_SHOULDER", BodyJoint_RIGHT_SHOULDER) + .value("LEFT_ELBOW", BodyJoint_LEFT_ELBOW) + .value("RIGHT_ELBOW", BodyJoint_RIGHT_ELBOW) + .value("LEFT_WRIST", BodyJoint_LEFT_WRIST) + .value("RIGHT_WRIST", BodyJoint_RIGHT_WRIST) + .value("LEFT_HAND", BodyJoint_LEFT_HAND) + .value("RIGHT_HAND", BodyJoint_RIGHT_HAND) + .value("NUM_JOINTS", BodyJoint_NUM_JOINTS); // Bind BodyJointPose struct (pose, is_valid). py::class_(m, "BodyJointPose") @@ -70,90 +70,85 @@ inline void bind_full_body(py::module& m) ")), is_valid=" + (self.is_valid() ? "True" : "False") + ")"; }); - // Bind BodyJointsPico struct (fixed-size array of 24 BodyJointPose). - py::class_(m, "BodyJointsPico") + // Bind BodyJoints struct (fixed-size array of 24 BodyJointPose). + py::class_(m, "BodyJoints") .def(py::init<>()) .def( "joints", - [](const BodyJointsPico& self, size_t index) -> const BodyJointPose* + [](const BodyJoints& self, size_t index) -> const BodyJointPose* { - if (index >= static_cast(BodyJointPico_NUM_JOINTS)) + if (index >= static_cast(BodyJoint_NUM_JOINTS)) { - throw py::index_error("BodyJointsPico index out of range (must be 0-23)"); + throw py::index_error("BodyJoints index out of range (must be 0-23)"); } return (*self.joints())[index]; }, py::arg("index"), py::return_value_policy::reference_internal, "Get the BodyJointPose at the specified index (0 to NUM_JOINTS-1).") - .def("__repr__", [](const BodyJointsPico&) { return "BodyJointsPico(joints=[...24 BodyJointPose entries...])"; }); + .def("__repr__", [](const BodyJoints&) { return "BodyJoints(joints=[...24 BodyJointPose entries...])"; }); - // Bind FullBodyPosePicoT class (FlatBuffers object API for tables). - py::class_>(m, "FullBodyPosePicoT") + // Bind FullBodyPoseT class (FlatBuffers object API for tables). + py::class_>(m, "FullBodyPoseT") .def(py::init( []() { - auto obj = std::make_shared(); - obj->joints = std::make_shared(); + auto obj = std::make_shared(); + obj->joints = std::make_shared(); return obj; })) .def(py::init( - [](const BodyJointsPico& joints) + [](const BodyJoints& joints) { - auto obj = std::make_shared(); - obj->joints = std::make_shared(joints); + auto obj = std::make_shared(); + obj->joints = std::make_shared(joints); return obj; }), py::arg("joints")) .def_property_readonly( - "joints", [](const FullBodyPosePicoT& self) -> const BodyJointsPico* { return self.joints.get(); }, + "joints", [](const FullBodyPoseT& self) -> const BodyJoints* { return self.joints.get(); }, py::return_value_policy::reference_internal) .def("__repr__", - [](const FullBodyPosePicoT& self) + [](const FullBodyPoseT& self) { std::string joints_str = "None"; if (self.joints) { - joints_str = "BodyJointsPico(joints=[...24 entries...])"; + joints_str = "BodyJoints(joints=[...24 entries...])"; } - return "FullBodyPosePicoT(joints=" + joints_str + ")"; + return "FullBodyPoseT(joints=" + joints_str + ")"; }); - py::class_>(m, "FullBodyPosePicoRecord") + py::class_>(m, "FullBodyPoseRecord") .def(py::init<>()) .def(py::init( - [](const FullBodyPosePicoT& data, const DeviceDataTimestamp& timestamp) + [](const FullBodyPoseT& data, const DeviceDataTimestamp& timestamp) { - auto obj = std::make_shared(); - obj->data = std::make_shared(data); + auto obj = std::make_shared(); + obj->data = std::make_shared(data); obj->timestamp = std::make_shared(timestamp); return obj; }), py::arg("data"), py::arg("timestamp")) .def_property_readonly( - "data", [](const FullBodyPosePicoRecordT& self) -> std::shared_ptr { return self.data; }) - .def_readonly("timestamp", &FullBodyPosePicoRecordT::timestamp) - .def("__repr__", - [](const FullBodyPosePicoRecordT& self) { - return "FullBodyPosePicoRecord(data=" + std::string(self.data ? "FullBodyPosePicoT(...)" : "None") + ")"; - }); + "data", [](const FullBodyPoseRecordT& self) -> std::shared_ptr { return self.data; }) + .def_readonly("timestamp", &FullBodyPoseRecordT::timestamp) + .def("__repr__", [](const FullBodyPoseRecordT& self) + { return "FullBodyPoseRecord(data=" + std::string(self.data ? "FullBodyPoseT(...)" : "None") + ")"; }); - py::class_>(m, "FullBodyPosePicoTrackedT") + py::class_>(m, "FullBodyPoseTrackedT") .def(py::init<>()) .def(py::init( - [](const FullBodyPosePicoT& data) + [](const FullBodyPoseT& data) { - auto obj = std::make_shared(); - obj->data = std::make_shared(data); + auto obj = std::make_shared(); + obj->data = std::make_shared(data); return obj; }), py::arg("data")) .def_property_readonly( - "data", [](const FullBodyPosePicoTrackedT& self) -> std::shared_ptr { return self.data; }) - .def("__repr__", - [](const FullBodyPosePicoTrackedT& self) { - return std::string("FullBodyPosePicoTrackedT(data=") + - (self.data ? "FullBodyPosePicoT(...)" : "None") + ")"; - }); + "data", [](const FullBodyPoseTrackedT& self) -> std::shared_ptr { return self.data; }) + .def("__repr__", [](const FullBodyPoseTrackedT& self) + { return std::string("FullBodyPoseTrackedT(data=") + (self.data ? "FullBodyPoseT(...)" : "None") + ")"; }); } } // namespace core diff --git a/src/core/schema/python/schema_init.py b/src/core/schema/python/schema_init.py index 29d9290d8..cc1d3265c 100644 --- a/src/core/schema/python/schema_init.py +++ b/src/core/schema/python/schema_init.py @@ -7,6 +7,8 @@ used in teleoperation, including poses, and controller data. """ +import warnings + from ._schema import ( # Timestamp types. DeviceDataTimestamp, @@ -62,14 +64,36 @@ FrameMetadataOakTrackedT, FrameMetadataOakRecord, # Full body-related types. - BodyJointPico, + BodyJoint, BodyJointPose, - BodyJointsPico, - FullBodyPosePicoT, - FullBodyPosePicoTrackedT, - FullBodyPosePicoRecord, + BodyJoints, + FullBodyPoseT, + FullBodyPoseTrackedT, + FullBodyPoseRecord, ) +# Deprecated aliases for the renamed full-body schema types, resolved lazily via +# __getattr__ so accessing them emits a DeprecationWarning. Omitted from __all__. +_DEPRECATED_ALIASES = { + "BodyJointPico": "BodyJoint", + "BodyJointsPico": "BodyJoints", + "FullBodyPosePicoT": "FullBodyPoseT", + "FullBodyPosePicoTrackedT": "FullBodyPoseTrackedT", + "FullBodyPosePicoRecord": "FullBodyPoseRecord", +} + + +def __getattr__(name: str): + new_name = _DEPRECATED_ALIASES.get(name) + if new_name is not None: + warnings.warn( + f"{name} is deprecated; use {new_name} instead.", + DeprecationWarning, + stacklevel=2, + ) + return globals()[new_name] + raise AttributeError(f"module {__name__!r} has no attribute {name!r}") + __all__ = [ # Timestamp types. @@ -126,9 +150,9 @@ "FrameMetadataOakRecord", # Full body types. "BodyJointPose", - "BodyJointsPico", - "BodyJointPico", - "FullBodyPosePicoT", - "FullBodyPosePicoTrackedT", - "FullBodyPosePicoRecord", + "BodyJoint", + "BodyJoints", + "FullBodyPoseT", + "FullBodyPoseTrackedT", + "FullBodyPoseRecord", ] diff --git a/src/core/schema/python/schema_module.cpp b/src/core/schema/python/schema_module.cpp index d4784a846..cbd2ca491 100644 --- a/src/core/schema/python/schema_module.cpp +++ b/src/core/schema/python/schema_module.cpp @@ -62,6 +62,6 @@ PYBIND11_MODULE(_schema, m) // Bind OAK types (StreamType enum, FrameMetadataOak table). core::bind_oak(m); - // Bind full body types (BodyJointPose, BodyJointsPico structs, FullBodyPosePicoT table). + // Bind full body types (BodyJointPose, BodyJoints structs, FullBodyPoseT table). core::bind_full_body(m); } diff --git a/src/core/schema_tests/cpp/test_full_body.cpp b/src/core/schema_tests/cpp/test_full_body.cpp index 614d94a43..33cebec4b 100644 --- a/src/core/schema_tests/cpp/test_full_body.cpp +++ b/src/core/schema_tests/cpp/test_full_body.cpp @@ -1,7 +1,7 @@ // SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. // SPDX-License-Identifier: Apache-2.0 -// Unit tests for the generated FullBodyPosePico FlatBuffer message. +// Unit tests for the generated FullBodyPose FlatBuffer message. #include #include @@ -20,16 +20,16 @@ // VT values are computed as: (field_id + 2) * 2. // ============================================================================= #define VT(field) (field + 2) * 2 -static_assert(core::FullBodyPosePico::VT_JOINTS == VT(0)); +static_assert(core::FullBodyPose::VT_JOINTS == VT(0)); -static_assert(core::FullBodyPosePicoRecord::VT_DATA == VT(0)); +static_assert(core::FullBodyPoseRecord::VT_DATA == VT(0)); // ============================================================================= // Compile-time verification of FlatBuffer field types. // These ensure schema field types remain stable across changes. // ============================================================================= -#define TYPE(field) decltype(std::declval().field()) -static_assert(std::is_same_v); +#define TYPE(field) decltype(std::declval().field()) +static_assert(std::is_same_v); // ============================================================================= // Compile-time verification of BodyJointPose struct. @@ -37,20 +37,76 @@ static_assert(std::is_same_v); static_assert(std::is_trivially_copyable_v, "BodyJointPose should be a trivially copyable struct"); // ============================================================================= -// Compile-time verification of BodyJointsPico struct. +// Compile-time verification of BodyJoints struct. // ============================================================================= -static_assert(std::is_trivially_copyable_v, "BodyJointsPico should be a trivially copyable struct"); +static_assert(std::is_trivially_copyable_v, "BodyJoints should be a trivially copyable struct"); -// BodyJointsPico should contain exactly 24 BodyJointPose entries. -static_assert(sizeof(core::BodyJointsPico) == 24 * sizeof(core::BodyJointPose), - "BodyJointsPico should contain exactly 24 BodyJointPose entries"); +// BodyJoints should contain exactly 24 BodyJointPose entries. +static_assert(sizeof(core::BodyJoints) == 24 * sizeof(core::BodyJointPose), + "BodyJoints should contain exactly 24 BodyJointPose entries"); // ============================================================================= -// Compile-time verification of BodyJointPico enum. +// Compile-time verification of the deprecated "...Pico" back-compat aliases. +// +// full_body_compat.hpp is an opt-in header: nothing else in-tree includes it and +// the generated header does not pull it in, so without this block a mistyped alias +// (e.g. BodyJointPico_HEAD mapped to BodyJoint_NECK) would compile and ship +// silently. Lock every alias to its renamed target here, mirroring the Python +// TestDeprecatedPicoAliases coverage. The references are intentionally to +// [[deprecated]] names, so suppress that diagnostic for this block only. // ============================================================================= -static_assert(core::BodyJointPico_PELVIS == 0, "PELVIS should be index 0"); -static_assert(core::BodyJointPico_RIGHT_HAND == 23, "RIGHT_HAND should be index 23"); -static_assert(core::BodyJointPico_NUM_JOINTS == 24, "NUM_JOINTS should be 24"); +#if defined(__GNUC__) || defined(__clang__) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#endif +#include + +// Type aliases resolve to the renamed generated types. +static_assert(std::is_same_v); +static_assert(std::is_same_v); +static_assert(std::is_same_v); +static_assert(std::is_same_v); +static_assert(std::is_same_v); +static_assert(std::is_same_v); +static_assert(std::is_same_v); +static_assert(std::is_same_v); + +// Every enumerator alias keeps its renamed target's numeric value. +static_assert(core::BodyJointPico_PELVIS == core::BodyJoint_PELVIS); +static_assert(core::BodyJointPico_LEFT_HIP == core::BodyJoint_LEFT_HIP); +static_assert(core::BodyJointPico_RIGHT_HIP == core::BodyJoint_RIGHT_HIP); +static_assert(core::BodyJointPico_SPINE1 == core::BodyJoint_SPINE1); +static_assert(core::BodyJointPico_LEFT_KNEE == core::BodyJoint_LEFT_KNEE); +static_assert(core::BodyJointPico_RIGHT_KNEE == core::BodyJoint_RIGHT_KNEE); +static_assert(core::BodyJointPico_SPINE2 == core::BodyJoint_SPINE2); +static_assert(core::BodyJointPico_LEFT_ANKLE == core::BodyJoint_LEFT_ANKLE); +static_assert(core::BodyJointPico_RIGHT_ANKLE == core::BodyJoint_RIGHT_ANKLE); +static_assert(core::BodyJointPico_SPINE3 == core::BodyJoint_SPINE3); +static_assert(core::BodyJointPico_LEFT_FOOT == core::BodyJoint_LEFT_FOOT); +static_assert(core::BodyJointPico_RIGHT_FOOT == core::BodyJoint_RIGHT_FOOT); +static_assert(core::BodyJointPico_NECK == core::BodyJoint_NECK); +static_assert(core::BodyJointPico_LEFT_COLLAR == core::BodyJoint_LEFT_COLLAR); +static_assert(core::BodyJointPico_RIGHT_COLLAR == core::BodyJoint_RIGHT_COLLAR); +static_assert(core::BodyJointPico_HEAD == core::BodyJoint_HEAD); +static_assert(core::BodyJointPico_LEFT_SHOULDER == core::BodyJoint_LEFT_SHOULDER); +static_assert(core::BodyJointPico_RIGHT_SHOULDER == core::BodyJoint_RIGHT_SHOULDER); +static_assert(core::BodyJointPico_LEFT_ELBOW == core::BodyJoint_LEFT_ELBOW); +static_assert(core::BodyJointPico_RIGHT_ELBOW == core::BodyJoint_RIGHT_ELBOW); +static_assert(core::BodyJointPico_LEFT_WRIST == core::BodyJoint_LEFT_WRIST); +static_assert(core::BodyJointPico_RIGHT_WRIST == core::BodyJoint_RIGHT_WRIST); +static_assert(core::BodyJointPico_LEFT_HAND == core::BodyJoint_LEFT_HAND); +static_assert(core::BodyJointPico_RIGHT_HAND == core::BodyJoint_RIGHT_HAND); +static_assert(core::BodyJointPico_NUM_JOINTS == core::BodyJoint_NUM_JOINTS); +#if defined(__GNUC__) || defined(__clang__) +# pragma GCC diagnostic pop +#endif + +// ============================================================================= +// Compile-time verification of BodyJoint enum. +// ============================================================================= +static_assert(core::BodyJoint_PELVIS == 0, "PELVIS should be index 0"); +static_assert(core::BodyJoint_RIGHT_HAND == 23, "RIGHT_HAND should be index 23"); +static_assert(core::BodyJoint_NUM_JOINTS == 24, "NUM_JOINTS should be 24"); // ============================================================================= // BodyJointPose Tests @@ -87,22 +143,22 @@ TEST_CASE("BodyJointPose default construction", "[full_body][struct]") } // ============================================================================= -// BodyJointsPico Tests +// BodyJoints Tests // ============================================================================= -TEST_CASE("BodyJointsPico struct has correct size", "[full_body][struct]") +TEST_CASE("BodyJoints struct has correct size", "[full_body][struct]") { - // BodyJointsPico should have exactly NUM_JOINTS entries (XR_BD_body_tracking). - core::BodyJointsPico joints; - CHECK(joints.joints()->size() == static_cast(core::BodyJointPico_NUM_JOINTS)); + // BodyJoints should have exactly NUM_JOINTS entries (XR_BD_body_tracking). + core::BodyJoints joints; + CHECK(joints.joints()->size() == static_cast(core::BodyJoint_NUM_JOINTS)); } -TEST_CASE("BodyJointsPico can be accessed by index", "[full_body][struct]") +TEST_CASE("BodyJoints can be accessed by index", "[full_body][struct]") { - core::BodyJointsPico joints; + core::BodyJoints joints; // Access first and last entries (returns pointers). const auto* first = (*joints.joints())[0]; - const auto* last = (*joints.joints())[core::BodyJointPico_NUM_JOINTS - 1]; + const auto* last = (*joints.joints())[core::BodyJoint_NUM_JOINTS - 1]; // Default values should be zero. CHECK(first->pose().position().x() == 0.0f); @@ -110,30 +166,30 @@ TEST_CASE("BodyJointsPico can be accessed by index", "[full_body][struct]") } // ============================================================================= -// FullBodyPosePicoT Tests +// FullBodyPoseT Tests // ============================================================================= -TEST_CASE("FullBodyPosePicoT default construction", "[full_body][native]") +TEST_CASE("FullBodyPoseT default construction", "[full_body][native]") { - auto body_pose = std::make_unique(); + auto body_pose = std::make_unique(); // Default values. CHECK(body_pose->joints == nullptr); } -TEST_CASE("FullBodyPosePicoT can store joints data", "[full_body][native]") +TEST_CASE("FullBodyPoseT can store joints data", "[full_body][native]") { - auto body_pose = std::make_unique(); + auto body_pose = std::make_unique(); // Create and set joints. - body_pose->joints = std::make_unique(); + body_pose->joints = std::make_unique(); - CHECK(body_pose->joints->joints()->size() == static_cast(core::BodyJointPico_NUM_JOINTS)); + CHECK(body_pose->joints->joints()->size() == static_cast(core::BodyJoint_NUM_JOINTS)); } -TEST_CASE("FullBodyPosePicoT joints can be mutated via flatbuffers Array", "[full_body][native]") +TEST_CASE("FullBodyPoseT joints can be mutated via flatbuffers Array", "[full_body][native]") { - auto body_pose = std::make_unique(); - body_pose->joints = std::make_unique(); + auto body_pose = std::make_unique(); + body_pose->joints = std::make_unique(); // Create a joint pose. core::Point position(1.0f, 2.0f, 3.0f); @@ -152,13 +208,13 @@ TEST_CASE("FullBodyPosePicoT joints can be mutated via flatbuffers Array", "[ful CHECK(first_joint->is_valid() == true); } -TEST_CASE("FullBodyPosePicoT serialization and deserialization", "[full_body][flatbuffers]") +TEST_CASE("FullBodyPoseT serialization and deserialization", "[full_body][flatbuffers]") { flatbuffers::FlatBufferBuilder builder(4096); - // Create FullBodyPosePicoT with all fields set. - auto body_pose = std::make_unique(); - body_pose->joints = std::make_unique(); + // Create FullBodyPoseT with all fields set. + auto body_pose = std::make_unique(); + body_pose->joints = std::make_unique(); // Set a few joint poses core::Point position(1.5f, 2.5f, 3.5f); @@ -169,15 +225,15 @@ TEST_CASE("FullBodyPosePicoT serialization and deserialization", "[full_body][fl body_pose->joints->mutable_joints()->Mutate(0, joint_pose); // Serialize. - auto offset = core::FullBodyPosePico::Pack(builder, body_pose.get()); + auto offset = core::FullBodyPose::Pack(builder, body_pose.get()); builder.Finish(offset); // Deserialize. auto buffer = builder.GetBufferPointer(); - auto deserialized = flatbuffers::GetRoot(buffer); + auto deserialized = flatbuffers::GetRoot(buffer); // Verify. - CHECK(deserialized->joints()->joints()->size() == static_cast(core::BodyJointPico_NUM_JOINTS)); + CHECK(deserialized->joints()->joints()->size() == static_cast(core::BodyJoint_NUM_JOINTS)); const auto* first_joint = (*deserialized->joints()->joints())[0]; CHECK(first_joint->pose().position().x() == Catch::Approx(1.5f)); @@ -186,13 +242,13 @@ TEST_CASE("FullBodyPosePicoT serialization and deserialization", "[full_body][fl CHECK(first_joint->is_valid() == true); } -TEST_CASE("FullBodyPosePicoT can be unpacked from buffer", "[full_body][flatbuffers]") +TEST_CASE("FullBodyPoseT can be unpacked from buffer", "[full_body][flatbuffers]") { flatbuffers::FlatBufferBuilder builder(4096); // Create and serialize. - auto original = std::make_unique(); - original->joints = std::make_unique(); + auto original = std::make_unique(); + original->joints = std::make_unique(); // Set multiple joint poses (--gen-mutable provides mutable_joints()). for (size_t i = 0; i < 24; ++i) @@ -204,13 +260,13 @@ TEST_CASE("FullBodyPosePicoT can be unpacked from buffer", "[full_body][flatbuff original->joints->mutable_joints()->Mutate(i, joint_pose); } - auto offset = core::FullBodyPosePico::Pack(builder, original.get()); + auto offset = core::FullBodyPose::Pack(builder, original.get()); builder.Finish(offset); - // Unpack to FullBodyPosePicoT. + // Unpack to FullBodyPoseT. auto buffer = builder.GetBufferPointer(); - auto body_pose_fb = flatbuffers::GetRoot(buffer); - auto unpacked = std::make_unique(); + auto body_pose_fb = flatbuffers::GetRoot(buffer); + auto unpacked = std::make_unique(); body_pose_fb->UnPackTo(unpacked.get()); // Check a few joints. @@ -225,10 +281,10 @@ TEST_CASE("FullBodyPosePicoT can be unpacked from buffer", "[full_body][flatbuff CHECK(joint_23->pose().position().z() == Catch::Approx(69.0f)); } -TEST_CASE("FullBodyPosePicoT all 24 joints can be set and verified", "[full_body][native]") +TEST_CASE("FullBodyPoseT all 24 joints can be set and verified", "[full_body][native]") { - auto body_pose = std::make_unique(); - body_pose->joints = std::make_unique(); + auto body_pose = std::make_unique(); + body_pose->joints = std::make_unique(); // Set all 24 joints with unique positions (--gen-mutable provides mutable_joints()). for (size_t i = 0; i < 24; ++i) @@ -252,7 +308,7 @@ TEST_CASE("FullBodyPosePicoT all 24 joints can be set and verified", "[full_body // ============================================================================= // Body Joint Index Tests (XR_BD_body_tracking joint mapping) // ============================================================================= -TEST_CASE("FullBodyPosePicoT joint indices correspond to body parts", "[full_body][joints]") +TEST_CASE("FullBodyPoseT joint indices correspond to body parts", "[full_body][joints]") { // This test documents the expected joint mapping from XR_BD_body_tracking. // Joint indices: @@ -261,8 +317,8 @@ TEST_CASE("FullBodyPosePicoT joint indices correspond to body parts", "[full_bod // 12: Neck, 13-14: Left/Right Collar, 15: Head, 16-17: Left/Right Shoulder, // 18-19: Left/Right Elbow, 20-21: Left/Right Wrist, 22-23: Left/Right Hand - auto body_pose = std::make_unique(); - body_pose->joints = std::make_unique(); + auto body_pose = std::make_unique(); + body_pose->joints = std::make_unique(); // Verify we can access all expected joint indices. REQUIRE(body_pose->joints->joints()->size() == 24); @@ -282,14 +338,14 @@ TEST_CASE("FullBodyPosePicoT joint indices correspond to body parts", "[full_bod // ============================================================================= // Edge Cases // ============================================================================= -TEST_CASE("FullBodyPosePicoT buffer size is reasonable", "[full_body][serialize]") +TEST_CASE("FullBodyPoseT buffer size is reasonable", "[full_body][serialize]") { flatbuffers::FlatBufferBuilder builder; - auto body_pose = std::make_unique(); - body_pose->joints = std::make_unique(); + auto body_pose = std::make_unique(); + body_pose->joints = std::make_unique(); - auto offset = core::FullBodyPosePico::Pack(builder, body_pose.get()); + auto offset = core::FullBodyPose::Pack(builder, body_pose.get()); builder.Finish(offset); // Buffer should be reasonably sized for 24 joints. @@ -299,42 +355,42 @@ TEST_CASE("FullBodyPosePicoT buffer size is reasonable", "[full_body][serialize] } // ============================================================================= -// BodyJointPico Enum Tests +// BodyJoint Enum Tests // ============================================================================= -TEST_CASE("BodyJointPico enum has correct values", "[full_body][enum]") +TEST_CASE("BodyJoint enum has correct values", "[full_body][enum]") { // Verify all 24 enum values match the XR_BD_body_tracking joint indices. - CHECK(core::BodyJointPico_PELVIS == 0); - CHECK(core::BodyJointPico_LEFT_HIP == 1); - CHECK(core::BodyJointPico_RIGHT_HIP == 2); - CHECK(core::BodyJointPico_SPINE1 == 3); - CHECK(core::BodyJointPico_LEFT_KNEE == 4); - CHECK(core::BodyJointPico_RIGHT_KNEE == 5); - CHECK(core::BodyJointPico_SPINE2 == 6); - CHECK(core::BodyJointPico_LEFT_ANKLE == 7); - CHECK(core::BodyJointPico_RIGHT_ANKLE == 8); - CHECK(core::BodyJointPico_SPINE3 == 9); - CHECK(core::BodyJointPico_LEFT_FOOT == 10); - CHECK(core::BodyJointPico_RIGHT_FOOT == 11); - CHECK(core::BodyJointPico_NECK == 12); - CHECK(core::BodyJointPico_LEFT_COLLAR == 13); - CHECK(core::BodyJointPico_RIGHT_COLLAR == 14); - CHECK(core::BodyJointPico_HEAD == 15); - CHECK(core::BodyJointPico_LEFT_SHOULDER == 16); - CHECK(core::BodyJointPico_RIGHT_SHOULDER == 17); - CHECK(core::BodyJointPico_LEFT_ELBOW == 18); - CHECK(core::BodyJointPico_RIGHT_ELBOW == 19); - CHECK(core::BodyJointPico_LEFT_WRIST == 20); - CHECK(core::BodyJointPico_RIGHT_WRIST == 21); - CHECK(core::BodyJointPico_LEFT_HAND == 22); - CHECK(core::BodyJointPico_RIGHT_HAND == 23); - CHECK(core::BodyJointPico_NUM_JOINTS == 24); + CHECK(core::BodyJoint_PELVIS == 0); + CHECK(core::BodyJoint_LEFT_HIP == 1); + CHECK(core::BodyJoint_RIGHT_HIP == 2); + CHECK(core::BodyJoint_SPINE1 == 3); + CHECK(core::BodyJoint_LEFT_KNEE == 4); + CHECK(core::BodyJoint_RIGHT_KNEE == 5); + CHECK(core::BodyJoint_SPINE2 == 6); + CHECK(core::BodyJoint_LEFT_ANKLE == 7); + CHECK(core::BodyJoint_RIGHT_ANKLE == 8); + CHECK(core::BodyJoint_SPINE3 == 9); + CHECK(core::BodyJoint_LEFT_FOOT == 10); + CHECK(core::BodyJoint_RIGHT_FOOT == 11); + CHECK(core::BodyJoint_NECK == 12); + CHECK(core::BodyJoint_LEFT_COLLAR == 13); + CHECK(core::BodyJoint_RIGHT_COLLAR == 14); + CHECK(core::BodyJoint_HEAD == 15); + CHECK(core::BodyJoint_LEFT_SHOULDER == 16); + CHECK(core::BodyJoint_RIGHT_SHOULDER == 17); + CHECK(core::BodyJoint_LEFT_ELBOW == 18); + CHECK(core::BodyJoint_RIGHT_ELBOW == 19); + CHECK(core::BodyJoint_LEFT_WRIST == 20); + CHECK(core::BodyJoint_RIGHT_WRIST == 21); + CHECK(core::BodyJoint_LEFT_HAND == 22); + CHECK(core::BodyJoint_RIGHT_HAND == 23); + CHECK(core::BodyJoint_NUM_JOINTS == 24); } -TEST_CASE("BodyJointPico enum can index BodyJointsPico", "[full_body][enum]") +TEST_CASE("BodyJoint enum can index BodyJoints", "[full_body][enum]") { - auto body_pose = std::make_unique(); - body_pose->joints = std::make_unique(); + auto body_pose = std::make_unique(); + body_pose->joints = std::make_unique(); // Set specific joints using enum values (--gen-mutable provides mutable_joints()). // Set HEAD joint with a recognizable position. @@ -342,65 +398,65 @@ TEST_CASE("BodyJointPico enum can index BodyJointsPico", "[full_body][enum]") core::Quaternion orientation(0.0f, 0.0f, 0.0f, 1.0f); core::Pose head_pose(head_position, orientation); core::BodyJointPose head_joint(head_pose, true); - body_pose->joints->mutable_joints()->Mutate(core::BodyJointPico_HEAD, head_joint); + body_pose->joints->mutable_joints()->Mutate(core::BodyJoint_HEAD, head_joint); // Set LEFT_HAND joint with a recognizable position. core::Point left_hand_position(-0.5f, 1.0f, 0.3f); core::Pose left_hand_pose(left_hand_position, orientation); core::BodyJointPose left_hand_joint(left_hand_pose, true); - body_pose->joints->mutable_joints()->Mutate(core::BodyJointPico_LEFT_HAND, left_hand_joint); + body_pose->joints->mutable_joints()->Mutate(core::BodyJoint_LEFT_HAND, left_hand_joint); // Verify using enum values to access. - const auto* head = (*body_pose->joints->joints())[core::BodyJointPico_HEAD]; + const auto* head = (*body_pose->joints->joints())[core::BodyJoint_HEAD]; CHECK(head->pose().position().y() == Catch::Approx(1.7f)); CHECK(head->is_valid() == true); - const auto* left_hand = (*body_pose->joints->joints())[core::BodyJointPico_LEFT_HAND]; + const auto* left_hand = (*body_pose->joints->joints())[core::BodyJoint_LEFT_HAND]; CHECK(left_hand->pose().position().x() == Catch::Approx(-0.5f)); CHECK(left_hand->is_valid() == true); } // ============================================================================= -// FullBodyPosePicoRecord Tests (timestamp lives on the Record wrapper) +// FullBodyPoseRecord Tests (timestamp lives on the Record wrapper) // ============================================================================= -TEST_CASE("FullBodyPosePicoRecord serialization with DeviceDataTimestamp", "[full_body][flatbuffers]") +TEST_CASE("FullBodyPoseRecord serialization with DeviceDataTimestamp", "[full_body][flatbuffers]") { flatbuffers::FlatBufferBuilder builder(4096); - auto record = std::make_shared(); - record->data = std::make_shared(); - record->data->joints = std::make_unique(); + auto record = std::make_shared(); + record->data = std::make_shared(); + record->data->joints = std::make_unique(); record->timestamp = std::make_shared(1000000000LL, 2000000000LL, 3000000000LL); - auto offset = core::FullBodyPosePicoRecord::Pack(builder, record.get()); + auto offset = core::FullBodyPoseRecord::Pack(builder, record.get()); builder.Finish(offset); - auto deserialized = flatbuffers::GetRoot(builder.GetBufferPointer()); + auto deserialized = flatbuffers::GetRoot(builder.GetBufferPointer()); CHECK(deserialized->timestamp()->available_time_local_common_clock() == 1000000000LL); CHECK(deserialized->timestamp()->sample_time_local_common_clock() == 2000000000LL); CHECK(deserialized->timestamp()->sample_time_raw_device_clock() == 3000000000LL); - CHECK(deserialized->data()->joints()->joints()->size() == static_cast(core::BodyJointPico_NUM_JOINTS)); + CHECK(deserialized->data()->joints()->joints()->size() == static_cast(core::BodyJoint_NUM_JOINTS)); } -TEST_CASE("FullBodyPosePicoRecord can be unpacked with DeviceDataTimestamp", "[full_body][flatbuffers]") +TEST_CASE("FullBodyPoseRecord can be unpacked with DeviceDataTimestamp", "[full_body][flatbuffers]") { flatbuffers::FlatBufferBuilder builder(4096); - auto original = std::make_shared(); - original->data = std::make_shared(); - original->data->joints = std::make_unique(); + auto original = std::make_shared(); + original->data = std::make_shared(); + original->data->joints = std::make_unique(); original->timestamp = std::make_shared(111LL, 222LL, 333LL); - auto offset = core::FullBodyPosePicoRecord::Pack(builder, original.get()); + auto offset = core::FullBodyPoseRecord::Pack(builder, original.get()); builder.Finish(offset); - auto fb = flatbuffers::GetRoot(builder.GetBufferPointer()); - auto unpacked = std::make_shared(); + auto fb = flatbuffers::GetRoot(builder.GetBufferPointer()); + auto unpacked = std::make_shared(); fb->UnPackTo(unpacked.get()); CHECK(unpacked->timestamp->available_time_local_common_clock() == 111LL); CHECK(unpacked->timestamp->sample_time_local_common_clock() == 222LL); CHECK(unpacked->timestamp->sample_time_raw_device_clock() == 333LL); - CHECK(unpacked->data->joints->joints()->size() == static_cast(core::BodyJointPico_NUM_JOINTS)); + CHECK(unpacked->data->joints->joints()->size() == static_cast(core::BodyJoint_NUM_JOINTS)); } diff --git a/src/core/schema_tests/python/test_full_body.py b/src/core/schema_tests/python/test_full_body.py index a1a7f32ed..9f0ed179a 100644 --- a/src/core/schema_tests/python/test_full_body.py +++ b/src/core/schema_tests/python/test_full_body.py @@ -1,18 +1,18 @@ # SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # SPDX-License-Identifier: Apache-2.0 -"""Unit tests for FullBodyPosePicoT and related types in isaacteleop.schema. +"""Unit tests for FullBodyPoseT and related types in isaacteleop.schema. -FullBodyPosePicoT is a FlatBuffers table that represents full body pose data: -- joints: BodyJointsPico struct containing 24 BodyJointPose entries (XR_BD_body_tracking) +FullBodyPoseT is a FlatBuffers table that represents full body pose data: +- joints: BodyJoints struct containing 24 BodyJointPose entries (XR_BD_body_tracking) -BodyJointsPico is a struct with a fixed-size array of 24 BodyJointPose entries. +BodyJoints is a struct with a fixed-size array of 24 BodyJointPose entries. BodyJointPose is a struct containing: - pose: The Pose (position and orientation) - is_valid: Whether this joint data is valid -Timestamps are carried by FullBodyPosePicoRecord, not FullBodyPosePicoT. +Timestamps are carried by FullBodyPoseRecord, not FullBodyPoseT. Joint indices follow XrBodyJointBD enum: 0: Pelvis, 1-2: Left/Right Hip, 3: Spine1, 4-5: Left/Right Knee, @@ -24,11 +24,11 @@ import pytest from isaacteleop.schema import ( - FullBodyPosePicoT, - FullBodyPosePicoRecord, - BodyJointsPico, + FullBodyPoseT, + FullBodyPoseRecord, + BodyJoints, BodyJointPose, - BodyJointPico, + BodyJoint, Pose, Point, Quaternion, @@ -98,12 +98,12 @@ def test_repr(self): assert "Pose" in repr_str -class TestBodyJointsPicoStruct: - """Tests for BodyJointsPico struct.""" +class TestBodyJointsStruct: + """Tests for BodyJoints struct.""" def test_joints_access(self): """Test accessing all 24 joints via joints() method.""" - body_joints = BodyJointsPico() + body_joints = BodyJoints() for i in range(24): joint = body_joints.joints(i) @@ -111,138 +111,138 @@ def test_joints_access(self): def test_joints_out_of_range(self): """Test that accessing out of range index raises IndexError.""" - body_joints = BodyJointsPico() + body_joints = BodyJoints() with pytest.raises(IndexError): _ = body_joints.joints(24) -class TestBodyJointsPicoRepr: - """Tests for BodyJointsPico __repr__ method.""" +class TestBodyJointsRepr: + """Tests for BodyJoints __repr__ method.""" def test_repr(self): """Test __repr__ returns a meaningful string.""" - body_joints = BodyJointsPico() + body_joints = BodyJoints() repr_str = repr(body_joints) - assert "BodyJointsPico" in repr_str + assert "BodyJoints" in repr_str -class TestFullBodyPosePicoTConstruction: - """Tests for FullBodyPosePicoT construction and basic properties.""" +class TestFullBodyPoseTConstruction: + """Tests for FullBodyPoseT construction and basic properties.""" def test_default_construction(self): - """Test default construction creates FullBodyPosePicoT with pre-populated joints.""" - body_pose = FullBodyPosePicoT() + """Test default construction creates FullBodyPoseT with pre-populated joints.""" + body_pose = FullBodyPoseT() assert body_pose is not None assert body_pose.joints is not None def test_parameterized_construction(self): """Test construction with joints.""" - joints = BodyJointsPico() - body_pose = FullBodyPosePicoT(joints) + joints = BodyJoints() + body_pose = FullBodyPoseT(joints) assert body_pose.joints is not None -class TestFullBodyPosePicoTRepr: - """Tests for FullBodyPosePicoT __repr__ method.""" +class TestFullBodyPoseTRepr: + """Tests for FullBodyPoseT __repr__ method.""" def test_repr_default(self): """Test __repr__ with default construction.""" - body_pose = FullBodyPosePicoT() + body_pose = FullBodyPoseT() repr_str = repr(body_pose) - assert "FullBodyPosePicoT" in repr_str + assert "FullBodyPoseT" in repr_str -class TestBodyJointPicoEnum: - """Tests for BodyJointPico enum (joint index mapping for XR_BD_body_tracking).""" +class TestBodyJointEnum: + """Tests for BodyJoint enum (joint index mapping for XR_BD_body_tracking).""" def test_all_joint_values_exist(self): - """Test that all expected BodyJointPico enum values exist.""" - assert BodyJointPico.PELVIS is not None - assert BodyJointPico.LEFT_HIP is not None - assert BodyJointPico.RIGHT_HIP is not None - assert BodyJointPico.SPINE1 is not None - assert BodyJointPico.LEFT_KNEE is not None - assert BodyJointPico.RIGHT_KNEE is not None - assert BodyJointPico.SPINE2 is not None - assert BodyJointPico.LEFT_ANKLE is not None - assert BodyJointPico.RIGHT_ANKLE is not None - assert BodyJointPico.SPINE3 is not None - assert BodyJointPico.LEFT_FOOT is not None - assert BodyJointPico.RIGHT_FOOT is not None - assert BodyJointPico.NECK is not None - assert BodyJointPico.LEFT_COLLAR is not None - assert BodyJointPico.RIGHT_COLLAR is not None - assert BodyJointPico.HEAD is not None - assert BodyJointPico.LEFT_SHOULDER is not None - assert BodyJointPico.RIGHT_SHOULDER is not None - assert BodyJointPico.LEFT_ELBOW is not None - assert BodyJointPico.RIGHT_ELBOW is not None - assert BodyJointPico.LEFT_WRIST is not None - assert BodyJointPico.RIGHT_WRIST is not None - assert BodyJointPico.LEFT_HAND is not None - assert BodyJointPico.RIGHT_HAND is not None + """Test that all expected BodyJoint enum values exist.""" + assert BodyJoint.PELVIS is not None + assert BodyJoint.LEFT_HIP is not None + assert BodyJoint.RIGHT_HIP is not None + assert BodyJoint.SPINE1 is not None + assert BodyJoint.LEFT_KNEE is not None + assert BodyJoint.RIGHT_KNEE is not None + assert BodyJoint.SPINE2 is not None + assert BodyJoint.LEFT_ANKLE is not None + assert BodyJoint.RIGHT_ANKLE is not None + assert BodyJoint.SPINE3 is not None + assert BodyJoint.LEFT_FOOT is not None + assert BodyJoint.RIGHT_FOOT is not None + assert BodyJoint.NECK is not None + assert BodyJoint.LEFT_COLLAR is not None + assert BodyJoint.RIGHT_COLLAR is not None + assert BodyJoint.HEAD is not None + assert BodyJoint.LEFT_SHOULDER is not None + assert BodyJoint.RIGHT_SHOULDER is not None + assert BodyJoint.LEFT_ELBOW is not None + assert BodyJoint.RIGHT_ELBOW is not None + assert BodyJoint.LEFT_WRIST is not None + assert BodyJoint.RIGHT_WRIST is not None + assert BodyJoint.LEFT_HAND is not None + assert BodyJoint.RIGHT_HAND is not None def test_joint_values(self): - """Test that BodyJointPico values match expected indices.""" - assert int(BodyJointPico.PELVIS) == 0 - assert int(BodyJointPico.LEFT_HIP) == 1 - assert int(BodyJointPico.RIGHT_HIP) == 2 - assert int(BodyJointPico.SPINE1) == 3 - assert int(BodyJointPico.LEFT_KNEE) == 4 - assert int(BodyJointPico.RIGHT_KNEE) == 5 - assert int(BodyJointPico.SPINE2) == 6 - assert int(BodyJointPico.LEFT_ANKLE) == 7 - assert int(BodyJointPico.RIGHT_ANKLE) == 8 - assert int(BodyJointPico.SPINE3) == 9 - assert int(BodyJointPico.LEFT_FOOT) == 10 - assert int(BodyJointPico.RIGHT_FOOT) == 11 - assert int(BodyJointPico.NECK) == 12 - assert int(BodyJointPico.LEFT_COLLAR) == 13 - assert int(BodyJointPico.RIGHT_COLLAR) == 14 - assert int(BodyJointPico.HEAD) == 15 - assert int(BodyJointPico.LEFT_SHOULDER) == 16 - assert int(BodyJointPico.RIGHT_SHOULDER) == 17 - assert int(BodyJointPico.LEFT_ELBOW) == 18 - assert int(BodyJointPico.RIGHT_ELBOW) == 19 - assert int(BodyJointPico.LEFT_WRIST) == 20 - assert int(BodyJointPico.RIGHT_WRIST) == 21 - assert int(BodyJointPico.LEFT_HAND) == 22 - assert int(BodyJointPico.RIGHT_HAND) == 23 + """Test that BodyJoint values match expected indices.""" + assert int(BodyJoint.PELVIS) == 0 + assert int(BodyJoint.LEFT_HIP) == 1 + assert int(BodyJoint.RIGHT_HIP) == 2 + assert int(BodyJoint.SPINE1) == 3 + assert int(BodyJoint.LEFT_KNEE) == 4 + assert int(BodyJoint.RIGHT_KNEE) == 5 + assert int(BodyJoint.SPINE2) == 6 + assert int(BodyJoint.LEFT_ANKLE) == 7 + assert int(BodyJoint.RIGHT_ANKLE) == 8 + assert int(BodyJoint.SPINE3) == 9 + assert int(BodyJoint.LEFT_FOOT) == 10 + assert int(BodyJoint.RIGHT_FOOT) == 11 + assert int(BodyJoint.NECK) == 12 + assert int(BodyJoint.LEFT_COLLAR) == 13 + assert int(BodyJoint.RIGHT_COLLAR) == 14 + assert int(BodyJoint.HEAD) == 15 + assert int(BodyJoint.LEFT_SHOULDER) == 16 + assert int(BodyJoint.RIGHT_SHOULDER) == 17 + assert int(BodyJoint.LEFT_ELBOW) == 18 + assert int(BodyJoint.RIGHT_ELBOW) == 19 + assert int(BodyJoint.LEFT_WRIST) == 20 + assert int(BodyJoint.RIGHT_WRIST) == 21 + assert int(BodyJoint.LEFT_HAND) == 22 + assert int(BodyJoint.RIGHT_HAND) == 23 def test_all_joint_indices_accessible(self): - """Test that all BodyJointPico values can be used to index BodyJointsPico.""" - body_joints = BodyJointsPico() + """Test that all BodyJoint values can be used to index BodyJoints.""" + body_joints = BodyJoints() all_joints = [ - BodyJointPico.PELVIS, - BodyJointPico.LEFT_HIP, - BodyJointPico.RIGHT_HIP, - BodyJointPico.SPINE1, - BodyJointPico.LEFT_KNEE, - BodyJointPico.RIGHT_KNEE, - BodyJointPico.SPINE2, - BodyJointPico.LEFT_ANKLE, - BodyJointPico.RIGHT_ANKLE, - BodyJointPico.SPINE3, - BodyJointPico.LEFT_FOOT, - BodyJointPico.RIGHT_FOOT, - BodyJointPico.NECK, - BodyJointPico.LEFT_COLLAR, - BodyJointPico.RIGHT_COLLAR, - BodyJointPico.HEAD, - BodyJointPico.LEFT_SHOULDER, - BodyJointPico.RIGHT_SHOULDER, - BodyJointPico.LEFT_ELBOW, - BodyJointPico.RIGHT_ELBOW, - BodyJointPico.LEFT_WRIST, - BodyJointPico.RIGHT_WRIST, - BodyJointPico.LEFT_HAND, - BodyJointPico.RIGHT_HAND, + BodyJoint.PELVIS, + BodyJoint.LEFT_HIP, + BodyJoint.RIGHT_HIP, + BodyJoint.SPINE1, + BodyJoint.LEFT_KNEE, + BodyJoint.RIGHT_KNEE, + BodyJoint.SPINE2, + BodyJoint.LEFT_ANKLE, + BodyJoint.RIGHT_ANKLE, + BodyJoint.SPINE3, + BodyJoint.LEFT_FOOT, + BodyJoint.RIGHT_FOOT, + BodyJoint.NECK, + BodyJoint.LEFT_COLLAR, + BodyJoint.RIGHT_COLLAR, + BodyJoint.HEAD, + BodyJoint.LEFT_SHOULDER, + BodyJoint.RIGHT_SHOULDER, + BodyJoint.LEFT_ELBOW, + BodyJoint.RIGHT_ELBOW, + BodyJoint.LEFT_WRIST, + BodyJoint.RIGHT_WRIST, + BodyJoint.LEFT_HAND, + BodyJoint.RIGHT_HAND, ] assert len(all_joints) == 24 @@ -251,20 +251,20 @@ def test_all_joint_indices_accessible(self): assert joint_pose is not None def test_enum_int_conversion(self): - """Test that BodyJointPico values can be converted to integers.""" - assert int(BodyJointPico.PELVIS) == 0 - assert int(BodyJointPico.HEAD) == 15 - assert int(BodyJointPico.RIGHT_HAND) == 23 + """Test that BodyJoint values can be converted to integers.""" + assert int(BodyJoint.PELVIS) == 0 + assert int(BodyJoint.HEAD) == 15 + assert int(BodyJoint.RIGHT_HAND) == 23 -class TestFullBodyPosePicoRecordTimestamp: - """Tests for FullBodyPosePicoRecord with DeviceDataTimestamp.""" +class TestFullBodyPoseRecordTimestamp: + """Tests for FullBodyPoseRecord with DeviceDataTimestamp.""" def test_construction_with_timestamp(self): - """Test FullBodyPosePicoRecord carries DeviceDataTimestamp.""" - data = FullBodyPosePicoT() + """Test FullBodyPoseRecord carries DeviceDataTimestamp.""" + data = FullBodyPoseT() ts = DeviceDataTimestamp(1000000000, 2000000000, 3000000000) - record = FullBodyPosePicoRecord(data, ts) + record = FullBodyPoseRecord(data, ts) assert record.timestamp.available_time_local_common_clock == 1000000000 assert record.timestamp.sample_time_local_common_clock == 2000000000 @@ -272,16 +272,41 @@ def test_construction_with_timestamp(self): assert record.data is not None def test_default_construction(self): - """Test default FullBodyPosePicoRecord has no data.""" - record = FullBodyPosePicoRecord() + """Test default FullBodyPoseRecord has no data.""" + record = FullBodyPoseRecord() assert record.data is None def test_timestamp_fields(self): """Test all three DeviceDataTimestamp fields are accessible.""" - data = FullBodyPosePicoT() + data = FullBodyPoseT() ts = DeviceDataTimestamp(111, 222, 333) - record = FullBodyPosePicoRecord(data, ts) + record = FullBodyPoseRecord(data, ts) assert record.timestamp.available_time_local_common_clock == 111 assert record.timestamp.sample_time_local_common_clock == 222 assert record.timestamp.sample_time_raw_device_clock == 333 + + +class TestDeprecatedPicoAliases: + """Deprecated ``...Pico`` names still resolve to the renamed generic types and + now emit a DeprecationWarning on access.""" + + def test_aliases_resolve_and_warn(self): + """Each legacy schema name warns and resolves to its renamed generic type.""" + from isaacteleop import schema + + cases = [ + ("FullBodyPosePicoT", "FullBodyPoseT"), + ("FullBodyPosePicoTrackedT", "FullBodyPoseTrackedT"), + ("FullBodyPosePicoRecord", "FullBodyPoseRecord"), + ("BodyJointsPico", "BodyJoints"), + ("BodyJointPico", "BodyJoint"), + ] + for old, new in cases: + with pytest.warns(DeprecationWarning, match=old): + deprecated = getattr(schema, old) + assert deprecated is getattr(schema, new) + + # The aliased enum still exposes the same joint members/values. + with pytest.warns(DeprecationWarning, match="BodyJointPico"): + assert int(schema.BodyJointPico.RIGHT_HAND) == 23 diff --git a/src/core/teleop_session_manager/python/config.py b/src/core/teleop_session_manager/python/config.py index 172ba6f16..4eac46a8b 100644 --- a/src/core/teleop_session_manager/python/config.py +++ b/src/core/teleop_session_manager/python/config.py @@ -28,7 +28,10 @@ from .teleop_state_manager_types import teleop_control_states if TYPE_CHECKING: - from isaacteleop.deviceio_session import McapRecordingConfig, McapReplayConfig + from isaacteleop.deviceio_session import ( + McapRecordingConfig, + McapReplayConfig, + ) from teleopcore.oxr import OpenXRSessionHandles diff --git a/src/core/teleop_session_manager/python/helpers.py b/src/core/teleop_session_manager/python/helpers.py index 6ebc83f58..1b41bf3e7 100644 --- a/src/core/teleop_session_manager/python/helpers.py +++ b/src/core/teleop_session_manager/python/helpers.py @@ -14,11 +14,31 @@ ) +def _get_sources_from_pipeline(pipeline: Any) -> List[Any]: + """Discover the DeviceIO source leaf nodes of a retargeting pipeline. + + Traverses the pipeline's leaf nodes and returns all ``IDeviceIOSource`` + instances. Each source owns the tracker it depends on (``get_tracker()``) + and its vendor selection (``get_vendor()``). + + Args: + pipeline: A connected retargeting pipeline (the object returned by + ``BaseRetargeter.connect(...)`` or an ``OutputCombiner``). + + Returns: + List of ``IDeviceIOSource`` instances used by the pipeline, in leaf order. + """ + from isaacteleop.retargeting_engine.deviceio_source_nodes import IDeviceIOSource + + leaf_nodes = pipeline.get_leaf_nodes() + return [node for node in leaf_nodes if isinstance(node, IDeviceIOSource)] + + def _get_trackers_from_pipeline(pipeline: Any) -> List[Any]: """Discover the DeviceIO trackers required by a retargeting pipeline. - Traverses the pipeline's leaf nodes, identifies all ``IDeviceIOSource`` - instances, and returns the tracker that each source depends on. + Identifies all ``IDeviceIOSource`` leaf nodes and returns the tracker that + each source depends on. Args: pipeline: A connected retargeting pipeline (the object returned by @@ -27,21 +47,49 @@ def _get_trackers_from_pipeline(pipeline: Any) -> List[Any]: Returns: List of tracker instances used by the pipeline. """ - from isaacteleop.retargeting_engine.deviceio_source_nodes import IDeviceIOSource + return [source.get_tracker() for source in _get_sources_from_pipeline(pipeline)] - leaf_nodes = pipeline.get_leaf_nodes() - sources = [node for node in leaf_nodes if isinstance(node, IDeviceIOSource)] - return [source.get_tracker() for source in sources] +def build_vendor_config_from_sources(sources: List[Any]) -> Any: + """Build a ``deviceio.VendorConfig`` from source-carried vendor selections. + + Each ``IDeviceIOSource`` carries its own vendor selection (``get_vendor()``); + sources that report ``None`` use their tracker's default vendor and are + omitted. An empty result is the native default; ``None`` is never returned, + since passing it to the bindings is a type error. Session construction and + extension discovery both route through this, so they resolve identical + vendor configurations. + + Args: + sources: ``IDeviceIOSource`` instances (e.g. a pipeline's DeviceIO leaf + nodes, or a session's input sources). + + Returns: + A ``deviceio.VendorConfig`` keyed by tracker instance. + """ + import isaacteleop.deviceio as deviceio + + vendor_entries = [ + (source.get_tracker(), vendor) + for source in sources + if (vendor := source.get_vendor()) is not None + ] + return deviceio.VendorConfig(vendor_entries) def get_required_oxr_extensions_from_pipeline(pipeline: Any) -> List[str]: """Return the OpenXR extensions required by a retargeting pipeline. - Convenience wrapper that discovers trackers via - :func:`_get_trackers_from_pipeline` and passes them to - ``DeviceIOSession.get_required_extensions()``, which aggregates extensions - for known live tracker types (see ``LiveDeviceIOFactory`` in C++). + Convenience wrapper that discovers the pipeline's DeviceIO sources and passes + their trackers to ``DeviceIOSession.get_required_extensions()``, which + aggregates extensions for known live tracker types (see ``LiveDeviceIOFactory`` + in C++). + + Extensions are vendor-dependent, but each source carries its own vendor + (``get_vendor()``), so the result already reflects those selections with no + extra argument — including in the external-``oxr_handles`` flow, where you + create the OpenXR session yourself. Set the vendor on the source (see + ``FullBodySource``) and the enabled extensions match the session built. Example:: @@ -56,8 +104,12 @@ def get_required_oxr_extensions_from_pipeline(pipeline: Any) -> List[str]: """ import isaacteleop.deviceio as deviceio - trackers = _get_trackers_from_pipeline(pipeline) - extensions = deviceio.DeviceIOSession.get_required_extensions(trackers) + sources = _get_sources_from_pipeline(pipeline) + trackers = [source.get_tracker() for source in sources] + vendor_config = build_vendor_config_from_sources(sources) + extensions = deviceio.DeviceIOSession.get_required_extensions( + trackers, vendor_config + ) # Deduplicate — multiple trackers may require the same extensions. return sorted(set(extensions)) diff --git a/src/core/teleop_session_manager/python/teleop_session.py b/src/core/teleop_session_manager/python/teleop_session.py index 5d40e8278..c32665d75 100644 --- a/src/core/teleop_session_manager/python/teleop_session.py +++ b/src/core/teleop_session_manager/python/teleop_session.py @@ -55,6 +55,7 @@ snapshot_retargeter_io, snapshot_pipeline_inputs, ) +from .helpers import build_vendor_config_from_sources from .teleop_state_manager_types import teleop_control_states @@ -212,6 +213,8 @@ def __init__(self, config: TeleopSessionConfig): self._in_context: bool = False # Discover sources and external leaves from pipeline self._discover_sources() + # Vendor selection is a live-only concern; reject it up front in replay. + self._reject_vendor_selection_in_replay() @property def oxr_session(self) -> Optional[oxr.OpenXRSession]: @@ -307,6 +310,30 @@ def _discover_sources(self) -> None: tracker = source.get_tracker() self._tracker_to_source[id(tracker)] = source + def _reject_vendor_selection_in_replay(self) -> None: + """Reject source-carried vendor selections when mode is REPLAY. + + Vendor selection is a live-session concern: the live path routes each + source's ``get_vendor()`` into a ``VendorConfig``, but replay reads the + recorded channel regardless of vendor. Without this guard a vendor set on + a source would be silently ignored in REPLAY. Fail fast instead, matching + the fail-fast convention on the live path (unknown vendor ids and + non-vendored trackers both raise). + """ + if self.config.mode != SessionMode.REPLAY: + return + vendored = [ + source for source in self._sources if source.get_vendor() is not None + ] + if vendored: + names = ", ".join(sorted(source.name for source in vendored)) + raise ValueError( + f"Vendor selection is only valid in SessionMode.LIVE, but " + f"source(s) {names} carry a vendor and mode is SessionMode.REPLAY. " + f"Replay reads the recorded channel regardless of vendor; remove " + f"the vendor selection or run in LIVE mode." + ) + def get_external_input_specs(self) -> Dict[str, RetargeterIOType]: """Get the input specifications for all external leaf nodes that need inputs. @@ -947,6 +974,11 @@ def __enter__(self): def _enter_resources(self, stack: ExitStack) -> None: """Acquire and initialize resources for one context-manager run.""" + # config is mutable between construction and entry, so revalidate the + # replay-only vendor guard here: a mode flipped to REPLAY after __init__ + # must still fail fast rather than silently ignore a source's vendor. + self._reject_vendor_selection_in_replay() + # Reset run-scoped plugin containers on each context entry. self.plugin_managers = [] self.plugin_contexts = [] @@ -997,9 +1029,13 @@ def _add_tracker(tracker: Any) -> None: for tracker in self.config.trackers: _add_tracker(tracker) + # Vendored trackers carry their vendor on the source, so it travels + # with the pipeline into the VendorConfig (see the builder's docstring). + vendor_config = build_vendor_config_from_sources(self._sources) + # Get required extensions from all trackers required_extensions = deviceio.DeviceIOSession.get_required_extensions( - trackers + trackers, vendor_config ) # Resolve OpenXR handles @@ -1013,7 +1049,9 @@ def _add_tracker(tracker: Any) -> None: # Create DeviceIO session with all trackers self.deviceio_session = stack.enter_context( - deviceio.DeviceIOSession.run(trackers, handles, mcap_config) + deviceio.DeviceIOSession.run( + trackers, handles, mcap_config, vendor_config + ) ) # Initialize plugins (if any) diff --git a/src/core/teleop_session_manager_tests/python/test_pipeline_introspection.py b/src/core/teleop_session_manager_tests/python/test_pipeline_introspection.py index 95387c434..edf28bcd9 100644 --- a/src/core/teleop_session_manager_tests/python/test_pipeline_introspection.py +++ b/src/core/teleop_session_manager_tests/python/test_pipeline_introspection.py @@ -12,8 +12,12 @@ import sys from unittest.mock import MagicMock +import pytest + +import isaacteleop.deviceio as deviceio from isaacteleop.retargeting_engine.deviceio_source_nodes import ( ControllersSource, + FullBodySource, HandsSource, HeadSource, ) @@ -151,3 +155,69 @@ def test_multiple_sources_combine_extensions(self): # No duplicates assert len(extensions) == len(set(extensions)) + + +# ============================================================================ +# Source-carried vendor selection +# ============================================================================ + + +class TestSourceCarriedVendor: + """Vendor selection rides on the source and flows through pipeline introspection. + + Extensions are vendor-dependent; because each source carries its own vendor + (``get_vendor()``), ``get_required_oxr_extensions_from_pipeline`` reflects the + selection with no extra argument -- the external-``oxr_handles`` flow gets the + right extensions straight from the pipeline. + """ + + def test_non_vendored_source_reports_no_vendor(self): + """Sources with no vendor argument default to None (tracker default vendor).""" + assert HandsSource(name="hands").get_vendor() is None + assert FullBodySource(name="full_body").get_vendor() is None + + def test_source_exposes_its_vendor(self): + """A vendor passed to the source is retrievable via get_vendor().""" + vendor = deviceio.TrackerVendor("body.pico-xr") + body = FullBodySource(name="full_body", vendor=vendor) + + assert body.get_vendor() is vendor + + def test_source_vendor_drives_extensions(self): + """A vendored source contributes its vendor's extensions with no extra argument.""" + body = FullBodySource( + name="full_body", vendor=deviceio.TrackerVendor("body.pico-xr") + ) + pipeline = _mock_pipeline_with_leaf_nodes([body]) + + extensions = get_required_oxr_extensions_from_pipeline(pipeline) + + # The Pico body vendor contributes its native body-tracking extension. + assert "XR_BD_body_tracking" in extensions + + def test_default_vendor_matches_explicit_default(self): + """Omitting the vendor equals selecting the current (default) Pico vendor.""" + default_pipeline = _mock_pipeline_with_leaf_nodes( + [FullBodySource(name="full_body")] + ) + selected_pipeline = _mock_pipeline_with_leaf_nodes( + [ + FullBodySource( + name="full_body", vendor=deviceio.TrackerVendor("body.pico-xr") + ) + ] + ) + + assert get_required_oxr_extensions_from_pipeline( + selected_pipeline + ) == get_required_oxr_extensions_from_pipeline(default_pipeline) + + def test_unknown_vendor_id_raises(self): + """An unknown vendor id on a source is rejected during extension discovery.""" + body = FullBodySource( + name="full_body", vendor=deviceio.TrackerVendor("body.does-not-exist") + ) + pipeline = _mock_pipeline_with_leaf_nodes([body]) + + with pytest.raises(ValueError): + get_required_oxr_extensions_from_pipeline(pipeline) diff --git a/src/core/teleop_session_manager_tests/python/test_teleop_session.py b/src/core/teleop_session_manager_tests/python/test_teleop_session.py index 0ba568eba..586b58033 100644 --- a/src/core/teleop_session_manager_tests/python/test_teleop_session.py +++ b/src/core/teleop_session_manager_tests/python/test_teleop_session.py @@ -724,7 +724,7 @@ def mock_session_dependencies( if collected_trackers is not None: - def get_ext_side_effect(trackers): + def get_ext_side_effect(trackers, vendor_config=None): collected_trackers.extend(trackers) return [] @@ -2663,6 +2663,75 @@ def test_default_mode_is_live(self): assert config.mode == SessionMode.LIVE +class TestReplayModeRejectsVendorSelection: + """Vendor selection is live-only; a vendored source in REPLAY must fail fast.""" + + @staticmethod + def _vendored_head_source(): + source = MockHeadSource(name="head") + # MockDeviceIOSource defaults get_vendor() to None; set any non-None + # selection to simulate a vendored source. + source._vendor = object() + return source + + def test_vendored_source_in_replay_raises(self): + """A source carrying a vendor is rejected at construction in REPLAY mode.""" + config = TeleopSessionConfig( + app_name="test", + pipeline=MockPipeline(leaf_nodes=[self._vendored_head_source()]), + mode=SessionMode.REPLAY, + mcap_config=MagicMock(), + ) + with pytest.raises(ValueError, match="Vendor selection is only valid"): + TeleopSession(config) + + def test_unvendored_source_in_replay_is_allowed(self): + """A default (unvendored) source constructs fine in REPLAY mode.""" + config = TeleopSessionConfig( + app_name="test", + pipeline=MockPipeline(leaf_nodes=[MockHeadSource(name="head")]), + mode=SessionMode.REPLAY, + mcap_config=MagicMock(), + ) + TeleopSession(config) # get_vendor() is None -> no raise + + def test_vendored_source_in_live_is_allowed(self): + """A vendored source does not trip the replay-only guard in LIVE mode. + + Construction must not raise: vendor validation is a live-session concern + deferred to session entry (``__enter__`` -> ``VendorConfig``), so unlike + REPLAY there is no construction-time rejection here. + """ + config = TeleopSessionConfig( + app_name="test", + pipeline=MockPipeline(leaf_nodes=[self._vendored_head_source()]), + mode=SessionMode.LIVE, + ) + TeleopSession(config) # replay-only guard does not fire in LIVE -> no raise + + def test_mode_flipped_to_replay_after_construction_rejects_on_enter(self): + """The guard re-runs on context entry, not just at construction. + + config is mutable, so a session built in LIVE with a vendored source + (which does not raise at construction) that is later switched to REPLAY + must still fail fast on ``__enter__`` rather than silently ignore the + source's vendor while replaying. Replay dependencies are fully mocked so + the only thing that can reject entry is the revalidated vendor guard. + """ + config = TeleopSessionConfig( + app_name="test", + pipeline=MockPipeline(leaf_nodes=[self._vendored_head_source()]), + mode=SessionMode.LIVE, + ) + session = TeleopSession(config) # LIVE at construction -> no raise + config.mode = SessionMode.REPLAY + config.mcap_config = MagicMock() + with mock_replay_dependencies(): + with pytest.raises(ValueError, match="Vendor selection is only valid"): + with session: + pass + + class TestReplayModeSessionEnter: """Tests for TeleopSession.__enter__ when mode is REPLAY."""