diff --git a/docs/source/device/body_tracking.rst b/docs/source/device/body_tracking.rst index 26de8d0a0..e247d86fc 100644 --- a/docs/source/device/body_tracking.rst +++ b/docs/source/device/body_tracking.rst @@ -193,6 +193,14 @@ The ``all_joint_poses_tracked`` quality flag indicates whether every joint was successfully tracked in the current frame. When it is false, consult individual joint ``is_valid`` flags to determine which joints have valid poses. +For a minimal C++ reader see ``examples/schemaio/full_body_printer.cpp``, which +creates the tracker, queries the required OpenXR extensions, and prints the +joint data each frame through ``DeviceIOSession``. The Python equivalent is +``examples/oxr/python/test_full_body_tracker.py``. Running +``python -m isaacteleop.rig rigs/full_body.yaml`` starts the CloudXR runtime, +this printer, and the C++ MCAP recorder together in one tmux session (see +:ref:`rig-launcher`). + Troubleshooting ~~~~~~~~~~~~~~~ @@ -204,6 +212,11 @@ Troubleshooting enterprise features (see above). Verify all PICO motion trackers are paired, powered on, and calibrated. Confirm the PICO browser is up to date. +- **The printer prints** ``[body tracking inactive]``. The headset is + connected but body tracking is unavailable: check that the Motion Trackers + are paired and calibrated and that the headset has enterprise body-tracking + support. A printer that never reaches ``[Step 4] Reading samples...`` is + still waiting for the runtime / headset instead. - **Some joints report** ``is_valid: false``. The PICO runtime may temporarily lose tracking for individual joints during fast movement or partial occlusion. These joints will recover automatically once tracking is @@ -217,6 +230,15 @@ same retargeting pipeline — no headset required during replay. See :doc:`../references/mcap_record_replay` for the recording / replay API and the ``record_full_body.py`` / ``replay_full_body.py`` example. +Recording is also available directly from C++: pass a ``McapRecordingConfig`` +to ``DeviceIOSession::run()`` with the tracker mapped to the ``full_body`` +channel base name. ``examples/mcap_record_replay/cpp/record_full_body.cpp`` +demonstrates this — a file recorded there replays unchanged with +``replay_full_body.py``. The ``rigs/full_body.yaml`` rig includes a recorder +pane running this example; each :kbd:`Enter` rerun in that pane writes a fresh +timestamped take into ``examples/mcap_record_replay/recordings/``, where +``replay_full_body.py`` looks by default. + .. figure:: ../_static/full-body-replay.gif :alt: Full body skeleton replayed from an MCAP recording in viser :class: no-image-zoom diff --git a/docs/source/device/trackers.rst b/docs/source/device/trackers.rst index 7bcac1bef..3334ff0f0 100644 --- a/docs/source/device/trackers.rst +++ b/docs/source/device/trackers.rst @@ -205,6 +205,13 @@ reads the PICO ``XR_BD_body_tracking`` extension directly. - :code-file:`src/core/schema_tests/python/test_full_body.py` - :code-file:`examples/oxr/python/test_full_body_tracker.py` +- Examples: + + - :code-file:`examples/schemaio/full_body_printer.cpp` + - :code-file:`examples/mcap_record_replay/cpp/record_full_body.cpp` + - :code-file:`examples/mcap_record_replay/python/record_full_body.py` + - :code-file:`examples/mcap_record_replay/python/replay_full_body.py` + .. note:: ``FullBodyTrackerPico`` remains available as a deprecated alias for diff --git a/docs/source/references/mcap_record_replay.rst b/docs/source/references/mcap_record_replay.rst index 9ed4d2edf..f72143099 100644 --- a/docs/source/references/mcap_record_replay.rst +++ b/docs/source/references/mcap_record_replay.rst @@ -133,6 +133,15 @@ additional source nodes (``HeadSource``, ``ControllersSource``, …) in For a live browser view of **all** human DeviceIO trackers at once (hands, head, controllers, and full body), see ``examples/deviceio_live_view/python/``. +A C++ recorder lives at ``examples/mcap_record_replay/cpp/``: + +- ``record_full_body.cpp`` — records the ``full_body`` channel by passing a + ``core::McapRecordingConfig`` to ``DeviceIOSession::run()``. It uses the + same channel base name as ``FullBodySource``, so the resulting file replays + with ``replay_full_body.py``. ``python -m isaacteleop.rig rigs/full_body.yaml`` + runs it together with the CloudXR runtime and the full-body printer (see + :ref:`rig-launcher`). + Recording ^^^^^^^^^ diff --git a/docs/source/references/rig.rst b/docs/source/references/rig.rst index 80a1eb470..d574dcc37 100644 --- a/docs/source/references/rig.rst +++ b/docs/source/references/rig.rst @@ -119,6 +119,10 @@ own: - name: se3 printer (requires headset) command: "install/examples/schemaio/se3_printer {collection_id}" +``rigs/full_body.yaml`` shows the other supported shape — no ``producers`` +key, because its consumers read the tracking data directly from the runtime, +so there is no ``collection_id`` to rendezvous on. + Top-level keys: .. list-table:: @@ -163,10 +167,10 @@ errors — a typo fails loudly at load time instead of misbehaving in a pane. .. important:: - Producers and consumers rendezvous on a shared ``collection_id``, and a - mismatch is **silent no-data** by design. Define it once under ``params`` - and reference it as ``{collection_id}`` in every command — then one edit - in one place changes both sides together. + In a rig with both producers and consumers, the two sides rendezvous on a + shared ``collection_id``, and a mismatch is **silent no-data** by design. + Define it once under ``params`` and reference it as ``{collection_id}`` in + every command — then one edit in one place changes both sides together. .. warning:: diff --git a/examples/mcap_record_replay/CMakeLists.txt b/examples/mcap_record_replay/CMakeLists.txt index 682432fef..7a977c5e2 100644 --- a/examples/mcap_record_replay/CMakeLists.txt +++ b/examples/mcap_record_replay/CMakeLists.txt @@ -3,5 +3,8 @@ cmake_minimum_required(VERSION 3.20) +# Build C++ examples +add_subdirectory(cpp) + include(${CMAKE_SOURCE_DIR}/cmake/InstallPythonExample.cmake) install_python_example(DESTINATION examples/mcap_record_replay/python) diff --git a/examples/mcap_record_replay/cpp/CMakeLists.txt b/examples/mcap_record_replay/cpp/CMakeLists.txt new file mode 100644 index 000000000..18d72260d --- /dev/null +++ b/examples/mcap_record_replay/cpp/CMakeLists.txt @@ -0,0 +1,24 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.20) + +find_package(Threads REQUIRED) + +# Full body MCAP recorder example +add_executable(record_full_body + record_full_body.cpp +) + +target_link_libraries(record_full_body PRIVATE + deviceio::deviceio_session + deviceio::deviceio_trackers + oxr::oxr_core + isaacteleop_schema + Threads::Threads +) + +# Install +install(TARGETS record_full_body + RUNTIME DESTINATION examples/mcap_record_replay/cpp +) diff --git a/examples/mcap_record_replay/cpp/record_full_body.cpp b/examples/mcap_record_replay/cpp/record_full_body.cpp new file mode 100644 index 000000000..b09c44038 --- /dev/null +++ b/examples/mcap_record_replay/cpp/record_full_body.cpp @@ -0,0 +1,161 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +/*! + * @file record_full_body.cpp + * @brief Record a live full-body tracking session to an MCAP file using only the C++ API. + * + * Creates a FullBodyTracker, opens an OpenXR session with its required extensions, and + * passes a McapRecordingConfig to DeviceIOSession::run() — the tracker impl then writes + * MCAP samples during each session->update() call. This application does not launch the + * CloudXR runtime; start it (and connect the headset) before running, or use the full_body + * rig (rigs/full_body.yaml) to launch the runtime, printer, and recorder together. + * + * Usage: + * record_full_body [duration_seconds] [output.mcap | output_dir/] + * + * Defaults: 5 seconds -> full_body_.mcap in the current directory. When the + * output argument is a directory (existing, or with a trailing slash), it is created if + * needed and each run writes a fresh timestamped file inside it. + * + * The recording is written to the standard "full_body" channel, so any Isaac Teleop + * replay tooling can read it (see the MCAP record & replay documentation). + */ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace +{ + +// Standard channel base name for full-body recordings. MCAP topics become +// "/", and replay tooling reads the "full_body" channel, so this +// must not change. +constexpr const char* MCAP_BASE_NAME = "full_body"; + +std::string default_output_path() +{ + const std::time_t now = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); + std::tm tm_buf{}; + localtime_r(&now, &tm_buf); + std::ostringstream name; + name << "full_body_" << std::put_time(&tm_buf, "%Y%m%d_%H%M%S") << ".mcap"; + return name.str(); +} + +// An existing directory (or a path with a trailing slash) receives a timestamped file +// inside it — created if needed — so repeated runs never clobber a previous take. +std::string resolve_output_path(const std::string& arg) +{ + const std::filesystem::path path(arg); + if (std::filesystem::is_directory(path) || (!arg.empty() && arg.back() == '/')) + { + std::filesystem::create_directories(path); + return (path / default_output_path()).string(); + } + return arg; +} + +uint32_t count_valid_joints(const core::FullBodyPoseT& data) +{ + uint32_t valid_count = 0; + for (uint32_t i = 0; i < core::FullBodyTracker::JOINT_COUNT; ++i) + { + if ((*data.joints->joints())[i]->is_valid()) + { + ++valid_count; + } + } + return valid_count; +} + +} // namespace + +int main(int argc, char** argv) +try +{ + const double duration_s = (argc > 1) ? std::stod(argv[1]) : 5.0; + const std::string mcap_path = (argc > 2) ? resolve_output_path(argv[2]) : default_output_path(); + + std::cout << "[record] writing " << mcap_path << " for " << duration_s << "s" << std::endl; + + // Step 1: Create the tracker + auto tracker = std::make_shared(); + + // Step 2: Get required extensions and create OpenXR session + std::vector> trackers = { tracker }; + auto required_extensions = core::DeviceIOSession::get_required_extensions(trackers); + + auto oxr_session = std::make_shared("McapFullBodyRecordExample", required_extensions); + + // Step 3: Create DeviceIOSession with recording enabled + core::McapRecordingConfig recording_config{ mcap_path, { { tracker.get(), MCAP_BASE_NAME } } }; + + auto session = core::DeviceIOSession::run(trackers, oxr_session->get_handles(), std::move(recording_config)); + + // Step 4: Update the session for the requested duration. The tracker impl writes the MCAP + // sample during each update(); no explicit write call is needed here. + const auto start = std::chrono::steady_clock::now(); + size_t frame_count = 0; + while (true) + { + const double elapsed_s = std::chrono::duration(std::chrono::steady_clock::now() - start).count(); + if (elapsed_s >= duration_s) + { + break; + } + + session->update(); + + if (frame_count % 60 == 0) + { + const auto& tracked = tracker->get_body_pose(*session); + std::cout << "[record] t=" << std::fixed << std::setprecision(2) << elapsed_s << "s frame=" << frame_count; + if (tracked.data) + { + std::cout << " joints=" << count_valid_joints(*tracked.data) << "/" + << core::FullBodyTracker::JOINT_COUNT; + } + else + { + std::cout << " [body tracking inactive]"; + } + std::cout << std::endl; + } + ++frame_count; + + // Tick at ~60 Hz. + std::this_thread::sleep_for(std::chrono::milliseconds(16)); + } + + // Destroying the session closes the MCAP writer and emits the summary/statistics block that + // replay tooling relies on — exit normally rather than aborting. + session.reset(); + + std::cout << "[record] done — " << mcap_path << std::endl; + return 0; +} +catch (const std::exception& e) +{ + std::cerr << argv[0] << ": " << e.what() << std::endl; + return 1; +} +catch (...) +{ + std::cerr << argv[0] << ": Unknown error occurred" << std::endl; + return 1; +} diff --git a/examples/schemaio/CMakeLists.txt b/examples/schemaio/CMakeLists.txt index cbd140f2d..938f8e441 100644 --- a/examples/schemaio/CMakeLists.txt +++ b/examples/schemaio/CMakeLists.txt @@ -47,6 +47,19 @@ target_link_libraries(se3_printer PRIVATE Threads::Threads ) +# Create full body tracker printer executable +add_executable(full_body_printer + full_body_printer.cpp +) + +target_link_libraries(full_body_printer PRIVATE + deviceio::deviceio_session + deviceio::deviceio_trackers + oxr::oxr_core + isaacteleop_schema + Threads::Threads +) + # Create frame metadata printer executable add_executable(frame_metadata_printer frame_metadata_printer.cpp @@ -62,6 +75,6 @@ target_link_libraries(frame_metadata_printer PRIVATE ) # Install -install(TARGETS pedal_pusher pedal_printer se3_printer frame_metadata_printer +install(TARGETS pedal_pusher pedal_printer se3_printer full_body_printer frame_metadata_printer RUNTIME DESTINATION examples/schemaio ) diff --git a/examples/schemaio/full_body_printer.cpp b/examples/schemaio/full_body_printer.cpp new file mode 100644 index 000000000..42f4a5db5 --- /dev/null +++ b/examples/schemaio/full_body_printer.cpp @@ -0,0 +1,151 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +/*! + * @file full_body_printer.cpp + * @brief Standalone application that reads and prints PICO full-body poses from the OpenXR runtime. + * + * This application demonstrates using FullBodyTracker (XR_BD_body_tracking) to read the + * 24-joint body skeleton through DeviceIOSession. Requires a runtime with body tracking support + * (e.g. CloudXR streaming from a PICO 4 Ultra Enterprise with Motion Trackers); when the system + * does not support body tracking the tracker runs in limp mode and no samples are printed. + * The full_body rig (rigs/full_body.yaml) launches this printer together with the CloudXR + * runtime and the MCAP recorder in one tmux session. + * + * To record a full-body session to MCAP from C++, see + * examples/mcap_record_replay/cpp/record_full_body.cpp. + */ + +#include "common_utils.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +using namespace schemaio_example; + +namespace +{ + +// Joint poses are unspecified while their tracking is lost — gate on is_valid per joint +// (all_joint_poses_tracked is a whole-skeleton quality flag only). +void print_joint(const char* label, const core::BodyJointPose& joint) +{ + if (joint.is_valid()) + { + const auto& position = joint.pose().position(); + std::cout << " " << label << "=[" << position.x() << ", " << position.y() << ", " << position.z() << "]"; + } + else + { + std::cout << " " << label << "=[not tracked]"; + } +} + +void print_body_pose(const core::FullBodyPoseT& data, size_t sample_count) +{ + const auto& joints = *data.joints->joints(); + + uint32_t valid_count = 0; + for (uint32_t i = 0; i < core::FullBodyTracker::JOINT_COUNT; ++i) + { + if (joints[i]->is_valid()) + { + ++valid_count; + } + } + + std::cout << "Sample " << sample_count << std::fixed << std::setprecision(3) << " valid=" << valid_count << "/" + << core::FullBodyTracker::JOINT_COUNT; + print_joint("pelvis", *joints[core::BodyJoint_PELVIS]); + print_joint("head", *joints[core::BodyJoint_HEAD]); + std::cout << std::endl; +} + +} // namespace + +int main(int argc, char** argv) +try +{ + std::cout << "Full Body Printer (XR_BD_body_tracking)" << std::endl; + + // Step 1: Create the tracker + std::cout << "[Step 1] Creating FullBodyTracker..." << std::endl; + auto tracker = std::make_shared(); + + // Step 2: Get required extensions and create OpenXR session + std::cout << "[Step 2] Creating OpenXR session with required extensions..." << std::endl; + + std::vector> trackers = { tracker }; + auto required_extensions = core::DeviceIOSession::get_required_extensions(trackers); + + auto oxr_session = std::make_shared("FullBodyPrinter", required_extensions); + + std::cout << " OpenXR session created" << std::endl; + + // Step 3: Create DeviceIOSession with the tracker + std::cout << "[Step 3] Creating DeviceIOSession..." << std::endl; + + std::unique_ptr session; + session = core::DeviceIOSession::run(trackers, oxr_session->get_handles()); + + // Step 4: Read samples by updating the session + std::cout << "[Step 4] Reading samples..." << std::endl; + + // Bound the loop so it cannot spin forever when no samples ever arrive (limp mode): a + // healthy session delivers a sample almost every tick, so twice the sample budget is ample. + constexpr size_t MAX_TICKS = 2 * MAX_SAMPLES; + + size_t received_count = 0; + size_t tick_count = 0; + while (received_count < MAX_SAMPLES && tick_count < MAX_TICKS) + { + // Update session (this calls update on all trackers). + session->update(); + + // Print current data if available. tracked.data is null only in limp mode (body tracking + // unsupported); a supported-but-untracked body still delivers data with valid=0/24 joints. + const auto& tracked = tracker->get_body_pose(*session); + if (tracked.data) + { + print_body_pose(*tracked.data, received_count++); + } + else if (tick_count % 30 == 0) + { + // Heartbeat once per second (~30th tick at 30 Hz) so an inactive session is visible + // instead of silent. Same literal as record_full_body.cpp so both siblings report + // this state identically. + std::cout << "[body tracking inactive]" << std::endl; + } + ++tick_count; + + // Tick at ~30 Hz. + std::this_thread::sleep_for(std::chrono::milliseconds(33)); + } + + if (received_count == 0) + { + std::cout << "\nNo body tracking samples received — body tracking is inactive or unsupported." << std::endl; + } + std::cout << "\nDone. Received " << received_count << " samples." << std::endl; + return 0; +} +catch (const std::exception& e) +{ + std::cerr << argv[0] << ": " << e.what() << std::endl; + return 1; +} +catch (...) +{ + std::cerr << argv[0] << ": Unknown error occurred" << std::endl; + return 1; +} diff --git a/rigs/full_body.yaml b/rigs/full_body.yaml new file mode 100644 index 000000000..ae33499d9 --- /dev/null +++ b/rigs/full_body.yaml @@ -0,0 +1,29 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# Run with: python -m isaacteleop.rig rigs/full_body.yaml +# +# No producers: the two consumer panes below are C++ readers pulling +# XR_BD_body_tracking joints directly from the OpenXR runtime — no producer +# plugin, no collection_id. The launcher always adds the CloudXR runtime pane +# (no `runtime:` override needed), so the session is three panes: runtime + +# the two consumers below. Requires a runtime with body tracking (e.g. CloudXR +# streaming from a PICO 4 Ultra Enterprise with Motion Trackers). A pane that +# started before the headset connected exits with 'Failed to get OpenXR +# system' — press Enter to rerun. The printer prints '[body tracking inactive]' +# once per second when the headset is connected but body tracking is +# unavailable (calibration / enterprise support). Each recorder run writes a +# fresh full_body_.mcap into examples/mcap_record_replay/recordings/ +# (gitignored; created if missing; recorder args: [duration_seconds, default +# 5 s] [output.mcap | output_dir/]); exiting after ~5 s is SUCCESS — replay +# headless with examples/mcap_record_replay/python/replay_full_body.py, which +# picks up the newest recording there by default. +# See rigs/se3_tracker.yaml for the fully annotated exemplar of every key. +name: full_body +description: CloudXR runtime + full-body joint printer + MCAP recorder (PICO 4 Ultra Enterprise with Motion Trackers) +cwd: .. # -> Teleop repo root +consumers: + - name: full body printer (requires headset + motion trackers) + command: "install/examples/schemaio/full_body_printer" + - name: full body recorder (check printer pane first; Enter = new timestamped take) + command: "install/examples/mcap_record_replay/cpp/record_full_body 5 examples/mcap_record_replay/recordings/" diff --git a/src/core/rig_tests/python/test_config.py b/src/core/rig_tests/python/test_config.py index 6618ddcd6..14a699041 100644 --- a/src/core/rig_tests/python/test_config.py +++ b/src/core/rig_tests/python/test_config.py @@ -21,6 +21,7 @@ REPO_ROOT = Path(__file__).resolve().parents[4] SE3_RIG = REPO_ROOT / "rigs" / "se3_tracker.yaml" +FULL_BODY_RIG = REPO_ROOT / "rigs" / "full_body.yaml" def write_rig(tmp_path: Path, body: str) -> Path: @@ -38,7 +39,7 @@ def write_rig(tmp_path: Path, body: str) -> Path: # --------------------------------------------------------------------------- -# Loading the shipped exemplar +# Loading the shipped rigs # --------------------------------------------------------------------------- @@ -67,6 +68,30 @@ def test_se3_rig_has_no_footguns(): ) +def test_load_full_body_rig(): + config = load_rig_config(FULL_BODY_RIG) + assert config.name == "full_body" + assert config.description + assert config.cwd == REPO_ROOT # cwd: .. resolves against rigs/ + assert config.runtime is None + assert config.runtime_command == DEFAULT_RUNTIME_COMMAND + # Consumer-only rig: both panes read the runtime directly, so there are no + # producers, no params, and no collection_id to rendezvous on. + assert config.params == {} + assert len(config.producers) == 0 + assert len(config.consumers) == 2 + + +def test_full_body_rig_has_no_footguns(): + config = load_rig_config(FULL_BODY_RIG) + assert ( + find_runtime_footguns( + [*config.producers, *config.consumers], runtime_managed=True + ) + == [] + ) + + # --------------------------------------------------------------------------- # Validation errors (each a hard error naming the file) # ---------------------------------------------------------------------------