From c813add8829d79f67e0d7433899d32b66b22f0fa Mon Sep 17 00:00:00 2001 From: zhimin Date: Fri, 16 Oct 2026 11:37:26 +0800 Subject: [PATCH 1/2] feat(ego): migrate stereo capture support to NVIDIA main Port the EGO plugin, live/replay trackers, structured schemas, MCAP media and timing tools, Python helpers, and stereo preview from orbbec/IsaacTeleop@e87ad563543aa8eec1ef0b341eb7884e3a6a28fb onto NVIDIA/IsaacCapture@ef804ef637fc6da25f22fdff5b34719e47a99a2b. Preserve main controller monitoring, gamepad/spacemouse support, packaging licenses and APIs, and category-based docs. Keep EGO schemas and golden files byte-identical to the source branch. Enable the EGO CI workflow on the Orbbec stereo and EGOPRO development branches. Validation: 218 C++ tests, 447 schema/session Python tests, 140 EGO/MCAP Python tests, 45 camera preview unit tests, all-files pre-commit, strict Sphinx build, and classic wheel/stub generation. The preview tests use a viz import stub in the BUILD_VIZ=OFF configuration; native preview/codec extensions were separately built and imported. Physical EGO FW 1.1.11 with SDK 20261012: stereo H264 1600x1300@30, 302 frames per eye, 4400 accel and gyro samples each, clean decode, embedded MCAP export matching sidecars, and native replay. Calibration reads were unavailable and remain an explicit hardware/SDK limitation. EGOPRO four-stream changes are outside this migration. Signed-off-by: zhimin --- .github/workflows/test-ego-software.yaml | 115 + .gitignore | 15 + CMakeLists.txt | 2 + docs/source/device/cameras/index.rst | 1 + docs/source/device/ego.rst | 317 ++ examples/camera_viz/README.md | 23 +- examples/camera_viz/camera_viz.py | 16 +- examples/camera_viz/camera_viz.sh | 6 +- examples/camera_viz/codec/__init__.py | 4 + examples/camera_viz/codec/codec_bindings.cpp | 9 +- examples/camera_viz/codec/h264_decoder.cpp | 17 +- .../camera_viz/codec/inc/h264_decoder.hpp | 12 +- examples/camera_viz/config.py | 34 +- examples/camera_viz/configs/ego.yaml | 28 + .../camera_viz/ego_preview/CMakeLists.txt | 39 + examples/camera_viz/ego_preview/README.md | 124 + examples/camera_viz/ego_preview/__init__.py | 19 + examples/camera_viz/ego_preview/build.sh | 43 + .../camera_viz/ego_preview/ego_capture.cpp | 219 ++ examples/camera_viz/pyproject.toml | 1 + examples/camera_viz/scripts/_install_deps.sh | 28 +- examples/camera_viz/sources/__init__.py | 23 +- examples/camera_viz/sources/_nv_decode.py | 23 +- examples/camera_viz/sources/ego.py | 313 ++ examples/camera_viz/sources/stereo_sbs.py | 73 + examples/mcap_record_replay/README.md | 57 +- .../mcap_record_replay/analyze_ego_sync.py | 75 + .../mcap_record_replay/calibrate_ego_sync.py | 219 ++ .../mcap_record_replay/replay_ego.py | 242 ++ examples/oxr/python/test_ego_camera.py | 126 + pyproject.toml | 3 + src/compat/pyproject.toml | 1 + .../ego_frame_metadata_tracker_base.hpp | 23 + .../deviceio_base/ego_imu_tracker_base.hpp | 23 + src/core/deviceio_trackers/cpp/CMakeLists.txt | 4 + .../cpp/ego_frame_metadata_tracker.cpp | 48 + .../deviceio_trackers/cpp/ego_imu_tracker.cpp | 49 + .../ego_frame_metadata_tracker.hpp | 65 + .../inc/deviceio_trackers/ego_imu_tracker.hpp | 56 + .../python/tracker_bindings.cpp | 25 + src/core/deviceio_trackers/trackers.toml | 24 + src/core/live_trackers/cpp/CMakeLists.txt | 4 + .../live_trackers/live_deviceio_factory.hpp | 7 + .../cpp/live_deviceio_factory.cpp | 41 + .../live_ego_frame_metadata_tracker_impl.cpp | 74 + .../live_ego_frame_metadata_tracker_impl.hpp | 53 + .../cpp/live_ego_imu_tracker_impl.cpp | 62 + .../cpp/live_ego_imu_tracker_impl.hpp | 46 + .../cpp/inc/plugin_manager/plugin.hpp | 5 +- .../cpp/inc/plugin_manager/plugin_manager.hpp | 4 +- src/core/plugin_manager/cpp/plugin.cpp | 15 +- .../plugin_manager/cpp/plugin_manager.cpp | 5 +- .../python/plugin_manager_bindings.cpp | 2 +- src/core/python/CMakeLists.txt | 1 + src/core/python/pyproject.toml.in | 1 + src/core/python/requirements-ego.txt | 5 + src/core/replay_trackers/cpp/CMakeLists.txt | 2 + .../replay_deviceio_factory.hpp | 7 + .../cpp/replay_deviceio_factory.cpp | 31 + .../cpp/replay_ego_tracker_impl.cpp | 88 + .../cpp/replay_ego_tracker_impl.hpp | 54 + .../cpp/inc/schema/ego_device_state_utils.hpp | 43 + src/core/schema/cpp/inc/schema/ego_limits.hpp | 16 + src/core/schema/fbs/ego_audio.fbs | 59 + src/core/schema/fbs/ego_calibration.fbs | 51 + src/core/schema/fbs/ego_camera.fbs | 64 + src/core/schema/fbs/ego_device_state.fbs | 65 + src/core/schema/fbs/ego_imu.fbs | 35 + src/core/schema/golden/ego_audio.bfbs | Bin 0 -> 2304 bytes src/core/schema/golden/ego_calibration.bfbs | Bin 0 -> 2180 bytes src/core/schema/golden/ego_camera.bfbs | Bin 0 -> 2752 bytes src/core/schema/golden/ego_device_state.bfbs | Bin 0 -> 3592 bytes src/core/schema/golden/ego_imu.bfbs | Bin 0 -> 1920 bytes src/core/schema/python/CMakeLists.txt | 2 + src/core/schema/python/ego_bindings.h | 501 ++++ src/core/schema/python/ego_camera_bindings.h | 123 + src/core/schema/python/schema_module.cpp | 4 + src/plugins/CMakeLists.txt | 3 + src/plugins/ego/CMakeLists.txt | 65 + src/plugins/ego/README.md | 589 ++++ src/plugins/ego/app/CMakeLists.txt | 26 + src/plugins/ego/app/main.cpp | 337 +++ src/plugins/ego/core/CMakeLists.txt | 64 + src/plugins/ego/core/calibration.cpp | 205 ++ src/plugins/ego/core/calibration.hpp | 18 + src/plugins/ego/core/device_controls.cpp | 215 ++ src/plugins/ego/core/device_controls.hpp | 29 + src/plugins/ego/core/device_diagnostics.cpp | 70 + src/plugins/ego/core/device_diagnostics.hpp | 34 + src/plugins/ego/core/diagnostics.hpp | 60 + src/plugins/ego/core/ego_camera.cpp | 2594 +++++++++++++++++ src/plugins/ego/core/frame_sink.cpp | 387 +++ .../ego/core/inc/ego_camera/cancellation.hpp | 40 + .../ego/core/inc/ego_camera/ego_camera.hpp | 242 ++ .../core/inc/ego_camera/time_alignment.hpp | 62 + src/plugins/ego/core/mcap_metadata_sink.cpp | 311 ++ src/plugins/ego/core/metadata_sink.cpp | 199 ++ src/plugins/ego/core/metadata_sink.hpp | 18 + src/plugins/ego/core/preview.cpp | 415 +++ src/plugins/ego/core/preview.hpp | 29 + src/plugins/ego/core/schema_metadata_sink.cpp | 349 +++ src/plugins/ego/core/time_alignment.cpp | 411 +++ src/plugins/ego/core/wav_writer.hpp | 137 + src/plugins/ego/ego.example.toml | 36 + src/plugins/ego/ego.sh | 986 +++++++ src/plugins/ego/mcap/CMakeLists.txt | 8 + .../ego/mcap/export_media_tool/CMakeLists.txt | 8 + .../export_embedded_media.cpp | 206 ++ src/plugins/ego/mcap/io/CMakeLists.txt | 9 + .../mcap/io/inc/ego_mcap/checked_write.hpp | 24 + src/plugins/ego/mcap/io/mcap_impl.cpp | 6 + src/plugins/ego/mcap/merge/CMakeLists.txt | 8 + .../ego/mcap/merge/inc/ego_mcap/merge.hpp | 14 + src/plugins/ego/mcap/merge/merge.cpp | 333 +++ .../ego/mcap/merge_tool/CMakeLists.txt | 9 + src/plugins/ego/mcap/merge_tool/main.cpp | 47 + src/plugins/ego/plugin.yaml | 11 + src/plugins/ego/tools/detect_media_mode.py | 15 + src/plugins/ego/tools/record_config.py | 82 + src/plugins/ego/tools/verify_profile.py | 77 + src/plugins/ego/tools/verify_sensor_topics.py | 471 +++ src/plugins/ego/tools/verify_video_timing.py | 191 ++ .../deviceio_trackers/__init__.py | 4 + src/python/isaaccapture/ego/__init__.py | 64 + src/python/isaaccapture/ego/_records.py | 231 ++ src/python/isaaccapture/ego/calibration.py | 380 +++ src/python/isaaccapture/ego/recording.py | 274 ++ src/python/isaaccapture/ego/sync.py | 1423 +++++++++ src/python/isaaccapture/schema/__init__.py | 66 + .../teleop_session_manager/config.py | 9 + .../teleop_session_manager/teleop_session.py | 7 +- tests/cpp/core/live_trackers/CMakeLists.txt | 1 + .../core/live_trackers/test_ego_dispatch.cpp | 52 + .../plugin_manager/test_plugin_manager.cpp | 52 + .../test_process/test_process.cpp | 15 +- .../replay_deviceio_session/CMakeLists.txt | 1 + .../test_ego_replay.cpp | 246 ++ tests/cpp/core/schema/CMakeLists.txt | 1 + tests/cpp/core/schema/test_ego_camera.cpp | 295 ++ tests/cpp/plugins/CMakeLists.txt | 12 + tests/cpp/plugins/ego/CMakeLists.txt | 24 + tests/cpp/plugins/ego/test_ego_camera.cpp | 900 ++++++ tests/cpp/plugins/ego/test_ego_script.sh | 697 +++++ tests/cpp/plugins/ego/test_time_alignment.cpp | 346 +++ tests/cpp/plugins/ego_mcap/CMakeLists.txt | 6 + tests/cpp/plugins/ego_mcap/test_merge.cpp | 336 +++ tests/cpp/plugins/ego_media/CMakeLists.txt | 9 + tests/cpp/plugins/ego_media/test_export.cpp | 163 ++ tests/python/CMakeLists.txt | 4 + tests/python/core/CMakeLists.txt | 1 + tests/python/core/ego/CMakeLists.txt | 18 + tests/python/core/ego/pyproject.toml | 13 + tests/python/core/ego/test_calibration.py | 327 +++ tests/python/core/ego/test_recording.py | 378 +++ tests/python/core/ego/test_sync.py | 1048 +++++++ tests/python/core/schema/test_ego_camera.py | 241 ++ .../test_teleop_session.py | 60 +- .../camera_viz/test_ego_and_stereo_sbs.py | 338 +++ .../examples/camera_viz/test_ego_failures.py | 361 +++ .../camera_viz/test_nv_decode_codec.py | 53 + .../mcap_record_replay/CMakeLists.txt | 26 + .../examples/mcap_record_replay/conftest.py | 16 + .../mcap_record_replay/pyproject.toml | 10 + .../test_ego_calibration_cli.py | 226 ++ .../mcap_record_replay/test_ego_replay.py | 190 ++ .../mcap_record_replay/test_ego_sync_cli.py | 140 + 166 files changed, 22470 insertions(+), 58 deletions(-) create mode 100644 .github/workflows/test-ego-software.yaml create mode 100644 docs/source/device/ego.rst create mode 100644 examples/camera_viz/configs/ego.yaml create mode 100644 examples/camera_viz/ego_preview/CMakeLists.txt create mode 100644 examples/camera_viz/ego_preview/README.md create mode 100644 examples/camera_viz/ego_preview/__init__.py create mode 100755 examples/camera_viz/ego_preview/build.sh create mode 100644 examples/camera_viz/ego_preview/ego_capture.cpp create mode 100644 examples/camera_viz/sources/ego.py create mode 100644 examples/camera_viz/sources/stereo_sbs.py create mode 100644 examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/analyze_ego_sync.py create mode 100644 examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/calibrate_ego_sync.py create mode 100644 examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/replay_ego.py create mode 100644 examples/oxr/python/test_ego_camera.py create mode 100644 src/core/deviceio_base/cpp/inc/deviceio_base/ego_frame_metadata_tracker_base.hpp create mode 100644 src/core/deviceio_base/cpp/inc/deviceio_base/ego_imu_tracker_base.hpp create mode 100644 src/core/deviceio_trackers/cpp/ego_frame_metadata_tracker.cpp create mode 100644 src/core/deviceio_trackers/cpp/ego_imu_tracker.cpp create mode 100644 src/core/deviceio_trackers/cpp/inc/deviceio_trackers/ego_frame_metadata_tracker.hpp create mode 100644 src/core/deviceio_trackers/cpp/inc/deviceio_trackers/ego_imu_tracker.hpp create mode 100644 src/core/live_trackers/cpp/live_ego_frame_metadata_tracker_impl.cpp create mode 100644 src/core/live_trackers/cpp/live_ego_frame_metadata_tracker_impl.hpp create mode 100644 src/core/live_trackers/cpp/live_ego_imu_tracker_impl.cpp create mode 100644 src/core/live_trackers/cpp/live_ego_imu_tracker_impl.hpp create mode 100644 src/core/python/requirements-ego.txt create mode 100644 src/core/replay_trackers/cpp/replay_ego_tracker_impl.cpp create mode 100644 src/core/replay_trackers/cpp/replay_ego_tracker_impl.hpp create mode 100644 src/core/schema/cpp/inc/schema/ego_device_state_utils.hpp create mode 100644 src/core/schema/cpp/inc/schema/ego_limits.hpp create mode 100644 src/core/schema/fbs/ego_audio.fbs create mode 100644 src/core/schema/fbs/ego_calibration.fbs create mode 100644 src/core/schema/fbs/ego_camera.fbs create mode 100644 src/core/schema/fbs/ego_device_state.fbs create mode 100644 src/core/schema/fbs/ego_imu.fbs create mode 100644 src/core/schema/golden/ego_audio.bfbs create mode 100644 src/core/schema/golden/ego_calibration.bfbs create mode 100644 src/core/schema/golden/ego_camera.bfbs create mode 100644 src/core/schema/golden/ego_device_state.bfbs create mode 100644 src/core/schema/golden/ego_imu.bfbs create mode 100644 src/core/schema/python/ego_bindings.h create mode 100644 src/core/schema/python/ego_camera_bindings.h create mode 100644 src/plugins/ego/CMakeLists.txt create mode 100644 src/plugins/ego/README.md create mode 100644 src/plugins/ego/app/CMakeLists.txt create mode 100644 src/plugins/ego/app/main.cpp create mode 100644 src/plugins/ego/core/CMakeLists.txt create mode 100644 src/plugins/ego/core/calibration.cpp create mode 100644 src/plugins/ego/core/calibration.hpp create mode 100644 src/plugins/ego/core/device_controls.cpp create mode 100644 src/plugins/ego/core/device_controls.hpp create mode 100644 src/plugins/ego/core/device_diagnostics.cpp create mode 100644 src/plugins/ego/core/device_diagnostics.hpp create mode 100644 src/plugins/ego/core/diagnostics.hpp create mode 100644 src/plugins/ego/core/ego_camera.cpp create mode 100644 src/plugins/ego/core/frame_sink.cpp create mode 100644 src/plugins/ego/core/inc/ego_camera/cancellation.hpp create mode 100644 src/plugins/ego/core/inc/ego_camera/ego_camera.hpp create mode 100644 src/plugins/ego/core/inc/ego_camera/time_alignment.hpp create mode 100644 src/plugins/ego/core/mcap_metadata_sink.cpp create mode 100644 src/plugins/ego/core/metadata_sink.cpp create mode 100644 src/plugins/ego/core/metadata_sink.hpp create mode 100644 src/plugins/ego/core/preview.cpp create mode 100644 src/plugins/ego/core/preview.hpp create mode 100644 src/plugins/ego/core/schema_metadata_sink.cpp create mode 100644 src/plugins/ego/core/time_alignment.cpp create mode 100644 src/plugins/ego/core/wav_writer.hpp create mode 100644 src/plugins/ego/ego.example.toml create mode 100755 src/plugins/ego/ego.sh create mode 100644 src/plugins/ego/mcap/CMakeLists.txt create mode 100644 src/plugins/ego/mcap/export_media_tool/CMakeLists.txt create mode 100644 src/plugins/ego/mcap/export_media_tool/export_embedded_media.cpp create mode 100644 src/plugins/ego/mcap/io/CMakeLists.txt create mode 100644 src/plugins/ego/mcap/io/inc/ego_mcap/checked_write.hpp create mode 100644 src/plugins/ego/mcap/io/mcap_impl.cpp create mode 100644 src/plugins/ego/mcap/merge/CMakeLists.txt create mode 100644 src/plugins/ego/mcap/merge/inc/ego_mcap/merge.hpp create mode 100644 src/plugins/ego/mcap/merge/merge.cpp create mode 100644 src/plugins/ego/mcap/merge_tool/CMakeLists.txt create mode 100644 src/plugins/ego/mcap/merge_tool/main.cpp create mode 100644 src/plugins/ego/plugin.yaml create mode 100644 src/plugins/ego/tools/detect_media_mode.py create mode 100644 src/plugins/ego/tools/record_config.py create mode 100644 src/plugins/ego/tools/verify_profile.py create mode 100644 src/plugins/ego/tools/verify_sensor_topics.py create mode 100644 src/plugins/ego/tools/verify_video_timing.py create mode 100644 src/python/isaaccapture/ego/__init__.py create mode 100644 src/python/isaaccapture/ego/_records.py create mode 100644 src/python/isaaccapture/ego/calibration.py create mode 100644 src/python/isaaccapture/ego/recording.py create mode 100644 src/python/isaaccapture/ego/sync.py create mode 100644 tests/cpp/core/live_trackers/test_ego_dispatch.cpp create mode 100644 tests/cpp/core/replay_deviceio_session/test_ego_replay.cpp create mode 100644 tests/cpp/core/schema/test_ego_camera.cpp create mode 100644 tests/cpp/plugins/ego/CMakeLists.txt create mode 100644 tests/cpp/plugins/ego/test_ego_camera.cpp create mode 100755 tests/cpp/plugins/ego/test_ego_script.sh create mode 100644 tests/cpp/plugins/ego/test_time_alignment.cpp create mode 100644 tests/cpp/plugins/ego_mcap/CMakeLists.txt create mode 100644 tests/cpp/plugins/ego_mcap/test_merge.cpp create mode 100644 tests/cpp/plugins/ego_media/CMakeLists.txt create mode 100644 tests/cpp/plugins/ego_media/test_export.cpp create mode 100644 tests/python/core/ego/CMakeLists.txt create mode 100644 tests/python/core/ego/pyproject.toml create mode 100644 tests/python/core/ego/test_calibration.py create mode 100644 tests/python/core/ego/test_recording.py create mode 100644 tests/python/core/ego/test_sync.py create mode 100644 tests/python/core/schema/test_ego_camera.py create mode 100644 tests/python/examples/camera_viz/test_ego_and_stereo_sbs.py create mode 100644 tests/python/examples/camera_viz/test_ego_failures.py create mode 100644 tests/python/examples/camera_viz/test_nv_decode_codec.py create mode 100644 tests/python/examples/mcap_record_replay/CMakeLists.txt create mode 100644 tests/python/examples/mcap_record_replay/conftest.py create mode 100644 tests/python/examples/mcap_record_replay/pyproject.toml create mode 100644 tests/python/examples/mcap_record_replay/test_ego_calibration_cli.py create mode 100644 tests/python/examples/mcap_record_replay/test_ego_replay.py create mode 100644 tests/python/examples/mcap_record_replay/test_ego_sync_cli.py diff --git a/.github/workflows/test-ego-software.yaml b/.github/workflows/test-ego-software.yaml new file mode 100644 index 0000000000..b9ba6f3262 --- /dev/null +++ b/.github/workflows/test-ego-software.yaml @@ -0,0 +1,115 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +name: Test EGO Software + +on: + push: + branches: [main, 'release/*.*.x', feature/Orbbec-Ego-stereo-camera, feature/Orbbec-EgoPro-quad-camera] + paths: &ego_paths + - '.github/workflows/test-ego-software.yaml' + - 'CMakeLists.txt' + - 'pyproject.toml' + - 'cmake/**' + - 'src/plugins/CMakeLists.txt' + - 'src/plugins/ego/**' + - 'src/core/schema/**' + - 'src/core/codegen/**' + - 'src/core/mcap/**' + - 'src/core/deviceio_base/**' + - 'src/core/deviceio_session/**' + - 'src/core/deviceio_trackers/**' + - 'src/core/live_trackers/**' + - 'src/core/replay_trackers/**' + - 'src/core/plugin_manager/**' + - 'src/core/python/**' + - 'src/compat/**' + - 'src/python/isaaccapture/**' + - 'examples/camera_viz/**' + - 'examples/mcap_record_replay/**' + - 'tests/**' + pull_request: + paths: *ego_paths + workflow_dispatch: + +concurrency: + group: ego-software-${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ github.event_name == 'pull_request' }} + +permissions: + contents: read + +jobs: + test-ego-software: + # The SDK runner must never execute untrusted fork code. + if: >- + github.event_name != 'pull_request' + || github.event.pull_request.head.repo.full_name == github.repository + # Verify repository/workflow access and an online runner with all labels. + # A blocked or unassigned job provides no build or test result. + runs-on: [self-hosted, linux, orbbec-sdk] + timeout-minutes: 45 + env: + ORBBEC_SDK_ROOT: ${{ vars.ORBBEC_SDK_ROOT }} + UV_CACHE_DIR: /tmp/isaaccapture-ego-uv-cache + + steps: + - uses: actions/checkout@v6 + with: + persist-credentials: false + + - name: Install pinned uv + uses: ./.github/actions/setup-uv + + - name: Check runner SDK + shell: bash + run: | + set -euo pipefail + if [[ -z "${ORBBEC_SDK_ROOT}" ]]; then + echo "Set the protected ORBBEC_SDK_ROOT Actions variable for the orbbec-sdk runner." + exit 1 + fi + for required in \ + "${ORBBEC_SDK_ROOT}/include/libobsensor/ObSensor.hpp" \ + "${ORBBEC_SDK_ROOT}/lib/OrbbecSDKConfig.cmake" \ + "${ORBBEC_SDK_ROOT}/lib/libOrbbecSDK.so.2"; do + [[ -f "${required}" ]] || { echo "Missing Orbbec SDK file: ${required}"; exit 1; } + done + printf 'EGO_BUILD_DIR=%s\n' "${RUNNER_TEMP}/ego-software-${GITHUB_RUN_ID}-${GITHUB_RUN_ATTEMPT}" >> "${GITHUB_ENV}" + + - name: Configure and build + shell: bash + run: | + set -euo pipefail + env -u PYTHONPATH -u AMENT_PREFIX_PATH -u COLCON_PREFIX_PATH \ + cmake -S . -B "${EGO_BUILD_DIR}" \ + -DISAAC_TELEOP_PYTHON_VERSION=3.11 \ + -DBUILD_VIZ=OFF \ + -DBUILD_TESTING=ON \ + -DBUILD_PLUGIN_EGO_CAMERA=ON \ + -DORBBEC_SDK_ROOT="${ORBBEC_SDK_ROOT}" + cmake --build "${EGO_BUILD_DIR}" --parallel 4 + + - name: Run hardware-free EGO tests + shell: bash + run: | + set -euo pipefail + env -u PYTHONPATH -u AMENT_PREFIX_PATH -u COLCON_PREFIX_PATH \ + ctest --test-dir "${EGO_BUILD_DIR}" --output-on-failure --parallel 4 \ + --no-tests=error -R '[Ee][Gg][Oo]' + + - name: Run full software suite + shell: bash + run: | + set -euo pipefail + env -u PYTHONPATH -u AMENT_PREFIX_PATH -u COLCON_PREFIX_PATH \ + ctest --test-dir "${EGO_BUILD_DIR}" --output-on-failure --parallel 4 + + - name: Upload CTest failure logs + if: failure() + uses: actions/upload-artifact@v6 + with: + name: ego-software-ctest-logs + path: ${{ env.EGO_BUILD_DIR }}/Testing/Temporary + if-no-files-found: warn + retention-days: 14 diff --git a/.gitignore b/.gitignore index f939df2136..c6addd5814 100644 --- a/.gitignore +++ b/.gitignore @@ -4,6 +4,17 @@ # macOS .DS_Store +# Local editor state and runtime logs +/.vscode/ +/Log/ + +# External Orbbec SDK downloads and extracts +OrbbecSDK*/ +OrbbecSDK*.zip +OrbbecSDK*.tar.gz +OrbbecSDK*.tgz +OrbbecSDK*.7z + # Node.js node_modules/ package-lock.json @@ -25,6 +36,9 @@ __pycache__/ *.pyc *.pyo *.pyd +.ruff_cache/ +.pytest_cache/ +.mypy_cache/ # Python virtual environments .venv/ venv/ @@ -37,6 +51,7 @@ uv.lock # Compiled files *.o *.so +*.so.* *.a *.exe diff --git a/CMakeLists.txt b/CMakeLists.txt index 498bd53482..5b885072ff 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -60,6 +60,8 @@ endif() option(BUILD_PLUGINS "Build plugins" ON) option(BUILD_PYTHON_BINDINGS "Build Python bindings" ON) option(BUILD_PLUGIN_OAK_CAMERA "Build OAK camera plugin (requires vcpkg for DepthAI v3.x)" OFF) +option(BUILD_PLUGIN_EGO_CAMERA "Build EGO camera plugin (requires Orbbec SDK v2)" OFF) +option(BUILD_EGO_MEDIA_TOOLS "Build SDK-free EGO MCAP media tools" OFF) option(BUILD_PLUGIN_NOITOM_MOCAP "Build Noitom mocap plugin (downloads MocapApi SDK)" OFF) option(BUILD_PLUGIN_OGLO "Build OGLO tactile glove plugin (BLE, Linux only; fetches SimpleBLE + nlohmann/json)" OFF) option(BUILD_PLUGIN_WUJI_GLOVE "Build Wuji glove plugin (requires the wuji_sdk C SDK)" OFF) diff --git a/docs/source/device/cameras/index.rst b/docs/source/device/cameras/index.rst index e94ec7b3d2..d7523f6ca2 100644 --- a/docs/source/device/cameras/index.rst +++ b/docs/source/device/cameras/index.rst @@ -12,3 +12,4 @@ camera feeds to clients, see :doc:`/references/camera_streaming`. :titlesonly: oak + ../ego diff --git a/docs/source/device/ego.rst b/docs/source/device/ego.rst new file mode 100644 index 0000000000..cabc06e24a --- /dev/null +++ b/docs/source/device/ego.rst @@ -0,0 +1,317 @@ +.. SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +.. SPDX-License-Identifier: Apache-2.0 + +EGO Camera Plugin +================= + +The ``ego_camera`` plugin connects EGO stereo cameras to Isaac Capture. +It records ColorLeft and ColorRight video, and captures accelerometer, +gyroscope, microphone, calibration, and device state when the device exposes +them. Detailed operator commands live in +:code-file:`src/plugins/ego/README.md`. + +.. contents:: On this page + :local: + :depth: 2 + +Supported streams and profiles +------------------------------ + +The plugin captures ``ColorLeft`` and ``ColorRight`` in MJPEG, H.264, or H.265. +The recording wrapper defaults to stereo H.264 ``1600x1300@30``. Use +``capabilities`` to inspect the connected device's available profiles; +unsupported simultaneous combinations fail without a fallback. The current +plugin rejects H.264/H.265 profiles above 30 FPS and does not capture depth or +infrared streams. + +Installation +------------ + +Use an extracted Linux OrbbecSDK v2 package and the normal Isaac Capture source +build dependencies, including ``nlohmann-json3-dev`` for the EGO adapter. +Install the SDK package's udev rules and reconnect the +camera before using it as a non-root user. The SDK is an external dependency; +the :code-file:`plugin README ` describes its expected +layout and advanced build options. + +From the repository root: + +.. code-block:: bash + + export ORBBEC_SDK_ROOT=/path/to/OrbbecSDK + ./src/plugins/ego/ego.sh doctor --sdk-root "$ORBBEC_SDK_ROOT" + ./src/plugins/ego/ego.sh build --sdk-root "$ORBBEC_SDK_ROOT" --jobs 8 + ./src/plugins/ego/ego.sh capabilities + +Preview is optional and requires SDL2 and FFmpeg development libraries; enable +it with the wrapper's ``build --preview`` flag. Recording alone needs neither. + +Capture and verify +------------------ + +The wrapper records the default stereo profile with available accelerometer +and gyroscope streams. Audio is off by default. Give each recording a fresh +output directory: + +.. code-block:: bash + + RUN="recordings/ego_$(date +%Y%m%d_%H%M%S)" + ./src/plugins/ego/ego.sh record --duration 30 --output "$RUN" + ./src/plugins/ego/ego.sh verify "$RUN" + +Raw video is an elementary MJPEG, H.264, or H.265 stream. Metadata, IMU, +calibration, and state are recorded in MCAP. The wrapper also preserves profile +and software/device provenance. + +Enable optional audio with ``--audio`` or ``audio = true`` in a TOML config; +``--no-audio`` overrides a config that enables it. Audio is saved as 48 kHz mono +S16_LE WAV, with audio offsets recorded in MCAP: + +.. code-block:: bash + + ./src/plugins/ego/ego.sh record --duration 30 --audio \ + --output recordings/ego_with_audio + +Raw PCM capture, storage and export are supported. The tested Ethernet path +has an SDK audio timestamp cadence anomaly, so audio/video and five-stream +physical synchronization have not passed acceptance. The recommended capture +workflow uses stereo RGB and IMU; including audio in synchronization requires +an independently validated audio time model. See "Optional audio and +synchronization limits" in the +:code-file:`plugin README `. + +For one self-contained MCAP with encoded video and any enabled audio PCM: + +.. code-block:: bash + + RUN="recordings/ego_embedded_$(date +%Y%m%d_%H%M%S)" + ./src/plugins/ego/ego.sh record --duration 30 \ + --output "$RUN" --mcap-media embedded + ./src/plugins/ego/ego.sh verify "$RUN" + ./src/plugins/ego/ego.sh export-media "$RUN" + +Embedded recording preserves the encoded media without transcoding. Export +produces ordinary video files and, when audio was enabled, WAV. A remaining +``.partial`` file means the capture did not finish successfully; preserve it +and the log for diagnosis. + +Data flow and published collections +----------------------------------- + +The plugin writes captured media and sensor records and can publish structured +data through SchemaPusher. DeviceIO trackers read the published collections +during session updates. + +With ``--collection-prefix=ego``, the collections and Python read APIs are: + +.. list-table:: + :header-rows: 1 + :widths: 30 40 30 + + * - Collection + - Python tracker + - Read method + * - ``ego/ColorLeft``, ``ego/ColorRight`` + - ``EgoFrameMetadataTracker`` + - ``get_stream_data`` + * - ``ego/Accel``, ``ego/Gyro`` + - ``EgoImuTracker`` + - ``get_stream_data`` + * - ``ego/Audio`` + - ``EgoAudioTracker`` + - ``get_data`` + * - ``ego/Calibration`` + - ``EgoCalibrationTracker`` + - ``get_data`` + * - ``ego/DeviceState`` + - ``EgoDeviceStateTracker`` + - ``get_data`` + +All reads return the schema payload directly or ``None`` when unavailable. +Audio records are available only when audio capture is enabled. +Camera and IMU readers select one configured stream by its index. Camera streams +use ``EgoCameraStream`` enums; IMU sensors default to Accel and Gyro. See the +:code-file:`plugin README ` for the full tracker +configuration example. + +Availability and diagnostics +---------------------------- + +The plugin integrates with :doc:`NV device monitoring <../references/device_provider_monitoring>`. +After ``session.step()``, query ``session.get_device_status(plugin_root_id + +"/camera/ego")`` for current availability. The provider ID is +``plugin/``. The existing capture checks report unknown, absent, +starting/recovering, streaming, or failed conditions; NVIDIA owns provider +failure/teardown and the one-second heartbeat / three-second stale-report rule. +No additional device polling thread is introduced. + +``EgoDeviceStateTracker`` carries historical diagnostic events, separately from +current availability. Version-2 payloads have ``device_uid``, ``capture_epoch``, +``observed_at_ns`` and one typed ``event``: + +- ``EgoSdkReport``: original SDK mode, flags, storage and report sequence. +- ``EgoCaptureSummary``: capture health, queue/loss totals and final-summary flag. +- ``EgoRecoveryEvent``: recovery stage, cause and attempt count. +- ``EgoDeviceSnapshot``: successful startup property reads and optional IMU temperature. + +An unavailable temperature is ``None``; a measured zero remains ``0.0``. Startup +samples are not repeated as live periodic measurements. These event views are +read-only and retain the backing buffer. This development-stage layout replaces +the previous diagnostic protocol: old diagnostic recordings are unsupported, +and the official Replay compatibility check remains enabled. + +Diagnostic MCAP recording defaults to on. Use +``--record-device-diagnostics=off`` or the TOML +``record_device_diagnostics = false`` option to omit it. Monitoring, live +publication, capture checks, recovery and logs continue. For NV sessions use +``isaaccapture.ego.configure_recording(config, record_device_diagnostics=False)`` +or the same keyword on ``embedded_recording``. Remove diagnostic trackers from +explicit recording lists and automatically recorded pipeline/control/sink +sources; the helper rejects conflicts before capture. The merger checks both +inputs, including custom or empty diagnostic channels. See the plugin README +for complete configuration and failure handling. + +Multi-device sessions +--------------------- + +Add the EGO trackers to a normal +:doc:`TeleopSession <../teleop/teleop_session>` configuration and its +``McapRecordingConfig`` tracker-name list, alongside the hand, head, or +controller trackers. Configure ``ego_camera`` through ``PluginConfig`` with +``--collection-prefix=ego`` and ``shutdown_timeout_seconds=15.0`` to allow +capture cleanup. Live publication requires a running OpenXR runtime. +Complete the normal full source build and install its Python wheel before +using the EGO tracker bindings and recording helper; the wrapper's +``build`` command builds only the native plugin tools. + +Call ``isaaccapture.ego.configure_recording`` before opening a normal NV +recording session. It retains or adds the calibration tracker through the +existing recording configuration, so EGO time-model snapshots are recorded. +``embedded_recording`` performs this configuration as part of its existing +finalization workflow. + +For one self-contained multi-device MCAP, use +``isaaccapture.ego.embedded_recording`` with exactly one EGO plugin configured +for embedded media. The full configuration, matching media-spool path, and +clean-stop procedure are documented under "TeleopSession: one MCAP with other +trackers" in the :code-file:`plugin README `. + +Replay and visualization +------------------------ + +The current structured replay example requires camera, IMU, audio, calibration +and any enabled diagnostic channels. Use an audio-enabled recording such as the +one above: + +.. code-block:: bash + + uv pip install -e ./examples/mcap_record_replay + python -m isaaccapture_examples.mcap_record_replay.replay_ego \ + recordings/ego_with_audio/metadata.mcap + +A recording without audio is valid; this example reports a missing Audio +channel because of its fixed tracker configuration. It reads structured +records and does not play sound or schedule synchronized media playback. Use +``export-media`` followed by FFmpeg for encoded media playback. For a live GPU +preview, build the standalone camera_viz binding: + +.. code-block:: bash + + examples/camera_viz/camera_viz.sh setup --with-ego \ + --sdk-root "$ORBBEC_SDK_ROOT" + examples/camera_viz/camera_viz.sh run \ + examples/camera_viz/configs/ego.yaml --mode window --stereo-debug sbs + +MJPEG uses SDK conversion and GPU upload; H.264/H.265 uses native NVDEC. Window +SBS is an optional left/right inspection image and is refused in XR mode. +XR mode submits separate eye textures using the existing visualization API. +The viewer and recording plugin each open the camera; run them one at a time. + +Sample-time association +----------------------- + +NV replay advances each tracker to its next record; a single update does not +automatically align video, IMU and audio. The EGO offline API associates only +the streams required by a consumer. It preserves the existing replay lifecycle +and introduces no video/audio player. + +Capture retains original timestamps and host arrival times separately. An +optional ``--time-alignment=PATH`` loads a validated reusable RGB/IMU model +bound to the device identity and actual stream profile. EGO calibration payloads +carry clock provenance and model snapshots through the normal NV recording +channel. Audio payloads carry retained sample indices and segment information. +Reusable profiles require the loaded SDK's SHA-256, unit scale and a verified +duration limit. Expiry preserves capture data with an explicit arrival-time +fallback; offline reads keep the validated interval separate from that tail. +Without a trustworthy model the recording remains unverified. An estimated +host-clock origin does not establish absolute host sampling accuracy or +synchronization with other devices. + +The audio source label ``sdk_audio_raw`` identifies the SDK's original +timestamp; its sampling-clock semantics depend on the SDK transport and do +not alone establish a common camera/IMU clock. Audio alignment needs an +independently validated sample-time model, including offset and sample-clock +drift. Retained audio sample indices do not prove physical continuity; this +needs separate evidence. Reconnects and detected clock discontinuities are +segmented, and the reader does not interpolate across these boundaries. + +Install the offline extra into an environment containing the built package: + +.. code-block:: bash + + uv pip install 'isaaccapture[ego]' + uv pip install -e ./examples/mcap_record_replay + python -m isaaccapture_examples.mcap_record_replay.analyze_ego_sync \ + "$RUN" --required-streams left_rgb right_rgb accel gyro \ + --output "$RUN/sync_audit.json" + +The recommended audit selects stereo RGB and IMU. Include ``audio`` only for +recordings with accessible audio media and an independently validated audio +time model; selecting the stream does not establish physical synchronization. + +Use ``isaaccapture.ego.sync.iter_sync_windows`` for Python consumers. Consecutive +reference video frames define half-open time windows containing frame media +references, all interval IMU samples, audio sample slices when requested, +model identity and completeness reasons. The default reference is left RGB; +right RGB is also selectable. Missing data remains incomplete without stale +fills. An absent required stream or trusted time model is an error. The audit +retains unpaired frames, tails and gaps. + +Standard sidecar recording directories and embedded MCAP are supported. +Metadata-only files need their accessible video sidecars and, when audio was +recorded, their WAV sidecar to form a complete dataset. Merged recordings use +canonical EGO topics and ignore duplicated +session observations. MCAP log times and latest-value tracker getters are not +used as sample times. + +A model recorded during capture is selected by default. For calibration made +after recording, pass ``--alignment PATH`` explicitly; the model must match +the recording/media fingerprints, identity and valid intervals. The +``calibrate_ego_sync`` example fits such a model from separately annotated +calibration and validation events and preserves evidence hashes. It does not +detect physical events or rewrite the MCAP. Both offline commands refuse to +overwrite output files and return nonzero for unverified/incomplete results. + +Physical acceptance requires independent beginning/middle/end measurements +over at least 15 minutes. Results apply to the validated duration. Corrected +maximum error plus measurement uncertainty +must be no more than half a configured video frame. Successful software +association alone does not establish that bound. Detailed model and event +manifest formats are in the +:code-file:`plugin README `. + +Troubleshooting +--------------- + +- **No camera or USB access:** install the selected SDK's udev rules, reconnect, + and re-run ``doctor`` and ``capabilities``. +- **Profile rejected:** choose an exact advertised left/right combination; + the current plugin rejects H.264/H.265 profiles above 30 FPS. +- **Partial MCAP:** the recording did not finish successfully. Preserve it and + the log for diagnosis before starting a new recording. +- **Publisher startup failure:** check the running OpenXR runtime and plugin + search path. +- **No embedded final output:** check normal context exit, committed fragment, + matching spool path, and the merge tool beside the selected plugin. +- **Native decoder unavailable:** build camera_viz's codec with CUDA, or choose + an advertised MJPEG profile for that preview. diff --git a/examples/camera_viz/README.md b/examples/camera_viz/README.md index f16f519512..237618661f 100644 --- a/examples/camera_viz/README.md +++ b/examples/camera_viz/README.md @@ -20,6 +20,7 @@ SPDX-License-Identifier: Apache-2.0 | `v4l2` | USB / UVC — anything `v4l2-ctl --list-formats-ext` shows | | `oakd` | OAK-D RGB / LEFT / RIGHT; mono or `stereo: true` (GRAY8 over USB, GPU-broadcast to RGBA; `stereo_rgb` for color). Needs the Luxonis udev rule — see below | | `zed` | ZED 2 / Mini / X One; mono or `stereo: true` (per-eye SDK retrieve, zero-copy GPU) | +| `ego` | Local EGO ColorLeft/ColorRight pair; `stereo: true` required. See the [EGO preview guide](../../src/plugins/ego/README.md#preview-and-gpu-stereo) | | `video` | Video-file replay (anything OpenCV/FFmpeg reads) — preview / testing without a camera. Loops by default; `stereo: true` splits side-by-side files into eyes (viewer only) | In XR mode the viewer **attaches to the CloudXR runtime + WSS proxy**, starting a background service if none is serving — nothing to start separately (`--accept-eula` for the first run; CloudXR.js is hosted at `https://:48322/client/` by default when this run starts the service — `--no-host-client` only applies then; stop and restart the service to change hosting; `camera_viz.py --help` for the rest). Output: XR headset (default) or desktop window (`run CONFIG --mode window`); one surface per camera — a flat plane (default), a cylinder arc, or an equirect sphere (`placements..shape`, XR only for the curved shapes). Stereo cameras render true SBS in XR; window mode shows the left eye. XR placements: `world` / `head` / `lazy` / `gimbal`. @@ -37,7 +38,7 @@ source examples/camera_viz/.venv/bin/activate camera_viz needs an `isaaccapture` new enough to carry the features it uses, so `setup` works down a ladder to get one: newest **final release** meeting that minimum; else newest **release candidate** (an rc is published from every release-branch commit, and PEP 440 keeps pre-releases out of a plain minimum-version specifier); else a **source build** of this checkout, after asking. Final releases win automatically whenever one qualifies. The minimum itself lives in `scripts/_install_deps.sh`. -Flags: `--no-{v4l2,oakd}`, `--with-rtp` (split mode / `loopback`; implied by `--sender-only`), `--with-zed`, `--sender-only`, `--jetson`. Pass `--venv PATH` to install into an existing venv (symlinks `.venv` → PATH so `run` / `loopback` pick it up too). +Flags: `--no-{v4l2,oakd}`, `--with-rtp` (split mode / `loopback`; implied by `--sender-only`), `--with-zed`, `--with-ego --sdk-root PATH`, `--sender-only`, `--jetson`. Pass `--venv PATH` to install into an existing venv (symlinks `.venv` → PATH so `run` / `loopback` pick it up too). > **OAK-D?** The camera needs a udev rule, or `depthai` reports `Insufficient permissions to communicate with X_LINK_UNBOOTED device` and never finds it. `setup` prompts to install one when an OAK-D is attached and no rule covers it; declining is not fatal. By hand: > ```bash @@ -56,7 +57,18 @@ Flags: `--no-{v4l2,oakd}`, `--with-rtp` (split mode / `loopback`; implied by `-- ./camera_viz.sh run configs/v4l2.yaml --mode window # desktop window instead ``` -Set `source: local`. Swap config for `oakd.yaml`, `zed.yaml`, `realsense.yaml`, `synthetic.yaml`, `synthetic_stereo.yaml`, `synthetic_xr_3up.yaml`, `multi_camera.yaml`, `replay.yaml` (file replay — point `path:` at any recording). +Set `source: local`. Swap config for `oakd.yaml`, `zed.yaml`, `ego.yaml`, `realsense.yaml`, `synthetic.yaml`, `synthetic_stereo.yaml`, `synthetic_xr_3up.yaml`, `multi_camera.yaml`, `replay.yaml` (file replay — point `path:` at any recording). + +For stereo sources, window mode shows the left eye by default. Use +`--stereo-debug sbs` to show both eyes side by side. Set +`display.window.stereo_debug: "sbs"` in YAML for the default; the CLI overrides +it. Quote `"off"` in YAML. This option is available only in window mode; +XR submits separate eye textures. + +EGO automatically uses the renderer's CUDA GPU for upload and decoding. +Fatal EGO producer errors propagate through the official runner to the main +thread instead of leaving the viewer waiting for frames. GPU configuration +and retry boundaries are described in the [EGO preview binding guide](ego_preview/README.md). In XR mode, when the CloudXR service hosts the client (default if this `run` starts the service), open CloudXR.js on the headset at @@ -119,11 +131,11 @@ encoder: auto | native | gstreamer cameras: - name: cam enabled: true - type: v4l2 # v4l2 | oakd | zed | synthetic | video + type: v4l2 # v4l2 | oakd | zed | ego | synthetic | video width: 2560 # video: optional — defaults to the file's size height: 720 fps: 30 - stereo: false # zed / oakd / synthetic / video — per-eye capture + SBS XR + stereo: false # zed / oakd / ego / synthetic / video — per-eye capture + SBS XR # … type-specific fields (e.g. synthetic: disparity_px; video: path, loop) rtp: port: 5000 # left eye when stereo @@ -332,7 +344,8 @@ camera_viz/ ├── controls/ — XR controller bindings, shapes, stereo geometry, HUD ├── pipeline/ — source ABC + threaded runner ├── placements/ — XR lock-mode strategies -├── sources/ — V4L2 / OAK-D / ZED / synthetic / video replay / rtp_h264 +├── sources/ — camera FrameSources + stereo_sbs.py desktop SBS adapter +├── ego_preview/ — EGO SDK capture binding (see ego_preview/README.md) ├── transports/ — RTP sender + receiver, native + GStreamer ├── codec/ — native NVENC/NVDEC pybind module ├── configs/ — one YAML per camera kind diff --git a/examples/camera_viz/camera_viz.py b/examples/camera_viz/camera_viz.py index e0b739510d..645351c4fe 100755 --- a/examples/camera_viz/camera_viz.py +++ b/examples/camera_viz/camera_viz.py @@ -63,6 +63,12 @@ def _parse_args(argv: Optional[list[str]]): "headset to connect. Negative waits forever (the default); 0 fails fast.", ) CloudXRLauncher.add_launcher_arguments(parser) + parser.add_argument( + "--stereo-debug", + choices=("off", "sbs"), + default=None, + help="Window stereo inspection: off (default) or left/right side by side.", + ) return parser.parse_args(argv) @@ -182,6 +188,7 @@ def main(argv: Optional[list[str]] = None) -> int: effective_mode = (args.mode or cfg.get("display", {}).get("mode", "xr")).lower() config.check_shapes_are_displayable(cfg, effective_mode) + stereo_debug = config.resolve_stereo_debug(cfg, args.stereo_debug, effective_mode) # CloudXR runtime settings (display.cloudxr) go through a generated # --cloudxr-env-config file rather than os.environ: an env file is the one @@ -269,9 +276,13 @@ def _interrupt(signum, frame): is_xr = session.is_xr_mode() if source_mode == "local": - entries = config.build_local_entries(cfg, is_xr) + # NV resolves Vulkan/CUDA identity; EGO consumes the resulting CUDA ID. + entries = config.build_local_entries( + cfg, is_xr, renderer_cuda_device_id=session.cuda_device_id + ) else: entries = config.build_rtp_entries(cfg, is_xr) + entries = config.apply_stereo_debug(entries, stereo_debug) # Shape switching needs every shape resident, and the shaped layers # are XR-only, so it is off outside XR regardless of the config. @@ -352,8 +363,9 @@ def _on_signal(signum, frame): # neither may happen at all if a worker thread is still alive. if controls is not None: controls.__enter__() - runner.start() try: + # Source startup (including EGO GPU validation) needs the same teardown. + runner.start() runner.wait( health_check=launcher.health_check if launcher is not None else None ) diff --git a/examples/camera_viz/camera_viz.sh b/examples/camera_viz/camera_viz.sh index 065be329d7..ecd5e7d6b7 100755 --- a/examples/camera_viz/camera_viz.sh +++ b/examples/camera_viz/camera_viz.sh @@ -391,12 +391,14 @@ cmd_service_restart() { # ────────────────────────────────────────────────────────────────────── show_help() { - cat < namespace py = pybind11; +using camera_viz::codec::DecoderCodec; using camera_viz::codec::DecoderConfig; using camera_viz::codec::EncoderConfig; using camera_viz::codec::H264Decoder; @@ -96,7 +97,7 @@ CudaBuf extract_cuda_buffer(const py::object& obj, std::uint32_t expected_width, PYBIND11_MODULE(_camera_viz_codec, m) { - m.doc() = "Native H.264 NVENC encoder + NVDEC decoder for camera_viz."; + m.doc() = "Native H.264 NVENC encoder + H.264/HEVC NVDEC decoder for camera_viz."; py::enum_(m, "PixelFormat").value("RGBA8", PixelFormat::kRGBA8).value("BGRA8", PixelFormat::kBGRA8); @@ -145,7 +146,10 @@ PYBIND11_MODULE(_camera_viz_codec, m) .def_readwrite("width", &DecoderConfig::width) .def_readwrite("height", &DecoderConfig::height) .def_readwrite("full_range", &DecoderConfig::full_range) - .def_readwrite("gpu_id", &DecoderConfig::gpu_id); + .def_readwrite("gpu_id", &DecoderConfig::gpu_id) + .def_readwrite("codec", &DecoderConfig::codec); + + py::enum_(m, "DecoderCodec").value("H264", DecoderCodec::kH264).value("HEVC", DecoderCodec::kHEVC); py::class_(m, "H264Decoder") .def(py::init(), py::arg("config")) @@ -175,4 +179,5 @@ PYBIND11_MODULE(_camera_viz_codec, m) py::arg("packet"), py::arg("rgba_out"), py::arg("width"), py::arg("height"), "Feed one Annex-B AU. Returns True iff a frame was written to rgba_out.") .def("reset", &H264Decoder::reset, "Tear down NVDEC state. Use after long stream silence."); + m.attr("VideoDecoder") = m.attr("H264Decoder"); } diff --git a/examples/camera_viz/codec/h264_decoder.cpp b/examples/camera_viz/codec/h264_decoder.cpp index 7d9bf9abe4..972f42234b 100644 --- a/examples/camera_viz/codec/h264_decoder.cpp +++ b/examples/camera_viz/codec/h264_decoder.cpp @@ -17,6 +17,18 @@ namespace camera_viz::codec namespace { +cudaVideoCodec cuda_codec(DecoderCodec codec) +{ + switch (codec) + { + case DecoderCodec::kH264: + return cudaVideoCodec_H264; + case DecoderCodec::kHEVC: + return cudaVideoCodec_HEVC; + } + throw std::invalid_argument("unsupported decoder codec"); +} + inline void check_cu(CUresult result, const char* what) { if (result != CUDA_SUCCESS) @@ -57,6 +69,7 @@ struct H264Decoder::Impl { throw std::runtime_error("H264Decoder: width/height must be > 0"); } + const auto codec = cuda_codec(cfg.codec); check_cu(cuInit(0), "cuInit"); check_cu(cuDeviceGet(&cu_device, cfg.gpu_id), "cuDeviceGet"); check_cu(cuDevicePrimaryCtxRetain(&cu_context, cu_device), "cuDevicePrimaryCtxRetain"); @@ -68,7 +81,7 @@ struct H264Decoder::Impl // Zero-latency, decode-order output. Output stays on GPU. decoder = std::make_unique(cu_context, true, // bUseDeviceFrame - cudaVideoCodec_H264, + codec, true, // bLowLatency false, // bDeviceFramePitched nullptr, // pCropRect @@ -190,7 +203,7 @@ struct H264Decoder::Impl try { decoder = std::make_unique( - cu_context, true, cudaVideoCodec_H264, true, false, nullptr, nullptr, false, 0, 0, 1000, true); + cu_context, true, cuda_codec(cfg.codec), true, false, nullptr, nullptr, false, 0, 0, 1000, true); } catch (...) { diff --git a/examples/camera_viz/codec/inc/h264_decoder.hpp b/examples/camera_viz/codec/inc/h264_decoder.hpp index fc3d8bfe3b..2aadfb7d12 100644 --- a/examples/camera_viz/codec/inc/h264_decoder.hpp +++ b/examples/camera_viz/codec/inc/h264_decoder.hpp @@ -1,7 +1,7 @@ // SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. // SPDX-License-Identifier: Apache-2.0 // -// NVDEC H.264 decoder + NV12 → RGBA8 colour conversion. Zero-latency +// NVDEC H.264/HEVC decoder + NV12 → RGBA8 colour conversion. Zero-latency // (no reorder buffer, decode-order output). Supports BT.709 limited // and BT.601 full range. @@ -13,6 +13,12 @@ namespace camera_viz::codec { +enum class DecoderCodec +{ + kH264, + kHEVC, +}; + struct DecoderConfig { uint32_t width = 0; @@ -20,6 +26,8 @@ struct DecoderConfig // BT.601 full-range (ITU-T T.871) when true; BT.709 limited (16-235) when false. bool full_range = false; int gpu_id = 0; + // Preserve existing H.264/RTP callers; HEVC streams opt in through this field. + DecoderCodec codec = DecoderCodec::kH264; }; class H264Decoder @@ -50,4 +58,6 @@ class H264Decoder std::unique_ptr impl_; }; +using VideoDecoder = H264Decoder; + } // namespace camera_viz::codec diff --git a/examples/camera_viz/config.py b/examples/camera_viz/config.py index abbd7df97b..bc2e2b73fd 100644 --- a/examples/camera_viz/config.py +++ b/examples/camera_viz/config.py @@ -15,12 +15,13 @@ from __future__ import annotations import sys -from dataclasses import dataclass +from dataclasses import dataclass, replace from typing import List, Optional, Tuple from pipeline import FrameSource from placements import PlacementConfig, PlacementStrategy, build as build_placement from sources import PairedFrameSource, RtpH264Source, build_local_camera +from sources.stereo_sbs import StereoSideBySideSource @dataclass @@ -53,6 +54,29 @@ class SourceEntry: equirect_yaw_deg: float = 0.0 +def resolve_stereo_debug(cfg: dict, override: Optional[str], mode: str) -> str: + value = override or cfg.get("display", {}).get("window", {}).get( + "stereo_debug", "off" + ) + if value not in ("off", "sbs"): + raise ValueError(f"camera_viz: stereo_debug must be off|sbs, got {value!r}") + if value == "sbs" and mode != "window": + raise ValueError("camera_viz: stereo_debug sbs requires --mode window") + return value + + +def apply_stereo_debug(entries: List[SourceEntry], mode: str) -> List[SourceEntry]: + if mode == "off": + return entries + # Replacing only the source and stereo flag retains NV placement/shape fields. + return [ + replace(e, source=StereoSideBySideSource(e.source), stereo=False) + if e.stereo + else e + for e in entries + ] + + VALID_SHAPES = ("quad", "cylinder", "equirect") _VALID_COMPOSITORS = ("openxr", "televiz") @@ -206,12 +230,16 @@ def _stereo_for(cam: dict, placements_cfg: dict) -> Tuple[bool, float]: return stereo, float(pspec.get("stereo_plane_distance_cm", 0.0)) -def build_local_entries(cfg: dict, is_xr: bool) -> List[SourceEntry]: +def build_local_entries( + cfg: dict, is_xr: bool, *, renderer_cuda_device_id: int | None = None +) -> List[SourceEntry]: """source=local: open each enabled camera directly.""" placements_cfg = cfg.get("display", {}).get("placements", {}) entries: List[SourceEntry] = [] for cam in _enabled_cameras(cfg): - cam_sources = build_local_camera(cam) + cam_sources = build_local_camera( + cam, renderer_cuda_device_id=renderer_cuda_device_id + ) # Aspect comes from the built source's spec, not the YAML — video # sources may omit width/height and size themselves from the file. first = cam_sources[0].spec diff --git a/examples/camera_viz/configs/ego.yaml b/examples/camera_viz/configs/ego.yaml new file mode 100644 index 0000000000..fb924d0d59 --- /dev/null +++ b/examples/camera_viz/configs/ego.yaml @@ -0,0 +1,28 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +source: local +verbose: false + +display: + mode: window + window: + # Set to sbs (or pass --stereo-debug sbs) to inspect ColorLeft|ColorRight + # in the desktop window. XR always submits the two eyes independently. + stereo_debug: "off" + placements: + ego: + lock_mode: world + size: [1.0, 0.8125] + +cameras: + - name: ego + enabled: true + type: ego + stereo: true + device_uid: "" + width: 1600 + height: 1300 + fps: 30 + format: h264 + gpu_id: 0 # Auto: follow the renderer's CUDA GPU; nonzero pins must match it. diff --git a/examples/camera_viz/ego_preview/CMakeLists.txt b/examples/camera_viz/ego_preview/CMakeLists.txt new file mode 100644 index 0000000000..042248301f --- /dev/null +++ b/examples/camera_viz/ego_preview/CMakeLists.txt @@ -0,0 +1,39 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.20) +project(camera_viz_ego LANGUAGES CXX) + +set(CMAKE_CXX_STANDARD 20) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +if(NOT ORBBEC_SDK_ROOT) + message(FATAL_ERROR "ORBBEC_SDK_ROOT is required") +endif() + +find_package(Python3 REQUIRED COMPONENTS Interpreter Development.Module) +execute_process( + COMMAND ${Python3_EXECUTABLE} -c "import pybind11; print(pybind11.get_cmake_dir())" + OUTPUT_VARIABLE pybind11_DIR + OUTPUT_STRIP_TRAILING_WHITESPACE + RESULT_VARIABLE _pybind11_rc +) +if(NOT _pybind11_rc EQUAL 0) + message(FATAL_ERROR "pybind11 not found in the active Python environment") +endif() +find_package(pybind11 REQUIRED CONFIG) + +# Keep vendor SDK identifiers separate from EGO product target names. +find_library(ORBBEC_SDK_LIBRARY NAMES OrbbecSDK PATHS ${ORBBEC_SDK_ROOT}/lib NO_DEFAULT_PATH REQUIRED) + +# This SDK bridge returns host payloads; GPU decoding stays in the shared codec. +pybind11_add_module(_ego_capture MODULE ego_capture.cpp) +target_include_directories(_ego_capture PRIVATE ${ORBBEC_SDK_ROOT}/include) +target_link_libraries(_ego_capture PRIVATE ${ORBBEC_SDK_LIBRARY}) +# Place the extension beside the package entry point for in-tree viewer imports. +set_target_properties(_ego_capture PROPERTIES + LIBRARY_OUTPUT_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + BUILD_RPATH ${ORBBEC_SDK_ROOT}/lib + INSTALL_RPATH "$ORIGIN" +) diff --git a/examples/camera_viz/ego_preview/README.md b/examples/camera_viz/ego_preview/README.md new file mode 100644 index 0000000000..0658555dd4 --- /dev/null +++ b/examples/camera_viz/ego_preview/README.md @@ -0,0 +1,124 @@ + + +# EGO live-preview binding + +This directory connects the OrbbecSDK to the local `camera_viz` EGO source. +It opens one camera's ColorLeft/ColorRight streams and returns paired host +payloads. The Python source publishes GPU images through NVIDIA's existing +`FrameSource` interface; the viewer keeps its normal rendering lifecycle. + +## Responsibilities and data flow + +| File or component | Responsibility | +|---|---| +| `ego_capture.cpp` | Own the SDK context, device and stereo pipeline; check pair completeness and device-time agreement; copy host payloads. | +| `__init__.py` | Export `Capture` from the native `_ego_capture` extension and report how to build it. | +| `CMakeLists.txt`, `build.sh` | Build the SDK-only extension for the active Python environment, beside the package entry point. | +| [`sources/ego.py`](../sources/ego.py) | Own the producer thread, retries, per-eye GPU conversion/decoding and stereo Frame publication. | +| [`codec/`](../codec/) | Provide the shared native NVDEC implementation used by the source for H.264/H.265. | +| [`sources/stereo_sbs.py`](../sources/stereo_sbs.py) | Optionally copy GPU eye images into one desktop image, left eye first. | + +```text +EGO ColorLeft/ColorRight -> SDK Capture -> paired host payloads + -> EgoSource -> GPU RGBA8 stereo Frame -> camera_viz + -> window: left eye, or StereoSideBySideSource for a 2W x H image + -> XR: separate eye images +``` + +MJPEG (`mjpg`) is converted to host RGB by the SDK, then staged in pinned memory +and uploaded to GPU RGBA8 images. H.264/H.265 (`h264`/`h265`) remain compressed +host bytes until the Python source passes them to one NVDEC decoder per eye. +Both paths finish GPU writes before publishing reusable triple buffers. +The binding itself does not decode video on the GPU or render a window. + +OAK-D uses the external DepthAI Python binding, so it does not need an equivalent +local SDK bridge in `camera_viz`. + +### GPU selection + +The viewer passes `VizSession.cuda_device_id` through the source factory. +NVIDIA already matches Vulkan and CUDA devices by UUID; EGO uses that CUDA ID +for both eye buffers, upload streams, and NVDEC decoders, including after a +capture retry. A Vulkan `gpu_index` is not a CUDA device ID. + +For EGO, omitted `gpu_id` or `gpu_id: 0` means automatic selection, following +the renderer. A nonzero `gpu_id` must match the renderer; a mismatch fails +before allocating EGO GPU buffers or opening the camera. This does not change +which GPU the renderer selects or enable cross-GPU copies. + +Standalone `EgoSource` callers can supply `renderer_cuda_device_id` as a +keyword argument. Without it, `gpu_id: 0` follows the calling thread's current +CUDA device at each start; a nonzero value selects that CUDA device directly. + +## Build and import + +Install the external OrbbecSDK and its udev rules, then run from the repository +root. Setup installs Python dependencies and builds both the SDK binding and +the shared native codec: + +```bash +examples/camera_viz/camera_viz.sh setup --with-ego \ + --sdk-root /path/to/OrbbecSDK +``` + +To rebuild only this binding with an already configured viewer environment: + +```bash +source examples/camera_viz/.venv/bin/activate +examples/camera_viz/ego_preview/build.sh --sdk-root /path/to/OrbbecSDK +cd examples/camera_viz +python -c 'from ego_preview import Capture' +``` + +The SDK root must contain `include/` and `lib/libOrbbecSDK.so`; the active Python +environment must provide pybind11. CMake builds one `_ego_capture` target and +sets its build RPATH to the selected SDK library directory. This standalone +binding does not link the recording plugin. H.264/H.265 preview also requires +the shared codec and a working CUDA/NVDEC environment; MJPEG requires CuPy for +GPU upload. See the [viewer guide](../README.md) for setup and run commands. + +## Pair contract and device ownership + +`Capture(device_uid, width, height, fps, format)` opens the requested stereo +profile. An empty UID selects the first enumerated device. `next_pair(timeout_ms)` +returns a dictionary with `left`/`right` payload bytes, `timestamp_ns`, +`device_timestamp_ns`, and `sequence_left`/`sequence_right`. + +- `timestamp_ns` is host monotonic time sampled after payload extraction, not + exposure time. The source preserves it in the published Frame. +- `device_timestamp_ns` is the average raw device timestamp of the two eyes, + in nanoseconds. Pairs whose device timestamps differ by more than 2 ms are + skipped before conversion or copying. +- SDK wait timeouts, missing eyes and null MJPEG conversions return `None`. + Invalid converted RGB dimensions or payload size raise an exception. +- Empty pairs check the SDK device list at most once per second. Disappearance + of the selected UID raises a disconnect exception and uses the source's + existing close/retry path. A confirmed removed instance releases SDK objects + through their destructors; ordinary pipeline-stop errors still propagate. +- Payload bytes own their data after the SDK frame is released. GPU Frames + refer to reusable source buffers and must be consumed through the viewer's + existing FrameSource lifecycle. + +The source producer serializes reads and SDK close. Open failures retry; +capture-read exceptions close/reopen the pipeline and reset encoded decoders. +Other producer failures are retained and raised by `latest()`, preserving the +original exception. NVIDIA's `VizRunner.wait()` reports them to the main thread +and the viewer runs its normal shutdown. The failure is visible before SDK +cleanup completes; a second cleanup exception cannot replace the first error. +Cleanup-only errors are logged and can reach `latest()` while it is still being +polled. Errors arriving during normal shutdown follow the runner's existing +cleanup policy. A fully stopped source resets errors and pending frames when +started again. + +`stop()` requests exit and waits up to five seconds; if the producer is still +live, it retains the SDK +handle rather than closing a device still in use. Native waits, conversion and +close release the Python GIL where needed for shutdown coordination. + +Preview and the [EGO recording plugin](../../../src/plugins/ego/README.md) +each open the physical camera; run one at a time. Sensor recording, controls +and restoration, OpenXR tracker publication, and MCAP output belong to the +plugin. This package only captures stereo video for live preview. diff --git a/examples/camera_viz/ego_preview/__init__.py b/examples/camera_viz/ego_preview/__init__.py new file mode 100644 index 0000000000..4dea3fd910 --- /dev/null +++ b/examples/camera_viz/ego_preview/__init__.py @@ -0,0 +1,19 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +"""SDK capture binding for the EGO live-preview source. + +Capture owns one stereo SDK pipeline and returns paired host payloads. +sources.ego handles GPU upload/decoding and the FrameSource lifecycle; +sensor recording and MCAP output belong to the separate EGO plugin. +See README.md here for build instructions and payload semantics. +""" + +try: + from ._ego_capture import Capture +except ImportError as exc: + raise ImportError( + "EGO camera_viz binding is not built. Run " + "`ego_preview/build.sh --sdk-root PATH`." + ) from exc + +__all__ = ["Capture"] diff --git a/examples/camera_viz/ego_preview/build.sh b/examples/camera_viz/ego_preview/build.sh new file mode 100755 index 0000000000..78a8de0cfb --- /dev/null +++ b/examples/camera_viz/ego_preview/build.sh @@ -0,0 +1,43 @@ +#!/usr/bin/env bash +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# Build the SDK-only preview binding for the active Python environment. +# GPU decoding is built separately in codec/; recording uses the EGO plugin. +set -euo pipefail + +here="$(cd "$(dirname "$0")" && pwd)" +camera_viz_dir="$(dirname "$here")" +sdk_root="${ORBBEC_SDK_ROOT:-}" +while (( $# )); do + case $1 in + --sdk-root) + (( $# >= 2 )) || { echo "build.sh: --sdk-root requires a directory" >&2; exit 1; } + sdk_root=$2; shift 2;; + *) echo "build.sh: unknown argument: $1" >&2; exit 1;; + esac +done +if [[ -z "$sdk_root" ]]; then + echo "build.sh: --sdk-root PATH (or ORBBEC_SDK_ROOT) is required" >&2 + exit 1 +fi +if [[ -z "${VIRTUAL_ENV:-}" ]]; then + echo "build.sh: activate examples/camera_viz/.venv first" >&2 + exit 1 +fi +if [[ ! -f "$sdk_root/lib/libOrbbecSDK.so" ]]; then + echo "build.sh: libOrbbecSDK.so was not found under: $sdk_root/lib" >&2 + exit 1 +fi + +generator=() +if command -v ninja >/dev/null 2>&1; then + generator=(-G Ninja) +fi +cmake -S "$here" -B "$here/build" "${generator[@]}" \ + -DCMAKE_BUILD_TYPE=Release \ + -DPython3_EXECUTABLE="$(command -v python)" \ + -UORBBEC_SDK_LIBRARY \ + -DORBBEC_SDK_ROOT="$sdk_root" +cmake --build "$here/build" --parallel +(cd "$camera_viz_dir" && python -c 'from ego_preview import Capture') diff --git a/examples/camera_viz/ego_preview/ego_capture.cpp b/examples/camera_viz/ego_preview/ego_capture.cpp new file mode 100644 index 0000000000..ddbc46e9e9 --- /dev/null +++ b/examples/camera_viz/ego_preview/ego_capture.cpp @@ -0,0 +1,219 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// SDK-only stereo capture for camera_viz. One Capture owns the device pipeline +// and copies paired host payloads; sources.ego owns the producer lifecycle +// and GPU publication. Sensor recording and MCAP output remain in the EGO plugin. + +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace py = pybind11; + +namespace +{ + +OBFormat format_from_name(const std::string& name) +{ + if (name == "mjpg" || name == "mjpeg") + return OB_FORMAT_MJPG; + if (name == "h264") + return OB_FORMAT_H264; + if (name == "h265" || name == "hevc") + return OB_FORMAT_H265; + throw std::invalid_argument("format must be mjpg, h264, or h265"); +} + +int64_t monotonic_ns() +{ + return std::chrono::duration_cast(std::chrono::steady_clock::now().time_since_epoch()).count(); +} + +// The source producer serializes reads and close; never close during a live read. +class Capture +{ +public: + Capture(std::string uid, uint32_t width, uint32_t height, uint32_t fps, std::string format) + : width_(width), height_(height), format_(format_from_name(format)) + { +#if defined(__linux__) || defined(__ANDROID__) + context_->setUvcBackendType(OB_UVC_BACKEND_TYPE_LIBUVC); +#endif + const auto devices = context_->queryDeviceList(); + for (uint32_t index = 0; index < devices->getCount(); ++index) + { + const auto candidate = devices->getDevice(index); + if (uid.empty() || candidate->getDeviceInfo()->getUid() == uid) + { + device_ = candidate; + selected_uid_ = candidate->getDeviceInfo()->getUid(); + break; + } + } + if (!device_) + throw std::runtime_error("EGO not found"); + pipeline_ = std::make_unique(device_); + auto config = std::make_shared(); + config->enableStream(device_->getSensor(OB_SENSOR_COLOR_LEFT) + ->getStreamProfileList() + ->getVideoStreamProfile(width, height, format_, fps)); + config->enableStream(device_->getSensor(OB_SENSOR_COLOR_RIGHT) + ->getStreamProfileList() + ->getVideoStreamProfile(width, height, format_, fps)); + pipeline_->start(config); + if (format_ == OB_FORMAT_MJPG) + { + left_converter_ = std::make_unique(); + right_converter_ = std::make_unique(); + left_converter_->setFormatConvertType(FORMAT_MJPG_TO_RGB); + right_converter_->setFormatConvertType(FORMAT_MJPG_TO_RGB); + } + } + + ~Capture() + { + try + { + close(); + } + catch (const ob::Error&) + { + } + } + + void close() + { + if (pipeline_) + { + // A confirmed removed instance has deactivated SDK components; release it through SDK destruction. + // Connected-device stop errors still propagate through the existing source lifecycle. + if (!device_disconnected_) + pipeline_->stop(); + pipeline_.reset(); + } + left_converter_.reset(); + right_converter_.reset(); + device_.reset(); + context_.reset(); + } + + py::object next_pair(uint32_t timeout_ms) + { + if (!pipeline_) + throw std::runtime_error("EGO capture is closed"); + if (device_disconnected_) + throw std::runtime_error("EGO device " + selected_uid_ + " disconnected"); + std::shared_ptr frame_set; + { + py::gil_scoped_release release; + frame_set = pipeline_->waitForFrameset(timeout_ms); + } + if (!frame_set) + return no_pair(); + const auto left_raw = frame_set->getFrame(OB_FRAME_COLOR_LEFT); + const auto right_raw = frame_set->getFrame(OB_FRAME_COLOR_RIGHT); + if (!left_raw || !right_raw) + return no_pair(); + const auto left = left_raw->as(); + const auto right = right_raw->as(); + const auto left_ts = static_cast(left->getTimeStampUs()) * 1000; + const auto right_ts = static_cast(right->getTimeStampUs()) * 1000; + // Match the eyes on raw device time before conversion or payload copies. + if (std::llabs(left_ts - right_ts) > 2'000'000) + return no_pair(); + const auto left_bytes = bytes(left); + const auto right_bytes = bytes(right); + if (left_bytes.is_none() || right_bytes.is_none()) + return no_pair(); + py::dict result; + result["left"] = left_bytes; + result["right"] = right_bytes; + // Preview time is host monotonic time after extraction, not exposure time. + result["timestamp_ns"] = monotonic_ns(); + result["device_timestamp_ns"] = (left_ts + right_ts) / 2; + result["sequence_left"] = left->getIndex(); + result["sequence_right"] = right->getIndex(); + return std::move(result); + } + +private: + py::object no_pair() + { + const auto now = std::chrono::steady_clock::now(); + if (now >= next_presence_check_) + { + next_presence_check_ = now + std::chrono::seconds(1); + bool present = false; + { + py::gil_scoped_release release; + const auto devices = context_->queryDeviceList(); + for (uint32_t index = 0; index < devices->getCount(); ++index) + if (selected_uid_ == devices->getUid(index)) + present = true; + } + // None also represents transient timeout or incomplete pairs; only confirmed UID loss means disconnect. + if (!present) + { + device_disconnected_ = true; + throw std::runtime_error("EGO device " + selected_uid_ + " disconnected"); + } + } + return py::none(); + } + + py::object bytes(const std::shared_ptr& frame) + { + std::shared_ptr output = frame; + if (format_ == OB_FORMAT_MJPG) + { + auto& converter = frame->getType() == OB_FRAME_COLOR_LEFT ? left_converter_ : right_converter_; + std::shared_ptr converted; + { + py::gil_scoped_release release; + converted = converter->process(frame); + } + // The SDK rejects undecodable JPEGs with a null frame; drop the pair. + if (!converted) + return py::none(); + output = converted->as(); + const auto rgb_bytes = static_cast(width_) * height_ * 3; + if (output->getFormat() != OB_FORMAT_RGB || output->getWidth() != width_ || + output->getHeight() != height_ || output->getDataSize() != rgb_bytes || !output->getData()) + throw std::runtime_error("EGO MJPEG conversion did not return the requested RGB frame"); + } + return py::bytes(reinterpret_cast(output->getData()), output->getDataSize()); + } + + std::unique_ptr context_ = std::make_unique(); + std::shared_ptr device_; + std::unique_ptr pipeline_; + std::unique_ptr left_converter_; + std::unique_ptr right_converter_; + std::string selected_uid_; + std::chrono::steady_clock::time_point next_presence_check_{}; + bool device_disconnected_ = false; + uint32_t width_; + uint32_t height_; + OBFormat format_; +}; + +} // namespace + +PYBIND11_MODULE(_ego_capture, module) +{ + module.doc() = "Native EGO stereo capture for camera_viz"; + py::class_(module, "Capture") + .def(py::init(), py::arg("device_uid") = "", + py::arg("width") = 1600, py::arg("height") = 1300, py::arg("fps") = 30, py::arg("format") = "h264", + py::call_guard()) + // SDK shutdown must not prevent the consumer thread from joining. + .def("close", &Capture::close, py::call_guard()) + .def("next_pair", &Capture::next_pair, py::arg("timeout_ms") = 100); +} diff --git a/examples/camera_viz/pyproject.toml b/examples/camera_viz/pyproject.toml index c2dd8a13da..9508709c87 100644 --- a/examples/camera_viz/pyproject.toml +++ b/examples/camera_viz/pyproject.toml @@ -30,6 +30,7 @@ v4l2 = ["opencv-python>=4.5"] # wheel as v4l2, listed separately so --no-v4l2 setups can still ask for it. video = ["opencv-python>=4.5"] oakd = ["depthai>=3.0"] +ego = ["pybind11>=2.11"] # pyzed isn't on PyPI — the ZED SDK's bundled ``get_python_api.py`` # produces a Python-version-specific wheel. ``camera_viz.sh setup --with-zed`` # runs that and installs the result. diff --git a/examples/camera_viz/scripts/_install_deps.sh b/examples/camera_viz/scripts/_install_deps.sh index dd46149e40..7c236abda9 100755 --- a/examples/camera_viz/scripts/_install_deps.sh +++ b/examples/camera_viz/scripts/_install_deps.sh @@ -55,6 +55,8 @@ WITH_OAKD=true # --sender-only forces it on below (streaming is the sender's whole job). WITH_RTP=false WITH_ZED=false +WITH_EGO=false +ORBBEC_SDK_DIR="${ORBBEC_SDK_ROOT:-}" ZED_SDK_DIR=/usr/local/zed # Skip the index probes and build isaaccapture from this checkout. Also the # non-interactive answer to the tier-3 prompt below. @@ -76,6 +78,10 @@ while (( $# )); do --no-oakd) WITH_OAKD=false; shift;; --with-rtp) WITH_RTP=true; shift;; --with-zed) WITH_ZED=true; shift;; + --with-ego) WITH_EGO=true; shift;; + --sdk-root) + (( $# >= 2 )) || die "--sdk-root requires a directory" + ORBBEC_SDK_DIR=$2; shift 2;; --zed-sdk) ZED_SDK_DIR=$2; shift 2;; --build-from-source) BUILD_FROM_SOURCE=true; shift;; *) die "unknown arg: $1";; @@ -87,6 +93,10 @@ done if [[ "$MODE" == sender ]]; then WITH_RTP=true fi +if $WITH_EGO; then + [[ -n "$ORBBEC_SDK_DIR" ]] || die "--with-ego requires --sdk-root or ORBBEC_SDK_ROOT" + [[ -f "$ORBBEC_SDK_DIR/include/libobsensor/ObSensor.hpp" && -f "$ORBBEC_SDK_DIR/lib/libOrbbecSDK.so" ]] || die "invalid OrbbecSDK root: $ORBBEC_SDK_DIR" +fi # Select the toolkit CuPy will discover, without guessing from the driver. cuda_info=$(python3 "$HERE/check_cuda.py" --detect) || die "CUDA toolkit detection failed" @@ -472,6 +482,7 @@ $WITH_V4L2 && EXTRAS+=(v4l2) $WITH_OAKD && EXTRAS+=(oakd) $WITH_RTP && EXTRAS+=(rtp) $WITH_ZED && EXTRAS+=(zed) +$WITH_EGO && EXTRAS+=(ego) step "camera_viz setup — ${MODE} mode" note "venv $VENV_DIR" note "python $PYTHON_VERSION" @@ -535,6 +546,9 @@ PKGS=("pyyaml>=6.0" "$target_cupy" "numpy>=1.23" "scipy>=1.15" "pillow>=10.0") $WITH_V4L2 && PKGS+=("opencv-python>=4.5") $WITH_OAKD && PKGS+=("depthai>=3.0") $WITH_RTP && PKGS+=("pybind11>=2.11" "PyGObject>=3.42,<3.52") +if $WITH_EGO && ! $WITH_RTP; then + PKGS+=("pybind11>=2.11") +fi # Local wheels keep version ``1.3+local`` across rebuilds; uv's --upgrade # no-ops on them. mtime probe forces a reinstall when the wheel's newer. @@ -585,7 +599,7 @@ fi # Native NVENC/NVDEC codec. Failures are non-fatal: the runtime falls # back to the GStreamer encoder when the native ``.so`` isn't importable. -if $WITH_RTP; then +if $WITH_RTP || $WITH_EGO; then CODEC_DIR="$CAMERA_VIZ_DIR/codec" if [[ -d "$CODEC_DIR" ]]; then step "building native NVENC/NVDEC codec" @@ -593,11 +607,23 @@ if $WITH_RTP; then source "$VENV_DIR/bin/activate" if ! "$CODEC_DIR/build.sh"; then warn "codec build failed — falling back to the GStreamer encoder at runtime" + if $WITH_EGO; then + warn "EGO H.264/H.265 requires the native decoder; use format: mjpg until it builds" + fi fi deactivate fi fi +if $WITH_EGO; then + [[ -n "$ORBBEC_SDK_DIR" ]] || die "--with-ego requires --sdk-root or ORBBEC_SDK_ROOT" + step "building EGO capture" + # shellcheck disable=SC1091 + source "$VENV_DIR/bin/activate" + "$CAMERA_VIZ_DIR/ego_preview/build.sh" --sdk-root "$ORBBEC_SDK_DIR" + deactivate +fi + # Smoke imports. ``gi`` is in the list under RTP to confirm PyGObject # built and installed cleanly into the venv. SMOKE_MODS="cupy yaml scipy.spatial.transform" diff --git a/examples/camera_viz/sources/__init__.py b/examples/camera_viz/sources/__init__.py index 89132a9c81..2f9cf5a8ee 100644 --- a/examples/camera_viz/sources/__init__.py +++ b/examples/camera_viz/sources/__init__.py @@ -15,6 +15,7 @@ from ._helpers import PairedFrameSource, set_notify_sink, set_verbose from .oakd import OakdSource +from .ego import EgoSource from .rtp_h264 import RtpH264Source from .synthetic import SyntheticSource, SyntheticStereoSource from .v4l2 import V4l2Source @@ -23,6 +24,7 @@ __all__ = [ "OakdSource", + "EgoSource", "PairedFrameSource", "RtpH264Source", "SyntheticSource", @@ -49,7 +51,9 @@ def resolve_video_paths(cfg: dict, base_dir) -> None: cam["path"] = str(p) -def build_local_camera(spec: dict) -> List[FrameSource]: +def build_local_camera( + spec: dict, *, renderer_cuda_device_id: int | None = None +) -> List[FrameSource]: """Build local FrameSource(s) for one ``cameras:`` entry. Mono → [source]; ``stereo: true`` → [PairedFrameSource]. v4l2 rejects @@ -121,6 +125,21 @@ def build_local_camera(spec: dict) -> List[FrameSource]: ) return [PairedFrameSource(name=name, left=eyes[0], right=eyes[1])] return eyes + if kind == "ego": + if not stereo: + raise ValueError("EGO always produces a stereo frame") + return [ + EgoSource( + name=name, + device_uid=spec.get("device_uid", ""), + width=int(spec.get("width", 1600)), + height=int(spec.get("height", 1300)), + fps=int(spec.get("fps", 30)), + format=spec.get("format", "h264"), + gpu_id=int(spec.get("gpu_id", 0)), + renderer_cuda_device_id=renderer_cuda_device_id, + ) + ] if kind == "video": # Stereo (side-by-side file) emits both eyes from one source, like # SyntheticStereoSource — viewer-only; camera_streamer's @@ -158,5 +177,5 @@ def build_local_camera(spec: dict) -> List[FrameSource]: return eyes raise ValueError( f"build_local_camera: unknown camera type {kind!r} " - "(known: synthetic, v4l2, oakd, zed, video)" + "(known: synthetic, v4l2, oakd, ego, zed, video)" ) diff --git a/examples/camera_viz/sources/_nv_decode.py b/examples/camera_viz/sources/_nv_decode.py index 3bcc84ec84..f0b6c84056 100644 --- a/examples/camera_viz/sources/_nv_decode.py +++ b/examples/camera_viz/sources/_nv_decode.py @@ -1,6 +1,6 @@ # SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # SPDX-License-Identifier: Apache-2.0 -"""NVDEC H.264 decoder — wrapper around the native ``codec`` module.""" +"""NVDEC H.264/HEVC decoder around the native ``codec`` module.""" from __future__ import annotations @@ -9,8 +9,8 @@ logger = logging.getLogger("isaaccapture.camera_viz.sources._nv_decode") -class NvH264Decoder: - """Annex-B H.264 packet → RGBA8 GPU buffer. +class NvVideoDecoder: + """Annex-B H.264/HEVC packet → RGBA8 GPU buffer. Decoder is created lazily on the first packet. Resolution is fixed at construction; streams that don't match drop frames with a warning. @@ -23,8 +23,12 @@ def __init__( full_range: bool = False, gpu_id: int = 0, low_latency: bool = True, + codec: str = "h264", ) -> None: del low_latency # native codec is always zero-latency + self._codec = codec.lower() + if self._codec not in ("h264", "h265", "hevc"): + raise ValueError(f"unsupported decoder codec: {codec!r}") self._width = width self._height = height @@ -39,7 +43,7 @@ def _ensure_initialized(self) -> None: import codec except ImportError as e: raise RuntimeError( - "NvH264Decoder requires the native codec. Run " + "NvVideoDecoder requires the native codec. Run " "`examples/camera_viz/codec/build.sh`." ) from e @@ -48,7 +52,12 @@ def _ensure_initialized(self) -> None: cfg.height = self._height cfg.full_range = self._full_range cfg.gpu_id = self._gpu_id - self._decoder = codec.H264Decoder(cfg) + cfg.codec = ( + codec.DecoderCodec.H264 + if self._codec == "h264" + else codec.DecoderCodec.HEVC + ) + self._decoder = codec.VideoDecoder(cfg) def decode(self, packet: bytes, rgba_out) -> bool: """Feed one Annex-B AU. Returns True iff a frame was written to ``rgba_out``.""" @@ -59,3 +68,7 @@ def reset(self) -> None: """Tear down NVDEC state. Use after stream-timeout / disconnect.""" if self._decoder is not None: self._decoder.reset() + + +class NvH264Decoder(NvVideoDecoder): + """Compatibility entry point for existing H.264 sources.""" diff --git a/examples/camera_viz/sources/ego.py b/examples/camera_viz/sources/ego.py new file mode 100644 index 0000000000..cf80e44621 --- /dev/null +++ b/examples/camera_viz/sources/ego.py @@ -0,0 +1,313 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +"""EGO stereo source for camera_viz. + +One source owns a native Capture instance and a producer thread. Each Frame +contains paired ColorLeft/ColorRight GPU RGBA8 images. The binding rejects +missing eyes or device timestamps more than 2 ms apart; publication uses +host monotonic time sampled after payload extraction, not camera exposure time. + +MJPEG uses SDK conversion to host RGB, pinned staging, and CUDA upload. +H.264/H.265 host payloads feed one shared-codec NVDEC decoder per eye. +Both paths finish GPU writes before publishing reusable triple buffers. +The viewer supplies its CUDA device ID; buffers and both decoders use that GPU. + +Open failures retry; capture-read exceptions reopen and reset the decoders. +Incomplete pairs and SDK timeouts are skipped. Other capture/decode failures +reach the viewer through latest() and exit through SDK cleanup. stop() requests +exit and waits up to five seconds, retaining the handle while the producer is live. + +This source opens the camera independently of the recording plugin. +Sensor recording, control restoration, and MCAP output belong to that plugin; +run preview and recording separately because each needs device ownership. +""" + +from __future__ import annotations + +import logging +import threading +import time +from typing import Optional + +import numpy as np + +from pipeline import Frame, FrameSource, SourceSpec +from ._helpers import alloc_pinned_host, notify, notify_verbose +from ._nv_decode import NvVideoDecoder + +logger = logging.getLogger("isaaccapture.camera_viz.sources.ego") + + +class EgoSource(FrameSource): + """One physical Ego producing timestamp-matched left/right GPU RGBA8.""" + + def __init__( + self, + name: str, + device_uid: str = "", + width: int = 1600, + height: int = 1300, + fps: int = 30, + format: str = "h264", + gpu_id: int = 0, + *, + renderer_cuda_device_id: int | None = None, + ) -> None: + if width <= 0 or height <= 0 or fps <= 0: + raise ValueError("EGO width, height, and fps must be positive") + self._spec = SourceSpec(name=name, width=width, height=height) + self._device_uid = device_uid + self._fps = fps + self._format = format.lower() + if self._format not in ("mjpg", "h264", "h265"): + raise ValueError("EGO format must be mjpg, h264, or h265") + self._requested_gpu_id = gpu_id + self._renderer_cuda_device_id = renderer_cuda_device_id + self._gpu_id = gpu_id + self._thread: Optional[threading.Thread] = None + self._stop = threading.Event() + self._lock = threading.Lock() + self._error: Optional[BaseException] = None + self._publish_idx = -1 + self._consumed_idx = -2 + self._timestamp_ns = 0 + self._write_idx = 0 + self._buffers_left = [] + self._buffers_right = [] + self._capture = None + self._incomplete_pairs = 0 + self._frames = 0 + + @property + def spec(self) -> SourceSpec: + return self._spec + + def start(self) -> None: + if self._thread is not None: + return + gpu_id = self._requested_gpu_id + renderer_gpu = self._renderer_cuda_device_id + if gpu_id < 0 or (renderer_gpu is not None and renderer_gpu < 0): + raise ValueError("EGO requires a nonnegative CUDA device ID") + if renderer_gpu is not None: + # Zero retains the source's auto-selection contract, not a pin to GPU 0. + if gpu_id != 0 and gpu_id != renderer_gpu: + raise ValueError( + f"EGO source {self._spec.name!r} requests CUDA GPU {gpu_id}, " + f"but the renderer uses CUDA GPU {renderer_gpu}; " + "set gpu_id to 0 to follow the renderer" + ) + gpu_id = renderer_gpu + try: + import cupy as cp + from ego_preview import Capture # noqa: F401 + except ImportError as exc: + raise RuntimeError( + "EgoSource requires CuPy and the native EGO binding. " + "Run `camera_viz.sh setup --with-ego " + "--sdk-root PATH`." + ) from exc + if renderer_gpu is None and gpu_id == 0: + gpu_id = int(cp.cuda.runtime.getDevice()) + self._gpu_id = gpu_id + with self._lock: + self._error = None + self._publish_idx = -1 + self._consumed_idx = -2 + self._timestamp_ns = 0 + self._write_idx = 0 + self._incomplete_pairs = 0 + self._frames = 0 + with cp.cuda.Device(self._gpu_id): + buffers_left = [ + cp.empty((self._spec.height, self._spec.width, 4), dtype=cp.uint8) + for _ in range(3) + ] + buffers_right = [ + cp.empty((self._spec.height, self._spec.width, 4), dtype=cp.uint8) + for _ in range(3) + ] + for buffer in buffers_left + buffers_right: + buffer[..., 3] = 255 + self._buffers_left, self._buffers_right = buffers_left, buffers_right + self._stop.clear() + self._thread = threading.Thread( + target=self._produce_loop, name=f"ego_{self._spec.name}", daemon=False + ) + try: + self._thread.start() + except Exception: + # An interrupt may arrive after the thread starts; retain it for stop(). + self._thread = None + self._buffers_left, self._buffers_right = [], [] + raise + + def stop(self) -> None: + self._stop.set() + if self._thread is not None: + self._thread.join(timeout=5.0) + if self._thread.is_alive(): + # Match NV's source stop budget; a live SDK call owns its handle. + logger.warning( + "EGO producer did not exit within 5s; retaining SDK handle" + ) + return + self._thread = None + self._capture = None + + def latest(self) -> Optional[Frame]: + with self._lock: + if self._error is not None: + raise RuntimeError( + f"EGO source {self._spec.name!r} failed: {self._error}" + ) from self._error + if self._publish_idx < 0 or self._publish_idx == self._consumed_idx: + return None + index = self._publish_idx + self._consumed_idx = index + timestamp_ns = self._timestamp_ns + return Frame( + image=self._buffers_left[index], + image_right=self._buffers_right[index], + timestamp_ns=timestamp_ns, + source_id=self._spec.name, + stream=0, + ) + + def _produce_loop(self) -> None: + try: + self._capture_loop() + except BaseException as exc: + # Report before SDK cleanup, which may block while latest() is polled. + self._record_error(exc) + logger.exception("EGO producer stopped after a capture or decode failure") + finally: + try: + self._close_capture() + except BaseException as exc: + self._record_error(exc) + logger.exception("EGO SDK close failed") + + def _record_error(self, exc: BaseException) -> None: + with self._lock: + # A cleanup failure must not replace the original producer failure. + if self._error is None: + self._error = exc + self._stop.set() + + def _close_capture(self) -> None: + capture, self._capture = self._capture, None + if capture is not None: + capture.close() + + def _capture_loop(self) -> None: + import cupy as cp + from ego_preview import Capture + + with cp.cuda.Device(self._gpu_id): + stream_left = cp.cuda.Stream(non_blocking=True) + stream_right = cp.cuda.Stream(non_blocking=True) + landing_left = landing_right = None + staging_left = staging_right = None + decoder_left = decoder_right = None + if self._format == "mjpg": + shape = (self._spec.height, self._spec.width, 3) + staging_left = alloc_pinned_host(shape, np.uint8) + staging_right = alloc_pinned_host(shape, np.uint8) + landing_left = cp.empty(shape, dtype=cp.uint8) + landing_right = cp.empty(shape, dtype=cp.uint8) + else: + decoder_left = NvVideoDecoder( + self._spec.width, + self._spec.height, + gpu_id=self._gpu_id, + codec=self._format, + ) + decoder_right = NvVideoDecoder( + self._spec.width, + self._spec.height, + gpu_id=self._gpu_id, + codec=self._format, + ) + + last_report = time.monotonic() + while not self._stop.is_set(): + if self._capture is None: + try: + notify("ego", "opening...") + self._capture = Capture( + self._device_uid, + self._spec.width, + self._spec.height, + self._fps, + self._format, + ) + notify("ego", "connected") + except Exception as exc: + notify("ego", f"open failed ({exc}); retrying", logging.WARNING) + self._stop.wait(5.0) + continue + try: + pair = self._capture.next_pair(100) + except Exception as exc: + notify( + "ego", + f"capture failed ({exc}); reconnecting", + logging.WARNING, + ) + self._close_capture() + if decoder_left: + decoder_left.reset() + decoder_right.reset() + continue + if pair is None: + self._incomplete_pairs += 1 + continue + + index = self._write_idx + if self._format == "mjpg": + np.copyto( + staging_left, + np.frombuffer(pair["left"], dtype=np.uint8).reshape( + self._spec.height, self._spec.width, 3 + ), + ) + np.copyto( + staging_right, + np.frombuffer(pair["right"], dtype=np.uint8).reshape( + self._spec.height, self._spec.width, 3 + ), + ) + with stream_left: + landing_left.set(staging_left) + self._buffers_left[index][..., :3] = landing_left + with stream_right: + landing_right.set(staging_right) + self._buffers_right[index][..., :3] = landing_right + stream_left.synchronize() + stream_right.synchronize() + produced = True + else: + produced_left = decoder_left.decode( + pair["left"], self._buffers_left[index] + ) + produced_right = decoder_right.decode( + pair["right"], self._buffers_right[index] + ) + produced = produced_left and produced_right + if not produced: + self._incomplete_pairs += 1 + continue + with self._lock: + self._publish_idx = index + self._timestamp_ns = int(pair["timestamp_ns"]) + self._write_idx = (index + 1) % len(self._buffers_left) + self._frames += 1 + + now = time.monotonic() + if now - last_report >= 5.0: + notify_verbose( + "ego", + f"frames={self._frames} incomplete={self._incomplete_pairs}", + ) + last_report = now diff --git a/examples/camera_viz/sources/stereo_sbs.py b/examples/camera_viz/sources/stereo_sbs.py new file mode 100644 index 0000000000..1dd5da6dfb --- /dev/null +++ b/examples/camera_viz/sources/stereo_sbs.py @@ -0,0 +1,73 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +"""Side-by-side desktop preview of an existing stereo source. + +The adapter delegates capture start and stop to its wrapped FrameSource. +It copies left/right GPU RGBA8 images into a reusable image of twice the width, +placing the left eye first and preserving the original pair timestamp. +Copying follows the producer stream and completes before publication. + +The viewer enables this adapter only in window mode; XR uses separate eye images. +The wrapped source remains responsible for device capture, pairing, and decoding. +""" + +from __future__ import annotations + +from pipeline import Frame, FrameSource, SourceSpec + + +class StereoSideBySideSource(FrameSource): + """Expose a stereo pair as one RGBA image, with left then right.""" + + def __init__(self, source: FrameSource) -> None: + self._source = source + spec = source.spec + self._spec = SourceSpec( + f"{spec.name}.sbs", spec.width * 2, spec.height, spec.pixel_format + ) + self._buffers = None + self._index = 0 + + @property + def spec(self) -> SourceSpec: + return self._spec + + def start(self) -> None: + self._source.start() + + def stop(self) -> None: + self._source.stop() + + def latest(self) -> Frame | None: + frame = self._source.latest() + if frame is None: + return None + if frame.image_right is None: + raise RuntimeError("Stereo SBS debug mode requires a paired source") + import cupy as cp + + width = self._source.spec.width + shape = (self.spec.height, width, 4) + if frame.image.shape != shape or frame.image_right.shape != shape: + raise ValueError( + "stereo debug requires matching left/right RGBA dimensions" + ) + with cp.cuda.Device(frame.image.device.id): + # Copy on the producer stream; preserve the Frame stream contract. + stream = ( + cp.cuda.ExternalStream(frame.stream) + if frame.stream + else cp.cuda.Stream.null + ) + with stream: + if self._buffers is None: + self._buffers = [ + cp.empty((self.spec.height, self.spec.width, 4), dtype=cp.uint8) + for _ in range(3) + ] + image = self._buffers[self._index] + image[:, :width] = frame.image + image[:, width:] = frame.image_right + stream.synchronize() + self._index = (self._index + 1) % len(self._buffers) + return Frame(image, frame.timestamp_ns, self.spec.name, stream=frame.stream) diff --git a/examples/mcap_record_replay/README.md b/examples/mcap_record_replay/README.md index 33d1cc9d4a..766cf9644b 100644 --- a/examples/mcap_record_replay/README.md +++ b/examples/mcap_record_replay/README.md @@ -5,8 +5,8 @@ SPDX-License-Identifier: Apache-2.0 # MCAP record / replay -Record DeviceIO tracking to an MCAP file and replay it into a viser 3D view. -Live viewers are included for watching without recording. +The viewer examples record DeviceIO tracking to an MCAP file and replay it +into a viser 3D view. Live viewers are included for watching without recording. ```bash uv pip install -e ./examples/mcap_record_replay @@ -15,7 +15,7 @@ python -m isaaccapture_examples.mcap_record_replay.replay_hand # newest tak ``` Recordings are written to `./recordings/` relative to where you run the -command; a replay given no path picks the newest matching recording there +command; a replay viewer given no path picks the newest matching recording there (e.g. `replay_hand` picks the newest `hands_*.mcap`) and exits if none match -- no silent fallback to a wrong-type recording. @@ -25,7 +25,7 @@ lockfile. The live and replay viewers bind every interface, so a browser on another machine can reach them at `http://:8080`. Pass `--host 127.0.0.1` to -keep a viewer local. Replay repeats until Ctrl+C; no loop flag is needed. +keep a viewer local. Replay viewers repeat until Ctrl+C; no loop flag is needed. | Channel | Live | Record | Replay | | --- | --- | --- | --- | @@ -34,9 +34,56 @@ keep a viewer local. Replay repeats until Ctrl+C; no loop flag is needed. | Full body | `live_full_body` | `record_full_body` | `replay_full_body` | | Raw hand joint SE3 poses | `live_joint_se3_pose` | `record_joint_se3_pose` | `replay_joint_se3_pose` | | VIVE SE3 trackers | — | `record_se3_vive` | `replay_se3_vive` | +| EGO structured data | — | [EGO recording CLI](../../src/plugins/ego/README.md) | [replay_ego MCAP](../../src/plugins/ego/README.md#replay-structured-ego-data) (headless; stops at EOF) | `record_*` takes an optional duration in seconds and an optional output path. -Recording needs a live OpenXR runtime; replay needs only the file. +These recorders need a live OpenXR runtime; replay needs only the file. + +The EGO recording wrapper captures stereo video and available IMU streams by +default. Audio is opt-in through `--audio` or `audio = true` in a TOML config; +`--no-audio` overrides a config that enables it. Raw PCM capture, storage and +export are supported, but the tested Ethernet path has an SDK audio timestamp +cadence anomaly. Audio/video and five-stream physical synchronization have not +passed acceptance. See [optional audio and synchronization limits](../../src/plugins/ego/README.md#optional-audio-and-synchronization-limits). + +The current `replay_ego` example requires an Audio channel alongside camera, +IMU, calibration and any enabled diagnostic channels, so use it with an +audio-enabled recording. A recording without audio is valid, but this example reports its +missing Audio channel. Each tracker reads its next record according to the +existing NV update behavior; one update need not represent one physical instant. + +For selected-stream sample association, install the built `isaaccapture` package +with its `ego` extra and use the recommended four-stream offline audit: + +```bash +uv pip install 'isaaccapture[ego]' +python -m isaaccapture_examples.mcap_record_replay.analyze_ego_sync \ + recordings/ego_run --required-streams left_rgb right_rgb accel gyro \ + --output recordings/ego_run/sync_audit.json +``` + +Include `audio` only when the recording contains accessible audio media and an +independently validated audio time model. Selecting it does not establish +physical synchronization. A valid recorded time model is used by default; a +later recording-bound model requires explicit `--alignment PATH`. Use +`--reference-stream right_rgb`, `--epoch N`, `--start-time-ns N` and +`--end-time-ns N` to select frame windows in aligned nanoseconds. The JSON +contains an association index, coverage, gaps and declared validation evidence. +It does not decode or play media, and an unverified/incomplete result exits +nonzero while preserving its report. + +Fit a recording-bound model from independently annotated physical events: + +```bash +python -m isaaccapture_examples.mcap_record_replay.calibrate_ego_sync \ + recordings/ego_run/events.json --output recordings/ego_run/alignment.json +``` + +Both commands require a fresh output path. Calibration uses separate training +and held-out events over at least 30 minutes, includes measurement uncertainty, +and does not open a camera or detect events. See the +[EGO time-alignment workflow](../../src/plugins/ego/README.md#sample-time-alignment-and-offline-reading) +for the manifest/model contracts and the Python `iter_sync_windows` API. A C++ recorder lives in `cpp/`. Docs: `docs/source/references/mcap_record_replay.rst`. diff --git a/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/analyze_ego_sync.py b/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/analyze_ego_sync.py new file mode 100644 index 0000000000..b71e912b92 --- /dev/null +++ b/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/analyze_ego_sync.py @@ -0,0 +1,75 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 ORBBEC INC. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Write an EGO timing audit and media/sample association index.""" + +from __future__ import annotations + +import argparse +import json +from pathlib import Path + +from isaaccapture.ego import analyze_recording_sync + + +def main(argv: list[str] | None = None) -> int: + parser = argparse.ArgumentParser( + description="Audit selected EGO streams without decoding or playing media." + ) + parser.add_argument( + "recording", type=Path, help="Recording directory or completed MCAP" + ) + parser.add_argument( + "--required-streams", + nargs="+", + required=True, + choices=("left_rgb", "right_rgb", "accel", "gyro", "audio"), + help="Streams required by this use; include the reference stream", + ) + parser.add_argument( + "--alignment", type=Path, help="Explicit recording-bound alignment JSON" + ) + parser.add_argument( + "--reference-stream", choices=("left_rgb", "right_rgb"), default="left_rgb" + ) + parser.add_argument("--epoch", type=int) + parser.add_argument("--start-time-ns", type=int) + parser.add_argument("--end-time-ns", type=int) + parser.add_argument( + "--output", required=True, type=Path, help="New JSON audit/index path" + ) + args = parser.parse_args(argv) + try: + if args.output.exists() or args.output.is_symlink(): + raise FileExistsError(f"Refusing to overwrite audit output: {args.output}") + report = analyze_recording_sync( + args.recording, + required_streams=args.required_streams, + alignment=args.alignment, + reference_stream=args.reference_stream, + epoch=args.epoch, + start_time_ns=args.start_time_ns, + end_time_ns=args.end_time_ns, + ) + encoded = json.dumps(report, indent=2, allow_nan=False) + "\n" + with args.output.open("x", encoding="utf-8") as output: + output.write(encoded) + except (OSError, ValueError, TypeError, KeyError, ImportError) as error: + parser.error(str(error)) + coverage = report["coverage"] + status = report["alignment"]["status"] + print( + f"EGO alignment status={status}; windows={coverage['window_count']}; " + f"complete={coverage['complete_windows']}; output={args.output}" + ) + return ( + 0 + if status == "validated" + and coverage["window_count"] > 0 + and coverage["incomplete_windows"] == 0 + else 2 + ) + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/calibrate_ego_sync.py b/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/calibrate_ego_sync.py new file mode 100644 index 0000000000..a3e1b9da3e --- /dev/null +++ b/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/calibrate_ego_sync.py @@ -0,0 +1,219 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 ORBBEC INC. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Fit EGO recording clocks from annotated physical events and held-out evidence.""" + +from __future__ import annotations + +import argparse +import json +from pathlib import Path + +from isaaccapture.ego.calibration import ( + PairedTimeEvent, + evidence_sha256, + fit_time_alignment, + make_recording_alignment, +) + + +def _object(value, name): + if not isinstance(value, dict): + raise ValueError(f"{name} must be an object") + return value + + +def _array(value, name): + if not isinstance(value, list): + raise ValueError(f"{name} must be an array") + return value + + +def _text(value, name): + if not isinstance(value, str) or not value.strip(): + raise ValueError(f"{name} must be a nonempty string") + return value + + +def _integer(value, name): + if type(value) is not int: + raise ValueError(f"{name} must be an integer") + return value + + +def _unique_object(pairs): + result = {} + for key, value in pairs: + if key in result: + raise ValueError(f"Duplicate manifest key: {key}") + result[key] = value + return result + + +def _reject_constant(value): + raise ValueError(f"Nonfinite JSON value: {value}") + + +def _file(base: Path, value, name: str) -> Path: + path = (base / _text(value, name)).resolve(strict=True) + if not path.is_file(): + raise ValueError(f"{name} must name a regular file: {path}") + return path + + +def calibrate_manifest(manifest_path: str | Path) -> dict: + """Resolve annotation evidence paths against the manifest, without opening devices.""" + path = Path(manifest_path).resolve(strict=True) + manifest = _object( + json.loads( + path.read_text(encoding="utf-8"), + object_pairs_hook=_unique_object, + parse_constant=_reject_constant, + ), + "manifest", + ) + if _integer(manifest["version"], "version") != 1: + raise ValueError("Only version-1 event manifests are supported") + model_id = _text(manifest["model_id"], "model_id") + identity = _object(manifest["identity"], "identity") + profile = _object(manifest["profile"], "profile") + fps = _integer(_object(profile["video"], "profile.video")["fps"], "fps") + recording = _file(path.parent, manifest["recording"], "recording") + media = [ + _file(path.parent, value, "media_paths") + for value in _array(manifest.get("media_paths", []), "media_paths") + ] + hashes: dict[Path, str] = {} + + def events(values, name): + result = [] + for value in _array(values, name): + value = _object(value, name) + evidence = _file(path.parent, value["evidence"], "event.evidence") + if evidence not in hashes: + hashes[evidence] = "sha256:" + evidence_sha256(evidence) + result.append( + PairedTimeEvent( + _text(value["event_id"], "event_id"), + _integer(value["source_value"], "source_value"), + _integer(value["reference_time_ns"], "reference_time_ns"), + _integer(value["uncertainty_ns"], "uncertainty_ns"), + hashes[evidence], + ) + ) + return result + + def fit(value, *, begin, end, audio=False): + training = events(value["calibration_events"], "calibration_events") + held_out = events(value["validation_events"], "validation_events") + validation_start = _integer(value["validation_start_ns"], "validation_start_ns") + validation_end = _integer(value["validation_end_ns"], "validation_end_ns") + if not audio and not validation_start <= begin < end <= validation_end: + raise ValueError( + "Effective epoch interval exceeds independent validation bounds" + ) + if audio: + first = _integer(value["first_sample_index"], "first_sample_index") + count = _integer(value["sample_count"], "sample_count") + if any( + not first <= event.source_value < first + count + for event in (*training, *held_out) + ): + raise ValueError("Audio event is outside its declared sample segment") + return fit_time_alignment( + training, + held_out, + fps=fps, + validation_start_ns=validation_start, + validation_end_ns=validation_end, + source_unit="samples" if audio else "ns", + sample_rate_hz=( + _integer( + _object(profile["audio"], "profile.audio")["sample_rate_hz"], + "sample_rate_hz", + ) + if audio + else None + ), + ) + + epochs = [] + for epoch in _array(manifest["epochs"], "epochs"): + epoch = _object(epoch, "epoch") + begin = _integer(epoch["valid_from_ns"], "valid_from_ns") + end = _integer(epoch["valid_to_ns"], "valid_to_ns") + streams = {} + for name, value in _object(epoch["streams"], "streams").items(): + value = _object(value, name) + if name == "audio": + segments = [] + for segment in _array(value["segments"], "audio.segments"): + segment = _object(segment, "audio segment") + continuity = segment.get("continuity_verified", False) + if type(continuity) is not bool: + raise ValueError("continuity_verified must be a boolean") + segments.append( + { + "segment_id": _integer(segment["segment_id"], "segment_id"), + "first_sample_index": _integer( + segment["first_sample_index"], "first_sample_index" + ), + "sample_count": _integer( + segment["sample_count"], "sample_count" + ), + "continuity_verified": continuity, + "fit": fit(segment, begin=begin, end=end, audio=True), + } + ) + streams[name] = segments + elif name in ("right_rgb", "accel", "gyro"): + streams[name] = fit(value, begin=begin, end=end) + else: + raise ValueError(f"Unsupported fitted stream: {name}") + epochs.append( + { + "epoch": _integer(epoch["epoch"], "epoch"), + "valid_from_ns": begin, + "valid_to_ns": end, + "streams": streams, + } + ) + return make_recording_alignment( + recording, + model_id=model_id, + identity=identity, + profile=profile, + epochs=epochs, + media_paths=media, + ) + + +def main(argv: list[str] | None = None) -> int: + parser = argparse.ArgumentParser( + description=( + "Fit EGO time alignment from manually annotated physical events. " + "This command does not detect events or access hardware." + ) + ) + parser.add_argument("manifest", type=Path, help="Version-1 event manifest JSON") + parser.add_argument( + "--output", required=True, type=Path, help="New alignment JSON path" + ) + args = parser.parse_args(argv) + try: + if args.output.exists() or args.output.is_symlink(): + raise FileExistsError( + f"Refusing to overwrite alignment output: {args.output}" + ) + document = calibrate_manifest(args.manifest) + encoded = json.dumps(document, indent=2, allow_nan=False) + "\n" + with args.output.open("x", encoding="utf-8") as output: + output.write(encoded) + except (OSError, ValueError, TypeError, KeyError) as error: + parser.error(str(error)) + print(f"EGO alignment status={document['status']}; output={args.output}") + return 0 if document["status"] == "validated" else 2 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/replay_ego.py b/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/replay_ego.py new file mode 100644 index 0000000000..f1d6142428 --- /dev/null +++ b/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/replay_ego.py @@ -0,0 +1,242 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Replay all structured EGO trackers from a recorded MCAP file. + +Pass the ``metadata.mcap`` produced by ``ego.sh record``, or the +final archive produced by ``isaaccapture.ego.embedded_recording``. +""" + +import argparse +import logging +from collections import Counter +from pathlib import Path + +from isaaccapture.deviceio_session import McapReplayConfig, ReplaySession +from isaaccapture.deviceio_trackers import ( + EgoFrameMetadataTracker, + EgoAudioTracker, + EgoCalibrationTracker, + EgoDeviceStateTracker, + EgoImuTracker, +) +from isaaccapture.schema import ( + EgoCameraStream, + EgoSdkReport, + EgoCaptureSummary, + EgoRecoveryEvent, + EgoDeviceSnapshot, +) +from mcap.reader import make_reader + +logger = logging.getLogger("isaaccapture.examples.mcap_record_replay.replay_ego") + + +def _update_count(path: Path, topics: set[str]) -> int: + with path.open("rb") as stream: + reader = make_reader(stream) + summary = reader.get_summary() + counts: Counter[str] = Counter() + if ( + summary is not None + and summary.statistics is not None + and summary.statistics.channel_message_counts.keys() + <= summary.channels.keys() + ): + for channel_id, count in summary.statistics.channel_message_counts.items(): + topic = summary.channels[channel_id].topic + if topic in topics: + counts[topic] += count + else: + for _, channel, _ in reader.iter_messages( + topics=topics, log_time_order=False + ): + counts[channel.topic] += 1 + return max(counts.values(), default=0) + + +def _diagnostic_policy(path: Path, expected: bool | None) -> bool: + policy = None + with path.open("rb") as stream: + reader = make_reader(stream) + for metadata in reader.iter_metadata(): + if metadata.name != "ego.recording": + continue + value = metadata.metadata.get("record_device_diagnostics") + if ( + value not in ("on", "off") + or metadata.metadata.get("diagnostic_version") != "2" + ): + raise ValueError("Unsupported EGO recording policy") + enabled = value == "on" + if policy is not None and policy != enabled: + raise ValueError("Conflicting EGO recording policies") + policy = enabled + if expected is not None and policy is not None and policy != expected: + raise ValueError("Requested and recorded diagnostic policies disagree") + enabled = policy if policy is not None else expected + if enabled is False: + summary = reader.get_summary() + if summary is None: + raise ValueError( + "Diagnostics-off replay requires a complete channel inventory" + ) + for channel in summary.channels.values(): + schema = summary.schemas.get(channel.schema_id) + if schema is not None and schema.name == "core.EgoDeviceStateRecord": + raise ValueError( + f"Diagnostics off but channel exists: {channel.topic}" + ) + if policy is None: + logger.warning( + "No EGO recording policy; current diagnostic schema is still required" + ) + return enabled is not False + + +def replay( + path: Path, max_updates: int, *, record_device_diagnostics: bool | None = None +) -> dict[str, int]: + diagnostics = _diagnostic_policy(path, record_device_diagnostics) + frame_tracker = EgoFrameMetadataTracker( + "ego_metadata", + [EgoCameraStream.ColorLeft, EgoCameraStream.ColorRight], + ) + imu_tracker = EgoImuTracker("ego_imu") + audio_tracker = EgoAudioTracker("ego_audio/Audio") + calibration_tracker = EgoCalibrationTracker("ego_calibration/Calibration") + state_tracker = EgoDeviceStateTracker("ego_device/DeviceState") + trackers = [ + (frame_tracker, "ego_metadata"), + (imu_tracker, "ego_imu"), + (audio_tracker, "ego_audio"), + (calibration_tracker, "ego_calibration"), + ] + counts = { + "ColorLeft": 0, + "ColorRight": 0, + "Accel": 0, + "Gyro": 0, + "Audio": 0, + "Calibration": 0, + "DeviceState": 0, + } + topics = { + "ego_metadata/ColorLeft", + "ego_metadata/ColorRight", + "ego_imu/Accel", + "ego_imu/Gyro", + "ego_audio/Audio", + "ego_calibration/Calibration", + "ego_device/DeviceState", + } + if diagnostics: + trackers.append((state_tracker, "ego_device")) + else: + del counts["DeviceState"] + topics.remove("ego_device/DeviceState") + # ReplaySession has no EOF accessor; payload-less records also return None. + updates = _update_count(path, topics) + if max_updates: + updates = min(updates, max_updates) + + with ReplaySession.run(McapReplayConfig(str(path), trackers)) as session: + for _ in range(updates): + session.update() + + for index, name in enumerate(("ColorLeft", "ColorRight")): + data = frame_tracker.get_stream_data(session, index) + if data is not None: + counts[name] += 1 + print( + f"{name}: epoch={data.capture_epoch} seq={data.sequence_number} " + f"profile={data.width}x{data.height}@{data.fps} " + f"format={data.pixel_format} bytes={data.encoded_bytes}" + ) + + for index, name in enumerate(("Accel", "Gyro")): + data = imu_tracker.get_stream_data(session, index) + if data is not None: + counts[name] += 1 + print( + f"{name}: epoch={data.capture_epoch} seq={data.sequence_number} " + f"rate_hz={data.sample_rate_hz} samples={len(data.samples)}" + ) + + audio = audio_tracker.get_data(session) + if audio is not None: + counts["Audio"] += 1 + print( + f"Audio: epoch={audio.capture_epoch} seq={audio.sequence_number} " + f"rate_hz={audio.sample_rate_hz} samples={audio.sample_count} " + f"bytes={audio.byte_count}" + ) + + calibration = calibration_tracker.get_data(session) + if calibration is not None: + counts["Calibration"] += 1 + print( + f"Calibration: epoch={calibration.capture_epoch} " + f"device_uid={calibration.device_uid}" + ) + + state = state_tracker.get_data(session) if diagnostics else None + if state is not None: + counts["DeviceState"] += 1 + event = state.event + if isinstance(event, EgoSdkReport): + detail = f"seq={event.sequence_number} work_mode={event.work_mode} status_flags={event.status_flags} error_flags={event.error_flags} storage_free_bytes={event.storage_free_bytes}" + elif isinstance(event, EgoCaptureSummary): + detail = f"health={event.capture_health} queue={event.queue_peak}/{event.queue_capacity} dropped_events={event.dropped_events} dropped_video_frame_sets={event.dropped_video_frame_sets} final={event.is_final} reason={event.reason}" + elif isinstance(event, EgoRecoveryEvent): + detail = f"stage={event.stage} attempt={event.reconnect_attempt} reason={event.reason}" + elif isinstance(event, EgoDeviceSnapshot): + detail = f"imu_temperature_c={event.imu_temperature_c} properties={len(event.properties)}" + else: + raise ValueError("Unsupported EGO diagnostic event") + print( + f"DeviceState: event={state.event_type} version={state.diagnostic_version} epoch={state.capture_epoch} device_uid={state.device_uid} observed_at_ns={state.observed_at_ns} {detail}" + ) + + return counts + + +def main() -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("mcap", type=Path, help="Path to an EGO metadata.mcap file") + parser.add_argument( + "--max-updates", + type=int, + default=0, + help="Stop after this many replay updates; 0 reads to end of file", + ) + parser.add_argument( + "--record-device-diagnostics", + choices=("on", "off"), + help="Expected policy; otherwise use MCAP Metadata (defaults to on when absent)", + ) + args = parser.parse_args() + if not args.mcap.is_file(): + parser.error(f"MCAP file does not exist: {args.mcap}") + if args.max_updates < 0: + parser.error("--max-updates must be non-negative") + + expected = ( + None + if args.record_device_diagnostics is None + else args.record_device_diagnostics == "on" + ) + counts = replay(args.mcap, args.max_updates, record_device_diagnostics=expected) + print( + "Replay counts: " + + ", ".join(f"{name}={count}" for name, count in counts.items()) + ) + if not all(counts.values()): + missing = ", ".join(name for name, count in counts.items() if count == 0) + logger.error("Replay incomplete; no records for: %s", missing) + return 1 + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/examples/oxr/python/test_ego_camera.py b/examples/oxr/python/test_ego_camera.py new file mode 100644 index 0000000000..04c1d07abf --- /dev/null +++ b/examples/oxr/python/test_ego_camera.py @@ -0,0 +1,126 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Record every EGO data class in one of the three supported modes.""" + +import argparse +import time +from pathlib import Path + +import isaaccapture.deviceio_trackers as deviceio_trackers +from isaaccapture.deviceio_session import DeviceIOSession, McapRecordingConfig +from isaaccapture.schema import EgoCameraStream +import isaaccapture.oxr as oxr +import isaaccapture.plugin_manager as plugin_manager + + +def main() -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--duration", type=float, default=15.0) + parser.add_argument("--plugin-root", type=Path, required=True) + parser.add_argument( + "--mode", + choices=["no-metadata", "plugin-mcap", "schema-pusher"], + default="schema-pusher", + ) + parser.add_argument("--format", choices=["mjpg", "h264", "h265"], default="h264") + parser.add_argument("--preview", action="store_true") + parser.add_argument("--mcap", type=Path, default=Path("ego.mcap")) + args = parser.parse_args() + + prefix = "ego" + frame = deviceio_trackers.EgoFrameMetadataTracker( + prefix, + [ + EgoCameraStream.ColorLeft, + EgoCameraStream.ColorRight, + ], + ) + imu = deviceio_trackers.EgoImuTracker(prefix) + audio = deviceio_trackers.EgoAudioTracker(prefix + "/Audio") + calibration = deviceio_trackers.EgoCalibrationTracker(prefix + "/Calibration") + state = deviceio_trackers.EgoDeviceStateTracker(prefix + "/DeviceState") + trackers = [frame, imu, audio, calibration, state] + + suffix = {"mjpg": "mjpg", "h264": "h264", "h265": "h265"}[args.format] + plugin_args = [ + f"--add-stream=camera=ColorLeft,output=recordings/left.{suffix},format={args.format},width=1600,height=1300,fps=30", + f"--add-stream=camera=ColorRight,output=recordings/right.{suffix},format={args.format},width=1600,height=1300,fps=30", + "--enable-imu", + "--imu-rate=1000", + "--audio-output=recordings/audio.wav", + "--calibration-output=recordings/calibration.json", + ] + if args.preview: + plugin_args.append("--preview") + if args.mode == "plugin-mcap": + plugin_args.append(f"--mcap-filename={args.mcap.resolve()}") + elif args.mode == "schema-pusher": + plugin_args.append(f"--collection-prefix={prefix}") + + manager = plugin_manager.PluginManager([str(args.plugin_root)]) + if "ego_camera" not in manager.get_plugin_names(): + raise RuntimeError("ego_camera plugin was not discovered") + + with manager.start( + "ego_camera", "ego_camera", plugin_args, shutdown_timeout_seconds=15.0 + ) as plugin: + if args.mode != "schema-pusher": + deadline = time.monotonic() + args.duration + while time.monotonic() < deadline: + plugin.check_health() + time.sleep(0.1) + return 0 + + extensions = DeviceIOSession.get_required_extensions(trackers) + recording = McapRecordingConfig( + str(args.mcap), + [ + (frame, "ego_metadata"), + (imu, "ego_imu"), + (audio, "ego_audio"), + (calibration, "ego_calibration"), + (state, "ego_device"), + ], + ) + with oxr.OpenXRSession("EgoCameraTest", extensions) as oxr_session: + with DeviceIOSession.run( + trackers, oxr_session.get_handles(), recording + ) as session: + + def all_data_received() -> bool: + return ( + not any( + frame.get_stream_data(session, i) is None for i in range(2) + ) + and not any( + imu.get_stream_data(session, i) is None for i in range(2) + ) + and audio.get_data(session) is not None + and calibration.get_data(session) is not None + and state.get_data(session) is not None + ) + + startup_deadline = time.monotonic() + 30.0 + while not all_data_received() and time.monotonic() < startup_deadline: + plugin.check_health() + session.update() + time.sleep(0.016) + if not all_data_received(): + raise RuntimeError( + "did not receive all EGO data classes within 30 seconds" + ) + + deadline = time.monotonic() + args.duration + while time.monotonic() < deadline: + plugin.check_health() + session.update() + time.sleep(0.016) + + if not all_data_received(): + raise RuntimeError("lost an EGO data class during recording") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/pyproject.toml b/pyproject.toml index 767461704b..9b4c84880f 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -29,6 +29,9 @@ dependencies = [ ] [project.optional-dependencies] +ego = [ + "mcap>=1.2,<2", +] dev = [ "pytest>=7.0.0", "pytest-cov>=4.0.0", diff --git a/src/compat/pyproject.toml b/src/compat/pyproject.toml index 1c373fac26..200e6c5a9c 100644 --- a/src/compat/pyproject.toml +++ b/src/compat/pyproject.toml @@ -14,6 +14,7 @@ license = "Apache-2.0" dependencies = ["isaaccapture>=1.6.dev0,<1.9"] [project.optional-dependencies] +ego = ["isaaccapture[ego]>=1.6.dev0,<1.9"] dev = ["isaaccapture[dev]>=1.6.dev0,<1.9"] ui = ["isaaccapture[ui]>=1.6.dev0,<1.9"] retargeters = ["isaaccapture[retargeters]>=1.6.dev0,<1.9"] diff --git a/src/core/deviceio_base/cpp/inc/deviceio_base/ego_frame_metadata_tracker_base.hpp b/src/core/deviceio_base/cpp/inc/deviceio_base/ego_frame_metadata_tracker_base.hpp new file mode 100644 index 0000000000..ab93bd0eff --- /dev/null +++ b/src/core/deviceio_base/cpp/inc/deviceio_base/ego_frame_metadata_tracker_base.hpp @@ -0,0 +1,23 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "tracker.hpp" + +#include + +#include + +namespace core +{ + +struct EgoFrameMetadata; + +class IEgoFrameMetadataTrackerImpl : public ITrackerImpl +{ +public: + virtual const Serialized& get_stream_data(size_t stream_index) const = 0; +}; + +} // namespace core diff --git a/src/core/deviceio_base/cpp/inc/deviceio_base/ego_imu_tracker_base.hpp b/src/core/deviceio_base/cpp/inc/deviceio_base/ego_imu_tracker_base.hpp new file mode 100644 index 0000000000..d1f533c745 --- /dev/null +++ b/src/core/deviceio_base/cpp/inc/deviceio_base/ego_imu_tracker_base.hpp @@ -0,0 +1,23 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "tracker.hpp" + +#include + +#include + +namespace core +{ + +struct EgoImuBatch; + +class IEgoImuTrackerImpl : public ITrackerImpl +{ +public: + virtual const Serialized& get_stream_data(size_t stream_index) const = 0; +}; + +} // namespace core diff --git a/src/core/deviceio_trackers/cpp/CMakeLists.txt b/src/core/deviceio_trackers/cpp/CMakeLists.txt index 46f6a03ed4..d0f4b77014 100644 --- a/src/core/deviceio_trackers/cpp/CMakeLists.txt +++ b/src/core/deviceio_trackers/cpp/CMakeLists.txt @@ -6,6 +6,8 @@ cmake_minimum_required(VERSION 3.20) # Public tracker classes (HeadTracker, HandTracker, etc.) add_library(deviceio_trackers STATIC hand_tracker.cpp + ego_frame_metadata_tracker.cpp + ego_imu_tracker.cpp head_tracker.cpp controller_tracker.cpp message_channel_tracker.cpp @@ -15,6 +17,8 @@ add_library(deviceio_trackers STATIC keyboard_tracker.cpp ${GENERATED_TRACKER_FACADE_SOURCES} inc/deviceio_trackers/head_tracker.hpp + inc/deviceio_trackers/ego_frame_metadata_tracker.hpp + inc/deviceio_trackers/ego_imu_tracker.hpp inc/deviceio_trackers/hand_tracker.hpp inc/deviceio_trackers/controller_tracker.hpp inc/deviceio_trackers/message_channel_tracker.hpp diff --git a/src/core/deviceio_trackers/cpp/ego_frame_metadata_tracker.cpp b/src/core/deviceio_trackers/cpp/ego_frame_metadata_tracker.cpp new file mode 100644 index 0000000000..9fc70cbbf1 --- /dev/null +++ b/src/core/deviceio_trackers/cpp/ego_frame_metadata_tracker.cpp @@ -0,0 +1,48 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/deviceio_trackers/ego_frame_metadata_tracker.hpp" + +#include +#include +#include + +namespace core +{ + +EgoFrameMetadataTracker::EgoFrameMetadataTracker(const std::string& collection_prefix, + const std::vector& streams, + size_t max_flatbuffer_size) + : collection_prefix_(collection_prefix), streams_(streams), max_flatbuffer_size_(max_flatbuffer_size) +{ + if (collection_prefix_.empty()) + throw std::invalid_argument("EgoFrameMetadataTracker: collection_prefix is required"); + if (streams_.empty()) + throw std::invalid_argument("EgoFrameMetadataTracker: at least one stream is required"); + if (max_flatbuffer_size_ == 0) + throw std::invalid_argument("EgoFrameMetadataTracker: max_flatbuffer_size must be positive"); + + std::set unique_streams; + for (const auto stream : streams_) + { + const char* name = EnumNameEgoCameraStream(stream); + if (name == nullptr || *name == '\0') + { + throw std::invalid_argument("EgoFrameMetadataTracker: invalid stream value " + + std::to_string(static_cast(stream))); + } + if (!unique_streams.insert(stream).second) + { + throw std::invalid_argument("EgoFrameMetadataTracker: duplicate stream " + std::string(name)); + } + stream_names_.emplace_back(name); + } +} + +const Serialized& EgoFrameMetadataTracker::get_stream_data(const ITrackerSession& session, + size_t stream_index) const +{ + return static_cast(session.get_tracker_impl(*this)).get_stream_data(stream_index); +} + +} // namespace core diff --git a/src/core/deviceio_trackers/cpp/ego_imu_tracker.cpp b/src/core/deviceio_trackers/cpp/ego_imu_tracker.cpp new file mode 100644 index 0000000000..4226a8a8ae --- /dev/null +++ b/src/core/deviceio_trackers/cpp/ego_imu_tracker.cpp @@ -0,0 +1,49 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/deviceio_trackers/ego_imu_tracker.hpp" + +#include +#include +#include + +namespace core +{ + +namespace +{ +void validate_common(const std::string& tracker, const std::string& prefix, size_t max_size) +{ + if (prefix.empty()) + throw std::invalid_argument(tracker + ": collection_prefix is required"); + if (max_size == 0) + throw std::invalid_argument(tracker + ": max_flatbuffer_size must be positive"); +} +} // namespace + +EgoImuTracker::EgoImuTracker(std::string collection_prefix, std::vector sensors, size_t max_flatbuffer_size) + : collection_prefix_(std::move(collection_prefix)), + sensors_(std::move(sensors)), + max_flatbuffer_size_(max_flatbuffer_size) +{ + validate_common("EgoImuTracker", collection_prefix_, max_flatbuffer_size_); + if (sensors_.empty()) + throw std::invalid_argument("EgoImuTracker: at least one sensor is required"); + std::set unique; + for (const auto sensor : sensors_) + { + const char* name = EnumNameEgoImuSensor(sensor); + if (name == nullptr || *name == '\0') + throw std::invalid_argument("EgoImuTracker: invalid sensor"); + if (!unique.insert(sensor).second) + throw std::invalid_argument("EgoImuTracker: duplicate sensor " + std::string(name)); + stream_names_.emplace_back(name); + } +} + +const Serialized& EgoImuTracker::get_stream_data(const ITrackerSession& session, size_t stream_index) const +{ + return static_cast(session.get_tracker_impl(*this)).get_stream_data(stream_index); +} + +} // namespace core diff --git a/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/ego_frame_metadata_tracker.hpp b/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/ego_frame_metadata_tracker.hpp new file mode 100644 index 0000000000..3798091394 --- /dev/null +++ b/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/ego_frame_metadata_tracker.hpp @@ -0,0 +1,65 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include + +#include +#include +#include + +namespace core +{ + +class EgoFrameMetadataTracker : public ITracker +{ +public: + static constexpr size_t DEFAULT_MAX_FLATBUFFER_SIZE = EGO_MAX_FLATBUFFER_SIZE; + + EgoFrameMetadataTracker(const std::string& collection_prefix, + const std::vector& streams, + size_t max_flatbuffer_size = DEFAULT_MAX_FLATBUFFER_SIZE); + + std::string_view get_name() const override + { + return "EgoFrameMetadataTracker"; + } + + const Serialized& get_stream_data(const ITrackerSession& session, size_t stream_index) const; + + size_t get_stream_count() const + { + return stream_names_.size(); + } + + const std::string& collection_prefix() const + { + return collection_prefix_; + } + + const std::vector& streams() const + { + return streams_; + } + + size_t max_flatbuffer_size() const + { + return max_flatbuffer_size_; + } + + const std::vector& get_stream_names() const + { + return stream_names_; + } + +private: + std::string collection_prefix_; + std::vector streams_; + size_t max_flatbuffer_size_; + std::vector stream_names_; +}; + +} // namespace core diff --git a/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/ego_imu_tracker.hpp b/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/ego_imu_tracker.hpp new file mode 100644 index 0000000000..d26145f8e6 --- /dev/null +++ b/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/ego_imu_tracker.hpp @@ -0,0 +1,56 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include + +#include +#include +#include + +namespace core +{ + +class EgoImuTracker : public ITracker +{ +public: + EgoImuTracker(std::string collection_prefix, + std::vector sensors = { EgoImuSensor_Accel, EgoImuSensor_Gyro }, + size_t max_flatbuffer_size = EGO_MAX_FLATBUFFER_SIZE); + std::string_view get_name() const override + { + return "EgoImuTracker"; + } + const Serialized& get_stream_data(const ITrackerSession& session, size_t stream_index) const; + const std::string& collection_prefix() const + { + return collection_prefix_; + } + const std::vector& sensors() const + { + return sensors_; + } + const std::vector& stream_names() const + { + return stream_names_; + } + size_t max_flatbuffer_size() const + { + return max_flatbuffer_size_; + } + size_t get_stream_count() const + { + return sensors_.size(); + } + +private: + std::string collection_prefix_; + std::vector sensors_; + std::vector stream_names_; + size_t max_flatbuffer_size_; +}; + +} // namespace core diff --git a/src/core/deviceio_trackers/python/tracker_bindings.cpp b/src/core/deviceio_trackers/python/tracker_bindings.cpp index cc01a09062..b8129da53a 100644 --- a/src/core/deviceio_trackers/python/tracker_bindings.cpp +++ b/src/core/deviceio_trackers/python/tracker_bindings.cpp @@ -4,6 +4,8 @@ #include "generated_tracker_binding_includes.inc" #include +#include +#include #include #include #include @@ -259,6 +261,29 @@ PYBIND11_MODULE(_deviceio_trackers, m) py::arg("session"), py::arg("endpoint"), "Get the latest haptic command for one endpoint (None when no data available)"); + py::class_>( + m, "EgoFrameMetadataTracker") + .def(py::init&, size_t>(), + py::arg("collection_prefix"), py::arg("streams"), + py::arg("max_flatbuffer_size") = core::EgoFrameMetadataTracker::DEFAULT_MAX_FLATBUFFER_SIZE) + .def( + "get_stream_data", + [](const core::EgoFrameMetadataTracker& self, const core::ITrackerSession& session, size_t stream_index) + { return to_python(self.get_stream_data(session, stream_index)); }, + py::arg("session"), py::arg("stream_index")) + .def_property_readonly("stream_count", &core::EgoFrameMetadataTracker::get_stream_count); + + py::class_>(m, "EgoImuTracker") + .def(py::init, size_t>(), py::arg("collection_prefix"), + py::arg("sensors") = std::vector{ core::EgoImuSensor_Accel, core::EgoImuSensor_Gyro }, + py::arg("max_flatbuffer_size") = core::EGO_MAX_FLATBUFFER_SIZE) + .def( + "get_stream_data", + [](const core::EgoImuTracker& self, const core::ITrackerSession& session, size_t stream_index) + { return to_python(self.get_stream_data(session, stream_index)); }, + py::arg("session"), py::arg("stream_index")) + .def_property_readonly("stream_count", &core::EgoImuTracker::get_stream_count); + // py::class_ blocks for every manifest tracker; the accessor name comes from the // manifest's python_accessor key. #include "generated_tracker_bindings.inc" diff --git a/src/core/deviceio_trackers/trackers.toml b/src/core/deviceio_trackers/trackers.toml index d27518d43c..3884af4383 100644 --- a/src/core/deviceio_trackers/trackers.toml +++ b/src/core/deviceio_trackers/trackers.toml @@ -93,6 +93,30 @@ channel = "oak" header = "frame_metadata_tracker_oak" max_flatbuffer_size = 128 +[[tracker]] +name = "ego_audio" +table = "EgoAudioChunk" +tensor_identifier = "audio_chunk" +max_flatbuffer_size = 32768 +mcap_channels = ["Audio", "Audio_tracked"] +replay_channels = ["Audio"] + +[[tracker]] +name = "ego_calibration" +table = "EgoCalibration" +tensor_identifier = "calibration" +max_flatbuffer_size = 32768 +mcap_channels = ["Calibration", "Calibration_tracked"] +replay_channels = ["Calibration"] + +[[tracker]] +name = "ego_device_state" +table = "EgoDeviceState" +tensor_identifier = "device_state" +max_flatbuffer_size = 32768 +mcap_channels = ["DeviceState", "DeviceState_tracked"] +replay_channels = ["DeviceState"] + [[tracker]] name = "steering_wheel" table = "SteeringWheelOutput" diff --git a/src/core/live_trackers/cpp/CMakeLists.txt b/src/core/live_trackers/cpp/CMakeLists.txt index fad1087cae..7d7e5cfff5 100644 --- a/src/core/live_trackers/cpp/CMakeLists.txt +++ b/src/core/live_trackers/cpp/CMakeLists.txt @@ -15,6 +15,10 @@ add_library(live_trackers STATIC live_full_body_tracker_xsens_impl.cpp live_tensor_push_tracker_impl.cpp live_haptic_command_reader_tracker_impl.cpp + live_ego_frame_metadata_tracker_impl.cpp + live_ego_frame_metadata_tracker_impl.hpp + live_ego_imu_tracker_impl.cpp + live_ego_imu_tracker_impl.hpp live_keyboard_tracker_impl.cpp ${GENERATED_TRACKER_LIVE_SOURCES} inc/live_trackers/schema_tracker_base.hpp 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 080ea5a4e4..a4c3804f95 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 @@ -22,6 +22,10 @@ namespace core { class ITracker; +class EgoFrameMetadataTracker; +class IEgoFrameMetadataTrackerImpl; +class EgoImuTracker; +class IEgoImuTrackerImpl; class ITrackerImpl; class ControllerTracker; class IControllerTrackerImpl; @@ -92,6 +96,9 @@ class LiveDeviceIOFactory const HapticCommandReaderTracker* tracker); std::unique_ptr create_keyboard_tracker_impl(const KeyboardTracker* tracker); // create__tracker_impl for every manifest tracker. + std::unique_ptr create_ego_frame_metadata_tracker_impl( + const EgoFrameMetadataTracker* tracker); + std::unique_ptr create_ego_imu_tracker_impl(const EgoImuTracker* tracker); #include "generated_live_factory_declarations.inc" private: diff --git a/src/core/live_trackers/cpp/live_deviceio_factory.cpp b/src/core/live_trackers/cpp/live_deviceio_factory.cpp index b0008ac1e5..17dce3da34 100644 --- a/src/core/live_trackers/cpp/live_deviceio_factory.cpp +++ b/src/core/live_trackers/cpp/live_deviceio_factory.cpp @@ -5,6 +5,8 @@ #include "generated_live_includes.inc" #include "live_controller_tracker_impl.hpp" +#include "live_ego_frame_metadata_tracker_impl.hpp" +#include "live_ego_imu_tracker_impl.hpp" #include "live_full_body_tracker_noitom_impl.hpp" #include "live_full_body_tracker_pico_impl.hpp" #include "live_full_body_tracker_xsens_impl.hpp" @@ -16,6 +18,8 @@ #include "live_tensor_push_tracker_impl.hpp" #include +#include +#include #include #include #include @@ -113,6 +117,19 @@ std::unique_ptr try_create_haptic_command_reader_impl(LiveDeviceIO return typed ? factory.create_haptic_command_reader_tracker_impl(typed) : nullptr; } +std::unique_ptr try_create_ego_frame_metadata_tracker_impl(LiveDeviceIOFactory& factory, + const ITracker& tracker) +{ + auto* typed = dynamic_cast(&tracker); + return typed ? factory.create_ego_frame_metadata_tracker_impl(typed) : nullptr; +} + +std::unique_ptr try_create_ego_imu_tracker_impl(LiveDeviceIOFactory& factory, const ITracker& tracker) +{ + auto* typed = dynamic_cast(&tracker); + return typed ? factory.create_ego_imu_tracker_impl(typed) : nullptr; +} + std::unique_ptr try_create_keyboard_impl(LiveDeviceIOFactory& factory, const ITracker& tracker) { auto* typed = dynamic_cast(&tracker); @@ -164,6 +181,9 @@ inline const TrackerDispatchEntry k_tracker_dispatch[] = { make_dispatch_entry(&try_create_tensor_push_impl), make_dispatch_entry( &try_create_haptic_command_reader_impl), + make_dispatch_entry( + &try_create_ego_frame_metadata_tracker_impl), + make_dispatch_entry(&try_create_ego_imu_tracker_impl), make_dispatch_entry(&try_create_keyboard_impl), // Manifest trackers are single-vendor, so their rows can sit last as a block. #include "generated_live_dispatch_rows.inc" @@ -499,6 +519,27 @@ std::unique_ptr LiveDeviceIOFactory::create_hap return std::make_unique(handles_, tracker); } +std::unique_ptr LiveDeviceIOFactory::create_ego_frame_metadata_tracker_impl( + const EgoFrameMetadataTracker* tracker) +{ + std::unique_ptr channels; + if (should_record(tracker)) + { + channels = LiveEgoFrameMetadataTrackerImpl::create_mcap_channels(*writer_, get_name(tracker), tracker); + } + return std::make_unique(handles_, tracker, std::move(channels)); +} + +std::unique_ptr LiveDeviceIOFactory::create_ego_imu_tracker_impl(const EgoImuTracker* tracker) +{ + std::unique_ptr channels; + if (should_record(tracker)) + { + channels = LiveEgoImuTrackerImpl::create_mcap_channels(*writer_, get_name(tracker), tracker); + } + return std::make_unique(handles_, tracker, std::move(channels)); +} + std::unique_ptr LiveDeviceIOFactory::create_keyboard_tracker_impl(const KeyboardTracker* tracker) { std::unique_ptr channels; diff --git a/src/core/live_trackers/cpp/live_ego_frame_metadata_tracker_impl.cpp b/src/core/live_trackers/cpp/live_ego_frame_metadata_tracker_impl.cpp new file mode 100644 index 0000000000..0a4ef03739 --- /dev/null +++ b/src/core/live_trackers/cpp/live_ego_frame_metadata_tracker_impl.cpp @@ -0,0 +1,74 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "live_ego_frame_metadata_tracker_impl.hpp" + +#include +#include + +#include +#include +#include + +namespace core +{ + +namespace +{ + +std::vector make_ego_tensor_configs(const EgoFrameMetadataTracker* tracker) +{ + std::vector configs; + configs.reserve(tracker->streams().size()); + for (const auto stream : tracker->streams()) + { + const char* name = EnumNameEgoCameraStream(stream); + SchemaTrackerConfig config; + config.collection_id = tracker->collection_prefix() + "/" + name; + config.max_flatbuffer_size = tracker->max_flatbuffer_size(); + config.tensor_identifier = "frame_metadata"; + config.localized_name = std::string("EgoFrameMetadataTracker_") + name; + configs.push_back(std::move(config)); + } + return configs; +} + +} // namespace + +std::unique_ptr LiveEgoFrameMetadataTrackerImpl::create_mcap_channels( + mcap::McapWriter& writer, std::string_view base_name, const EgoFrameMetadataTracker* tracker) +{ + return std::make_unique(writer, base_name, tracker->get_stream_names()); +} + +LiveEgoFrameMetadataTrackerImpl::LiveEgoFrameMetadataTrackerImpl(const OpenXRSessionHandles& handles, + const EgoFrameMetadataTracker* tracker, + std::unique_ptr mcap_channels) + : mcap_channels_(std::move(mcap_channels)) +{ + auto configs = make_ego_tensor_configs(tracker); + for (auto& config : configs) + { + StreamState state; + state.reader = std::make_unique( + handles, std::move(config), mcap_channels_.get(), streams_.size()); + streams_.push_back(std::move(state)); + } +} + +void LiveEgoFrameMetadataTrackerImpl::update(int64_t /*monotonic_time_ns*/) +{ + for (auto& stream : streams_) + stream.reader->update(stream.tracked); +} + +const Serialized& LiveEgoFrameMetadataTrackerImpl::get_stream_data(size_t stream_index) const +{ + if (stream_index >= streams_.size()) + { + throw std::out_of_range("EgoFrameMetadataTracker: invalid stream index " + std::to_string(stream_index)); + } + return streams_[stream_index].tracked; +} + +} // namespace core diff --git a/src/core/live_trackers/cpp/live_ego_frame_metadata_tracker_impl.hpp b/src/core/live_trackers/cpp/live_ego_frame_metadata_tracker_impl.hpp new file mode 100644 index 0000000000..dd72caa8a9 --- /dev/null +++ b/src/core/live_trackers/cpp/live_ego_frame_metadata_tracker_impl.hpp @@ -0,0 +1,53 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "inc/live_trackers/schema_tracker.hpp" + +#include +#include +#include + +#include +#include +#include +#include + +namespace core +{ + +using EgoFrameMetadataMcapChannels = McapTrackerChannels; +using EgoFrameMetadataSchemaTracker = SchemaTracker; + +class LiveEgoFrameMetadataTrackerImpl : public IEgoFrameMetadataTrackerImpl +{ +public: + static std::vector required_extensions() + { + return SchemaTrackerBase::get_required_extensions(); + } + + static std::unique_ptr create_mcap_channels(mcap::McapWriter& writer, + std::string_view base_name, + const EgoFrameMetadataTracker* tracker); + + LiveEgoFrameMetadataTrackerImpl(const OpenXRSessionHandles& handles, + const EgoFrameMetadataTracker* tracker, + std::unique_ptr mcap_channels); + + void update(int64_t monotonic_time_ns) override; + const Serialized& get_stream_data(size_t stream_index) const override; + +private: + struct StreamState + { + std::unique_ptr reader; + Serialized tracked; + }; + + std::unique_ptr mcap_channels_; + std::vector streams_; +}; + +} // namespace core diff --git a/src/core/live_trackers/cpp/live_ego_imu_tracker_impl.cpp b/src/core/live_trackers/cpp/live_ego_imu_tracker_impl.cpp new file mode 100644 index 0000000000..61bd09765d --- /dev/null +++ b/src/core/live_trackers/cpp/live_ego_imu_tracker_impl.cpp @@ -0,0 +1,62 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "live_ego_imu_tracker_impl.hpp" + +#include +#include + +#include +#include + +namespace core +{ +namespace +{ +SchemaTrackerConfig config(std::string collection_id, size_t max_size, const char* tensor, const char* name) +{ + return { .collection_id = std::move(collection_id), + .max_flatbuffer_size = max_size, + .tensor_identifier = tensor, + .localized_name = name }; +} +} // namespace + +std::unique_ptr LiveEgoImuTrackerImpl::create_mcap_channels(mcap::McapWriter& writer, + std::string_view base_name, + const EgoImuTracker* tracker) +{ + return std::make_unique(writer, base_name, tracker->stream_names()); +} + +LiveEgoImuTrackerImpl::LiveEgoImuTrackerImpl(const OpenXRSessionHandles& handles, + const EgoImuTracker* tracker, + std::unique_ptr channels) + : channels_(std::move(channels)) +{ + for (const auto& name : tracker->stream_names()) + { + StreamState state; + state.reader = std::make_unique>( + handles, + config(tracker->collection_prefix() + "/" + name, tracker->max_flatbuffer_size(), "imu_batch", + "EGO IMU batch"), + channels_.get(), streams_.size()); + streams_.push_back(std::move(state)); + } +} + +void LiveEgoImuTrackerImpl::update(int64_t /*monotonic_time_ns*/) +{ + for (auto& stream : streams_) + stream.reader->update(stream.tracked); +} + +const Serialized& LiveEgoImuTrackerImpl::get_stream_data(size_t stream_index) const +{ + if (stream_index >= streams_.size()) + throw std::out_of_range("EgoImuTracker: invalid stream index"); + return streams_[stream_index].tracked; +} + +} // namespace core diff --git a/src/core/live_trackers/cpp/live_ego_imu_tracker_impl.hpp b/src/core/live_trackers/cpp/live_ego_imu_tracker_impl.hpp new file mode 100644 index 0000000000..659fbedd61 --- /dev/null +++ b/src/core/live_trackers/cpp/live_ego_imu_tracker_impl.hpp @@ -0,0 +1,46 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "inc/live_trackers/schema_tracker.hpp" + +#include +#include + +#include +#include +#include + +namespace core +{ + +using EgoImuMcapChannels = McapTrackerChannels; + +class LiveEgoImuTrackerImpl : public IEgoImuTrackerImpl +{ +public: + static std::vector required_extensions() + { + return SchemaTrackerBase::get_required_extensions(); + } + static std::unique_ptr create_mcap_channels(mcap::McapWriter& writer, + std::string_view base_name, + const EgoImuTracker* tracker); + LiveEgoImuTrackerImpl(const OpenXRSessionHandles& handles, + const EgoImuTracker* tracker, + std::unique_ptr channels); + void update(int64_t monotonic_time_ns) override; + const Serialized& get_stream_data(size_t stream_index) const override; + +private: + struct StreamState + { + std::unique_ptr> reader; + Serialized tracked; + }; + std::unique_ptr channels_; + std::vector streams_; +}; + +} // namespace core diff --git a/src/core/plugin_manager/cpp/inc/plugin_manager/plugin.hpp b/src/core/plugin_manager/cpp/inc/plugin_manager/plugin.hpp index 379e3745b8..173a1746be 100644 --- a/src/core/plugin_manager/cpp/inc/plugin_manager/plugin.hpp +++ b/src/core/plugin_manager/cpp/inc/plugin_manager/plugin.hpp @@ -68,11 +68,13 @@ class Plugin * @param working_dir The directory to run the plugin in (where metadata was found) * @param plugin_root_id The root ID for the plugin * @param plugin_args Optional list of arguments to append to the command + * @param shutdown_timeout_seconds Grace period before forcing the process to stop */ Plugin(const std::string& command, const std::string& working_dir, const std::string& plugin_root_id, - const std::vector& plugin_args = {}); + const std::vector& plugin_args = {}, + double shutdown_timeout_seconds = 2.0); /** * @brief Destructor - stops the plugin process @@ -114,6 +116,7 @@ class Plugin int m_pid = -1; #endif bool m_stop_requested = false; + const double m_shutdown_timeout_seconds; ProcessSnapshot m_process_snapshot; std::mutex m_process_mutex; }; diff --git a/src/core/plugin_manager/cpp/inc/plugin_manager/plugin_manager.hpp b/src/core/plugin_manager/cpp/inc/plugin_manager/plugin_manager.hpp index d236fae618..a610362cd9 100644 --- a/src/core/plugin_manager/cpp/inc/plugin_manager/plugin_manager.hpp +++ b/src/core/plugin_manager/cpp/inc/plugin_manager/plugin_manager.hpp @@ -66,11 +66,13 @@ class PluginManager * @param plugin_name The name of the plugin to start. * @param plugin_root_id The root ID for the plugin. * @param plugin_args Optional arguments appended after plugin.yaml args. + * @param shutdown_timeout_seconds Grace period before forcing the process to stop. * @return A unique pointer to the Plugin instance (stops on destruction). */ std::unique_ptr start(const std::string& plugin_name, const std::string& plugin_root_id, - const std::vector& plugin_args = {}); + const std::vector& plugin_args = {}, + double shutdown_timeout_seconds = 2.0); private: void discover_plugins(); diff --git a/src/core/plugin_manager/cpp/plugin.cpp b/src/core/plugin_manager/cpp/plugin.cpp index 600ea872ed..69af404414 100644 --- a/src/core/plugin_manager/cpp/plugin.cpp +++ b/src/core/plugin_manager/cpp/plugin.cpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -165,8 +166,14 @@ std::string child_launch_error_message(const ChildLaunchError& error, Plugin::Plugin(const std::string& command, const std::string& working_dir, const std::string& plugin_root_id, - const std::vector& plugin_args) + const std::vector& plugin_args, + double shutdown_timeout_seconds) + : m_shutdown_timeout_seconds(shutdown_timeout_seconds) { + if (!std::isfinite(shutdown_timeout_seconds) || shutdown_timeout_seconds <= 0.0) + { + throw std::invalid_argument("shutdown_timeout_seconds must be finite and positive"); + } start_process(command, working_dir, plugin_root_id, plugin_args); } @@ -478,8 +485,10 @@ void Plugin::stop_process() throw PluginCrashException(m_process_snapshot.error); } - const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(2); - while (m_pid != -1 && std::chrono::steady_clock::now() < deadline) + // Longer cleanup is opt-in per instance; the default grace period remains unchanged. + const auto stop_started = std::chrono::steady_clock::now(); + while (m_pid != -1 && std::chrono::duration(std::chrono::steady_clock::now() - stop_started).count() < + m_shutdown_timeout_seconds) { refresh_process_snapshot_locked(false); if (m_pid != -1) diff --git a/src/core/plugin_manager/cpp/plugin_manager.cpp b/src/core/plugin_manager/cpp/plugin_manager.cpp index 00283e6a27..b3a5cdbeb5 100644 --- a/src/core/plugin_manager/cpp/plugin_manager.cpp +++ b/src/core/plugin_manager/cpp/plugin_manager.cpp @@ -160,7 +160,8 @@ std::vector PluginManager::query_devices(const std::string& plugin_ std::unique_ptr PluginManager::start(const std::string& plugin_name, const std::string& plugin_root_id, - const std::vector& plugin_args) + const std::vector& plugin_args, + double shutdown_timeout_seconds) { auto it = m_discovered_plugins.find(plugin_name); if (it == m_discovered_plugins.end()) @@ -171,7 +172,7 @@ std::unique_ptr PluginManager::start(const std::string& plugin_name, const auto& info = it->second; std::vector args = info.args; args.insert(args.end(), plugin_args.begin(), plugin_args.end()); - return std::make_unique(info.command, info.working_dir, plugin_root_id, args); + return std::make_unique(info.command, info.working_dir, plugin_root_id, args, shutdown_timeout_seconds); } } // namespace core diff --git a/src/core/plugin_manager/python/plugin_manager_bindings.cpp b/src/core/plugin_manager/python/plugin_manager_bindings.cpp index 3e8fcba078..f7188ae129 100644 --- a/src/core/plugin_manager/python/plugin_manager_bindings.cpp +++ b/src/core/plugin_manager/python/plugin_manager_bindings.cpp @@ -90,6 +90,6 @@ PYBIND11_MODULE(_plugin_manager, m) .def("query_devices", &PluginManager::query_devices, py::arg("plugin_name"), "Query available devices from a plugin") .def("start", &PluginManager::start, py::arg("plugin_name"), py::arg("plugin_root_id"), - py::arg("plugin_args") = std::vector{}, + py::arg("plugin_args") = std::vector{}, py::arg("shutdown_timeout_seconds") = 2.0, "Start a plugin and return a RAII handle. plugin_args are appended after plugin.yaml args."); } diff --git a/src/core/python/CMakeLists.txt b/src/core/python/CMakeLists.txt index 35ffc430ed..657fb639e8 100644 --- a/src/core/python/CMakeLists.txt +++ b/src/core/python/CMakeLists.txt @@ -45,6 +45,7 @@ add_custom_target(python_package ALL COMMAND ${CMAKE_COMMAND} -E copy "${CMAKE_SOURCE_DIR}/LICENSE.md" "${CMAKE_BINARY_DIR}/python_package/$/LICENSE.md" COMMAND ${CMAKE_COMMAND} -E copy "${CMAKE_SOURCE_DIR}/README.md" "${CMAKE_BINARY_DIR}/python_package/$/README.md" COMMAND ${CMAKE_COMMAND} -E copy "${CMAKE_CURRENT_SOURCE_DIR}/requirements.txt" "${CMAKE_BINARY_DIR}/python_package/$/" + COMMAND ${CMAKE_COMMAND} -E copy "${CMAKE_CURRENT_SOURCE_DIR}/requirements-ego.txt" "${CMAKE_BINARY_DIR}/python_package/$/" COMMAND ${CMAKE_COMMAND} -E copy "${CMAKE_CURRENT_SOURCE_DIR}/requirements-dev.txt" "${CMAKE_BINARY_DIR}/python_package/$/" COMMAND ${CMAKE_COMMAND} -E copy "${CMAKE_CURRENT_SOURCE_DIR}/requirements-ui.txt" "${CMAKE_BINARY_DIR}/python_package/$/" COMMAND ${CMAKE_COMMAND} -E copy "${CMAKE_CURRENT_SOURCE_DIR}/requirements-cloudxr.txt" "${CMAKE_BINARY_DIR}/python_package/$/" diff --git a/src/core/python/pyproject.toml.in b/src/core/python/pyproject.toml.in index fff1897dca..8c1578f724 100644 --- a/src/core/python/pyproject.toml.in +++ b/src/core/python/pyproject.toml.in @@ -27,6 +27,7 @@ dynamic = ["dependencies", "optional-dependencies"] [tool.setuptools.dynamic] dependencies = {file = ["requirements.txt"]} +optional-dependencies.ego = {file = ["requirements-ego.txt"]} optional-dependencies.dev = {file = ["requirements-dev.txt"]} optional-dependencies.ui = {file = ["requirements-ui.txt"]} optional-dependencies.retargeters = {file = ["requirements-retargeters.txt"]} diff --git a/src/core/python/requirements-ego.txt b/src/core/python/requirements-ego.txt new file mode 100644 index 0000000000..b8becb8239 --- /dev/null +++ b/src/core/python/requirements-ego.txt @@ -0,0 +1,5 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 ORBBEC INC. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# Keep the root pyproject.toml and compatibility-package extra in sync. +mcap>=1.2,<2 diff --git a/src/core/replay_trackers/cpp/CMakeLists.txt b/src/core/replay_trackers/cpp/CMakeLists.txt index 1716c44ea5..621e31f5cf 100644 --- a/src/core/replay_trackers/cpp/CMakeLists.txt +++ b/src/core/replay_trackers/cpp/CMakeLists.txt @@ -12,6 +12,8 @@ add_library(replay_trackers STATIC replay_message_channel_tracker_impl.cpp replay_tensor_push_tracker_impl.cpp replay_haptic_command_reader_tracker_impl.cpp + replay_ego_tracker_impl.cpp + replay_ego_tracker_impl.hpp replay_keyboard_tracker_impl.cpp ${GENERATED_TRACKER_REPLAY_SOURCES} inc/replay_trackers/replay_deviceio_factory.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 c9be254b31..985676fdfa 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 @@ -16,6 +16,10 @@ namespace core { class ITracker; +class EgoFrameMetadataTracker; +class IEgoFrameMetadataTrackerImpl; +class EgoImuTracker; +class IEgoImuTrackerImpl; class ITrackerImpl; class ControllerTracker; class IControllerTrackerImpl; @@ -72,6 +76,9 @@ class ReplayDeviceIOFactory const HapticCommandReaderTracker* tracker); std::unique_ptr create_keyboard_tracker_impl(const KeyboardTracker* tracker); // create__tracker_impl for every manifest tracker. + std::unique_ptr create_ego_frame_metadata_tracker_impl( + const EgoFrameMetadataTracker* tracker); + std::unique_ptr create_ego_imu_tracker_impl(const EgoImuTracker* tracker); #include "generated_replay_factory_declarations.inc" private: diff --git a/src/core/replay_trackers/cpp/replay_deviceio_factory.cpp b/src/core/replay_trackers/cpp/replay_deviceio_factory.cpp index b1ef3c71d0..3623bf0fb5 100644 --- a/src/core/replay_trackers/cpp/replay_deviceio_factory.cpp +++ b/src/core/replay_trackers/cpp/replay_deviceio_factory.cpp @@ -5,6 +5,7 @@ #include "generated_replay_includes.inc" #include "replay_controller_tracker_impl.hpp" +#include "replay_ego_tracker_impl.hpp" #include "replay_full_body_tracker_impl.hpp" #include "replay_hand_tracker_impl.hpp" #include "replay_haptic_command_reader_tracker_impl.hpp" @@ -14,6 +15,8 @@ #include "replay_tensor_push_tracker_impl.hpp" #include +#include +#include #include #include #include @@ -108,6 +111,19 @@ std::unique_ptr try_create_haptic_command_reader_impl(ReplayDevice return typed ? factory.create_haptic_command_reader_tracker_impl(typed) : nullptr; } +std::unique_ptr try_create_ego_frame_metadata_tracker_impl(ReplayDeviceIOFactory& factory, + const ITracker& tracker) +{ + auto* typed = dynamic_cast(&tracker); + return typed ? factory.create_ego_frame_metadata_tracker_impl(typed) : nullptr; +} + +std::unique_ptr try_create_ego_imu_tracker_impl(ReplayDeviceIOFactory& factory, const ITracker& tracker) +{ + auto* typed = dynamic_cast(&tracker); + return typed ? factory.create_ego_imu_tracker_impl(typed) : nullptr; +} + std::unique_ptr try_create_keyboard_impl(ReplayDeviceIOFactory& factory, const ITracker& tracker) { auto* typed = dynamic_cast(&tracker); @@ -120,6 +136,8 @@ using TryCreateFn = std::unique_ptr (*)(ReplayDeviceIOFactory&, co inline const TryCreateFn k_tracker_dispatch[] = { &try_create_head_impl, + &try_create_ego_frame_metadata_tracker_impl, + &try_create_ego_imu_tracker_impl, &try_create_hand_impl, &try_create_controller_impl, &try_create_full_body_impl, @@ -206,6 +224,19 @@ std::unique_ptr ReplayDeviceIOFactory::create_h return std::make_unique(); } +std::unique_ptr ReplayDeviceIOFactory::create_ego_frame_metadata_tracker_impl( + const EgoFrameMetadataTracker* tracker) +{ + return std::make_unique( + open_reader(filename_), get_name(tracker), tracker->get_stream_names(), recorded_schemas_); +} + +std::unique_ptr ReplayDeviceIOFactory::create_ego_imu_tracker_impl(const EgoImuTracker* tracker) +{ + return std::make_unique( + open_reader(filename_), get_name(tracker), tracker->stream_names(), recorded_schemas_); +} + std::unique_ptr ReplayDeviceIOFactory::create_keyboard_tracker_impl(const KeyboardTracker* tracker) { return std::make_unique( diff --git a/src/core/replay_trackers/cpp/replay_ego_tracker_impl.cpp b/src/core/replay_trackers/cpp/replay_ego_tracker_impl.cpp new file mode 100644 index 0000000000..0506066a3a --- /dev/null +++ b/src/core/replay_trackers/cpp/replay_ego_tracker_impl.cpp @@ -0,0 +1,88 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "replay_ego_tracker_impl.hpp" + +#include +#include + +#include +#include + +namespace core +{ + +ReplayEgoFrameMetadataTrackerImpl::ReplayEgoFrameMetadataTrackerImpl(std::unique_ptr reader, + std::string_view base_name, + const std::vector& channels, + const RecordedSchemas& recorded) + : viewers_(std::make_unique>( + std::move(reader), base_name, channels, recorded)), + streams_(channels.size()), + logger_(isaaccapture::Logger::get("isaaccapture.core.ReplayEgoFrameMetadataTrackerImpl")), + warned_no_data_(channels.size(), false) +{ +} + +void ReplayEgoFrameMetadataTrackerImpl::update(int64_t /*monotonic_time_ns*/) +{ + for (size_t index = 0; index < streams_.size(); ++index) + { + auto record = viewers_->read(index); + streams_[index] = record ? record.narrow(record->data()) : Serialized{}; + if (record) + { + warned_no_data_[index] = false; + } + else if (!warned_no_data_[index]) + { + logger_->warn("no data (EOF or gap), stream_index={}", index); + warned_no_data_[index] = true; + } + } +} + +const Serialized& ReplayEgoFrameMetadataTrackerImpl::get_stream_data(size_t index) const +{ + if (index >= streams_.size()) + throw std::out_of_range("EgoFrameMetadataTracker: invalid stream index"); + return streams_[index]; +} + +ReplayEgoImuTrackerImpl::ReplayEgoImuTrackerImpl(std::unique_ptr reader, + std::string_view base_name, + const std::vector& channels, + const RecordedSchemas& recorded) + : viewers_(std::make_unique>(std::move(reader), base_name, channels, recorded)), + streams_(channels.size()), + logger_(isaaccapture::Logger::get("isaaccapture.core.ReplayEgoImuTrackerImpl")), + warned_no_data_(channels.size(), false) +{ +} + +void ReplayEgoImuTrackerImpl::update(int64_t /*monotonic_time_ns*/) +{ + for (size_t index = 0; index < streams_.size(); ++index) + { + auto record = viewers_->read(index); + streams_[index] = record ? record.narrow(record->data()) : Serialized{}; + if (record) + { + warned_no_data_[index] = false; + } + else if (!warned_no_data_[index]) + { + logger_->warn("no data (EOF or gap), stream_index={}", index); + warned_no_data_[index] = true; + } + } +} + +const Serialized& ReplayEgoImuTrackerImpl::get_stream_data(size_t index) const +{ + if (index >= streams_.size()) + throw std::out_of_range("EgoImuTracker: invalid stream index"); + return streams_[index]; +} + +} // namespace core diff --git a/src/core/replay_trackers/cpp/replay_ego_tracker_impl.hpp b/src/core/replay_trackers/cpp/replay_ego_tracker_impl.hpp new file mode 100644 index 0000000000..57e7f8e7e2 --- /dev/null +++ b/src/core/replay_trackers/cpp/replay_ego_tracker_impl.hpp @@ -0,0 +1,54 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace core +{ + +class ReplayEgoFrameMetadataTrackerImpl : public IEgoFrameMetadataTrackerImpl +{ +public: + ReplayEgoFrameMetadataTrackerImpl(std::unique_ptr reader, + std::string_view base_name, + const std::vector& channels, + const RecordedSchemas& recorded); + void update(int64_t monotonic_time_ns) override; + const Serialized& get_stream_data(size_t stream_index) const override; + +private: + std::unique_ptr> viewers_; + std::vector> streams_; + std::shared_ptr logger_; + std::vector warned_no_data_; +}; + +class ReplayEgoImuTrackerImpl : public IEgoImuTrackerImpl +{ +public: + ReplayEgoImuTrackerImpl(std::unique_ptr reader, + std::string_view base_name, + const std::vector& channels, + const RecordedSchemas& recorded); + void update(int64_t monotonic_time_ns) override; + const Serialized& get_stream_data(size_t stream_index) const override; + +private: + std::unique_ptr> viewers_; + std::vector> streams_; + std::shared_ptr logger_; + std::vector warned_no_data_; +}; + +} // namespace core diff --git a/src/core/schema/cpp/inc/schema/ego_device_state_utils.hpp b/src/core/schema/cpp/inc/schema/ego_device_state_utils.hpp new file mode 100644 index 0000000000..c15d691941 --- /dev/null +++ b/src/core/schema/cpp/inc/schema/ego_device_state_utils.hpp @@ -0,0 +1,43 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include + +#include +#include + +namespace core +{ +inline constexpr uint32_t EGO_DIAGNOSTIC_VERSION = 2; + +inline void validate_ego_diagnostic(const EgoDeviceState& value) +{ + if (value.diagnostic_version() != EGO_DIAGNOSTIC_VERSION || value.observed_at_ns() <= 0 || !value.device_uid() || + value.device_uid()->size() == 0 || !value.event()) + throw std::invalid_argument("EGO diagnostics require version 2, a device UID, observation time, and an event"); + switch (value.event_type()) + { + case EgoDiagnosticEvent_EgoSdkReport: + break; + case EgoDiagnosticEvent_EgoCaptureSummary: + if (const auto health = value.event_as_EgoCaptureSummary()->capture_health(); + health < EgoCaptureHealth_MIN || health > EgoCaptureHealth_MAX) + throw std::invalid_argument("Invalid EGO capture health"); + break; + case EgoDiagnosticEvent_EgoRecoveryEvent: + if (const auto stage = value.event_as_EgoRecoveryEvent()->stage(); + stage < EgoRecoveryStage_MIN || stage > EgoRecoveryStage_MAX) + throw std::invalid_argument("Invalid EGO recovery stage"); + break; + case EgoDiagnosticEvent_EgoDeviceSnapshot: + if (const auto temperature = value.event_as_EgoDeviceSnapshot()->imu_temperature_c(); + temperature && !std::isfinite(*temperature)) + throw std::invalid_argument("EGO IMU temperature must be finite or absent"); + break; + default: + throw std::invalid_argument("Unknown EGO diagnostic event type"); + } +} +} // namespace core diff --git a/src/core/schema/cpp/inc/schema/ego_limits.hpp b/src/core/schema/cpp/inc/schema/ego_limits.hpp new file mode 100644 index 0000000000..14b0a18a11 --- /dev/null +++ b/src/core/schema/cpp/inc/schema/ego_limits.hpp @@ -0,0 +1,16 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include + +namespace core +{ + +// Shared by EGO SchemaPushers and trackers. 32 KiB accommodates the +// 32-sample IMU batches and EGO calibration YAML while remaining below the +// CloudXR runtime's IPC tensor-payload limit. +inline constexpr size_t EGO_MAX_FLATBUFFER_SIZE = 32 * 1024; + +} // namespace core diff --git a/src/core/schema/fbs/ego_audio.fbs b/src/core/schema/fbs/ego_audio.fbs new file mode 100644 index 0000000000..43652717dc --- /dev/null +++ b/src/core/schema/fbs/ego_audio.fbs @@ -0,0 +1,59 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +include "timestamp.fbs"; + +namespace core; + +enum EgoAudioSampleFormat : byte { S16LE = 0 } + +// Indices count retained PCM frames per channel within a continuous segment. +// continuity_known is false when the SDK cannot report physical sample loss. +table EgoAudioTiming { + segment_id: uint64 (id: 0); + first_sample_index: uint64 (id: 1); + continuity_known: bool (id: 2); + discontinuity: bool (id: 3); +} + +table EgoAudioChunk { + sequence_number: uint64 (id: 0); + sample_rate_hz: uint32 (id: 1); + channel_count: uint16 (id: 2); + bits_per_sample: uint16 (id: 3); + sample_format: EgoAudioSampleFormat (id: 4); + sample_count: uint32 (id: 5); + wav_data_offset: uint64 (id: 6); + byte_count: uint32 (id: 7); + capture_epoch: uint32 (id: 8); + timing: EgoAudioTiming (id: 9); +} + +table EgoAudioChunkTracked { data: EgoAudioChunk (id: 0); } + +table EgoAudioChunkRecord { + data: EgoAudioChunk (id: 0); + timestamp: DeviceDataTimestamp (id: 1); +} + +// PCM block retained verbatim in an embedded-media MCAP. The existing +// EgoAudioChunk remains the lightweight WAV-sidecar index used by the +// metadata-only and SchemaPusher paths. +table EgoPcmAudioChunk { + sequence_number: uint64 (id: 0); + sample_rate_hz: uint32 (id: 1); + channel_count: uint16 (id: 2); + bits_per_sample: uint16 (id: 3); + sample_format: EgoAudioSampleFormat (id: 4); + sample_count: uint32 (id: 5); + pcm_data: [ubyte] (id: 6); + capture_epoch: uint32 (id: 7); + timing: EgoAudioTiming (id: 8); +} + +table EgoPcmAudioChunkRecord { + data: EgoPcmAudioChunk (id: 0); + timestamp: DeviceDataTimestamp (id: 1); +} + +root_type EgoAudioChunkRecord; diff --git a/src/core/schema/fbs/ego_calibration.fbs b/src/core/schema/fbs/ego_calibration.fbs new file mode 100644 index 0000000000..b6d3e2cfdb --- /dev/null +++ b/src/core/schema/fbs/ego_calibration.fbs @@ -0,0 +1,51 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +include "timestamp.fbs"; + +namespace core; + +table EgoCameraIntrinsics { + width: uint32 (id: 0); + height: uint32 (id: 1); + fx: float (id: 2); + fy: float (id: 3); + cx: float (id: 4); + cy: float (id: 5); + distortion_model: int32 (id: 6); + distortion: [float] (id: 7); +} + +table EgoExtrinsics { + rotation: [float] (id: 0); + translation_mm: [float] (id: 1); +} + +// Versioned EGO timing provenance/model transported by the existing calibration channel. +table EgoTimeAlignment { + json: string (id: 0); +} + +table EgoCalibration { + device_uid: string (id: 0); + color_left: EgoCameraIntrinsics (id: 1); + color_right: EgoCameraIntrinsics (id: 2); + left_to_right: EgoExtrinsics (id: 3); + accel_intrinsics: [float] (id: 4); + gyro_intrinsics: [float] (id: 5); + accel_to_left: EgoExtrinsics (id: 6); + gyro_to_left: EgoExtrinsics (id: 7); + raw_alignment_yaml: string (id: 8); + raw_imu_yaml: string (id: 9); + capture_epoch: uint32 (id: 10); + time_alignment: EgoTimeAlignment (id: 11); +} + +table EgoCalibrationTracked { data: EgoCalibration (id: 0); } + +table EgoCalibrationRecord { + data: EgoCalibration (id: 0); + timestamp: DeviceDataTimestamp (id: 1); +} + +root_type EgoCalibrationRecord; diff --git a/src/core/schema/fbs/ego_camera.fbs b/src/core/schema/fbs/ego_camera.fbs new file mode 100644 index 0000000000..e39e394888 --- /dev/null +++ b/src/core/schema/fbs/ego_camera.fbs @@ -0,0 +1,64 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +include "timestamp.fbs"; + +namespace core; + +// The two RGB sensors exposed by the EGO camera. +enum EgoCameraStream : byte { ColorLeft = 0, ColorRight = 1 } + +// Values are append-only: existing recordings use Mjpg == 0. +enum EgoPixelFormat : byte { Mjpg = 0, H264 = 1, H265 = 2 } + +// SDK metadata is deliberately represented as numeric key/value pairs. Orbbec may +// add frame metadata keys without requiring a wire-schema revision. +struct EgoFrameMetadataEntry { + key: int32; + value: int64; +} + +// Per-frame metadata pushed independently for each recorded stream. +table EgoFrameMetadata { + stream: EgoCameraStream (id: 0); + sequence_number: uint64 (id: 1); + width: uint32 (id: 2); + height: uint32 (id: 3); + fps: uint32 (id: 4); + pixel_format: EgoPixelFormat (id: 5); + encoded_bytes: uint64 (id: 6); + sdk_metadata: [EgoFrameMetadataEntry] (id: 7); + capture_epoch: uint32 (id: 8); +} + +// In-memory tracker wrapper. data is null before a sample arrives. +table EgoFrameMetadataTracked { + data: EgoFrameMetadata (id: 0); +} + +// MCAP record wrapper. DeviceDataTimestamp carries host and device clock values. +table EgoFrameMetadataRecord { + data: EgoFrameMetadata (id: 0); + timestamp: DeviceDataTimestamp (id: 1); +} + +// Encoded access unit retained verbatim in an embedded-media MCAP. This is +// deliberately separate from EgoFrameMetadata: metadata-only recordings +// stay small, while an archive can opt in to the high-throughput payload. +table EgoEncodedVideoFrame { + stream: EgoCameraStream (id: 0); + sequence_number: uint64 (id: 1); + width: uint32 (id: 2); + height: uint32 (id: 3); + fps: uint32 (id: 4); + pixel_format: EgoPixelFormat (id: 5); + encoded_data: [ubyte] (id: 6); + capture_epoch: uint32 (id: 7); +} + +table EgoEncodedVideoFrameRecord { + data: EgoEncodedVideoFrame (id: 0); + timestamp: DeviceDataTimestamp (id: 1); +} + +root_type EgoFrameMetadataRecord; diff --git a/src/core/schema/fbs/ego_device_state.fbs b/src/core/schema/fbs/ego_device_state.fbs new file mode 100644 index 0000000000..c12da80538 --- /dev/null +++ b/src/core/schema/fbs/ego_device_state.fbs @@ -0,0 +1,65 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +include "timestamp.fbs"; + +namespace core; + +enum EgoCaptureHealth : byte { Healthy = 0, Warning = 1, Incomplete = 2 } +enum EgoRecoveryStage : byte { Started = 0, Recovered = 1, Failed = 2 } + +struct EgoDevicePropertyValue { + property_id: int32; + value: double; +} + +table EgoSdkReport { + sequence_number: uint64 (id: 0); + work_mode: int32 (id: 1); + status_flags: uint64 (id: 2); + error_flags: uint64 (id: 3); + storage_free_bytes: uint64 (id: 4); +} + +table EgoCaptureSummary { + capture_health: EgoCaptureHealth = Healthy (id: 0); + reason: string (id: 1); + queue_capacity: uint32 (id: 2); + queue_peak: uint32 (id: 3); + dropped_events: uint64 (id: 4); + dropped_video_frame_sets: uint64 (id: 5); + is_final: bool (id: 6); +} + +table EgoRecoveryEvent { + stage: EgoRecoveryStage = Started (id: 0); + reason: string (id: 1); + reconnect_attempt: uint32 (id: 2); +} + +table EgoDeviceSnapshot { + // Absence is a failed/unavailable read; zero is a measured temperature. + imu_temperature_c: float = null (id: 0); + // Successful reads only, not an inventory of every supported property. + properties: [EgoDevicePropertyValue] (id: 1); +} + +// Each event carries only its own observations; never infer NV availability from it. +union EgoDiagnosticEvent { EgoSdkReport, EgoCaptureSummary, EgoRecoveryEvent, EgoDeviceSnapshot } + +table EgoDeviceState { + device_uid: string (id: 0); + capture_epoch: uint32 (id: 1); + observed_at_ns: int64 (id: 2); + diagnostic_version: uint32 (id: 3); + event: EgoDiagnosticEvent (id: 5); +} + +table EgoDeviceStateTracked { data: EgoDeviceState (id: 0); } + +table EgoDeviceStateRecord { + data: EgoDeviceState (id: 0); + timestamp: DeviceDataTimestamp (id: 1); +} + +root_type EgoDeviceStateRecord; diff --git a/src/core/schema/fbs/ego_imu.fbs b/src/core/schema/fbs/ego_imu.fbs new file mode 100644 index 0000000000..a000fbe7f6 --- /dev/null +++ b/src/core/schema/fbs/ego_imu.fbs @@ -0,0 +1,35 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +include "timestamp.fbs"; + +namespace core; + +enum EgoImuSensor : byte { Accel = 0, Gyro = 1 } + +struct EgoImuSample { + x_si: double; + y_si: double; + z_si: double; + temperature_c: double; + sample_time_local_common_clock_ns: int64; + sample_time_raw_device_clock_ns: int64; +} + +table EgoImuBatch { + sensor: EgoImuSensor (id: 0); + sequence_number: uint64 (id: 1); + sample_rate_hz: uint32 (id: 2); + full_scale: float (id: 3); + samples: [EgoImuSample] (id: 4); + capture_epoch: uint32 (id: 5); +} + +table EgoImuBatchTracked { data: EgoImuBatch (id: 0); } + +table EgoImuBatchRecord { + data: EgoImuBatch (id: 0); + timestamp: DeviceDataTimestamp (id: 1); +} + +root_type EgoImuBatchRecord; diff --git a/src/core/schema/golden/ego_audio.bfbs b/src/core/schema/golden/ego_audio.bfbs new file mode 100644 index 0000000000000000000000000000000000000000..f15a9e26965b75f5e92b6bf4372aa519c7f2be47 GIT binary patch literal 2304 zcmah~Jx?1!5FLX7g9uoWBM}8fjzmF$Adyf+k%ACFDJW2&ponxnpBHRl`y77&iTDAO z6ciLG@&i&(q@bXnNWo7?K}kW8GE$V!%X_=G=4=dEYrJ1Gvv1$b?2U@Z+~V9yO41US zgbYejQZj4+kO@c{!ue6RKuhEqq*IGwF$d{|toDgq#6@;tGtC~GU$EW<;R#rh z^Y20M5_%l7eZ~AUCY;mP1olMk;%uT5;A2Z-Ys)ByazF`&$)FMog8Ct8okAr3FDZ8B4a#@K7;-}?WT$<)SHGXd&(&{N?BEvBXAzg%RCf$j~&P?gm`Uy zuO%h+_zLh%NPv&l)~9*JT1(C@1-IwyA@?IWyBq2gKZQKlco(rpd^Tr4hB~w8uhp|I zrq!zJZK@phM>S?z@+FG49)lYg#<6U2FsU<;Jn}S?gEHI+QwkQ*mxwO}eJnxJ5PJva z$kdeD@Ew=0&E$HvCam9iDCE&kLy8!mpudD1V0;c?T==$KkBP_{-iQQFv-z@b>gmRM z+k<|zK2Ib1jAr&N5-Y5hPtavul5d@h<~?f3t!;Np*P`REuh&#vE};{x$Fzy1#zW@Y z>v06aCI+>($ha$wB^;S<7!2ixz&{Yd3{V<=3b$%rRCgc|(iM;E%YNv(~4{x#kx_=N!pA zgX}=4PhFi`^J=T$sd;=JRwJJjIYA7&B6*NEG41C$NKaimVO)8TQ+2nUoOb1SMc>c-)%$+9AM_6~a Q+(OaK7L}u+c5*HM0I=zzm;e9( literal 0 HcmV?d00001 diff --git a/src/core/schema/golden/ego_calibration.bfbs b/src/core/schema/golden/ego_calibration.bfbs new file mode 100644 index 0000000000000000000000000000000000000000..488ffd25a12b4d24d220d4dbf12195adddb88ec0 GIT binary patch literal 2180 zcmaJ?D{mB06h7N-cefAP4HN=oLrQ|EXsD^EK#wS)4X7i!6Z_Kyx7Ow?H+}GU#8Uf{R1^t(Qcuat%5ti+mmx8Nr&;v6bex!QdVE zxaMeK{|p=M0hO1y+xsyXG_ansXMJ2!#y!v_&^eHwq`~F2c5+Qc;YO;mFzMb7aM;4# zI`EiEC8^|kzx~u_GdS0J#xZAM9P~03)4yB1FO_rt`Co>gHj!vwOJxd^EP<1*!Q7q7IX#c1_JXD>${kjF~7#y zKIT5=bLjHPrraUX)#D)yBi1H%p#{uOiEQbndjr+x5>vCFiu~4 zH%+`O@uFZeqrO#(tDv4;y?q(N(KqG$K~w-fwzx5w&h5Of&!#6C8y1b$6?U^U?Dj&x zCoe3f49}f!dj(8A*HZxD?32Zf3Zrjgzc={XH`+*_Xkr?+J9KW&_)!``Cpz3)i=&N< z32Vboq9nDbrp2%i=5a?m>-beMv!9tY+KDmaWK3XVWSFRSr~`fBe6%svIVW!`co_PD zw;i@nZ{RKV?OWWlZ`ofwXTDu6uI=QZl%EAVot_W+%*1~hxGYuOUZnfni(|^G!v|I! zU!LL^)4GZ_FX%WKS@b(iGA)QGRwmy-Q^7ppTZs1n#F#OzjLm7#;$qNFJpbs$xxBg2 z!#UAkLG-r?dIDMiunf8Z+5s_V&i8974pMdVsF-lamzUY7p)UR&RpSp#VLx2LCGD$& zyP`8m?i%(S#z&o(u$Lt%pGD|KEZGyp(U+R3n<~uGxK_^V#YrpBy}J%fu^+A&%p@_) zslSW7f~Q#j_lJ3Q@RoyzweEEHhj=z2hrP|Zc0A9Ac_8NyJgj5KV{2c(SFC_eU>e25 z(9{M0>2Wb;tbH?F%tXuJyYeV(WgX13h&XGjYGcOiQ7=Ym8Gn?=oHM74C*Oy~#r$ic zs{&88yaB;<%}-vP|5e>EtN4^O3!fi(1)oQt&&kw%Co)bwj5+(Khfh@$cvR%^t+l=9 zM~VLc*S3D9U{eKa;*Q>m0~6C+-i0A~kf9>aPvSV~dR8K1z}}z@^B~5+y&qWOJbyoO SxfgM+9;z@>8_v|@M9UvWik#5^ literal 0 HcmV?d00001 diff --git a/src/core/schema/golden/ego_camera.bfbs b/src/core/schema/golden/ego_camera.bfbs new file mode 100644 index 0000000000000000000000000000000000000000..5204dfcd827206f88c41e7dcf051a6a5d281fe00 GIT binary patch literal 2752 zcmai0F>D+~6#ZwP?;Pu}PjE;9PH;#kDN>|NK}qR?GKC8l6ci~bQc$FzKq!FWcyDHBJ+?Mtt#`XK^Z%ds-~7Mp zwuszZyLrDQ9jVH!G^8mlxr*5~?jkpUC7=Uvd|VY+iS&W8YYLP7ipU1;b)b1gqsIYW#lo;l5(FqG~elZn8V&%AmZ!|Y1&v! zkWWhc>VnEWMboR@AZ#9`Iz$!Fs*7! zYGgPxai^`lnuvGFmvm(X^VE*bmQ)jZ>Ak_uZbsjR=5Bqv zSm$0%9S;w<$lmI~m^exk(bqtt!{pvKREDi)sLejE2Mfj00K#;@d#wYxaCx;|sG-;KXC=UKSQYxMPp z*+Hs2HB5qCjGq~fl^PUvwXW%ofYR~+5xFhw&0diE{{z1H@Qu$hIR4wuw8_h^j`_PYEMtKLEps(GU ze*#O^-vfXezXRAa)HQcUWj@8sYcGO7OfJ;kI(i0kudKcAxbD>6IJYNB@}s`?ymqn1 zccHDlDvW5;2gtjW+eYdKdn&{-Y?@G--pty)X0$(fEMl5x(ZnC$&D57WvTt!Ml<{i8 z!+kT+n=!5fmg^>jWZ$g-KLOW4d=q$y=P~dY&(DAecoXB&Txg3kWVP6O3E8`#3AfTd z;TMYRXHkn6ue!+@60+~yyxBt+^PXSeIZy-UGYa>-I6r#EY=8s%kY_`?V^e2EZL)uD z?`<7rYJ~9$eB9pScLvk{+k0d9xxE*fn$!97Y}k8`VdM7RH;#?ldrx8K_TCSUo!fga zVCVMUBaE?CkzZNw8O=?t@t4Xh;n&nwtEes7<+VlnrG42o+EiFiJSUzFee9VmFRPuz z3-s6Q+V&RZ)qOj}y$iezl-ASgewH4IoFeZEqT2Ht8y&82AJ!GMIP{|fCGTQi(?OWJ zbk(rRtE%@jc{Yl-@%j5$>Ws-`_tX*eXzPe|M5Yh_+c9_1`I0|*SZ>sMdAay{>2w+E z1Q-Lfr>)MnuMP)+>f;Ty;e77oN9Z|){cKFWSKw0Q%Xi5LpT`&FY03<~=OmC8W|0-p5@-Pwkm}?W`2mE_4~{+9eK{dgVt!o{*@yW| z6LJlaQwN}9QsfzwJOSet=GK(Rb1*>!7FhkezYT{Al)-6ut< zCTUT|(b>LEN@@Qy9L{5(=Q-Q(DR=)i`lsnT^?wASxbDZ1UQJY1Y^B8Gz9&y!H=bAW zfWpXAKjbgzDDCTfU=PRIqakp>O!tWw$J`rxw)3kx`)iezyG^hD;?BC&V7BhWs;oF_b{};Z!a2At%1shLd4nWkk zh1g!)H8$>{%}$2?-vX_H0?;QQhr4rpwN#rL>(6l(d5DYi8=rE|vjJ%(=TzL$ZP;Gn zM1G#<#*W9&ysXZlh}0qDeB1T-7!%l$mxk-GL6LqDA6mqm^ZmVs%~O9`f63wddmsOK zRsiz-t%7x4t4_Ivy(gaizQ4Dz?(`w8<(ni*yIDt<0I7*;7?1Be{(JKD>wA?aak2^7 zW|hx2H|DWNEM_V%@rnJ0!QY)vd28@McHwUe|6M+n*reU;7u5AR$i=oQic*!D@aM$i z@m~fn)aT@=2fRWIur&=b7&2aE+|@;?x*5JZs3ob@NBjYN!}4^!;^*8}rNSh4pr5_P z+4mI5LO*ER7ohjpww9}Cn<0NXcFxTYoFk~?1@D{=&S73Qao7^*F1~r@70@Hl$DsF| zEzfK}sO^&ACx-;O4kdW_jNl| zWtbH3A9@*Qcg~!Ry*cL~_#@l6LNuL-wXjCK5ogDL-tr%GKD14f4SO+Sbsas-&%>8+ zo_Tl!6o5Vjjp03N&MS;F%%*KV=9563{=CKf&@^=te`1*BX{Pfs*6^)`Ra-%fx=J8hw(blLPVS+VGnmt}T&sfd-!XSaeUxWQR`!2=o_UFy zHGMq5{t5hd{;a?m=Ja)da3Av(5bI|b^cb{^bw6fRpB!Ck8SI(_<}UwQ*3JoHN@CpN zBHV}*)xoFS!#-=u@vm6^{p2G**V~9^hIH-6ZJmZ2x$5e$&?TZP@+#AY%%*25C^J7oy$SEU$|F0qA_p=wa2BzpM z@2sn7z8!YcHd<^ID{7fEHNfudYBjQPdiE)IwB3#$2Zo21N&75ng@Leav0mY_@eD#gYQ(Qr{xjsuR*i3E3^JI9R`eD z-*6tm*|ytZX>3f&5wlV^XWE?NOWh6omcT!Q6XQ_z(LsntK8x(7X1?qx%gVH~)Ryp% ze$XCgBe7as9K8;V(@fUKplcxVlh^odoj!x7!J7A+kcYr_0_QoMTNdllUB-K*z|*6n zLmm|7>Z(IJ>ZB2>_icRZReXIMAB^cEKE6KaP@NF(xo(<-l}2K~dZMq#Ko&>)4!~c7 XQB|8GJC9;r_3;o^>(12osh0l$a+jXu literal 0 HcmV?d00001 diff --git a/src/core/schema/golden/ego_imu.bfbs b/src/core/schema/golden/ego_imu.bfbs new file mode 100644 index 0000000000000000000000000000000000000000..186cf2126a1bd4c39651b56b536daa5912038688 GIT binary patch literal 1920 zcmaJ?y;2iV6h46@1dI@wj9M@nXDBQfVo5=v3@Xl8I6{%anRVG*NQT{P%x(~(Y>-RHQ03 zsmpC}=dg<`0SiC_AiY))NEtr1OAFWX8Yls0_-Tzn56n+MwI~8oTgOpazoO_AGCXs2 zKo>!frk&3;BD3(z$e>RW@y+7*0nh-*AB~F~{*EFXaWcBpr>{0}AGiTH{y;50-u9n) z!*$gQ`~i4XZJm~xJF_-^sLwTuqKIdLaXsB1_}Cy@)pVja4<74N(^e=!`3mUsc$m&3 z$5Af!v%gW4^;JdWjJE|efSW)j-W5A^IxiJCNa!7Oaa|}#Ij!Xj#0lOEpktlH9(Fwy zgtphm`2+6@#Z>+NFHD_kGJ!MuM^CI1*34D$)N>QSD$~XSki>WjMSA6290T6~=G_9H^GEynxWSSI z%uPwAwLSKC&D5pBIIaVZ-4BNYWvRaJbU^b}FxrYtfy%B4lOsx1ghV#aGBUlG=kyGH zF;>^fh$alz&!RkEt$=uPFS!V8V)_F|h zV0>5uJ5*NZKxF<8=lXnpMz9C5DaJ{E9HcjJ^nK4u{(Ft=p%;~@i zy4d6g?h2@4BKz?Q+5K2{gu6tJKA@h)L7RKATpbh5ui^w^V2}CbHoFv-K+OY;Ke5G` zX*ScZj_w=>-~`wMC{J1a4p()dSMkZ&$b6>c3uZo>bH}`av}gQg{;y}aPUo~3&uO+U x$2ogWJziItmVaNLJHG$` literal 0 HcmV?d00001 diff --git a/src/core/schema/python/CMakeLists.txt b/src/core/schema/python/CMakeLists.txt index cee33a811b..5311d878b0 100644 --- a/src/core/schema/python/CMakeLists.txt +++ b/src/core/schema/python/CMakeLists.txt @@ -3,6 +3,8 @@ pybind11_add_module(schema_py oak_bindings.h + ego_camera_bindings.h + ego_bindings.h controller_bindings.h full_body_bindings.h gamepad_bindings.h diff --git a/src/core/schema/python/ego_bindings.h b/src/core/schema/python/ego_bindings.h new file mode 100644 index 0000000000..5cbac78950 --- /dev/null +++ b/src/core/schema/python/ego_bindings.h @@ -0,0 +1,501 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "schema_serialized.h" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace core +{ + +inline void bind_ego(py::module& m) +{ + + py::enum_(m, "EgoImuSensor").value("Accel", EgoImuSensor_Accel).value("Gyro", EgoImuSensor_Gyro); + + py::class_(m, "EgoImuSample") + .def(py::init(), py::arg("x_si") = 0.0, py::arg("y_si") = 0.0, + py::arg("z_si") = 0.0, py::arg("temperature_c") = 0.0, py::arg("sample_time_local_common_clock_ns") = 0, + py::arg("sample_time_raw_device_clock_ns") = 0) + .def_property("x_si", &EgoImuSample::x_si, &EgoImuSample::mutate_x_si) + .def_property("y_si", &EgoImuSample::y_si, &EgoImuSample::mutate_y_si) + .def_property("z_si", &EgoImuSample::z_si, &EgoImuSample::mutate_z_si) + .def_property("temperature_c", &EgoImuSample::temperature_c, &EgoImuSample::mutate_temperature_c) + .def_property("sample_time_local_common_clock_ns", &EgoImuSample::sample_time_local_common_clock_ns, + &EgoImuSample::mutate_sample_time_local_common_clock_ns) + .def_property("sample_time_raw_device_clock_ns", &EgoImuSample::sample_time_raw_device_clock_ns, + &EgoImuSample::mutate_sample_time_raw_device_clock_ns); + + serialized_class(m, "EgoImuBatch", "Encoded EGO payload.") + .def(py::init( + [](EgoImuSensor sensor, uint64_t sequence_number, uint32_t sample_rate_hz, float full_scale, + const std::vector& samples, uint32_t capture_epoch) + { + EgoImuBatchT native; + native.sensor = sensor; + native.sequence_number = sequence_number; + native.sample_rate_hz = sample_rate_hz; + native.full_scale = full_scale; + native.samples = samples; + native.capture_epoch = capture_epoch; + return pack(native); + }), + py::arg("sensor") = EgoImuSensor_Accel, py::arg("sequence_number") = 0, py::arg("sample_rate_hz") = 0, + py::arg("full_scale") = 0.0f, py::arg("samples") = std::vector{}, py::arg("capture_epoch") = 0) + .def_property_readonly("sensor", field(&EgoImuBatch::sensor)) + .def_property_readonly("sequence_number", field(&EgoImuBatch::sequence_number)) + .def_property_readonly("sample_rate_hz", field(&EgoImuBatch::sample_rate_hz)) + .def_property_readonly("full_scale", field(&EgoImuBatch::full_scale)) + .def_property_readonly("samples", + [](const Serialized& self) + { + std::vector values; + const auto* encoded = self->samples(); + if (encoded != nullptr) + { + values.reserve(encoded->size()); + for (const auto* value : *encoded) + { + values.push_back(*value); + } + } + return values; + }) + .def_property_readonly("capture_epoch", field(&EgoImuBatch::capture_epoch)); + + bind_record(m, "EgoImuBatchRecord", "EgoImuBatch"); + + py::enum_(m, "EgoAudioSampleFormat").value("S16LE", EgoAudioSampleFormat_S16LE); + + serialized_class(m, "EgoAudioTiming", "PCM segment provenance; no implied hardware continuity.") + .def(py::init( + [](uint64_t segment_id, uint64_t first_sample_index, bool continuity_known, bool discontinuity) + { + EgoAudioTimingT native; + native.segment_id = segment_id; + native.first_sample_index = first_sample_index; + native.continuity_known = continuity_known; + native.discontinuity = discontinuity; + return pack(native); + }), + py::arg("segment_id") = 0, py::arg("first_sample_index") = 0, py::arg("continuity_known") = false, + py::arg("discontinuity") = false) + .def_property_readonly("segment_id", field(&EgoAudioTiming::segment_id)) + .def_property_readonly("first_sample_index", field(&EgoAudioTiming::first_sample_index)) + .def_property_readonly("continuity_known", field(&EgoAudioTiming::continuity_known)) + .def_property_readonly("discontinuity", field(&EgoAudioTiming::discontinuity)); + + serialized_class(m, "EgoAudioChunk", "Encoded EGO payload.") + .def(py::init( + [](uint64_t sequence_number, uint32_t sample_rate_hz, uint16_t channel_count, uint16_t bits_per_sample, + EgoAudioSampleFormat sample_format, uint32_t sample_count, uint64_t wav_data_offset, + uint32_t byte_count, uint32_t capture_epoch, const Serialized* timing) + { + EgoAudioChunkT native; + native.sequence_number = sequence_number; + native.sample_rate_hz = sample_rate_hz; + native.channel_count = channel_count; + native.bits_per_sample = bits_per_sample; + native.sample_format = sample_format; + native.sample_count = sample_count; + native.wav_data_offset = wav_data_offset; + native.byte_count = byte_count; + native.capture_epoch = capture_epoch; + if (timing != nullptr && *timing) + { + native.timing = std::make_shared(); + (*timing)->UnPackTo(native.timing.get()); + } + return pack(native); + }), + py::arg("sequence_number") = 0, py::arg("sample_rate_hz") = 0, py::arg("channel_count") = 0, + py::arg("bits_per_sample") = 0, py::arg("sample_format") = EgoAudioSampleFormat_S16LE, + py::arg("sample_count") = 0, py::arg("wav_data_offset") = 0, py::arg("byte_count") = 0, + py::arg("capture_epoch") = 0, py::arg("timing") = nullptr) + .def_property_readonly("sequence_number", field(&EgoAudioChunk::sequence_number)) + .def_property_readonly("sample_rate_hz", field(&EgoAudioChunk::sample_rate_hz)) + .def_property_readonly("channel_count", field(&EgoAudioChunk::channel_count)) + .def_property_readonly("bits_per_sample", field(&EgoAudioChunk::bits_per_sample)) + .def_property_readonly("sample_format", field(&EgoAudioChunk::sample_format)) + .def_property_readonly("sample_count", field(&EgoAudioChunk::sample_count)) + .def_property_readonly("wav_data_offset", field(&EgoAudioChunk::wav_data_offset)) + .def_property_readonly("byte_count", field(&EgoAudioChunk::byte_count)) + .def_property_readonly("capture_epoch", field(&EgoAudioChunk::capture_epoch)) + .def_property_readonly("timing", + [](const Serialized& self) -> py::object + { return self->timing() ? py::cast(self.narrow(self->timing())) : py::none(); }); + + bind_record(m, "EgoAudioChunkRecord", "EgoAudioChunk"); + + serialized_class(m, "EgoPcmAudioChunk", "Encoded EGO payload.") + .def(py::init( + [](uint64_t sequence_number, uint32_t sample_rate_hz, uint16_t channel_count, uint16_t bits_per_sample, + EgoAudioSampleFormat sample_format, uint32_t sample_count, const std::vector& pcm_data, + uint32_t capture_epoch, const Serialized* timing) + { + EgoPcmAudioChunkT native; + native.sequence_number = sequence_number; + native.sample_rate_hz = sample_rate_hz; + native.channel_count = channel_count; + native.bits_per_sample = bits_per_sample; + native.sample_format = sample_format; + native.sample_count = sample_count; + native.pcm_data = pcm_data; + native.capture_epoch = capture_epoch; + if (timing != nullptr && *timing) + { + native.timing = std::make_shared(); + (*timing)->UnPackTo(native.timing.get()); + } + return pack(native); + }), + py::arg("sequence_number") = 0, py::arg("sample_rate_hz") = 0, py::arg("channel_count") = 0, + py::arg("bits_per_sample") = 0, py::arg("sample_format") = EgoAudioSampleFormat_S16LE, + py::arg("sample_count") = 0, py::arg("pcm_data") = std::vector{}, py::arg("capture_epoch") = 0, + py::arg("timing") = nullptr) + .def_property_readonly("sequence_number", field(&EgoPcmAudioChunk::sequence_number)) + .def_property_readonly("sample_rate_hz", field(&EgoPcmAudioChunk::sample_rate_hz)) + .def_property_readonly("channel_count", field(&EgoPcmAudioChunk::channel_count)) + .def_property_readonly("bits_per_sample", field(&EgoPcmAudioChunk::bits_per_sample)) + .def_property_readonly("sample_format", field(&EgoPcmAudioChunk::sample_format)) + .def_property_readonly("sample_count", field(&EgoPcmAudioChunk::sample_count)) + .def_property_readonly("pcm_data", vector_field(&EgoPcmAudioChunk::pcm_data)) + .def_property_readonly("capture_epoch", field(&EgoPcmAudioChunk::capture_epoch)) + .def_property_readonly("timing", + [](const Serialized& self) -> py::object + { return self->timing() ? py::cast(self.narrow(self->timing())) : py::none(); }); + + bind_record(m, "EgoPcmAudioChunkRecord", "EgoPcmAudioChunk"); + + serialized_class(m, "EgoCameraIntrinsics", "Encoded EGO payload.") + .def(py::init( + [](uint32_t width, uint32_t height, float fx, float fy, float cx, float cy, int32_t distortion_model, + const std::vector& distortion) + { + EgoCameraIntrinsicsT native; + native.width = width; + native.height = height; + native.fx = fx; + native.fy = fy; + native.cx = cx; + native.cy = cy; + native.distortion_model = distortion_model; + native.distortion = distortion; + return pack(native); + }), + py::arg("width") = 0, py::arg("height") = 0, py::arg("fx") = 0.0f, py::arg("fy") = 0.0f, + py::arg("cx") = 0.0f, py::arg("cy") = 0.0f, py::arg("distortion_model") = 0, + py::arg("distortion") = std::vector{}) + .def_property_readonly("width", field(&EgoCameraIntrinsics::width)) + .def_property_readonly("height", field(&EgoCameraIntrinsics::height)) + .def_property_readonly("fx", field(&EgoCameraIntrinsics::fx)) + .def_property_readonly("fy", field(&EgoCameraIntrinsics::fy)) + .def_property_readonly("cx", field(&EgoCameraIntrinsics::cx)) + .def_property_readonly("cy", field(&EgoCameraIntrinsics::cy)) + .def_property_readonly("distortion_model", field(&EgoCameraIntrinsics::distortion_model)) + .def_property_readonly("distortion", vector_field(&EgoCameraIntrinsics::distortion)); + + serialized_class(m, "EgoExtrinsics", "Encoded EGO payload.") + .def(py::init( + [](const std::vector& rotation, const std::vector& translation_mm) + { + EgoExtrinsicsT native; + native.rotation = rotation; + native.translation_mm = translation_mm; + return pack(native); + }), + py::arg("rotation") = std::vector{}, py::arg("translation_mm") = std::vector{}) + .def_property_readonly("rotation", vector_field(&EgoExtrinsics::rotation)) + .def_property_readonly("translation_mm", vector_field(&EgoExtrinsics::translation_mm)); + + serialized_class(m, "EgoTimeAlignment", "Versioned EGO timing provenance JSON.") + .def(py::init( + [](const std::string& json) + { + EgoTimeAlignmentT native; + native.json = json; + return pack(native); + }), + py::arg("json") = std::string{}) + .def_property_readonly("json", string_field(&EgoTimeAlignment::json)); + + serialized_class(m, "EgoCalibration", "Encoded EGO payload.") + .def(py::init( + [](const std::string& device_uid, const Serialized* color_left, + const Serialized* color_right, const Serialized* left_to_right, + const std::vector& accel_intrinsics, const std::vector& gyro_intrinsics, + const Serialized* accel_to_left, const Serialized* gyro_to_left, + const std::string& raw_alignment_yaml, const std::string& raw_imu_yaml, uint32_t capture_epoch, + const Serialized* time_alignment) + { + EgoCalibrationT native; + native.device_uid = device_uid; + if (color_left != nullptr && *color_left) + { + native.color_left = std::make_shared(); + (*color_left)->UnPackTo(native.color_left.get()); + } + if (color_right != nullptr && *color_right) + { + native.color_right = std::make_shared(); + (*color_right)->UnPackTo(native.color_right.get()); + } + if (left_to_right != nullptr && *left_to_right) + { + native.left_to_right = std::make_shared(); + (*left_to_right)->UnPackTo(native.left_to_right.get()); + } + native.accel_intrinsics = accel_intrinsics; + native.gyro_intrinsics = gyro_intrinsics; + if (accel_to_left != nullptr && *accel_to_left) + { + native.accel_to_left = std::make_shared(); + (*accel_to_left)->UnPackTo(native.accel_to_left.get()); + } + if (gyro_to_left != nullptr && *gyro_to_left) + { + native.gyro_to_left = std::make_shared(); + (*gyro_to_left)->UnPackTo(native.gyro_to_left.get()); + } + native.raw_alignment_yaml = raw_alignment_yaml; + native.raw_imu_yaml = raw_imu_yaml; + native.capture_epoch = capture_epoch; + if (time_alignment != nullptr && *time_alignment) + { + native.time_alignment = std::make_shared(); + (*time_alignment)->UnPackTo(native.time_alignment.get()); + } + return pack(native); + }), + py::arg("device_uid") = std::string{}, py::arg("color_left") = nullptr, py::arg("color_right") = nullptr, + py::arg("left_to_right") = nullptr, py::arg("accel_intrinsics") = std::vector{}, + py::arg("gyro_intrinsics") = std::vector{}, py::arg("accel_to_left") = nullptr, + py::arg("gyro_to_left") = nullptr, py::arg("raw_alignment_yaml") = std::string{}, + py::arg("raw_imu_yaml") = std::string{}, py::arg("capture_epoch") = 0, py::arg("time_alignment") = nullptr) + .def_property_readonly("device_uid", string_field(&EgoCalibration::device_uid)) + .def_property_readonly("color_left", + [](const Serialized& self) -> py::object + { + const auto* value = self->color_left(); + return value != nullptr ? py::cast(self.narrow(value)) : py::none(); + }) + .def_property_readonly("color_right", + [](const Serialized& self) -> py::object + { + const auto* value = self->color_right(); + return value != nullptr ? py::cast(self.narrow(value)) : py::none(); + }) + .def_property_readonly("left_to_right", + [](const Serialized& self) -> py::object + { + const auto* value = self->left_to_right(); + return value != nullptr ? py::cast(self.narrow(value)) : py::none(); + }) + .def_property_readonly("accel_intrinsics", vector_field(&EgoCalibration::accel_intrinsics)) + .def_property_readonly("gyro_intrinsics", vector_field(&EgoCalibration::gyro_intrinsics)) + .def_property_readonly("accel_to_left", + [](const Serialized& self) -> py::object + { + const auto* value = self->accel_to_left(); + return value != nullptr ? py::cast(self.narrow(value)) : py::none(); + }) + .def_property_readonly("gyro_to_left", + [](const Serialized& self) -> py::object + { + const auto* value = self->gyro_to_left(); + return value != nullptr ? py::cast(self.narrow(value)) : py::none(); + }) + .def_property_readonly("raw_alignment_yaml", string_field(&EgoCalibration::raw_alignment_yaml)) + .def_property_readonly("raw_imu_yaml", string_field(&EgoCalibration::raw_imu_yaml)) + .def_property_readonly("capture_epoch", field(&EgoCalibration::capture_epoch)) + .def_property_readonly( + "time_alignment", + [](const Serialized& self) -> py::object + { return self->time_alignment() ? py::cast(self.narrow(self->time_alignment())) : py::none(); }); + + bind_record(m, "EgoCalibrationRecord", "EgoCalibration"); + + py::enum_(m, "EgoCaptureHealth") + .value("Healthy", EgoCaptureHealth_Healthy) + .value("Warning", EgoCaptureHealth_Warning) + .value("Incomplete", EgoCaptureHealth_Incomplete); + + py::enum_(m, "EgoRecoveryStage") + .value("Started", EgoRecoveryStage_Started) + .value("Recovered", EgoRecoveryStage_Recovered) + .value("Failed", EgoRecoveryStage_Failed); + + py::enum_(m, "EgoDiagnosticEvent") + .value("SdkReport", EgoDiagnosticEvent_EgoSdkReport) + .value("CaptureSummary", EgoDiagnosticEvent_EgoCaptureSummary) + .value("RecoveryEvent", EgoDiagnosticEvent_EgoRecoveryEvent) + .value("DeviceSnapshot", EgoDiagnosticEvent_EgoDeviceSnapshot); + + py::class_(m, "EgoDevicePropertyValue") + .def(py::init(), py::arg("property_id") = 0, py::arg("value") = 0.0) + .def_property("property_id", &EgoDevicePropertyValue::property_id, &EgoDevicePropertyValue::mutate_property_id) + .def_property("value", &EgoDevicePropertyValue::value, &EgoDevicePropertyValue::mutate_value); + + serialized_class(m, "EgoSdkReport", "Typed EGO diagnostic event.") + .def(py::init( + [](uint64_t sequence_number, int32_t work_mode, uint64_t status_flags, uint64_t error_flags, + uint64_t storage_free_bytes) + { + EgoSdkReportT native; + native.sequence_number = sequence_number; + native.work_mode = work_mode; + native.status_flags = status_flags; + native.error_flags = error_flags; + native.storage_free_bytes = storage_free_bytes; + return pack(native); + }), + py::arg("sequence_number"), py::arg("work_mode"), py::arg("status_flags"), py::arg("error_flags"), + py::arg("storage_free_bytes")) + .def_property_readonly("sequence_number", field(&EgoSdkReport::sequence_number)) + .def_property_readonly("work_mode", field(&EgoSdkReport::work_mode)) + .def_property_readonly("status_flags", field(&EgoSdkReport::status_flags)) + .def_property_readonly("error_flags", field(&EgoSdkReport::error_flags)) + .def_property_readonly("storage_free_bytes", field(&EgoSdkReport::storage_free_bytes)); + + serialized_class(m, "EgoCaptureSummary", "Typed EGO diagnostic event.") + .def(py::init( + [](EgoCaptureHealth capture_health, const std::string& reason, uint32_t queue_capacity, + uint32_t queue_peak, uint64_t dropped_events, uint64_t dropped_video_frame_sets, bool is_final) + { + if (capture_health < EgoCaptureHealth_MIN || capture_health > EgoCaptureHealth_MAX) + throw py::value_error("Invalid EGO capture health"); + EgoCaptureSummaryT native; + native.capture_health = capture_health; + native.reason = reason; + native.queue_capacity = queue_capacity; + native.queue_peak = queue_peak; + native.dropped_events = dropped_events; + native.dropped_video_frame_sets = dropped_video_frame_sets; + native.is_final = is_final; + return pack(native); + }), + py::arg("capture_health"), py::arg("reason"), py::arg("queue_capacity"), py::arg("queue_peak"), + py::arg("dropped_events"), py::arg("dropped_video_frame_sets"), py::arg("is_final")) + .def_property_readonly("capture_health", field(&EgoCaptureSummary::capture_health)) + .def_property_readonly("reason", string_field(&EgoCaptureSummary::reason)) + .def_property_readonly("queue_capacity", field(&EgoCaptureSummary::queue_capacity)) + .def_property_readonly("queue_peak", field(&EgoCaptureSummary::queue_peak)) + .def_property_readonly("dropped_events", field(&EgoCaptureSummary::dropped_events)) + .def_property_readonly("dropped_video_frame_sets", field(&EgoCaptureSummary::dropped_video_frame_sets)) + .def_property_readonly("is_final", field(&EgoCaptureSummary::is_final)); + + serialized_class(m, "EgoRecoveryEvent", "Typed EGO diagnostic event.") + .def(py::init( + [](EgoRecoveryStage stage, const std::string& reason, uint32_t reconnect_attempt) + { + if (stage < EgoRecoveryStage_MIN || stage > EgoRecoveryStage_MAX) + throw py::value_error("Invalid EGO recovery stage"); + EgoRecoveryEventT native; + native.stage = stage; + native.reason = reason; + native.reconnect_attempt = reconnect_attempt; + return pack(native); + }), + py::arg("stage"), py::arg("reason"), py::arg("reconnect_attempt")) + .def_property_readonly("stage", field(&EgoRecoveryEvent::stage)) + .def_property_readonly("reason", string_field(&EgoRecoveryEvent::reason)) + .def_property_readonly("reconnect_attempt", field(&EgoRecoveryEvent::reconnect_attempt)); + + serialized_class( + m, "EgoDeviceSnapshot", "Successful startup reads; unavailable temperature is None.") + .def(py::init( + [](std::optional imu_temperature_c, const std::vector& properties) + { + if (imu_temperature_c && !std::isfinite(*imu_temperature_c)) + throw py::value_error("EGO IMU temperature must be finite or None"); + EgoDeviceSnapshotT native; + if (imu_temperature_c) + native.imu_temperature_c = *imu_temperature_c; + native.properties = properties; + return pack(native); + }), + py::arg("imu_temperature_c"), py::arg("properties")) + .def_property_readonly("imu_temperature_c", + [](const Serialized& self) -> py::object + { + const auto temperature = self->imu_temperature_c(); + return temperature ? py::cast(*temperature) : py::none(); + }) + .def_property_readonly("properties", + [](const Serialized& self) + { + std::vector values; + if (self->properties()) + for (const auto* value : *self->properties()) + values.push_back(*value); + return values; + }); + + using DiagnosticPayload = std::variant, Serialized, + Serialized, Serialized>; + serialized_class(m, "EgoDeviceState", "One diagnostic event, separate from NV current availability.") + .def(py::init( + [](const std::string& device_uid, uint32_t capture_epoch, int64_t observed_at_ns, + const DiagnosticPayload& event) + { + EgoDeviceStateT native; + native.device_uid = device_uid; + native.capture_epoch = capture_epoch; + native.observed_at_ns = observed_at_ns; + native.diagnostic_version = EGO_DIAGNOSTIC_VERSION; + std::visit( + [&](const auto& value) + { + using Table = std::remove_cvref_t; + typename Table::NativeTableType payload; + value->UnPackTo(&payload); + native.event.Set(std::move(payload)); + }, + event); + auto encoded = pack(native); + validate_ego_diagnostic(*encoded); + return encoded; + }), + py::arg("device_uid"), py::arg("capture_epoch"), py::arg("observed_at_ns"), py::arg("event")) + .def_property_readonly("device_uid", string_field(&EgoDeviceState::device_uid)) + .def_property_readonly("capture_epoch", field(&EgoDeviceState::capture_epoch)) + .def_property_readonly("observed_at_ns", field(&EgoDeviceState::observed_at_ns)) + .def_property_readonly("diagnostic_version", field(&EgoDeviceState::diagnostic_version)) + .def_property_readonly("event_type", field(&EgoDeviceState::event_type)) + .def_property_readonly("event", + [](const Serialized& self) -> py::object + { + validate_ego_diagnostic(*self); + // Nested event views retain the record buffer after the parent is released. + switch (self->event_type()) + { + case EgoDiagnosticEvent_EgoSdkReport: + return py::cast(self.narrow(self->event_as_EgoSdkReport())); + case EgoDiagnosticEvent_EgoCaptureSummary: + return py::cast(self.narrow(self->event_as_EgoCaptureSummary())); + case EgoDiagnosticEvent_EgoRecoveryEvent: + return py::cast(self.narrow(self->event_as_EgoRecoveryEvent())); + case EgoDiagnosticEvent_EgoDeviceSnapshot: + return py::cast(self.narrow(self->event_as_EgoDeviceSnapshot())); + default: + throw py::value_error("Unknown EGO diagnostic event type"); + } + }); + + bind_record(m, "EgoDeviceStateRecord", "EgoDeviceState"); +} + +} // namespace core diff --git a/src/core/schema/python/ego_camera_bindings.h b/src/core/schema/python/ego_camera_bindings.h new file mode 100644 index 0000000000..a0302f42ea --- /dev/null +++ b/src/core/schema/python/ego_camera_bindings.h @@ -0,0 +1,123 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "schema_serialized.h" + +#include +#include + +#include +#include +#include +#include + +namespace core +{ + +inline void bind_ego_camera(py::module& m) +{ + + py::enum_(m, "EgoCameraStream") + .value("ColorLeft", EgoCameraStream_ColorLeft) + .value("ColorRight", EgoCameraStream_ColorRight); + + py::enum_(m, "EgoPixelFormat") + .value("Mjpg", EgoPixelFormat_Mjpg) + .value("H264", EgoPixelFormat_H264) + .value("H265", EgoPixelFormat_H265); + + // Struct vectors contain values; Serialized handles describe tables only. + py::class_(m, "EgoFrameMetadataEntry") + .def(py::init(), py::arg("key") = 0, py::arg("value") = 0) + .def_property("key", &EgoFrameMetadataEntry::key, &EgoFrameMetadataEntry::mutate_key) + .def_property("value", &EgoFrameMetadataEntry::value, &EgoFrameMetadataEntry::mutate_value); + + serialized_class(m, "EgoFrameMetadata", "Encoded EGO payload.") + .def(py::init( + [](EgoCameraStream stream, uint64_t sequence_number, uint32_t width, uint32_t height, uint32_t fps, + EgoPixelFormat pixel_format, uint64_t encoded_bytes, + const std::vector& sdk_metadata, uint32_t capture_epoch) + { + EgoFrameMetadataT native; + native.stream = stream; + native.sequence_number = sequence_number; + native.width = width; + native.height = height; + native.fps = fps; + native.pixel_format = pixel_format; + native.encoded_bytes = encoded_bytes; + native.sdk_metadata = sdk_metadata; + native.capture_epoch = capture_epoch; + return pack(native); + }), + py::arg("stream") = EgoCameraStream_ColorLeft, py::arg("sequence_number") = 0, py::arg("width") = 0, + py::arg("height") = 0, py::arg("fps") = 0, py::arg("pixel_format") = EgoPixelFormat_Mjpg, + py::arg("encoded_bytes") = 0, py::arg("sdk_metadata") = std::vector{}, + py::arg("capture_epoch") = 0) + .def_property_readonly("stream", field(&EgoFrameMetadata::stream)) + .def_property_readonly("sequence_number", field(&EgoFrameMetadata::sequence_number)) + .def_property_readonly("width", field(&EgoFrameMetadata::width)) + .def_property_readonly("height", field(&EgoFrameMetadata::height)) + .def_property_readonly("fps", field(&EgoFrameMetadata::fps)) + .def_property_readonly("pixel_format", field(&EgoFrameMetadata::pixel_format)) + .def_property_readonly("encoded_bytes", field(&EgoFrameMetadata::encoded_bytes)) + .def_property_readonly("sdk_metadata", + [](const Serialized& self) + { + std::vector values; + const auto* encoded = self->sdk_metadata(); + if (encoded != nullptr) + { + values.reserve(encoded->size()); + for (const auto* value : *encoded) + { + values.push_back(*value); + } + } + return values; + }) + .def_property_readonly("capture_epoch", field(&EgoFrameMetadata::capture_epoch)) + .def("__repr__", + [](const Serialized& self) + { + return "EgoFrameMetadata(stream=" + std::string(EnumNameEgoCameraStream(self->stream())) + + ", sequence_number=" + std::to_string(self->sequence_number()) + ")"; + }); + + bind_record(m, "EgoFrameMetadataRecord", "EgoFrameMetadata"); + + serialized_class(m, "EgoEncodedVideoFrame", "Encoded EGO payload.") + .def(py::init( + [](EgoCameraStream stream, uint64_t sequence_number, uint32_t width, uint32_t height, uint32_t fps, + EgoPixelFormat pixel_format, const std::vector& encoded_data, uint32_t capture_epoch) + { + EgoEncodedVideoFrameT native; + native.stream = stream; + native.sequence_number = sequence_number; + native.width = width; + native.height = height; + native.fps = fps; + native.pixel_format = pixel_format; + native.encoded_data = encoded_data; + native.capture_epoch = capture_epoch; + return pack(native); + }), + py::arg("stream") = EgoCameraStream_ColorLeft, py::arg("sequence_number") = 0, py::arg("width") = 0, + py::arg("height") = 0, py::arg("fps") = 0, py::arg("pixel_format") = EgoPixelFormat_Mjpg, + py::arg("encoded_data") = std::vector{}, py::arg("capture_epoch") = 0) + .def_property_readonly("stream", field(&EgoEncodedVideoFrame::stream)) + .def_property_readonly("sequence_number", field(&EgoEncodedVideoFrame::sequence_number)) + .def_property_readonly("width", field(&EgoEncodedVideoFrame::width)) + .def_property_readonly("height", field(&EgoEncodedVideoFrame::height)) + .def_property_readonly("fps", field(&EgoEncodedVideoFrame::fps)) + .def_property_readonly("pixel_format", field(&EgoEncodedVideoFrame::pixel_format)) + .def_property_readonly("encoded_data", vector_field(&EgoEncodedVideoFrame::encoded_data)) + .def_property_readonly("capture_epoch", field(&EgoEncodedVideoFrame::capture_epoch)); + + bind_record( + m, "EgoEncodedVideoFrameRecord", "EgoEncodedVideoFrame"); +} + +} // namespace core diff --git a/src/core/schema/python/schema_module.cpp b/src/core/schema/python/schema_module.cpp index 80d32c9fca..a02f9a048c 100644 --- a/src/core/schema/python/schema_module.cpp +++ b/src/core/schema/python/schema_module.cpp @@ -7,6 +7,8 @@ // Include binding definitions. #include "controller_bindings.h" +#include "ego_bindings.h" +#include "ego_camera_bindings.h" #include "full_body_bindings.h" #include "gamepad_bindings.h" #include "hand_bindings.h" @@ -91,6 +93,8 @@ PYBIND11_MODULE(_schema, m) // Bind OAK types (StreamType enum, FrameMetadataOak table). core::bind_oak(m); + core::bind_ego_camera(m); + core::bind_ego(m); // Bind full body types (BodyJointPose, BodyJoints structs, FullBodyPose table). core::bind_full_body(m); diff --git a/src/plugins/CMakeLists.txt b/src/plugins/CMakeLists.txt index 4e5fa231f1..a523d19cfc 100644 --- a/src/plugins/CMakeLists.txt +++ b/src/plugins/CMakeLists.txt @@ -32,6 +32,9 @@ endif() if(BUILD_PLUGIN_OAK_CAMERA) add_subdirectory(oak) endif() +if(BUILD_PLUGIN_EGO_CAMERA OR BUILD_EGO_MEDIA_TOOLS) + add_subdirectory(ego) +endif() if(BUILD_PLUGIN_OGLO) add_subdirectory(oglo_tactile) endif() diff --git a/src/plugins/ego/CMakeLists.txt b/src/plugins/ego/CMakeLists.txt new file mode 100644 index 0000000000..609bd4820e --- /dev/null +++ b/src/plugins/ego/CMakeLists.txt @@ -0,0 +1,65 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +include(GNUInstallDirs) +option(EGO_ENABLE_PREVIEW "Build the EGO SDL/FFmpeg preview" OFF) +set(EGO_PLUGIN_OUTPUT_DIR "${CMAKE_BINARY_DIR}/src/plugins/ego_camera") +add_subdirectory(mcap) + +if(BUILD_PLUGIN_EGO_CAMERA) + if(NOT DEFINED ORBBEC_SDK_ROOT AND DEFINED ENV{ORBBEC_SDK_ROOT}) + set(ORBBEC_SDK_ROOT "$ENV{ORBBEC_SDK_ROOT}" CACHE PATH "Path to the OrbbecSDK v2 package root") + endif() + + if(NOT ORBBEC_SDK_ROOT) + message(FATAL_ERROR "BUILD_PLUGIN_EGO_CAMERA=ON requires ORBBEC_SDK_ROOT, the OrbbecSDK v2 package root.") + endif() + + foreach(_ego_required + "${ORBBEC_SDK_ROOT}/include/libobsensor/ObSensor.hpp" + "${ORBBEC_SDK_ROOT}/lib/OrbbecSDKConfig.cmake" + "${ORBBEC_SDK_ROOT}/lib/libOrbbecSDK.so.2" + "${ORBBEC_SDK_ROOT}/lib/extensions") + if(NOT EXISTS "${_ego_required}") + message(FATAL_ERROR "Invalid ORBBEC_SDK_ROOT='${ORBBEC_SDK_ROOT}': missing ${_ego_required}") + endif() + endforeach() + + find_package(OrbbecSDK CONFIG REQUIRED + PATHS "${ORBBEC_SDK_ROOT}/lib" + NO_DEFAULT_PATH + ) + + set(EGO_PLUGIN_MANIFEST "${CMAKE_CURRENT_SOURCE_DIR}/plugin.yaml") + + # The SDK SONAME patch level is release-specific. Package every libOrbbecSDK + # symlink/real-library pair supplied by the selected SDK rather than assuming + # the development machine's 2.9.0 filename. + file(GLOB ORBBEC_SDK_LIBRARY_FILES "${ORBBEC_SDK_ROOT}/lib/libOrbbecSDK.so*") + if(NOT ORBBEC_SDK_LIBRARY_FILES) + message(FATAL_ERROR "Invalid ORBBEC_SDK_ROOT='${ORBBEC_SDK_ROOT}': no libOrbbecSDK.so* files found") + endif() + + add_subdirectory(core) + add_subdirectory(app) + + install(FILES + "${CMAKE_CURRENT_SOURCE_DIR}/plugin.yaml" + "${CMAKE_CURRENT_SOURCE_DIR}/ego.example.toml" + "${CMAKE_CURRENT_SOURCE_DIR}/README.md" + "${ORBBEC_SDK_ROOT}/lib/OrbbecSDKConfig.xml" + ${ORBBEC_SDK_LIBRARY_FILES} + DESTINATION plugins/ego + COMPONENT ego + ) + install(PROGRAMS + "${CMAKE_CURRENT_SOURCE_DIR}/ego.sh" + DESTINATION plugins/ego + COMPONENT ego + ) + install(DIRECTORY "${ORBBEC_SDK_ROOT}/lib/extensions" + DESTINATION plugins/ego + COMPONENT ego + ) + install(DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/tools" DESTINATION plugins/ego COMPONENT ego) +endif() diff --git a/src/plugins/ego/README.md b/src/plugins/ego/README.md new file mode 100644 index 0000000000..1af865ce68 --- /dev/null +++ b/src/plugins/ego/README.md @@ -0,0 +1,589 @@ + + +# EGO Camera Plugin + +C++ plugin that records EGO ColorLeft and ColorRight video and optionally captures +IMU, audio, calibration, and device diagnostics in Isaac Capture. + +The examples below use the native `ego_camera_plugin` executable. The optional +`ego.sh` helper provides a quick way to build, record, and verify a dataset. + +## Features + +- Record MJPEG, H.264, or H.265 video as elementary streams. +- Capture accelerometer and gyroscope data, plus optional 48 kHz mono S16_LE audio. +- Write structured data to a local MCAP or publish it to a multi-device TeleopSession. +- Embed encoded video and PCM audio in a private MCAP and export without transcoding. +- Inspect device profiles, configure controls, and preview stereo video. +- Recover the same physical camera after a disconnect. + +The plugin does not provide depth, IR, D2C, point clouds, or YUYV streams. + +## Build + +### Prerequisites + +For Linux x86_64, use an external OrbbecSDK v2 package compatible with the EGO +camera and host architecture. Its root must contain: + +```text +include/libobsensor/ObSensor.hpp +lib/OrbbecSDKConfig.cmake +lib/libOrbbecSDK.so.2 +lib/extensions/ +``` + +Install CMake 3.24 or newer, Python 3.11, `uv`, and the normal Isaac Capture +build dependencies. These packages cover the common native build and media tools: + +```bash +sudo apt update +sudo apt install -y build-essential cmake ccache clang-format-14 patchelf \ + pkg-config libudev-dev nlohmann-json3-dev libssl-dev ffmpeg jq usbutils +``` + +Install the udev rules from the same SDK package, then reconnect the camera: + +```bash +sudo /path/to/OrbbecSDK/shared/install_udev_rules.sh +``` + +SDL preview is optional. To build it, also install: + +```bash +sudo apt install -y libsdl2-dev libavcodec-dev libavutil-dev libswscale-dev +``` + +### Configure and Build + +Run from the repository root and replace the SDK path with your extracted package: + +```bash +export ORBBEC_SDK=/absolute/path/to/OrbbecSDK +cmake -S . -B build-ego-py3.11 \ + -DISAAC_TELEOP_PYTHON_VERSION=3.11 \ + -DBUILD_VIZ=OFF -DBUILD_PLUGIN_EGO_CAMERA=ON \ + -DORBBEC_SDK_ROOT="$ORBBEC_SDK" +cmake --build build-ego-py3.11 \ + --target ego_camera_plugin ego_mcap_export_media ego_mcap_merge --parallel + +export EGO_BIN="$PWD/build-ego-py3.11/src/plugins/ego_camera" +export EGO_PLUGIN="$EGO_BIN/ego_camera_plugin" +"$EGO_PLUGIN" --help +``` + +Add `-DEGO_ENABLE_PREVIEW=ON` to the configure command for SDL preview. +Recording alone does not require SDL2 or FFmpeg development packages. +The native tools install together under `plugins/ego`; after installation, +set `EGO_BIN` and `EGO_PLUGIN` to that directory and executable instead. + +## Usage + +### Inspect the Camera + +Before recording, inspect the connected device and its supported profiles: + +```bash +"$EGO_PLUGIN" --list-capabilities +``` + +This lists firmware, SDK identity, serial/UID, connection, sensor profiles, and +property ranges. Profiles are advertised per sensor; starting capture checks +the exact simultaneous combination. Unsupported combinations fail without a +fallback. H.264/H.265 profiles above 30 FPS are rejected, including profiles +resolved through `fps=0`. + +### Record Stereo Video + +Create a new output directory and request both eyes explicitly: + +```bash +RUN="recordings/ego_$(date +%Y%m%d_%H%M%S)" +mkdir -p "$RUN/raw" +"$EGO_PLUGIN" --width=1600 --height=1300 --fps=30 \ + --add-stream=camera=ColorLeft,output="$RUN/raw/ColorLeft.h264",format=h264 \ + --add-stream=camera=ColorRight,output="$RUN/raw/ColorRight.h264",format=h264 +``` + +Press `Ctrl+C` once to stop. Wait for final statistics and the shell prompt so +the plugin can drain queues, close outputs, and restore changed controls. +Use fresh output paths: recording refuses to overwrite existing files. + +This command records raw video only. Native defaults are MJPEG, SDK-selected +resolution/FPS, IMU off, audio off, and no MCAP. H.264 `1600x1300@30` above is +an explicit example; use a profile advertised by your camera. + +### Select a Profile or Device + +Stream fields override global dimensions/FPS. For example, to record one eye +from a selected device with an advertised H.265 profile: + +```bash +"$EGO_PLUGIN" --device-uid=DEVICE_UID \ + --add-stream=camera=ColorLeft,output=recordings/left.h265,format=h265,width=1600,height=1300,fps=30 +``` + +Use `format=mjpg` with a `.mjpg` output for MJPEG. The native executable permits +one or both eyes; it never adds a second stream implicitly. + +### Optional IMU and Preview + +Add these flags to a recording command as needed: + +| Option | Effect | +|---|---| +| `--enable-imu --imu-rate=400` | Request available accelerometer and gyroscope streams. Use MCAP or TeleopSession to retain their records. | +| `--preview` | Show SDL side-by-side preview; requires a preview-enabled build and desktop session. | +| `--calibration-output="$RUN/calibration.json"` | Export calibration and the original SDK YAML. | + +The SDL preview decodes on the CPU and may drop stale display frames without +changing the recording. For GPU preview, use the independent viewer: + +```bash +examples/camera_viz/camera_viz.sh setup --with-ego --sdk-root "$ORBBEC_SDK" +examples/camera_viz/camera_viz.sh run \ + examples/camera_viz/configs/ego.yaml --mode window --stereo-debug sbs +``` + +H.264/H.265 GPU preview requires CUDA/NVDEC; MJPEG uses SDK conversion and GPU +upload. The viewer and recording plugin each open the camera; run one at a time. + +### Optional audio and synchronization limits + +For a WAV sidecar, add `--audio-output="$RUN/Audio.wav"` to the recording command. +For audio embedded in MCAP without a WAV sidecar, use `--enable-audio` instead. +Audio is disabled by default; enabling IMU does not enable audio. + +Audio capture, storage, and export are supported, but cross-stream audio +synchronization has not passed physical acceptance. One tested Ethernet setup +with firmware 1.1.10 and an SDK 2.9.0 build returned audio timestamps advancing +about 1.875 times faster than the 48 kHz PCM sample timeline. This observation +does not establish the same fault for other SDK builds, devices, or USB paths. +Successful WAV playback does not establish synchronization. Offline audio +association requires an independently validated time model and continuity +evidence; disabling audio does not establish RGB/IMU physical synchronization. + +### Record Data in MCAP + +Choose where structured data is recorded: + +| Workflow | How to select it | Result | +|---|---|---| +| Raw media only | Omit both metadata destination options. | Video files and optional WAV/calibration sidecars. | +| Local MCAP | `--mcap-filename=PATH` | Plugin-owned metadata, enabled sensors, calibration, and diagnostics. | +| TeleopSession | `--collection-prefix=PREFIX` | Structured data published through OpenXR and recorded with other trackers. | + +`--mcap-filename` and `--collection-prefix` are mutually exclusive. + +#### Local MCAP + +To record stereo video, IMU, and calibration with a local MCAP: + +```bash +RUN="recordings/ego_mcap_$(date +%Y%m%d_%H%M%S)" +mkdir -p "$RUN/raw" +"$EGO_PLUGIN" --width=1600 --height=1300 --fps=30 \ + --add-stream=camera=ColorLeft,output="$RUN/raw/ColorLeft.h264",format=h264 \ + --add-stream=camera=ColorRight,output="$RUN/raw/ColorRight.h264",format=h264 \ + --enable-imu --mcap-filename="$RUN/metadata.mcap" \ + --calibration-output="$RUN/calibration.json" +``` + +No OpenXR runtime or host tracker is needed here. By default, video and WAV are +sidecars; MCAP holds structured records and media indices. + +#### TeleopSession: one MCAP with other trackers + +Live publication requires a running OpenXR runtime and the built/installed +Python package. Complete the full build and install the wheel first: + +```bash +cmake --build build-ego-py3.11 --parallel +cmake --install build-ego-py3.11 --prefix install-ego-py3.11 +uv pip install --reinstall install-ego-py3.11/wheels/isaaccapture-*.whl +``` + +This extends an existing live `TeleopSessionConfig` named `config`, with its +pipeline and other devices already configured, but no EGO plugin or trackers +added yet. It requests stereo RGB and IMU and preserves existing manual recording entries: + +```python +from pathlib import Path +import isaaccapture.deviceio_trackers as dt +from isaaccapture.deviceio_session import McapRecordingConfig +from isaaccapture.ego import configure_recording +from isaaccapture.schema import EgoCameraStream +from isaaccapture.teleop_session_manager import PluginConfig, TeleopSession + +run = Path("recordings/teleop_ego").resolve() +(run / "raw").mkdir(parents=True, exist_ok=True) +ego_entries = [ + (dt.EgoFrameMetadataTracker( + "ego", [EgoCameraStream.ColorLeft, EgoCameraStream.ColorRight] + ), "ego_metadata"), + (dt.EgoImuTracker("ego"), "ego_imu"), + (dt.EgoCalibrationTracker("ego/Calibration"), "ego_calibration"), + (dt.EgoDeviceStateTracker("ego/DeviceState"), "ego_device"), +] +existing = config.mcap_config.get_tracker_names() if config.mcap_config else [] +config.trackers.extend(tracker for tracker, _ in ego_entries) +config.mcap_config = McapRecordingConfig(str(run / "metadata.mcap"), existing + ego_entries) +config.plugins.append(PluginConfig( + plugin_name="ego_camera", + plugin_root_id="ego_camera", + search_paths=[Path("build-ego-py3.11/src/plugins").resolve()], + required=True, + shutdown_timeout_seconds=15.0, + plugin_args=[ + "--collection-prefix=ego", "--width=1600", "--height=1300", "--fps=30", + f"--add-stream=camera=ColorLeft,output={run}/raw/ColorLeft.h264,format=h264", + f"--add-stream=camera=ColorRight,output={run}/raw/ColorRight.h264,format=h264", + "--enable-imu", + ], +)) +config = configure_recording(config) +with TeleopSession(config) as session: + try: + while True: + session.step() + except KeyboardInterrupt: + pass +``` + +Plugin search paths point to the parent containing plugin folders; for an +installation, use `install-ego-py3.11/plugins`. Keep the 15-second EGO shutdown +budget and absolute output paths. Collection prefixes and MCAP source names +serve different purposes; retain these canonical names for the EGO offline reader. + +For audio, add `dt.EgoAudioTracker("ego/Audio")` with recording name `ego_audio` +and an absolute `--audio-output=...` path. Current availability is queried through +`session.get_device_status("ego_camera/camera/ego")` after `session.step()`; +`EgoDeviceStateTracker` carries historical diagnostic events. Version-2 +diagnostics replace the old EGO layout; old diagnostic recordings are unsupported. + +### Store Media Inside MCAP + +Media layout is separate from the recording workflow: + +| Layout | Option | Media location | +|---|---|---| +| Sidecar (default) | `--mcap-media=metadata-only` | Video/WAV files beside structured MCAP data. | +| Embedded | `--mcap-media=embedded` | Encoded video and enabled PCM audio inside MCAP, without transcoding. | + +For a self-contained local recording, omit video output paths: + +```bash +RUN="recordings/ego_embedded_$(date +%Y%m%d_%H%M%S)" +mkdir -p "$RUN" +"$EGO_PLUGIN" --width=1600 --height=1300 --fps=30 \ + --add-stream=camera=ColorLeft,format=h264 \ + --add-stream=camera=ColorRight,format=h264 \ + --enable-imu --mcap-filename="$RUN/metadata.mcap" --mcap-media=embedded +``` + +Add `--enable-audio` to embed PCM. To retain sidecars too, add +`--keep-media-sidecars`, supply each stream's `output=...`, and use +`--audio-output=PATH.wav` if a WAV sidecar is wanted. + +For embedded multi-device recording, replace the execution block in the +TeleopSession example with the following, before opening the session: + +If that configuration includes audio, replace `--audio-output=...` with +`--enable-audio` for embedded-only PCM, or explicitly retain media sidecars. + +```python +from isaaccapture.ego import embedded_recording + +fragment = run / "ego_fragment.mcap" +config.mcap_config.filename = str(run / "final.mcap") +config.plugins[-1].plugin_args.extend([ + "--mcap-media=embedded", f"--mcap-media-spool={fragment}", +]) +with embedded_recording(config, media_fragment=fragment) as session: + try: + while True: + session.step() + except KeyboardInterrupt: + pass +``` + +The helper accepts exactly one enabled EGO plugin and merges its fragment with +the other devices' session MCAP after clean shutdown. The fragment must match +`--mcap-media-spool`; `final.mcap`, `final.session.mcap`, the fragment, and their +`.partial` paths must be unused. `ego_mcap_merge` must be beside the plugin. +Cleanup/merge failures propagate and preserve inputs for diagnosis. Catch +`Ctrl+C` inside the context to request normal finalization. + +## Configuration + +### Native CLI Options + +These entries describe the native executable, not wrapper presets: + +| Option | Native default | Purpose | +|---|---|---| +| `--add-stream=camera=...,output=...[,format=...,width=...,height=...,fps=...]` | None; at least one required | Select `ColorLeft` or `ColorRight`. Format defaults to `mjpg`; output is required unless embedded media has no sidecars. | +| `--width=N --height=N --fps=N` | `0` | Global profile fields; zero selects an SDK default. Nonzero stream fields take precedence. | +| `--device-uid=UID` | Automatic selection | Select a device reported by `--list-capabilities`. | +| `--enable-imu --imu-rate=400\|1000` | Off; rate `400` | Enable available IMU. Setting rate alone does not enable it. | +| `--accel-full-scale=N --gyro-full-scale=N` | `24` g / `2000` dps | Requested IMU ranges, subject to advertised support. | +| `--enable-audio` / `--audio-output=PATH.wav` | Off | Enable PCM; the output option also writes WAV. | +| `--preview` | Off | Open SDL preview. | +| `--mcap-filename=PATH` / `--collection-prefix=PREFIX` | Neither | Choose local MCAP or live publication. | +| `--mcap-media=metadata-only\|embedded` | `metadata-only` | Choose media layout; embedded requires an MCAP/publication destination. | +| `--keep-media-sidecars` | Off | Retain sidecars in embedded mode. | +| `--mcap-media-spool=PATH` | None | Required fragment for embedded TeleopSession. | +| `--record-device-diagnostics=on\|off` | `on` | Include diagnostic events in MCAP. | +| `--calibration-output=PATH.json` | None | Export calibration as a sidecar. | +| `--time-alignment=PATH` | None | Load a validated reusable RGB/IMU capture profile. | +| `--bitrate=N --dynamic-bitrate=on\|off` | Leave device values unchanged | Set SDK bitrate properties; inspect the advertised range. | +| `--reconnect-timeout=N` | `30` seconds | Wait for the same physical camera; `0` disables recovery. | +| `--reconnect-interval-ms=N` | `1000` | Re-enumeration interval; must be positive. | +| `--persist-controls` | Off | Keep modified camera properties after stopping. | + +### Device Controls + +Inspect `--list-capabilities` before requesting exposure, gain, white balance, +brightness, sharpness, saturation, contrast, or power-frequency values. For example, +after confirming the property ranges for the intended profile: + +```bash +"$EGO_PLUGIN" \ + --add-stream=camera=ColorLeft,output=recordings/manual.h264,format=h264,width=1600,height=1300,fps=30 \ + --set-property=OB_PROP_COLOR_AUTO_EXPOSURE_BOOL=0 --exposure=1000 --gain=100 +``` + +Controls are validated against the active profile and restored after capture +stops. Use `--persist-controls` only when device changes should remain. +`--set-property=SDK_PROPERTY_NAME=VALUE` supports other writable SDK properties. +EGO has no `--quality` option; bitrate values are SDK property values, not OAK's units. + +To omit stored diagnostic events, use `--record-device-diagnostics=off`. +Capture checks, recovery, logs, live diagnostics, and NV monitoring remain active. +For TeleopSession, use `config = configure_recording(config, +record_device_diagnostics=False)` or the same keyword on `embedded_recording`. +Remove diagnostic trackers from explicit recording lists and automatically +recorded pipeline/control/sink sources; conflicts fail before capture. The +merger also rejects diagnostic channels in either input under this policy. + +## Architecture + +```text +EGO camera -> OrbbecSDK -> ego_camera_plugin + |-> video/WAV/calibration sidecars + |-> local MCAP: structured data + optional media + `-> OpenXR publication -> DeviceIO trackers -> session MCAP +``` + +Embedded TeleopSession additionally writes an EGO capture fragment, then merges +it with the session MCAP into one final file after both producers stop. + +## Output and Verification + +### Recording Files + +The local MCAP example produces: + +```text +recordings/ego_mcap_/ +├── raw/ColorLeft.h264 +├── raw/ColorRight.h264 +├── calibration.json +└── metadata.mcap +``` + +Audio adds `Audio.wav` only when requested with an output path. Embedded mode +stores media in MCAP and omits video/WAV sidecars unless explicitly retained. + +Canonical structured topics are `ego_metadata/ColorLeft`, `ego_metadata/ColorRight`, +`ego_imu/Accel`, `ego_imu/Gyro`, `ego_calibration/Calibration`, and +`ego_device/DeviceState`; audio adds `ego_audio/Audio`. IMU/audio topics depend +on enabled and available sensors, and diagnostics can be omitted. Embedded +media uses `ego_media/ColorLeft`, `ego_media/ColorRight`, and optional `ego_media/Audio`. + +MCAP is written as `PATH.partial` and promoted to `PATH` only after successful +capture and cleanup. A remaining `.partial`, nonzero exit, or queue/sequence +loss is a failed recording; preserve it with its logs for diagnosis. +Recovery opens a new `capture_epoch`; continuity is checked within each epoch, +and a different physical device is never substituted. + +### Verify a Recording + +Decode both H.264 streams completely and inspect the actual profile: + +```bash +ffmpeg -v error -xerror -f h264 -i "$RUN/raw/ColorLeft.h264" -f null - +ffmpeg -v error -xerror -f h264 -i "$RUN/raw/ColorRight.h264" -f null - +ffprobe -v error -select_streams v:0 \ + -show_entries stream=codec_name,width,height -of default=noprint_wrappers=1 \ + "$RUN/raw/ColorLeft.h264" +``` + +For the two-eye local MCAP layout above, the helper also checks MCAP topics, +video counts/cadence, enabled sensors, and capture health: + +```bash +python3 -m pip install --user mcap # If the MCAP reader is not installed. +./src/plugins/ego/ego.sh verify "$RUN" --plugin "$EGO_PLUGIN" --expect-imu +``` + +Omit `--expect-imu` if IMU was not requested; add `--expect-audio` when audio was +requested. This verifier expects `metadata.mcap` and two video streams; it is +not a verifier for raw-only or single-eye capture. Native recording does not +create `requested_profile.json` or `logs/capture.log`: without those files, +request matching or log-based loss checks are skipped with a warning. +Success prints `Verification passed` and writes `verification_report.txt`; +failure writes `verification_report.failed.txt` and invalidates an old success report. +These checks establish recording integrity, not physical cross-stream synchronization. + +### Export and Play Media + +Export a completed embedded MCAP with the native tool. Choose a fresh output +directory because the exporter overwrites same-name media files: + +```bash +"$EGO_BIN/ego_mcap_export_media" "$RUN/metadata.mcap" "$RUN/exported_media" +``` + +It writes `ColorLeft.`, `ColorRight.`, and `Audio.wav` when PCM +was recorded. Do not export a `.partial` capture as a completed dataset. + +Elementary streams are not MP4 files. To play or remux H.264 sidecars: + +```bash +ffplay -f h264 -framerate 30 "$RUN/raw/ColorLeft.h264" +ffmpeg -f h264 -framerate 30 -i "$RUN/raw/ColorLeft.h264" -c:v copy "$RUN/ColorLeft.mp4" +``` + +For H.265, use `-f hevc` and the `.h265` file. For MJPEG, use `-f mjpeg` and +transcode with `-c:v libx264 -pix_fmt yuv420p`. For embedded captures, use the +exported paths. Opening video and WAV separately does not demonstrate synchronized playback. + +### Structured Replay and Offline Reading + +With the installed Python package, inspect an audio-enabled recording using: + +```bash +uv pip install -e ./examples/mcap_record_replay +python -m isaaccapture_examples.mcap_record_replay.replay_ego \ + /absolute/path/to/audio_enabled_recording/metadata.mcap +``` + +This example requires RGB, IMU, Audio, calibration, and enabled diagnostic +channels. A missing Audio channel is expected for audio-disabled recordings; +it does not make them invalid. Replay reads structured records, not decoded +video or synchronized sound, and one update is not a synchronized bundle. + +For sample-time association, install the offline extra and select only the +streams needed, supplying a validated model when none was recorded: + +```bash +uv pip install 'isaaccapture[ego]' +python -m isaaccapture_examples.mcap_record_replay.analyze_ego_sync \ + "$RUN" --required-streams left_rgb right_rgb accel gyro \ + --alignment /absolute/path/to/alignment.json --output "$RUN/sync_audit.json" +``` + +The reader needs accessible sidecar media or a completed embedded MCAP. +Missing required streams/models fail; missing samples remain incomplete. +Add audio only with a validated audio model and continuity evidence. The audit +does not establish synchronization with other devices or absolute host sampling accuracy. + +
+Advanced: capture profiles and recording-bound time models + +A reusable profile supplied through `--time-alignment=PATH` has version 1, +`scope="capture_profile"`, `status="validated"`, `model_id`, and +`reference_stream="left_rgb"`. Its identity binds serial, firmware, SDK version, +and the loaded SDK library SHA-256; its profile binds video format/dimensions/FPS, +IMU rate, and audio format. Enabled RGB/IMU mappings provide `offset_ns` and +`scale=1`, with left RGB as the identity mapping. Audio remains an arrival-time +fallback during capture. The file is limited to 8 KiB. + +Recording-bound models use `scope="recording"` and bind identity/profile, +MCAP/media fingerprints, and epoch validity intervals. RGB/IMU mappings include +`source_anchor_ns`, `reference_anchor_ns`, positive `scale`, `validation_start_ns`, +and `validation_end_ns`. Audio segments include `segment_id`, `first_sample_index`, +`sample_count`, `reference_anchor_ns`, `period_ns`, `continuity_verified`, and +the same validation bounds. Supply these models explicitly to offline reads; +they do not rewrite MCAP or automatically become reusable capture profiles. + +To fit such a model from independently annotated events: + +```bash +python -m isaaccapture_examples.mcap_record_replay.calibrate_ego_sync \ + "$RUN/events.json" --output "$RUN/alignment.json" +``` + +The version-1 event manifest contains `model_id`, `recording`, optional +`media_paths`, `identity`, `profile`, and `epochs`. Each epoch defines `epoch`, +`valid_from_ns`, `valid_to_ns`, and `streams`. Each fitted stream supplies +`calibration_events`, separate `validation_events`, and validation start/end +nanoseconds. Events contain `event_id`, `source_value` (raw nanoseconds or audio +sample index), `reference_time_ns`, `uncertainty_ns`, and an `evidence` path. +Audio uses `streams.audio.segments` with `segment_id`, `first_sample_index`, +`sample_count`, and `continuity_verified` (default false). Paths resolve relative +to the manifest; evidence files are hashed. + +Validation requires at least three calibration events and three separate +validation events spanning the beginning, middle, and end of at least 15 minutes. +Maximum error plus uncertainty must be within half a configured video frame. +Reusable profiles declare `validation` evidence and `max_capture_duration_ns` +within the validated duration; expiry returns capture to an unverified +arrival-time fallback. Reconnects and clock discontinuities form new epochs; +models do not interpolate across them. Calibration/audit tools refuse to +overwrite outputs and return nonzero for unverified or incomplete results. + +
+ +## Troubleshooting + +| Symptom | What to check | +|---|---| +| No camera or permission denied | Install the selected SDK's udev rules, reconnect, and run `--list-capabilities`. | +| Requested profile rejected | Choose advertised profiles for each eye; the simultaneous combination must also start successfully. H.264/H.265 are limited to 30 FPS. | +| `--preview` rejected | Reconfigure with `-DEGO_ENABLE_PREVIEW=ON`, install SDL/FFmpeg development packages, and use a desktop session. | +| Camera busy or preview cannot open | Stop other processes using that EGO, including `camera_viz`. | +| Recording waits after disconnect | Reconnect the same physical camera within `--reconnect-timeout`; UID may change after USB re-enumeration. | +| Nonzero exit or `.partial` remains | Preserve capture and logs; check queue loss, output failures, reconnect timeout, and control restoration. | +| MCAP verifier cannot read a native run | Use the two-eye `metadata.mcap` layout; install the Python MCAP reader. Missing wrapper manifests/logs reduce the checks performed. | +| Video does not open in a desktop player | Decode it with FFmpeg, then remux/transcode to MP4. Keep other SEI data intact when diagnosing decode errors. | +| TeleopSession cannot publish EGO data | Check the running OpenXR runtime, installed Python bindings, plugin search path, and collection prefix. | +| Embedded session has no final file | Check clean context exit, matching spool/fragment paths, fresh output paths, and `ego_mcap_merge` beside the plugin. | +| Audio cadence or sync audit fails | Preserve data and logs; audio requires a validated time model and continuity evidence. | + +For SDK API background, consult the [Orbbec SDK v2 API guide](https://orbbec.github.io/docs/OrbbecSDKv2_API_User_Guide/) +and [timestamp guide](https://orbbec.github.io/OrbbecSDK_v2/docs/tutorial/timestamp.html). +These describe SDK behavior; EGO profiles and control ranges must be checked on the connected camera. + +## Quick Start with ego.sh (Optional) + +The helper automates dependency checks, native builds, run-directory creation, +and post-recording verification. It does not install packages or download SDKs. +Unlike the native executable, `record` requests both eyes at H.264 +`1600x1300@30`, available IMU, calibration, and local MCAP by default; audio is off. + +```bash +./src/plugins/ego/ego.sh doctor --sdk-root "$ORBBEC_SDK" +./src/plugins/ego/ego.sh build --sdk-root "$ORBBEC_SDK" --jobs 8 +./src/plugins/ego/ego.sh capabilities +./src/plugins/ego/ego.sh record --duration 30 +./src/plugins/ego/ego.sh record --help +``` + +For repeatable settings, copy `src/plugins/ego/ego.example.toml` outside the +source tree and run `ego.sh record --config /path/to/run.toml`. Later CLI options +override TOML values; arguments after `--` pass through to the native plugin: + +```bash +./src/plugins/ego/ego.sh record --config /path/to/run.toml \ + --duration 60 --no-audio -- --bitrate=8 --dynamic-bitrate=on +``` + +Helper runs additionally save capabilities/provenance, `requested_profile.json`, +`logs/capture.log`, and a verification report. `--duration` includes startup +time; omit it for recording until `Ctrl+C`. The native executable does not +accept `--duration`, `--config`, or wrapper-style `--format` options. diff --git a/src/plugins/ego/app/CMakeLists.txt b/src/plugins/ego/app/CMakeLists.txt new file mode 100644 index 0000000000..a49222155b --- /dev/null +++ b/src/plugins/ego/app/CMakeLists.txt @@ -0,0 +1,26 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +add_executable(ego_camera_plugin + main.cpp +) + +target_link_libraries(ego_camera_plugin + PRIVATE + ego::camera_core + log_bridge::log_bridge_core +) + +target_compile_definitions(ego_camera_plugin + PRIVATE + EGO_ENABLE_PREVIEW=$ +) + +set_target_properties(ego_camera_plugin PROPERTIES + RUNTIME_OUTPUT_DIRECTORY "${EGO_PLUGIN_OUTPUT_DIR}" + BUILD_RPATH "${ORBBEC_SDK_ROOT}/lib" + INSTALL_RPATH "$ORIGIN" +) + +configure_file("${EGO_PLUGIN_MANIFEST}" "${EGO_PLUGIN_OUTPUT_DIR}/plugin.yaml" COPYONLY) +install(TARGETS ego_camera_plugin RUNTIME DESTINATION plugins/ego COMPONENT ego) diff --git a/src/plugins/ego/app/main.cpp b/src/plugins/ego/app/main.cpp new file mode 100644 index 0000000000..7adce67691 --- /dev/null +++ b/src/plugins/ego/app/main.cpp @@ -0,0 +1,337 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace +{ + +std::atomic stop_requested{ false }; + +void signal_handler(int signal) +{ + if (signal == SIGINT || signal == SIGTERM) + stop_requested.store(true, std::memory_order_relaxed); +} + +core::EgoCameraStream parse_camera_name(const std::string& name) +{ + if (name == "ColorLeft") + return core::EgoCameraStream_ColorLeft; + if (name == "ColorRight") + return core::EgoCameraStream_ColorRight; + throw std::runtime_error("Unknown camera name: '" + name + "'. Expected ColorLeft or ColorRight."); +} + +core::EgoPixelFormat parse_pixel_format(const std::string& value) +{ + if (value == "mjpg" || value == "mjpeg") + return core::EgoPixelFormat_Mjpg; + if (value == "h264") + return core::EgoPixelFormat_H264; + if (value == "h265" || value == "hevc") + return core::EgoPixelFormat_H265; + throw std::runtime_error("Unknown format '" + value + "'. Expected mjpg, h264, or h265."); +} + +bool parse_on_off(const std::string& value) +{ + if (value == "on" || value == "true" || value == "1") + return true; + if (value == "off" || value == "false" || value == "0") + return false; + throw std::runtime_error("Expected on or off, got '" + value + "'"); +} + +plugins::ego::StreamConfig parse_stream_arg(const std::string& arg) +{ + plugins::ego::StreamConfig config{}; + bool has_camera = false; + std::istringstream input(arg); + std::string token; + while (std::getline(input, token, ',')) + { + const auto equals = token.find('='); + if (equals == std::string::npos) + throw std::runtime_error("Invalid key=value in --add-stream: '" + token + "'"); + const auto key = token.substr(0, equals); + const auto value = token.substr(equals + 1); + if (key == "camera") + { + config.camera = parse_camera_name(value); + has_camera = true; + } + else if (key == "output") + { + config.output_path = value; + } + else if (key == "format") + config.pixel_format = parse_pixel_format(value); + else if (key == "width") + config.width = std::stoul(value); + else if (key == "height") + config.height = std::stoul(value); + else if (key == "fps") + config.fps = std::stoul(value); + else + { + throw std::runtime_error("Unknown key in --add-stream: '" + key + "'"); + } + } + if (!has_camera) + throw std::runtime_error("--add-stream requires camera="); + return config; +} + +plugins::ego::McapMediaMode parse_mcap_media_mode(const std::string& value) +{ + if (value == "metadata-only") + return plugins::ego::McapMediaMode::MetadataOnly; + if (value == "embedded") + return plugins::ego::McapMediaMode::Embedded; + throw std::runtime_error("--mcap-media must be metadata-only or embedded"); +} + +void print_usage(const char* program) +{ + std::cout << "Usage: " << program << " [options] --add-stream=camera=[,output=]\n\n" + << "Streams (repeatable):\n" + << " --add-stream=camera=ColorLeft[,output=][,format=mjpg|h264|h265,width=N,height=N,fps=N]\n" + << " --add-stream=camera=ColorRight[,output=][,format=mjpg|h264|h265,width=N,height=N,fps=N]\n\n" + << "Capture:\n" + << " --width=N --height=N --fps=N 0 selects the SDK default profile\n" + << " --bitrate=N --dynamic-bitrate=on|off\n" + << " --device-uid=UID Select a specific Ego device\n" + << " --reconnect-timeout=N Wait N seconds for the same physical device; 0 disables recovery " + "(default 30)\n" + << " --reconnect-interval-ms=N Device re-enumeration interval (default 1000)\n" + << " --preview SDL side-by-side latest-frame preview\n" + << " --enable-imu --imu-rate=400|1000\n" + << " --accel-full-scale= --gyro-full-scale=\n" + << " --enable-audio Capture PCM audio (embedded MCAP needs no WAV path)\n" + << " --audio-output=PATH.wav Capture audio and write a WAV sidecar\n" + << "Metadata (mutually exclusive):\n" + << " --mcap-filename=PATH Write a local MCAP\n" + << " --collection-prefix=PREFIX Publish metadata via OpenXR tensors\n" + << " --mcap-media=metadata-only|embedded (default metadata-only)\n" + << " --keep-media-sidecars Keep raw video/WAV with embedded MCAP\n" + << " --mcap-media-spool=PATH Embedded-media fragment for TeleopSession merger\n" + << " --record-device-diagnostics=on|off Save detailed diagnostics in local MCAP (default on)\n" + << "Device controls:\n" + << " --exposure=N --gain=N --white-balance=N --brightness=N\n" + << " --sharpness=N --saturation=N --contrast=N --power-frequency=N\n" + << " --set-property=SDK_PROPERTY_NAME=VALUE [--persist-controls]\n" + << " --calibration-output=PATH.json --list-capabilities\n" + << " --time-alignment=PATH Validated EGO capture timing profile\n" + << "Ego has no quality property; use bitrate/dynamic-bitrate. Ego exposes no Depth/IR/point cloud.\n" + << "Advertised profiles are per sensor; capture validates the exact simultaneous combination without " + "fallback.\n" + << "Encoded profiles above 30 FPS remain uncertified and are rejected after SDK profile resolution.\n" + << " --plugin-root-id=ID PluginManager status collection identity\n"; +} + +} // namespace + +int main(int argc, char** argv) +try +{ + plugins::ego::CaptureConfig capture_config; + std::map stream_map; + bool list_capabilities = false; + for (int index = 1; index < argc; ++index) + { + const std::string argument(argv[index]); + if (argument == "--help" || argument == "-h") + { + print_usage(argv[0]); + return 0; + } + if (argument.rfind("--add-stream=", 0) == 0) + { + auto stream = parse_stream_arg(argument.substr(13)); + if (!stream_map.emplace(stream.camera, std::move(stream)).second) + throw std::runtime_error("Duplicate camera in --add-stream"); + } + else if (argument.rfind("--width=", 0) == 0) + capture_config.width = std::stoul(argument.substr(8)); + else if (argument.rfind("--height=", 0) == 0) + capture_config.height = std::stoul(argument.substr(9)); + else if (argument.rfind("--fps=", 0) == 0) + capture_config.fps = std::stoul(argument.substr(6)); + else if (argument.rfind("--device-uid=", 0) == 0) + capture_config.device_uid = argument.substr(13); + else if (argument.rfind("--reconnect-timeout=", 0) == 0) + capture_config.reconnect_timeout_seconds = std::stoul(argument.substr(20)); + else if (argument.rfind("--reconnect-interval-ms=", 0) == 0) + capture_config.reconnect_interval_milliseconds = std::stoul(argument.substr(24)); + else if (argument.rfind("--bitrate=", 0) == 0) + capture_config.bitrate = std::stoul(argument.substr(10)); + else if (argument.rfind("--dynamic-bitrate=", 0) == 0) + { + capture_config.dynamic_bitrate = parse_on_off(argument.substr(18)); + capture_config.dynamic_bitrate_set = true; + } + else if (argument == "--preview") + capture_config.preview = true; + else if (argument == "--enable-imu") + capture_config.enable_imu = true; + else if (argument == "--enable-audio") + capture_config.enable_audio = true; + else if (argument.rfind("--imu-rate=", 0) == 0) + capture_config.imu_rate = std::stoul(argument.substr(11)); + else if (argument.rfind("--accel-full-scale=", 0) == 0) + capture_config.accel_full_scale_g = std::stof(argument.substr(19)); + else if (argument.rfind("--gyro-full-scale=", 0) == 0) + capture_config.gyro_full_scale_dps = std::stof(argument.substr(18)); + else if (argument.rfind("--audio-output=", 0) == 0) + { + capture_config.audio_output = argument.substr(15); + capture_config.enable_audio = true; + } + else if (argument.rfind("--collection-prefix=", 0) == 0) + capture_config.collection_prefix = argument.substr(20); + else if (argument.rfind("--mcap-filename=", 0) == 0) + capture_config.mcap_filename = argument.substr(16); + else if (argument.rfind("--mcap-media=", 0) == 0) + capture_config.mcap_media_mode = parse_mcap_media_mode(argument.substr(13)); + else if (argument == "--keep-media-sidecars") + capture_config.keep_media_sidecars = true; + else if (argument.rfind("--mcap-media-spool=", 0) == 0) + capture_config.mcap_media_spool = argument.substr(19); + else if (argument.rfind("--calibration-output=", 0) == 0) + capture_config.calibration_output = argument.substr(21); + else if (argument.rfind("--time-alignment=", 0) == 0) + { + capture_config.time_alignment = argument.substr(17); + if (capture_config.time_alignment.empty()) + throw std::invalid_argument("--time-alignment requires a JSON profile path"); + } + else if (argument == "--persist-controls") + capture_config.persist_controls = true; + else if (argument == "--list-capabilities") + list_capabilities = true; + else if (argument.rfind("--set-property=", 0) == 0) + { + const auto setting = argument.substr(15); + const auto equals = setting.find('='); + if (equals == std::string::npos) + throw std::runtime_error("--set-property requires SDK_PROPERTY_NAME=VALUE"); + capture_config.properties.push_back({ setting.substr(0, equals), std::stod(setting.substr(equals + 1)) }); + } + else if (argument.rfind("--exposure=", 0) == 0) + capture_config.properties.push_back({ "OB_PROP_COLOR_EXPOSURE_INT", std::stod(argument.substr(11)) }); + else if (argument.rfind("--gain=", 0) == 0) + capture_config.properties.push_back({ "OB_PROP_COLOR_GAIN_INT", std::stod(argument.substr(7)) }); + else if (argument.rfind("--white-balance=", 0) == 0) + capture_config.properties.push_back({ "OB_PROP_COLOR_WHITE_BALANCE_INT", std::stod(argument.substr(16)) }); + else if (argument.rfind("--brightness=", 0) == 0) + capture_config.properties.push_back({ "OB_PROP_COLOR_BRIGHTNESS_INT", std::stod(argument.substr(13)) }); + else if (argument.rfind("--sharpness=", 0) == 0) + capture_config.properties.push_back({ "OB_PROP_COLOR_SHARPNESS_INT", std::stod(argument.substr(12)) }); + else if (argument.rfind("--saturation=", 0) == 0) + capture_config.properties.push_back({ "OB_PROP_COLOR_SATURATION_INT", std::stod(argument.substr(13)) }); + else if (argument.rfind("--contrast=", 0) == 0) + capture_config.properties.push_back({ "OB_PROP_COLOR_CONTRAST_INT", std::stod(argument.substr(11)) }); + else if (argument.rfind("--power-frequency=", 0) == 0) + capture_config.properties.push_back( + { "OB_PROP_COLOR_POWER_LINE_FREQUENCY_INT", std::stod(argument.substr(18)) }); + else if (argument.rfind("--plugin-root-id=", 0) == 0) + capture_config.plugin_root_id = argument.substr(17); + else if (argument.rfind("--record-device-diagnostics=", 0) == 0) + capture_config.record_device_diagnostics = parse_on_off(argument.substr(28)); + else + { + std::cerr << "Unknown option: " << argument << std::endl; + print_usage(argv[0]); + return 1; + } + } + if (!capture_config.collection_prefix.empty() && !capture_config.mcap_filename.empty()) + throw std::runtime_error("--collection-prefix and --mcap-filename are mutually exclusive"); +#if !EGO_ENABLE_PREVIEW + if (capture_config.preview) + { + throw std::runtime_error("--preview requires a preview-enabled build. Reconfigure with -DEGO_ENABLE_PREVIEW=ON."); + } +#endif + if (capture_config.imu_rate != 400 && capture_config.imu_rate != 1000) + throw std::runtime_error("--imu-rate must be 400 or 1000"); + if (capture_config.reconnect_interval_milliseconds == 0) + throw std::runtime_error("--reconnect-interval-ms must be greater than zero"); + const bool embedded_media = capture_config.mcap_media_mode == plugins::ego::McapMediaMode::Embedded; + if (embedded_media && capture_config.collection_prefix.empty() && capture_config.mcap_filename.empty()) + throw std::runtime_error("--mcap-media=embedded requires --mcap-filename or --collection-prefix"); + if (embedded_media && !capture_config.collection_prefix.empty() && capture_config.mcap_media_spool.empty()) + throw std::runtime_error("--mcap-media=embedded with --collection-prefix requires --mcap-media-spool=PATH"); + if (!embedded_media && capture_config.keep_media_sidecars) + throw std::runtime_error("--keep-media-sidecars is only valid with --mcap-media=embedded"); + if (embedded_media && !capture_config.keep_media_sidecars && !capture_config.audio_output.empty()) + throw std::runtime_error( + "--audio-output requires --keep-media-sidecars with --mcap-media=embedded; use --enable-audio"); + if (list_capabilities) + { + plugins::ego::EgoCamera::list_capabilities(capture_config); + return 0; + } + if (stream_map.empty()) + throw std::runtime_error("At least one --add-stream is required."); + + std::vector streams; + streams.reserve(stream_map.size()); + for (auto& [_, stream] : stream_map) + { + if ((embedded_media && capture_config.keep_media_sidecars) || !embedded_media) + { + if (stream.output_path.empty()) + throw std::runtime_error( + "--add-stream requires output= unless --mcap-media=embedded is used without --keep-media-sidecars"); + } + plugins::ego::validate_stream_config(stream, capture_config); + streams.push_back(std::move(stream)); + } + + plugins::ego::validate_time_alignment_profile(capture_config.time_alignment); + std::signal(SIGINT, signal_handler); + std::signal(SIGTERM, signal_handler); + capture_config.stop_requested = [] { return stop_requested.load(std::memory_order_relaxed); }; + auto sink = plugins::ego::create_frame_sink(streams, capture_config); + plugins::ego::EgoCamera camera(capture_config, streams, std::move(sink)); + const auto stats_interval = std::chrono::seconds(5); + auto last_stats = std::chrono::steady_clock::now(); + while (!stop_requested.load(std::memory_order_relaxed)) + { + camera.update(); + if (camera.preview_closed()) + break; + const auto now = std::chrono::steady_clock::now(); + if (now - last_stats >= stats_interval) + { + camera.print_stats(); + last_stats = now; + } + } + camera.close(); + camera.print_stats(); + return 0; +} +catch (const plugins::ego::CaptureCancelled& error) +{ + isaaccapture::Logger::get("isaaccapture.plugins.ego.main")->info("{}", error.what()); + return 130; +} +catch (const std::exception& error) +{ + isaaccapture::Logger::get("isaaccapture.plugins.ego.main")->error("{}: {}", argv[0], error.what()); + return 1; +} diff --git a/src/plugins/ego/core/CMakeLists.txt b/src/plugins/ego/core/CMakeLists.txt new file mode 100644 index 0000000000..22923f5eec --- /dev/null +++ b/src/plugins/ego/core/CMakeLists.txt @@ -0,0 +1,64 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +set(ego_camera_core_sources + time_alignment.cpp + inc/ego_camera/time_alignment.hpp + ego_camera.cpp + calibration.cpp + calibration.hpp + device_diagnostics.cpp + device_diagnostics.hpp + device_controls.cpp + device_controls.hpp + wav_writer.hpp + frame_sink.cpp + metadata_sink.cpp + metadata_sink.hpp + mcap_metadata_sink.cpp + schema_metadata_sink.cpp + inc/ego_camera/ego_camera.hpp +) + +if(EGO_ENABLE_PREVIEW) + find_package(PkgConfig REQUIRED) + pkg_check_modules(EGO_PREVIEW REQUIRED IMPORTED_TARGET sdl2 libavcodec libavutil libswscale) + list(APPEND ego_camera_core_sources + preview.cpp + preview.hpp + ) +endif() + +add_library(ego_camera_core STATIC ${ego_camera_core_sources}) + +if(NOT TARGET nlohmann_json::nlohmann_json) + find_package(nlohmann_json CONFIG REQUIRED) +endif() +find_package(OpenSSL REQUIRED COMPONENTS Crypto) + +target_include_directories(ego_camera_core + INTERFACE + $ +) + +target_link_libraries(ego_camera_core + PUBLIC + isaacteleop_schema + PRIVATE + nlohmann_json::nlohmann_json + OpenSSL::Crypto + ${CMAKE_DL_LIBS} + ob::OrbbecSDK + ego::mcap_support + log_bridge::log_bridge_core + oxr::oxr_core + pusherio::pusherio + Teleop::plugin_utils +) + +if(EGO_ENABLE_PREVIEW) + target_compile_definitions(ego_camera_core PRIVATE EGO_ENABLE_PREVIEW=1) + target_link_libraries(ego_camera_core PRIVATE PkgConfig::EGO_PREVIEW) +endif() + +add_library(ego::camera_core ALIAS ego_camera_core) diff --git a/src/plugins/ego/core/calibration.cpp b/src/plugins/ego/core/calibration.cpp new file mode 100644 index 0000000000..67db5b095e --- /dev/null +++ b/src/plugins/ego/core/calibration.cpp @@ -0,0 +1,205 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "calibration.hpp" + +#include +#include +#include +#include +#include + +namespace plugins::ego +{ + +std::vector yaml_numbers(const std::string& text) +{ + std::vector numbers; + const char* cursor = text.c_str(); + while (*cursor != '\0') + { + char* end = nullptr; + const float value = std::strtof(cursor, &end); + if (end != cursor) + { + numbers.push_back(value); + cursor = end; + } + else + ++cursor; + } + return numbers; +} + +bool populate_stereo_calibration_from_yaml(const std::string& yaml, core::EgoCalibrationT& calibration) +{ + struct Camera + { + uint32_t width = 0; + uint32_t height = 0; + float fx = 0; + float fy = 0; + float cx = 0; + float cy = 0; + std::vector distortion; + std::vector rotation; + std::vector translation; + }; + std::vector cameras; + enum class Section + { + kNone, + kIntrinsics, + kDistortion, + kRotation, + }; + Section section = Section::kNone; + std::istringstream input(yaml); + std::string line; + while (std::getline(input, line)) + { + const auto first = line.find_first_not_of(" \t\r"); + if (first == std::string::npos) + continue; + std::string key = line.substr(first); + if (!key.empty() && key.back() == '\r') + key.pop_back(); + if (key.rfind("- id:", 0) == 0) + { + cameras.emplace_back(); + section = Section::kNone; + continue; + } + if (cameras.empty()) + continue; + auto& camera = cameras.back(); + if (key == "intrinsics:") + { + section = Section::kIntrinsics; + continue; + } + if (key == "distortion:") + { + section = Section::kDistortion; + continue; + } + if (key == "rotation:") + { + section = Section::kRotation; + continue; + } + if (key.rfind("translation:", 0) == 0) + { + camera.translation = yaml_numbers(key); + section = Section::kNone; + continue; + } + if (section == Section::kRotation && key.rfind("- [", 0) == 0) + { + const auto values = yaml_numbers(key); + camera.rotation.insert(camera.rotation.end(), values.begin(), values.end()); + continue; + } + const auto colon = key.find(':'); + if (colon == std::string::npos) + continue; + const auto values = yaml_numbers(key.substr(colon + 1)); + if (values.empty()) + continue; + if (key.rfind("image_width:", 0) == 0) + camera.width = static_cast(values.front()); + else if (key.rfind("image_height:", 0) == 0) + camera.height = static_cast(values.front()); + else if (section == Section::kIntrinsics) + { + if (key.rfind("fx:", 0) == 0) + camera.fx = values.front(); + else if (key.rfind("fy:", 0) == 0) + camera.fy = values.front(); + else if (key.rfind("cx:", 0) == 0) + camera.cx = values.front(); + else if (key.rfind("cy:", 0) == 0) + camera.cy = values.front(); + } + else if (section == Section::kDistortion && (key.rfind("k", 0) == 0 || key.rfind("p", 0) == 0)) + camera.distortion.push_back(values.front()); + } + if (cameras.size() < 2 || cameras[0].width == 0 || cameras[1].width == 0 || cameras[0].fx == 0 || cameras[1].fx == 0) + return false; + const auto intrinsics = [](const Camera& camera) + { + auto result = std::make_shared(); + result->width = camera.width; + result->height = camera.height; + result->fx = camera.fx; + result->fy = camera.fy; + result->cx = camera.cx; + result->cy = camera.cy; + result->distortion_model = OB_DISTORTION_KANNALA_BRANDT4; + result->distortion = camera.distortion; + return result; + }; + calibration.color_left = intrinsics(cameras[0]); + calibration.color_right = intrinsics(cameras[1]); + if (cameras[1].rotation.size() == 9 && cameras[1].translation.size() == 3) + { + calibration.left_to_right = std::make_shared(); + calibration.left_to_right->rotation = cameras[1].rotation; + calibration.left_to_right->translation_mm = cameras[1].translation; + } + return true; +} + +std::string raw_data(const std::shared_ptr& device, OBPropertyID id) +{ + std::vector result; + bool failed = false; + device->getRawData(id, + [&result, &failed](OBDataTranState state, OBDataChunk* chunk) + { + if (state == DATA_TRAN_STAT_TRANSFERRING && chunk && chunk->data && chunk->size) + { + if (result.size() < chunk->fullDataSize) + result.resize(chunk->fullDataSize); + std::copy(chunk->data, chunk->data + chunk->size, result.begin() + chunk->offset); + } + else if (state < 0) + failed = true; + }); + if (failed) + throw std::runtime_error("Ego raw calibration transfer failed"); + return std::string(result.begin(), result.end()); +} + +std::string json_escape(const std::string& input) +{ + std::string output; + output.reserve(input.size()); + for (const char character : input) + { + switch (character) + { + case '\\': + output += "\\\\"; + break; + case '"': + output += "\\\""; + break; + case '\n': + output += "\\n"; + break; + case '\r': + output += "\\r"; + break; + case '\t': + output += "\\t"; + break; + default: + output += character; + break; + } + } + return output; +} + +} // namespace plugins::ego diff --git a/src/plugins/ego/core/calibration.hpp b/src/plugins/ego/core/calibration.hpp new file mode 100644 index 0000000000..e6a191ee4b --- /dev/null +++ b/src/plugins/ego/core/calibration.hpp @@ -0,0 +1,18 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "inc/ego_camera/ego_camera.hpp" + +#include + +#include +#include + +namespace plugins::ego +{ +bool populate_stereo_calibration_from_yaml(const std::string& yaml, core::EgoCalibrationT& calibration); +std::string raw_data(const std::shared_ptr& device, OBPropertyID id); +std::string json_escape(const std::string& input); +} // namespace plugins::ego diff --git a/src/plugins/ego/core/device_controls.cpp b/src/plugins/ego/core/device_controls.cpp new file mode 100644 index 0000000000..867bd87a9a --- /dev/null +++ b/src/plugins/ego/core/device_controls.cpp @@ -0,0 +1,215 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "device_controls.hpp" + +#include +#include +#include +#include +#include +#include + +namespace plugins::ego +{ + +OBPropertyItem find_property(const std::shared_ptr& device, const std::string& name) +{ + for (int index = 0; index < device->getSupportedPropertyCount(); ++index) + { + const auto item = device->getSupportedProperty(static_cast(index)); + if (name == item.name) + return item; + } + throw std::invalid_argument("Unsupported Ego property: " + name); +} + +double read_property(const std::shared_ptr& device, const OBPropertyItem& item) +{ + switch (item.type) + { + case OB_BOOL_PROPERTY: + return device->getBoolProperty(item.id) ? 1.0 : 0.0; + case OB_INT_PROPERTY: + return device->getIntProperty(item.id); + case OB_FLOAT_PROPERTY: + return device->getFloatProperty(item.id); + default: + throw std::invalid_argument(std::string(item.name) + " is not a scalar property"); + } +} + +void validate_property_value(const std::shared_ptr& device, + const OBPropertyItem& item, + double value, + bool validate_step) +{ + if ((item.permission & OB_PERMISSION_WRITE) == 0) + throw std::invalid_argument(std::string(item.name) + " is read-only"); + if (!std::isfinite(value)) + throw std::out_of_range(std::string(item.name) + " requires a finite value"); + switch (item.type) + { + case OB_BOOL_PROPERTY: + if (value != 0.0 && value != 1.0) + throw std::out_of_range(std::string(item.name) + " accepts only 0 or 1"); + break; + case OB_INT_PROPERTY: + { + const auto range = device->getIntPropertyRange(item.id); + if (value < std::numeric_limits::min() || value > std::numeric_limits::max()) + throw std::out_of_range(std::string(item.name) + " value is outside the int32 range"); + const auto integer = static_cast(value); + if (validate_step && range.max > range.min && range.step > range.max - range.min) + { + throw std::runtime_error(std::string(item.name) + + " reports an invalid SDK range/step; refusing to change a property that " + "cannot be restored safely"); + } + if (value != integer || integer < range.min || integer > range.max || + (validate_step && range.step > 0 && (integer - range.min) % range.step != 0)) + { + throw std::out_of_range(std::string(item.name) + " requested=" + std::to_string(value) + " range=[" + + std::to_string(range.min) + "," + std::to_string(range.max) + + "] step=" + std::to_string(range.step)); + } + break; + } + case OB_FLOAT_PROPERTY: + { + const auto range = device->getFloatPropertyRange(item.id); + const double tolerance = + std::max(std::max(1.0, std::abs(value)) * 1e-6, static_cast(std::abs(range.step)) * 1e-6); + const double steps = range.step > 0 ? (value - range.min) / range.step : 0.0; + const double nearest = range.min + std::round(steps) * range.step; + if (value < range.min - tolerance || value > range.max + tolerance || + (validate_step && range.step > 0 && std::abs(value - nearest) > tolerance)) + { + throw std::out_of_range(std::string(item.name) + " requested=" + std::to_string(value) + " range=[" + + std::to_string(range.min) + "," + std::to_string(range.max) + + "] step=" + std::to_string(range.step)); + } + break; + } + default: + throw std::invalid_argument(std::string(item.name) + " is not a scalar property"); + } +} + +void write_property(const std::shared_ptr& device, const OBPropertyItem& item, double value, bool validate_step) +{ + validate_property_value(device, item, value, validate_step); + switch (item.type) + { + case OB_BOOL_PROPERTY: + device->setBoolProperty(item.id, value != 0.0); + break; + case OB_INT_PROPERTY: + device->setIntProperty(item.id, static_cast(value)); + break; + case OB_FLOAT_PROPERTY: + device->setFloatProperty(item.id, static_cast(value)); + break; + default: + throw std::invalid_argument(std::string(item.name) + " is not a scalar property"); + } +} + +void verify_property_readback(const std::shared_ptr& device, + const OBPropertyItem& item, + double requested, + std::string_view operation) +{ + const double actual = read_property(device, item); + const double tolerance = item.type == OB_FLOAT_PROPERTY ? std::max(1.0, std::abs(requested)) * 1e-6 : 0.0; + if (std::abs(actual - requested) > tolerance) + { + throw std::runtime_error(std::string(item.name) + " " + std::string(operation) + " readback=" + + std::to_string(actual) + " differs from requested=" + std::to_string(requested)); + } +} + +void print_capabilities(const std::shared_ptr& device) +{ + const auto info = device->getDeviceInfo(); + std::cout << "SDK version=" << ob::Version::getMajor() << "." << ob::Version::getMinor() << "." + << ob::Version::getPatch() << " full=" << ob::Version::getVersion() + << " stage=" << ob::Version::getStageVersion() << std::endl; + std::cout << info->getName() << " uid=" << info->getUid() << " serial=" << info->getSerialNumber() + << " firmware=" << info->getFirmwareVersion() << " vid=0x" << std::hex << info->getVid() << " pid=0x" + << info->getPid() << std::dec << " usb=" << info->getConnectionType() + << " global_timestamp_supported=" << std::boolalpha << device->isGlobalTimestampSupported() << std::endl; + std::cout << "Profiles are per-sensor advertisements; capture validates the exact simultaneous combination." + << std::endl; + std::cout << "Certification policy: encoded profiles above 30 FPS are advertised but rejected by this build; " + "sustained recording validation must pass before they are enabled." + << std::endl; + const auto sensors = device->getSensorList(); + for (uint32_t sensor_index = 0; sensor_index < sensors->getCount(); ++sensor_index) + { + const auto sensor_type = sensors->getSensorType(sensor_index); + std::cout << "Sensor " << ob::TypeHelper::convertOBSensorTypeToString(sensor_type) << std::endl; + const auto profiles = device->getSensor(sensor_type)->getStreamProfileList(); + for (uint32_t profile_index = 0; profile_index < profiles->getCount(); ++profile_index) + { + const auto profile = profiles->getProfile(profile_index); + std::cout << " " << ob::TypeHelper::convertOBFormatTypeToString(profile->getFormat()); + if (profile->is()) + { + const auto video = profile->as(); + std::cout << " " << video->getWidth() << "x" << video->getHeight() << "@" << video->getFps(); + } + else if (profile->is()) + { + const auto imu = profile->as(); + std::cout << " rate=" << ob::TypeHelper::convertOBIMUSampleRateTypeToValue(imu->getSampleRate()) + << "Hz full_scale=" + << ob::TypeHelper::convertOBAccelFullScaleRangeTypeToString(imu->getFullScaleRange()); + } + else if (profile->is()) + { + const auto imu = profile->as(); + std::cout << " rate=" << ob::TypeHelper::convertOBIMUSampleRateTypeToValue(imu->getSampleRate()) + << "Hz full_scale=" + << ob::TypeHelper::convertOBGyroFullScaleRangeTypeToString(imu->getFullScaleRange()); + } + else if (profile->is()) + { + const auto audio = profile->as(); + std::cout << " " << audio->getSampleRate() << "Hz " << audio->getChannelCount() << "ch " + << audio->getBitsPerSample() << "bit"; + } + std::cout << std::endl; + } + } + std::cout << "Properties (ranges can depend on the most recently active video profile):" << std::endl; + for (int index = 0; index < device->getSupportedPropertyCount(); ++index) + { + const auto item = device->getSupportedProperty(static_cast(index)); + std::cout << " " << item.name << " id=" << item.id << " type=" << item.type << " permission=" << item.permission; + try + { + if (item.type == OB_INT_PROPERTY) + { + const auto range = device->getIntPropertyRange(item.id); + std::cout << " range=[" << range.min << "," << range.max << "] step=" << range.step; + } + else if (item.type == OB_FLOAT_PROPERTY) + { + const auto range = device->getFloatPropertyRange(item.id); + std::cout << " range=[" << range.min << "," << range.max << "] step=" << range.step; + } + if ((item.permission & OB_PERMISSION_READ) != 0 && + (item.type == OB_BOOL_PROPERTY || item.type == OB_INT_PROPERTY || item.type == OB_FLOAT_PROPERTY)) + { + std::cout << " value=" << read_property(device, item); + } + } + catch (const ob::Error&) + { + } + std::cout << std::endl; + } +} + +} // namespace plugins::ego diff --git a/src/plugins/ego/core/device_controls.hpp b/src/plugins/ego/core/device_controls.hpp new file mode 100644 index 0000000000..d06d5f9760 --- /dev/null +++ b/src/plugins/ego/core/device_controls.hpp @@ -0,0 +1,29 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include + +#include +#include +#include + +namespace plugins::ego +{ +OBPropertyItem find_property(const std::shared_ptr& device, const std::string& name); +double read_property(const std::shared_ptr& device, const OBPropertyItem& item); +void validate_property_value(const std::shared_ptr& device, + const OBPropertyItem& item, + double value, + bool validate_step = true); +void write_property(const std::shared_ptr& device, + const OBPropertyItem& item, + double value, + bool validate_step = true); +void verify_property_readback(const std::shared_ptr& device, + const OBPropertyItem& item, + double requested, + std::string_view operation); +void print_capabilities(const std::shared_ptr& device); +} // namespace plugins::ego diff --git a/src/plugins/ego/core/device_diagnostics.cpp b/src/plugins/ego/core/device_diagnostics.cpp new file mode 100644 index 0000000000..339be070f6 --- /dev/null +++ b/src/plugins/ego/core/device_diagnostics.cpp @@ -0,0 +1,70 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "device_diagnostics.hpp" + +#include + +#include + +namespace plugins::ego +{ +namespace +{ +template +core::EgoDeviceStateT event(const DiagnosticOrigin& origin, Payload payload) +{ + core::EgoDeviceStateT value; + value.diagnostic_version = core::EGO_DIAGNOSTIC_VERSION; + value.observed_at_ns = origin.observed_at_ns; + value.device_uid = origin.uid; + value.capture_epoch = origin.epoch; + value.event.Set(std::move(payload)); + return value; +} +} // namespace + +core::EgoDeviceStateT sdk_report(const DiagnosticOrigin& origin, + uint64_t sequence, + int32_t work_mode, + uint64_t status_flags, + uint64_t error_flags, + uint64_t storage_free_bytes) +{ + core::EgoSdkReportT payload; + payload.sequence_number = sequence; + payload.work_mode = work_mode; + payload.status_flags = status_flags; + payload.error_flags = error_flags; + payload.storage_free_bytes = storage_free_bytes; + return event(origin, std::move(payload)); +} + +core::EgoDeviceStateT capture_summary(const DiagnosticOrigin& origin, core::EgoCaptureSummaryT summary) +{ + return event(origin, std::move(summary)); +} + +core::EgoDeviceStateT recovery_event(const DiagnosticOrigin& origin, + core::EgoRecoveryStage stage, + const std::string& reason, + uint32_t reconnect_attempt) +{ + core::EgoRecoveryEventT payload; + payload.stage = stage; + payload.reason = reason; + payload.reconnect_attempt = reconnect_attempt; + return event(origin, std::move(payload)); +} + +core::EgoDeviceStateT device_snapshot(const DiagnosticOrigin& origin, + std::optional temperature, + const std::vector& properties) +{ + core::EgoDeviceSnapshotT payload; + if (temperature && std::isfinite(*temperature)) + payload.imu_temperature_c = *temperature; + payload.properties = properties; + return event(origin, std::move(payload)); +} +} // namespace plugins::ego diff --git a/src/plugins/ego/core/device_diagnostics.hpp b/src/plugins/ego/core/device_diagnostics.hpp new file mode 100644 index 0000000000..ae900e75fc --- /dev/null +++ b/src/plugins/ego/core/device_diagnostics.hpp @@ -0,0 +1,34 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include + +#include +#include +#include + +namespace plugins::ego +{ +struct DiagnosticOrigin +{ + std::string uid; + uint32_t epoch; + int64_t observed_at_ns; +}; +core::EgoDeviceStateT sdk_report(const DiagnosticOrigin&, + uint64_t sequence, + int32_t work_mode, + uint64_t status_flags, + uint64_t error_flags, + uint64_t storage_free_bytes); +core::EgoDeviceStateT capture_summary(const DiagnosticOrigin&, core::EgoCaptureSummaryT); +core::EgoDeviceStateT recovery_event(const DiagnosticOrigin&, + core::EgoRecoveryStage, + const std::string& reason, + uint32_t reconnect_attempt); +core::EgoDeviceStateT device_snapshot(const DiagnosticOrigin&, + std::optional temperature, + const std::vector& properties); +} // namespace plugins::ego diff --git a/src/plugins/ego/core/diagnostics.hpp b/src/plugins/ego/core/diagnostics.hpp new file mode 100644 index 0000000000..2a1f75e999 --- /dev/null +++ b/src/plugins/ego/core/diagnostics.hpp @@ -0,0 +1,60 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 ORBBEC INC. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include + +#include +#include + +namespace plugins::ego::detail +{ +class DiagnosticLine +{ +public: + explicit DiagnosticLine(spdlog::level::level_enum level) : level_(level) + { + } + ~DiagnosticLine() noexcept + { + try + { + auto message = stream_.str(); + while (!message.empty() && (message.back() == '\n' || message.back() == '\r')) + message.pop_back(); + isaaccapture::Logger::get("isaaccapture.plugins.ego.capture")->log(level_, "{}", message); + } + catch (...) + { + } + } + template + DiagnosticLine& operator<<(T&& value) + { + stream_ << std::forward(value); + return *this; + } + DiagnosticLine& operator<<(std::ostream& (*manipulator)(std::ostream&)) + { + manipulator(stream_); + return *this; + } + +private: + spdlog::level::level_enum level_; + std::ostringstream stream_; +}; +inline DiagnosticLine log_info() +{ + return DiagnosticLine(spdlog::level::info); +} +inline DiagnosticLine log_error() +{ + return DiagnosticLine(spdlog::level::err); +} +inline DiagnosticLine log_warning() +{ + return DiagnosticLine(spdlog::level::warn); +} +} // namespace plugins::ego::detail diff --git a/src/plugins/ego/core/ego_camera.cpp b/src/plugins/ego/core/ego_camera.cpp new file mode 100644 index 0000000000..5d4dbbd311 --- /dev/null +++ b/src/plugins/ego/core/ego_camera.cpp @@ -0,0 +1,2594 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#if defined(EGO_ENABLE_PREVIEW) +# include "preview.hpp" +#endif + +#include "calibration.hpp" +#include "device_controls.hpp" +#include "device_diagnostics.hpp" +#include "diagnostics.hpp" +#include "inc/ego_camera/cancellation.hpp" +#include "inc/ego_camera/ego_camera.hpp" +#include "inc/ego_camera/time_alignment.hpp" +#include "wav_writer.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace plugins::ego +{ + +namespace +{ + +// SDK 2.9 builds place EGO callbacks on Device or DataAcquisitionDevice. +// Keep handle adoption and destruction matched to the selected SDK headers. +#if __has_include() +using EgoSdkDevice = ob::DataAcquisitionDevice; +#else +using EgoSdkDevice = ob::Device; +#endif + +std::shared_ptr adopt_ego_device(std::shared_ptr device) +{ +#if __has_include() + return EgoSdkDevice::fromDevice(std::move(device)); +#else + return device; +#endif +} + +const char* prepare_sdk_logging() +{ + static std::once_flag once; + std::call_once(once, + [] + { + const auto logger = isaaccapture::Logger::get("isaaccapture.plugins.ego.sdk"); + // Global severity resets every SDK sink; disable native sinks after it. + ob::Context::setLoggerSeverity(OB_LOG_SEVERITY_DEBUG); + ob::Context::setLoggerToConsole(OB_LOG_SEVERITY_NONE); + ob::Context::setLoggerToFile(OB_LOG_SEVERITY_NONE, ""); + ob::Context::setLoggerToCallback( + OB_LOG_SEVERITY_DEBUG, + [logger](OBLogSeverity severity, const char* message) + { + const auto level = severity == OB_LOG_SEVERITY_DEBUG ? spdlog::level::debug : + severity == OB_LOG_SEVERITY_INFO ? spdlog::level::info : + severity == OB_LOG_SEVERITY_WARN ? spdlog::level::warn : + severity == OB_LOG_SEVERITY_FATAL ? spdlog::level::critical : + spdlog::level::err; + // Vendor callbacks must not unwind into the SDK. + try + { + std::string line = message ? message : ""; + while (!line.empty() && (line.back() == '\n' || line.back() == '\r')) + line.pop_back(); + logger->log(level, "{}", line); + } + catch (...) + { + } + }); + }); + return ""; +} + +constexpr size_t kMaxQueuedEvents = 4096; +constexpr size_t kMaxQueuedVideoFrameSets = 256; + +class RecoverableCaptureError : public std::runtime_error +{ +public: + using std::runtime_error::runtime_error; +}; + +class FatalReconnectError : public std::runtime_error +{ +public: + using std::runtime_error::runtime_error; +}; + +OBIMUSampleRate imu_rate(uint32_t rate) +{ + if (rate == 400) + return OB_SAMPLE_RATE_400_HZ; + if (rate == 1000) + return OB_SAMPLE_RATE_1_KHZ; + throw std::invalid_argument("EGO IMU rate must be 400 or 1000 Hz"); +} + +OBAccelFullScaleRange accel_scale(float scale) +{ + if (scale == 2) + return OB_ACCEL_FS_2g; + if (scale == 3) + return OB_ACCEL_FS_3g; + if (scale == 4) + return OB_ACCEL_FS_4g; + if (scale == 6) + return OB_ACCEL_FS_6g; + if (scale == 8) + return OB_ACCEL_FS_8g; + if (scale == 12) + return OB_ACCEL_FS_12g; + if (scale == 16) + return OB_ACCEL_FS_16g; + if (scale == 24) + return OB_ACCEL_FS_24g; + throw std::invalid_argument("Unsupported Ego accelerometer full scale"); +} + +OBGyroFullScaleRange gyro_scale(float scale) +{ + if (scale == 16) + return OB_GYRO_FS_16dps; + if (scale == 31) + return OB_GYRO_FS_31dps; + if (scale == 62) + return OB_GYRO_FS_62dps; + if (scale == 125) + return OB_GYRO_FS_125dps; + if (scale == 250) + return OB_GYRO_FS_250dps; + if (scale == 400) + return OB_GYRO_FS_400dps; + if (scale == 500) + return OB_GYRO_FS_500dps; + if (scale == 800) + return OB_GYRO_FS_800dps; + if (scale == 1000) + return OB_GYRO_FS_1000dps; + if (scale == 2000) + return OB_GYRO_FS_2000dps; + throw std::invalid_argument("Unsupported Ego gyroscope full scale"); +} + +OBFormat to_ob_format(core::EgoPixelFormat format) +{ + switch (format) + { + case core::EgoPixelFormat_Mjpg: + return OB_FORMAT_MJPG; + case core::EgoPixelFormat_H264: + return OB_FORMAT_H264; + case core::EgoPixelFormat_H265: + return OB_FORMAT_H265; + default: + throw std::invalid_argument("Unsupported Ego pixel format"); + } +} + +OBFrameType to_ob_frame(core::EgoCameraStream stream) +{ + switch (stream) + { + case core::EgoCameraStream_ColorLeft: + return OB_FRAME_COLOR_LEFT; + case core::EgoCameraStream_ColorRight: + return OB_FRAME_COLOR_RIGHT; + default: + throw std::invalid_argument("Unsupported Ego camera stream"); + } +} + +OBSensorType to_ob_sensor(core::EgoCameraStream stream) +{ + switch (stream) + { + case core::EgoCameraStream_ColorLeft: + return OB_SENSOR_COLOR_LEFT; + case core::EgoCameraStream_ColorRight: + return OB_SENSOR_COLOR_RIGHT; + default: + throw std::invalid_argument("Unsupported Ego camera stream"); + } +} + +bool has_sensor(const std::shared_ptr& device, OBSensorType sensor) +{ + const auto sensors = device->getSensorList(); + for (uint32_t index = 0; index < sensors->getCount(); ++index) + { + if (sensors->getSensorType(index) == sensor) + return true; + } + return false; +} + +void print_profiles(const std::shared_ptr& device, core::EgoCameraStream stream) +{ + const auto profiles = device->getSensor(to_ob_sensor(stream))->getStreamProfileList(); + std::ostringstream profiles_message; + profiles_message << "Available " << core::EnumNameEgoCameraStream(stream) << " profiles:"; + for (uint32_t index = 0; index < profiles->getCount(); ++index) + { + const auto profile = profiles->getProfile(index); + if (!profile->is()) + continue; + const auto video = profile->as(); + profiles_message << " " << video->getWidth() << "x" << video->getHeight() << "@" << video->getFps() << " " + << ob::TypeHelper::convertOBFormatTypeToString(video->getFormat()); + } + detail::log_warning() << profiles_message.str(); +} + +std::shared_ptr select_profile(const std::shared_ptr& device, + const StreamConfig& stream, + const CaptureConfig& config) +{ + validate_stream_config(stream, config); + const auto width = stream.width != 0 ? stream.width : config.width; + const auto height = stream.height != 0 ? stream.height : config.height; + const auto fps = stream.fps != 0 ? stream.fps : config.fps; + const auto format = to_ob_format(stream.pixel_format); + try + { + const auto profile = device->getSensor(to_ob_sensor(stream.camera)) + ->getStreamProfileList() + ->getVideoStreamProfile( + static_cast(width), static_cast(height), format, static_cast(fps)); + detail::log_info() << "Selected " << core::EnumNameEgoCameraStream(stream.camera) + << " profile: " << profile->getWidth() << "x" << profile->getHeight() << "@" + << profile->getFps() << " " << core::EnumNameEgoPixelFormat(stream.pixel_format) << std::endl; + return profile; + } + catch (const ob::Error& error) + { + print_profiles(device, stream.camera); + throw std::runtime_error("No matching Ego profile for " + + std::string(core::EnumNameEgoCameraStream(stream.camera)) + ": " + error.what()); + } +} + +using ActiveProfiles = std::map>; + +std::shared_ptr make_video_config(const ActiveProfiles& profiles, const std::vector& streams) +{ + auto config = std::make_shared(); + for (const auto& [_, profile] : profiles) + config->enableStream(profile); + const bool has_interframe_video = std::any_of(streams.begin(), streams.end(), + [](const StreamConfig& stream) { + return stream.pixel_format == core::EgoPixelFormat_H264 || + stream.pixel_format == core::EgoPixelFormat_H265; + }); + // Single-eye capture must not wait for an unrequested color frame. + // MJPG capture must retain an available eye even when its stereo partner is absent. + config->setFrameAggregateOutputMode(profiles.size() == 1 ? OB_FRAME_AGGREGATE_OUTPUT_ANY_SITUATION : + has_interframe_video ? OB_FRAME_AGGREGATE_OUTPUT_COLOR_FRAME_REQUIRE : + OB_FRAME_AGGREGATE_OUTPUT_ANY_SITUATION); + return config; +} + +std::string describe_profiles(const ActiveProfiles& profiles) +{ + std::ostringstream output; + bool first = true; + for (const auto& [stream, profile] : profiles) + { + if (!first) + output << ", "; + first = false; + output << core::EnumNameEgoCameraStream(stream) << "=" << profile->getWidth() << "x" << profile->getHeight() + << "@" << profile->getFps() << " " << ob::TypeHelper::convertOBFormatTypeToString(profile->getFormat()); + } + return output.str(); +} + +} // namespace + +class EgoCamera::Impl +{ +public: + Impl(const CaptureConfig& config, const std::vector& streams, std::unique_ptr sink) + : config_(config), + time_alignment_(config.time_alignment), + context_(std::make_unique(prepare_sdk_logging())), + streams_(streams), + sink_(std::move(sink)) + { +#if !defined(EGO_ENABLE_PREVIEW) + if (config_.preview) + { + throw std::runtime_error( + "SDL preview is not available in this build. Reconfigure with -DEGO_ENABLE_PREVIEW=ON."); + } +#endif +#if defined(__linux__) || defined(__ANDROID__) + context_->setUvcBackendType(OB_UVC_BACKEND_TYPE_LIBUVC); +#endif + publish_capture_status(CaptureAvailability::Unknown); + check_cancelled(); + const auto devices = context_->queryDeviceList(); + if (devices->getCount() == 0) + { + publish_capture_status(CaptureAvailability::Absent); + throw std::runtime_error("No Ego devices found. Check USB connection and udev permissions."); + } + + for (uint32_t index = 0; index < devices->getCount(); ++index) + { + check_cancelled(); + const auto candidate = devices->getDevice(index); + if (!config.device_uid.empty() && candidate->getDeviceInfo()->getUid() != config.device_uid) + continue; + bool supports_requested_streams = true; + for (const auto& stream : streams_) + supports_requested_streams = + supports_requested_streams && has_sensor(candidate, to_ob_sensor(stream.camera)); + if (supports_requested_streams) + { + // Adopt the EGO callback owner before pipelines; newer SDK + // wrappers transfer the base device handle. + device_ = adopt_ego_device(candidate); + break; + } + } + if (!device_) + throw std::runtime_error("No Ego device matches the requested UID and ColorLeft/ColorRight sensors."); + const auto selected_info = device_->getDeviceInfo(); + selected_device_uid_ = selected_info->getUid(); + selected_device_serial_ = selected_info->getSerialNumber(); + selected_device_vid_ = selected_info->getVid(); + selected_device_pid_ = selected_info->getPid(); + selected_firmware_ = selected_info->getFirmwareVersion(); + publish_capture_status(CaptureAvailability::Starting); + + for (const auto& stream : streams_) + { + const auto profile = select_profile(device_, stream, config); + active_profiles_.emplace(stream.camera, profile); + } + initial_profile_description_ = describe_profiles(active_profiles_); + configure_time_alignment(); + try + { + for (const auto& stream : streams_) + validate_resolved_stream_config(stream, active_profiles_.at(stream.camera)->getFps()); + } + catch (const std::exception& error) + { + throw std::runtime_error("Ego uid=" + selected_device_uid_ + " firmware=" + selected_firmware_ + + " resolved profiles [" + describe_profiles(active_profiles_) + + "] are not certified: " + error.what()); + } + const auto pipeline_config = make_video_config(active_profiles_, streams_); + + std::vector settings = config.properties; + if (config.bitrate != 0) + settings.push_back({ "OB_PROP_COLOR_BITRATE_INT", static_cast(config.bitrate) }); + if (config.dynamic_bitrate_set) + settings.push_back({ "OB_PROP_COLOR_DYNAMIC_BITRATE_ENABLE_BOOL", config.dynamic_bitrate ? 1.0 : 0.0 }); + struct CaptureReadiness + { + std::mutex mutex; + std::condition_variable wake; + std::vector seen; + std::vector active_seen; + std::string error; + std::atomic complete{ false }; + std::atomic active_complete{ false }; + }; + struct ExpectedFrame + { + OBFrameType frame_type; + uint32_t width; + uint32_t height; + uint32_t fps; + OBFormat format; + std::string name; + }; + std::vector expected_frames; + expected_frames.reserve(active_profiles_.size()); + for (const auto& [stream, profile] : active_profiles_) + { + expected_frames.push_back({ to_ob_frame(stream), profile->getWidth(), profile->getHeight(), + profile->getFps(), profile->getFormat(), core::EnumNameEgoCameraStream(stream) }); + } + auto readiness = std::make_shared(); + readiness->seen.resize(expected_frames.size()); + readiness->active_seen.resize(expected_frames.size()); + const auto wait_for_video_readiness = [this, &readiness, &expected_frames] + { + std::string readiness_error; + { + std::unique_lock lock(readiness->mutex); + const bool ready = wait_capture_ready( + readiness->wake, lock, std::chrono::seconds(10), + [&readiness] + { return readiness->complete.load(std::memory_order_acquire) || !readiness->error.empty(); }, + config_.stop_requested); + if (!ready) + { + readiness_error = "timed out waiting for"; + for (size_t index = 0; index < expected_frames.size(); ++index) + { + if (!readiness->seen[index]) + readiness_error += " " + expected_frames[index].name; + } + } + else + readiness_error = readiness->error; + } + if (!readiness_error.empty()) + { + throw std::runtime_error("Ego uid=" + selected_device_uid_ + " firmware=" + selected_firmware_ + + " active profiles [" + describe_profiles(active_profiles_) + + "] failed readiness: " + readiness_error); + } + }; + const auto wait_for_active_video_readiness = [this, &readiness, &expected_frames] + { + std::string readiness_error; + { + std::unique_lock lock(readiness->mutex); + const bool ready = wait_capture_ready( + readiness->wake, lock, std::chrono::seconds(10), + [&readiness] + { return readiness->active_complete.load(std::memory_order_acquire) || !readiness->error.empty(); }, + config_.stop_requested); + if (!ready) + { + readiness_error = "timed out waiting for post-boundary"; + for (size_t index = 0; index < expected_frames.size(); ++index) + { + if (!readiness->active_seen[index]) + readiness_error += " " + expected_frames[index].name; + } + } + else + readiness_error = readiness->error; + } + if (!readiness_error.empty()) + { + throw std::runtime_error("Ego uid=" + selected_device_uid_ + " firmware=" + selected_firmware_ + + " active profiles [" + describe_profiles(active_profiles_) + + "] failed readiness: " + readiness_error); + } + }; + try + { + if (!settings.empty()) + { + try + { + prepare_controls(settings); + } + catch (const std::exception& error) + { + throw std::runtime_error("Ego uid=" + selected_device_uid_ + " firmware=" + selected_firmware_ + + " controls for profiles [" + describe_profiles(active_profiles_) + + "] failed: " + error.what()); + } + } + capture_device_snapshot(); + pipeline_ = std::make_unique(device_); +#if defined(EGO_ENABLE_PREVIEW) + if (config_.preview) + preview_ = std::make_unique(); +#endif + publish_calibration(pipeline_config); + start_device_state(); + if (config_.enable_imu) + start_imu(); + if (config_.enable_audio || !config_.audio_output.empty()) + start_audio(); + wait_for_auxiliary_readiness(); + try + { + pipeline_->start( + pipeline_config, + [this, readiness, expected_frames](std::shared_ptr frame_set) + { + if (!frame_set || !accepting_callbacks_.load(std::memory_order_acquire)) + return; + const bool active_capture = capture_active_.load(std::memory_order_acquire); + std::vector observed(expected_frames.size()); + std::string observation_error; + if (!readiness->complete.load(std::memory_order_acquire) || + (active_capture && !readiness->active_complete.load(std::memory_order_acquire))) + { + try + { + for (size_t index = 0; index < expected_frames.size(); ++index) + { + const auto& expected = expected_frames[index]; + const auto raw_frame = frame_set->getFrame(expected.frame_type); + if (!raw_frame) + continue; + const auto frame = raw_frame->as(); + const auto profile = + frame ? frame->getStreamProfile()->as() : nullptr; + if (!frame || !profile || frame->getWidth() != expected.width || + frame->getHeight() != expected.height || + frame->getFormat() != expected.format || profile->getFps() != expected.fps) + { + observation_error = + "SDK returned a different active profile for " + expected.name; + if (frame && profile) + { + observation_error += + ": actual=" + std::to_string(frame->getWidth()) + "x" + + std::to_string(frame->getHeight()) + "@" + + std::to_string(profile->getFps()) + " " + + ob::TypeHelper::convertOBFormatTypeToString(frame->getFormat()); + } + break; + } + observed[index] = true; + } + } + catch (const std::exception& error) + { + observation_error = error.what(); + } + catch (...) + { + observation_error = "unknown error while inspecting a frameset"; + } + } + if (!observation_error.empty()) + { + std::lock_guard lock(readiness->mutex); + readiness->error = std::move(observation_error); + readiness->wake.notify_one(); + return; + } + const auto report_observed = [&readiness, &observed](bool active) + { + auto& complete = active ? readiness->active_complete : readiness->complete; + auto& seen = active ? readiness->active_seen : readiness->seen; + if (complete.load(std::memory_order_relaxed)) + return; + std::lock_guard lock(readiness->mutex); + for (size_t index = 0; index < observed.size(); ++index) + seen[index] = seen[index] || observed[index]; + if (std::all_of(seen.begin(), seen.end(), [](bool value) { return value; })) + { + complete.store(true, std::memory_order_release); + readiness->wake.notify_one(); + } + }; + if (!active_capture) + { + report_observed(false); + return; + } + const int64_t arrival_time_local_common_clock_ns = core::os_monotonic_now_ns(); + { + std::lock_guard lock(video_queue_mutex_); + if (!accepting_callbacks_.load(std::memory_order_relaxed)) + return; + if (video_frame_sets_.size() >= kMaxQueuedVideoFrameSets) + { + ++auxiliary_stats_.dropped_video_frame_sets; + { + std::lock_guard readiness_lock(readiness->mutex); + readiness->error = "video callback queue filled before capture became ready"; + } + readiness->wake.notify_one(); + set_async_error("Ego video callback queue is full; capture stopped to avoid silent loss"); + return; + } + video_frame_sets_.emplace_back(std::move(frame_set), arrival_time_local_common_clock_ns); + } + report_observed(true); + video_queue_cv_.notify_one(); + }); + video_pipeline_started_ = true; + } + catch (const ob::Error& error) + { + throw std::runtime_error("Ego uid=" + selected_device_uid_ + " firmware=" + selected_firmware_ + + " rejected simultaneous profiles [" + describe_profiles(active_profiles_) + + "]: " + error.what()); + } + wait_for_video_readiness(); + // SDK stream startup can reset image controls; reapply before capture begins. + reapply_controls("startup"); + verify_active_controls(); + capture_active_.store(true, std::memory_order_release); + capture_epoch_started_ns_ = core::os_monotonic_now_ns(); + publish_periodic_device_state(); + wait_for_active_video_readiness(); + wait_for_active_auxiliary_readiness(); + device_changed_callback_id_ = context_->registerDeviceChangedCallback( + [this](const std::shared_ptr& removed, const std::shared_ptr&) + { + if (device_list_contains_uid(removed, selected_device_uid_)) + device_removed_.store(true, std::memory_order_release); + }); + device_changed_callback_registered_ = true; + publish_capture_status(CaptureAvailability::Streaming); + detail::log_info() << "Ego pipeline started for " << selected_device_uid_ << std::endl; + } + catch (...) + { + try + { + shutdown(false); + } + catch (const std::exception& error) + { + detail::log_error() << "Ego cleanup after startup error failed: " << error.what() << std::endl; + } + throw; + } + } + + ~Impl() + { + shutdown_noexcept(false); + } + + static bool is_removed_device_cleanup_error(const ob::Error& error, bool device_removal_confirmed) noexcept + { + // SDK 2.9 reports deactivated components as call-sequence errors. Only + // the removed capture instance may tolerate this during reconnect teardown. + return device_removal_confirmed && error.getStatus() == OB_ERROR_WRONG_API_CALL_SEQUENCE && + error.getExceptionType() == OB_EXCEPTION_TYPE_WRONG_API_CALL_SEQUENCE && error.what() && + std::string_view(error.what()).find("Device is deactivated/disconnected! DeviceComponentId:") == 0; + } + + static std::string stop_pipeline_noexcept(std::unique_ptr& pipeline, + bool& started, + const char* name, + bool device_removal_confirmed = false) noexcept + { + if (!pipeline) + return {}; + const auto stop_started = std::chrono::steady_clock::now(); + std::string failure; + if (started) + { + try + { + pipeline->stop(); + } + catch (const ob::Error& error) + { + if (is_removed_device_cleanup_error(error, device_removal_confirmed)) + detail::log_info() + << "Ego " << name << " pipeline already inactive after confirmed removal: " << error.what(); + else + failure = std::string("Ego ") + name + " pipeline stop failed: " + error.what(); + } + catch (const std::exception& error) + { + failure = std::string("Ego ") + name + " pipeline stop failed: " + error.what(); + } + catch (...) + { + failure = std::string("Ego ") + name + " pipeline stop failed: unknown error"; + } + } + started = false; + pipeline.reset(); + detail::log_info() + << "Ego " << name << " pipeline stop completed in " + << std::chrono::duration(std::chrono::steady_clock::now() - stop_started).count() + << " ms"; + return failure; + } + + static bool device_list_contains_uid(const std::shared_ptr& devices, const std::string& uid) + { + if (!devices) + return false; + for (uint32_t index = 0; index < devices->getCount(); ++index) + { + try + { + // Read the removal snapshot without trying to open a disconnected device. + const auto* candidate_uid = devices->getUid(index); + if (candidate_uid && uid == candidate_uid) + return true; + } + catch (const std::exception&) + { + } + } + return false; + } + + bool is_selected_physical_device(const std::shared_ptr& info) const + { + if (!info) + return false; + // SDK 2.9.0 EGO UIDs include the USB enumeration address and can change after a physical reconnect. + if (!selected_device_serial_.empty()) + { + return info->getSerialNumber() == selected_device_serial_ && info->getVid() == selected_device_vid_ && + info->getPid() == selected_device_pid_; + } + return info->getUid() == selected_device_uid_; + } + + std::shared_ptr find_selected_device() + { + const auto devices = context_->queryDeviceList(); + for (uint32_t index = 0; index < devices->getCount(); ++index) + { + const auto candidate = devices->getDevice(index); + if (!is_selected_physical_device(candidate->getDeviceInfo())) + continue; + if (std::all_of(streams_.begin(), streams_.end(), + [&candidate](const StreamConfig& stream) + { return has_sensor(candidate, to_ob_sensor(stream.camera)); })) + { + return candidate; + } + } + return nullptr; + } + + void publish_capture_status(CaptureAvailability availability, const std::string& error = {}) noexcept + { + try + { + if (auto* metadata = sink_->metadata_sink()) + metadata->on_capture_status({ availability, core::os_monotonic_now_ns(), error }); + } + catch (const std::exception& failure) + { + detail::log_warning() << "Ego device status notification failed: " << failure.what(); + } + } + + DiagnosticOrigin diagnostic_origin(int64_t observed_at_ns) const + { + return { selected_device_uid_, capture_epoch_, observed_at_ns }; + } + + void publish_recovery_event(core::EgoRecoveryStage stage, const std::string& reason) + { + const auto now = core::os_monotonic_now_ns(); + auto state = recovery_event(diagnostic_origin(now), stage, reason, auxiliary_stats_.reconnect_attempts); + enqueue(DeviceStateEvent{ std::move(state), now }); + drain_events(); + } + + void reset_stream_readiness() + { + accel_ready_.store(false, std::memory_order_release); + gyro_ready_.store(false, std::memory_order_release); + audio_ready_.store(false, std::memory_order_release); + active_accel_ready_.store(false, std::memory_order_release); + active_gyro_ready_.store(false, std::memory_order_release); + active_audio_ready_.store(false, std::memory_order_release); + last_accel_arrival_ns_.store(0, std::memory_order_release); + last_gyro_arrival_ns_.store(0, std::memory_order_release); + last_audio_arrival_ns_.store(0, std::memory_order_release); + last_video_arrival_ns_.clear(); + } + + void stop_capture_for_reconnect(bool device_removal_confirmed = false) + { + accepting_callbacks_.store(false, std::memory_order_release); + capture_active_.store(false, std::memory_order_release); + try + { + if (device_) + device_->clearEgoStateCallback(); + } + catch (const ob::Error& error) + { + if (is_removed_device_cleanup_error(error, device_removal_confirmed)) + { + detail::log_info() + << "Ego device-state callback already inactive after confirmed removal: " << error.what(); + } + else + { + cleanup_error_ += + std::string("Ego device-state callback clear during reconnect failed: ") + error.what() + "; "; + detail::log_warning() << cleanup_error_; + } + } + catch (const std::exception& error) + { + cleanup_error_ += + std::string("Ego device-state callback clear during reconnect failed: ") + error.what() + "; "; + detail::log_warning() << cleanup_error_; + } + catch (...) + { + cleanup_error_ += "Ego device-state callback clear during reconnect failed: unknown error; "; + detail::log_warning() << cleanup_error_; + } + const auto report_stop_error = [this](const std::string& error) + { + if (!error.empty()) + { + cleanup_error_ += error + "; "; + detail::log_warning() << "Warning: " << error << std::endl; + } + }; + report_stop_error(stop_pipeline_noexcept(pipeline_, video_pipeline_started_, "video", device_removal_confirmed)); + if (audio_sensor_) + { + if (audio_started_) + { + try + { + const auto audio_stop_started = std::chrono::steady_clock::now(); + audio_sensor_->stop(); + detail::log_info() << "Ego audio stop completed in " + << std::chrono::duration(std::chrono::steady_clock::now() - + audio_stop_started) + .count() + << " ms"; + } + catch (const ob::Error& error) + { + if (is_removed_device_cleanup_error(error, device_removal_confirmed)) + { + detail::log_info() + << "Ego audio sensor already inactive after confirmed removal: " << error.what(); + } + else + { + cleanup_error_ += std::string("Ego audio stop during reconnect failed: ") + error.what() + "; "; + detail::log_warning() << cleanup_error_; + } + } + catch (const std::exception& error) + { + cleanup_error_ += std::string("Ego audio stop during reconnect failed: ") + error.what() + "; "; + detail::log_warning() << cleanup_error_; + } + } + audio_started_ = false; + audio_sensor_.reset(); + } + report_stop_error(stop_pipeline_noexcept(imu_pipeline_, imu_pipeline_started_, "IMU", device_removal_confirmed)); + drain_video_frames(false); + flush_imu(core::EgoImuSensor_Accel); + flush_imu(core::EgoImuSensor_Gyro); + drain_events(); + active_profiles_.clear(); + device_.reset(); + reset_stream_readiness(); + } + + void begin_recovery(const std::string& reason) + { + publish_capture_status(device_removed_.load(std::memory_order_acquire) ? CaptureAvailability::Absent : + CaptureAvailability::Recovering, + reason); + if (config_.reconnect_timeout_seconds == 0) + throw RecoverableCaptureError(reason + "; automatic reconnect is disabled"); + if (recovering_) + return; + recovering_ = true; + recovery_device_present_ = !device_removed_.load(std::memory_order_acquire); + recovery_reason_ = reason; + recovery_last_error_.clear(); + const auto now = std::chrono::steady_clock::now(); + recovery_deadline_ = now + std::chrono::seconds(config_.reconnect_timeout_seconds); + next_reconnect_attempt_ = now; + publish_recovery_event(core::EgoRecoveryStage_Started, reason); + detail::log_error() << "Ego device serial=" << selected_device_serial_ << " uid=" << selected_device_uid_ + << " disconnected; waiting up to " << config_.reconnect_timeout_seconds + << " seconds for the same physical device" << std::endl; + stop_capture_for_reconnect(device_removed_.load(std::memory_order_acquire)); + } + + void start_recovered_video(const std::shared_ptr& pipeline_config) + { + pipeline_ = std::make_unique(device_); + pipeline_->start( + pipeline_config, + [this](std::shared_ptr frame_set) + { + if (!frame_set || !accepting_callbacks_.load(std::memory_order_acquire) || + !capture_active_.load(std::memory_order_acquire)) + { + return; + } + const int64_t arrival_time_local_common_clock_ns = core::os_monotonic_now_ns(); + { + std::lock_guard lock(video_queue_mutex_); + if (video_frame_sets_.size() >= kMaxQueuedVideoFrameSets) + { + ++auxiliary_stats_.dropped_video_frame_sets; + set_async_error("Ego video callback queue is full; capture stopped to avoid silent loss"); + return; + } + video_frame_sets_.emplace_back(std::move(frame_set), arrival_time_local_common_clock_ns); + } + video_queue_cv_.notify_one(); + }); + video_pipeline_started_ = true; + } + + void reapply_controls(std::string_view operation = "reconnect") + { + check_cancelled(); + try + { + for (const auto& setting : requested_controls_) + { + check_cancelled(); + const auto item = find_property(device_, setting.name); + validate_property_value(device_, item, setting.value); + if ((item.permission & OB_PERMISSION_READ) != 0) + { + // Bitrate cannot be rewritten while streaming; retain controls already at the requested value. + const double tolerance = + item.type == OB_FLOAT_PROPERTY ? std::max(1.0, std::abs(setting.value)) * 1e-6 : 0.0; + if (std::abs(read_property(device_, item) - setting.value) <= tolerance) + continue; + } + write_property(device_, item, setting.value); + if ((item.permission & OB_PERMISSION_READ) != 0) + verify_property_readback(device_, item, setting.value, operation); + } + } + catch (const CaptureCancelled&) + { + throw; + } + catch (const std::exception& error) + { + throw FatalReconnectError("failed to restore requested controls: " + std::string(error.what())); + } + } + + void wait_for_recovered_video_readiness() + { + const auto ready = [this] + { + return std::all_of(streams_.begin(), streams_.end(), + [this](const StreamConfig& stream) + { return stats_.at(stream.camera).epoch_frame_count > 0; }); + }; + const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(10); + while (!ready() && std::chrono::steady_clock::now() < deadline) + { + check_cancelled(); + drain_video_frames(); + drain_events(); + if (const auto failure = sink_->metadata_error(); !failure.empty()) + throw std::runtime_error("Ego metadata publication failed: " + failure); + std::lock_guard lock(queue_mutex_); + if (!async_error_.empty()) + throw std::runtime_error(async_error_); + } + if (ready()) + return; + std::string missing; + for (const auto& stream : streams_) + { + if (stats_.at(stream.camera).epoch_frame_count == 0) + missing += " " + std::string(core::EnumNameEgoCameraStream(stream.camera)); + } + throw std::runtime_error("Ego uid=" + selected_device_uid_ + " firmware=" + selected_firmware_ + + " active profiles [" + describe_profiles(active_profiles_) + + "] timed out waiting for video streams after reconnect:" + missing); + } + + void wait_for_recovery_stability() const + { + const bool expect_audio = config_.enable_audio || !config_.audio_output.empty(); + const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(5); + while (std::chrono::steady_clock::now() < deadline) + { + check_cancelled(); + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + } + + const int64_t now = core::os_monotonic_now_ns(); + const auto is_recent = [now](const std::atomic& arrival) + { return now - arrival.load(std::memory_order_acquire) <= 500'000'000LL; }; + std::string stalled; + if (config_.enable_imu && !is_recent(last_accel_arrival_ns_)) + stalled += " Accel"; + if (config_.enable_imu && !is_recent(last_gyro_arrival_ns_)) + stalled += " Gyro"; + if (expect_audio && !is_recent(last_audio_arrival_ns_)) + stalled += " Audio"; + if (!stalled.empty()) + { + throw std::runtime_error("Ego uid=" + selected_device_uid_ + + " auxiliary streams did not remain active during reconnect readiness:" + stalled); + } + } + + void check_cancelled() const + { + check_capture_cancelled(config_.stop_requested); + } + + void attempt_reconnect() + { + check_cancelled(); + const auto now = std::chrono::steady_clock::now(); + if (now >= recovery_deadline_) + { + std::string reason = "Ego device serial=" + selected_device_serial_ + " last_uid=" + selected_device_uid_ + + " did not recover within " + std::to_string(config_.reconnect_timeout_seconds) + + " seconds"; + if (!recovery_last_error_.empty()) + reason += "; last restart error: " + recovery_last_error_; + publish_recovery_event(core::EgoRecoveryStage_Failed, reason); + throw std::runtime_error(reason); + } + if (now < next_reconnect_attempt_) + { + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + return; + } + next_reconnect_attempt_ = now + std::chrono::milliseconds(config_.reconnect_interval_milliseconds); + ++auxiliary_stats_.reconnect_attempts; + try + { + auto recovered_device = find_selected_device(); + if (!recovered_device) + { + recovery_device_present_ = false; + publish_capture_status(CaptureAvailability::Absent, recovery_reason_); + return; + } + recovery_device_present_ = true; + publish_capture_status(CaptureAvailability::Recovering, recovery_reason_); + const auto recovered_info = recovered_device->getDeviceInfo(); + const auto recovered_uid = recovered_info->getUid(); + const auto firmware = recovered_info->getFirmwareVersion(); + if (firmware != selected_firmware_) + throw FatalReconnectError("firmware changed from " + selected_firmware_ + " to " + firmware); + if (recovered_uid != selected_device_uid_) + { + detail::log_info() + << "Ego device serial=" << selected_device_serial_ + << " re-enumerated from uid=" << selected_device_uid_ << " to uid=" << recovered_uid << std::endl; + } + device_ = adopt_ego_device(std::move(recovered_device)); + selected_device_uid_ = recovered_uid; + for (const auto& stream : streams_) + { + const auto profile = select_profile(device_, stream, config_); + validate_resolved_stream_config(stream, profile->getFps()); + active_profiles_.emplace(stream.camera, profile); + } + if (describe_profiles(active_profiles_) != initial_profile_description_) + { + throw FatalReconnectError("resolved profiles changed to [" + describe_profiles(active_profiles_) + "]"); + } + + accepting_callbacks_.store(true, std::memory_order_release); + reset_stream_readiness(); + const auto pipeline_config = make_video_config(active_profiles_, streams_); + start_device_state(); + if (config_.enable_imu) + start_imu(); + if (config_.enable_audio || !config_.audio_output.empty()) + start_audio(); + wait_for_auxiliary_readiness(); + start_recovered_video(pipeline_config); + wait_for_recovery_stability(); + reapply_controls(); + verify_active_controls(); + publish_calibration(pipeline_config); + + // A restart may reset device counters; reset continuity checks and encoded-video gates. + ++capture_epoch_; + auxiliary_stats_.capture_epoch = capture_epoch_; + for (auto& [_, stats] : stats_) + stats.epoch_frame_count = 0; + sink_->begin_capture_epoch(); + capture_epoch_started_ns_ = core::os_monotonic_now_ns(); + capture_active_.store(true, std::memory_order_release); + wait_for_recovered_video_readiness(); + wait_for_active_auxiliary_readiness(); + publish_periodic_device_state(); + ++auxiliary_stats_.successful_reconnects; + recovering_ = false; + device_removed_.store(false, std::memory_order_release); + publish_recovery_event(core::EgoRecoveryStage_Recovered, recovery_reason_); + publish_capture_status(CaptureAvailability::Streaming); + detail::log_info() << "Ego device " << selected_device_uid_ << " recovered as capture epoch " + << capture_epoch_ << std::endl; + } + catch (const CaptureCancelled&) + { + throw; + } + catch (const FatalReconnectError& error) + { + recovery_last_error_ = error.what(); + try + { + stop_capture_for_reconnect(); + publish_recovery_event(core::EgoRecoveryStage_Failed, recovery_last_error_); + } + catch (const std::exception& cleanup_error) + { + recovery_last_error_ += "; cleanup failed: " + std::string(cleanup_error.what()); + } + throw std::runtime_error("Ego reconnect cannot safely continue: " + recovery_last_error_); + } + catch (const std::exception& error) + { + recovery_last_error_ = error.what(); + const auto sink_failure = sink_->metadata_error(); + detail::log_error() << "Ego reconnect attempt " << auxiliary_stats_.reconnect_attempts + << " failed: " << recovery_last_error_ << std::endl; + try + { + stop_capture_for_reconnect(); + } + catch (const std::exception& cleanup_error) + { + recovery_last_error_ += "; cleanup failed: " + std::string(cleanup_error.what()); + } + if (!sink_failure.empty()) + throw std::runtime_error("Ego metadata publication failed during reconnect: " + sink_failure); + } + } + + void prepare_controls(const std::vector& settings) + { + requested_controls_.clear(); + for (const auto& setting : settings) + { + const auto expected = std::find_if(requested_controls_.begin(), requested_controls_.end(), + [&setting](const auto& current) { return current.name == setting.name; }); + if (expected == requested_controls_.end()) + requested_controls_.push_back(setting); + else + *expected = setting; + const auto item = find_property(device_, setting.name); + if ((item.permission & OB_PERMISSION_WRITE) == 0) + throw std::invalid_argument(std::string(item.name) + " is read-only"); + if (!config_.persist_controls && (item.permission & OB_PERMISSION_READ) == 0) + { + throw std::invalid_argument(std::string(item.name) + + " is write-only; use --persist-controls to acknowledge it cannot be restored"); + } + if (!config_.persist_controls && + std::none_of(original_properties_.begin(), original_properties_.end(), + [&setting](const PropertySetting& original) { return original.name == setting.name; })) + { + original_properties_.push_back({ setting.name, read_property(device_, item) }); + } + } + + struct PreflightProfile + { + OBFrameType frame_type; + uint32_t width; + uint32_t height; + uint32_t fps; + OBFormat format; + std::string name; + }; + std::vector expected_profiles; + expected_profiles.reserve(active_profiles_.size()); + for (const auto& [stream, profile] : active_profiles_) + { + expected_profiles.push_back({ to_ob_frame(stream), profile->getWidth(), profile->getHeight(), + profile->getFps(), profile->getFormat(), + core::EnumNameEgoCameraStream(stream) }); + } + struct PreflightState + { + std::mutex mutex; + std::condition_variable wake; + std::vector seen; + std::string error; + bool accepting = true; + }; + auto preflight_state = std::make_shared(); + preflight_state->seen.resize(expected_profiles.size()); + auto preflight = std::make_unique(device_); + bool preflight_started = false; + try + { + // EGO encoded streams can time out in SDK pull mode while callback delivery works, + // so control preflight mirrors the capture callback path. + preflight->start( + make_video_config(active_profiles_, streams_), + [preflight_state, expected_profiles](std::shared_ptr frame_set) + { + if (!frame_set) + return; + std::lock_guard lock(preflight_state->mutex); + if (!preflight_state->accepting) + return; + try + { + for (size_t index = 0; index < expected_profiles.size(); ++index) + { + const auto& expected = expected_profiles[index]; + const auto raw_frame = frame_set->getFrame(expected.frame_type); + if (!raw_frame) + continue; + const auto frame = raw_frame->as(); + if (!frame) + continue; + const auto profile = frame->getStreamProfile()->as(); + if (!profile || frame->getFormat() != expected.format || frame->getWidth() != expected.width || + frame->getHeight() != expected.height || profile->getFps() != expected.fps) + { + preflight_state->error = "SDK returned a different profile for " + expected.name; + break; + } + preflight_state->seen[index] = true; + } + } + catch (const std::exception& error) + { + preflight_state->error = error.what(); + } + const bool complete = std::all_of( + preflight_state->seen.begin(), preflight_state->seen.end(), [](bool seen) { return seen; }); + if (complete || !preflight_state->error.empty()) + preflight_state->wake.notify_one(); + }); + preflight_started = true; + std::string preflight_error; + { + std::unique_lock lock(preflight_state->mutex); + const bool finished = wait_capture_ready( + preflight_state->wake, lock, std::chrono::seconds(10), + [&preflight_state] + { + return !preflight_state->error.empty() || + std::all_of(preflight_state->seen.begin(), preflight_state->seen.end(), + [](bool seen) { return seen; }); + }, + config_.stop_requested); + if (!finished) + { + preflight_error = "timed out waiting for"; + for (size_t index = 0; index < expected_profiles.size(); ++index) + { + if (!preflight_state->seen[index]) + preflight_error += " " + expected_profiles[index].name; + } + } + else + preflight_error = preflight_state->error; + preflight_state->accepting = false; + } + if (!preflight_error.empty()) + throw std::runtime_error(preflight_error); + } + catch (const std::exception& error) + { + { + std::lock_guard lock(preflight_state->mutex); + preflight_state->accepting = false; + } + const auto stop_error = stop_pipeline_noexcept(preflight, preflight_started, "control preflight"); + original_properties_.clear(); + std::string message = "Ego uid=" + selected_device_uid_ + " firmware=" + selected_firmware_ + + " control preflight rejected simultaneous profiles [" + + describe_profiles(active_profiles_) + "]: " + error.what(); + if (!stop_error.empty()) + message += "; " + stop_error; + throw std::runtime_error(message); + } + { + std::lock_guard lock(preflight_state->mutex); + preflight_state->accepting = false; + } + if (const auto error = stop_pipeline_noexcept(preflight, preflight_started, "control preflight"); !error.empty()) + { + original_properties_.clear(); + throw std::runtime_error(error); + } + + try + { + for (const auto& setting : settings) + validate_property_value(device_, find_property(device_, setting.name), setting.value); + for (const auto& setting : settings) + { + const auto item = find_property(device_, setting.name); + controls_applied_ = true; + write_property(device_, item, setting.value); + if ((item.permission & OB_PERMISSION_READ) != 0) + verify_property_readback(device_, item, setting.value, "set"); + detail::log_info() << "Set " << item.name << "=" << setting.value << std::endl; + } + } + catch (...) + { + if (controls_applied_) + { + try + { + restore_properties(); + } + catch (const std::exception& error) + { + detail::log_error() + << "Failed to restore Ego controls after configuration error: " << error.what() << std::endl; + } + } + else + original_properties_.clear(); + throw; + } + } + + void verify_active_controls() + { + for (const auto& setting : requested_controls_) + { + const auto item = find_property(device_, setting.name); + if ((item.permission & OB_PERMISSION_READ) == 0) + continue; + try + { + verify_property_readback(device_, item, setting.value, "active-profile"); + } + catch (const std::exception& error) + { + throw std::runtime_error("Ego uid=" + selected_device_uid_ + " firmware=" + selected_firmware_ + + " active profiles [" + describe_profiles(active_profiles_) + + "] changed a requested control: " + error.what()); + } + } + } + + void capture_device_snapshot() + { + std::vector properties; + for (int index = 0; index < device_->getSupportedPropertyCount(); ++index) + { + const auto item = device_->getSupportedProperty(static_cast(index)); + if ((item.permission & OB_PERMISSION_READ) == 0 || + (item.type != OB_BOOL_PROPERTY && item.type != OB_INT_PROPERTY && item.type != OB_FLOAT_PROPERTY)) + { + continue; + } + try + { + properties.emplace_back(static_cast(item.id), read_property(device_, item)); + } + catch (const ob::Error& error) + { + detail::log_warning() << "Ego property snapshot read failed for " << item.id << ": " << error.what(); + } + } + std::optional temperature_c; + try + { + OBDeviceTemperature temperature{}; + uint32_t size = sizeof(temperature); + device_->getStructuredData(OB_STRUCT_DEVICE_TEMPERATURE, reinterpret_cast(&temperature), &size); + if (size >= sizeof(temperature)) + temperature_c = temperature.imuTemp; + } + catch (const ob::Error& error) + { + detail::log_warning() << "Ego IMU temperature snapshot unavailable: " << error.what(); + } + // Publish at observation time; recovery never relabels these startup-only reads. + const auto now = core::os_monotonic_now_ns(); + auto state = device_snapshot(diagnostic_origin(now), temperature_c, properties); + enqueue(DeviceStateEvent{ std::move(state), now }); + } + + void reacquire_device() + { + const auto devices = context_->queryDeviceList(); + for (uint32_t index = 0; index < devices->getCount(); ++index) + { + const auto candidate = devices->getDevice(index); + if (is_selected_physical_device(candidate->getDeviceInfo())) + { + selected_device_uid_ = candidate->getDeviceInfo()->getUid(); + device_ = adopt_ego_device(candidate); + return; + } + } + throw std::runtime_error("Ego device " + selected_device_uid_ + " disconnected before controls restored"); + } + + void close() + { + shutdown(true); + } + + void shutdown(bool promote_metadata) + { + const auto shutdown_started = std::chrono::steady_clock::now(); + if (shutdown_complete_) + return; + accepting_callbacks_.store(false, std::memory_order_release); + capture_active_.store(false, std::memory_order_release); + std::string shutdown_error = cleanup_error_; + if (recovering_) + shutdown_error += "Ego capture stopped before device recovery completed"; + const auto remember_error = [&shutdown_error](const std::string& error) + { + if (error.empty()) + return; + if (!shutdown_error.empty()) + shutdown_error += "; "; + shutdown_error += error; + }; + if (device_changed_callback_registered_) + { + try + { + context_->unregisterDeviceChangedCallback(device_changed_callback_id_); + device_changed_callback_registered_ = false; + } + catch (const std::exception& error) + { + remember_error(std::string("Ego device-change callback unregister failed: ") + error.what()); + } + } + try + { + if (device_) + device_->clearEgoStateCallback(); + } + catch (const ob::Error& error) + { + remember_error(std::string("Ego device-state callback clear failed: ") + error.what()); + } + catch (...) + { + remember_error("Ego device-state callback clear failed: unknown error"); + } + // Freeze callbacks before stopping producers so SDK stop latency cannot + // turn post-stop frames into queue overflow or a longer recording tail. + remember_error(stop_pipeline_noexcept(pipeline_, video_pipeline_started_, "video")); + if (audio_sensor_) + { + if (audio_started_) + { + try + { + const auto audio_stop_started = std::chrono::steady_clock::now(); + audio_sensor_->stop(); + detail::log_info() << "Ego audio stop completed in " + << std::chrono::duration(std::chrono::steady_clock::now() - + audio_stop_started) + .count() + << " ms"; + } + catch (const ob::Error& error) + { + remember_error(std::string("Ego audio stop failed: ") + error.what()); + } + catch (const std::exception& error) + { + remember_error(std::string("Ego audio stop failed: ") + error.what()); + } + catch (...) + { + remember_error("Ego audio stop failed: unknown error"); + } + } + audio_started_ = false; + audio_sensor_.reset(); + } + remember_error(stop_pipeline_noexcept(imu_pipeline_, imu_pipeline_started_, "IMU")); + const auto finalize = [&remember_error](const char* operation, auto&& function) + { + try + { + function(); + } + catch (const std::exception& error) + { + remember_error(std::string("Ego ") + operation + " failed: " + error.what()); + } + catch (...) + { + remember_error(std::string("Ego ") + operation + " failed: unknown error"); + } + }; + finalize("video drain", [this] { drain_video_frames(false); }); + finalize("media sidecar close", [this] { sink_->close_media(); }); + for (const auto& [stream, stats] : stats_) + { + if (stats.sequence_gaps != 0) + { + remember_error("Ego " + std::string(core::EnumNameEgoCameraStream(stream)) + " recorded " + + std::to_string(stats.sequence_gaps) + " sequence gaps"); + } + } + finalize("IMU flush", + [this] + { + flush_imu(core::EgoImuSensor_Accel); + flush_imu(core::EgoImuSensor_Gyro); + }); + finalize("event drain", [this] { drain_events(); }); + uint64_t dropped_video_frame_sets = 0; + { + std::lock_guard lock(video_queue_mutex_); + dropped_video_frame_sets = auxiliary_stats_.dropped_video_frame_sets; + } + if (dropped_video_frame_sets != 0) + { + remember_error("Ego capture dropped " + std::to_string(dropped_video_frame_sets) + " video frame sets"); + } + { + std::lock_guard lock(queue_mutex_); + if (!async_error_.empty()) + remember_error(async_error_); + if (auxiliary_stats_.dropped_events != 0) + { + remember_error("Ego capture dropped " + std::to_string(auxiliary_stats_.dropped_events) + + " metadata events"); + } + } + if (const auto failure = sink_->metadata_error(); !failure.empty()) + remember_error("Ego metadata publication failed: " + failure); + finalize("WAV close", [this] { wav_writer_.close(); }); + try + { + restore_properties(); + } + catch (const std::exception& error) + { + remember_error(error.what()); + } + finalize("SDK resource release", + [this] + { + device_.reset(); + context_.reset(); + }); + // Aborted capture must not produce a healthy final summary, even if cleanup succeeds. + const auto final_diagnostic_error = !shutdown_error.empty() ? shutdown_error : + promote_metadata ? std::string{} : + "Capture aborted before completion"; + finalize("final diagnostic summary", + [this, &final_diagnostic_error] { publish_periodic_device_state(final_diagnostic_error, true); }); + if (promote_metadata && shutdown_error.empty()) + { + finalize("metadata close", [this] { sink_->close_metadata(); }); + if (shutdown_error.empty()) + finalize("metadata commit", [this] { sink_->commit_metadata(); }); + } + else + finalize("metadata abort", [this] { sink_->abort_metadata(); }); + shutdown_complete_ = true; + detail::log_info() + << "Ego shutdown completed in " + << std::chrono::duration(std::chrono::steady_clock::now() - shutdown_started).count() + << " ms; archive_committed=" << (promote_metadata && shutdown_error.empty()); + if (!shutdown_error.empty()) + throw std::runtime_error(shutdown_error); + } + + void shutdown_noexcept(bool promote_metadata) noexcept + { + try + { + shutdown(promote_metadata); + } + catch (const std::exception& error) + { + detail::log_error() << "Ego shutdown failed: " << error.what() << std::endl; + } + catch (...) + { + detail::log_error() << "Ego shutdown failed: unknown error" << std::endl; + } + } + + void restore_properties() + { + if (!controls_applied_ || original_properties_.empty()) + return; + reacquire_device(); + std::string restore_error; + for (auto it = original_properties_.rbegin(); it != original_properties_.rend(); ++it) + { + try + { + const auto item = find_property(device_, it->name); + write_property(device_, item, it->value, false); + verify_property_readback(device_, item, it->value, "restore"); + detail::log_info() << "Restored " << item.name << "=" << it->value << std::endl; + } + catch (const std::exception& error) + { + if (!restore_error.empty()) + restore_error += "; "; + restore_error += "failed to restore " + it->name + ": " + error.what(); + } + } + original_properties_.clear(); + controls_applied_ = false; + if (!restore_error.empty()) + throw std::runtime_error(restore_error); + } + + void update() + { + try + { + update_capture(); + } + catch (const CaptureCancelled&) + { + throw; + } + catch (const std::exception& error) + { + cleanup_error_ = error.what(); + publish_capture_status(CaptureAvailability::Failed, error.what()); + throw; + } + } + + void update_capture() + { + if (recovering_) + { + publish_capture_status( + recovery_device_present_ ? CaptureAvailability::Recovering : CaptureAvailability::Absent, + recovery_reason_); + attempt_reconnect(); + return; + } + try + { + if (device_removed_.load(std::memory_order_acquire)) + throw RecoverableCaptureError("SDK reported removal of UID " + selected_device_uid_); + { + std::lock_guard lock(queue_mutex_); + if (!async_error_.empty()) + throw std::runtime_error(async_error_); + } + if (const auto error = sink_->metadata_error(); !error.empty()) + throw std::runtime_error("Ego metadata publication failed: " + error); + drain_events(); + drain_video_frames(); + validate_video_liveness(); + validate_auxiliary_liveness(); + publish_capture_status(CaptureAvailability::Streaming); + if (std::chrono::steady_clock::now() - last_device_poll_ >= std::chrono::seconds(5)) + publish_periodic_device_state(); + drain_events(); + if (const auto error = sink_->metadata_error(); !error.empty()) + throw std::runtime_error("Ego metadata publication failed: " + error); + } + catch (const RecoverableCaptureError& error) + { + begin_recovery(error.what()); + } + } + + void validate_video_liveness() const + { + if (!capture_active_.load(std::memory_order_acquire)) + return; + const int64_t now = core::os_monotonic_now_ns(); + for (const auto& stream : streams_) + { + const auto observed = last_video_arrival_ns_.find(stream.camera); + const int64_t reference = + observed == last_video_arrival_ns_.end() ? capture_epoch_started_ns_ : observed->second; + if (now - reference > 5'000'000'000LL) + { + throw RecoverableCaptureError("Ego " + std::string(core::EnumNameEgoCameraStream(stream.camera)) + + " stopped delivering frames for five seconds"); + } + } + } + + void validate_auxiliary_liveness() const + { + if (!capture_active_.load(std::memory_order_acquire)) + return; + const int64_t now = core::os_monotonic_now_ns(); + const auto validate = [this, now](bool expected, const std::atomic& last_arrival, const char* name) + { + if (!expected) + return; + const int64_t observed = last_arrival.load(std::memory_order_acquire); + const int64_t reference = observed == 0 ? capture_epoch_started_ns_ : observed; + if (now - reference > 5'000'000'000LL) + throw RecoverableCaptureError(std::string("Ego ") + name + " stopped delivering samples for five seconds"); + }; + validate(config_.enable_imu, last_accel_arrival_ns_, "Accel"); + validate(config_.enable_imu, last_gyro_arrival_ns_, "Gyro"); + validate(config_.enable_audio || !config_.audio_output.empty(), last_audio_arrival_ns_, "Audio"); + } + + void drain_video_frames(bool wait_for_frame = true) + { + std::deque, int64_t>> pending; + { + std::unique_lock lock(video_queue_mutex_); + if (wait_for_frame && video_frame_sets_.empty()) + video_queue_cv_.wait_for(lock, std::chrono::milliseconds(20)); + pending.swap(video_frame_sets_); + } + for (const auto& [frame_set, arrival_time_local_common_clock_ns] : pending) + process_frame_set(frame_set, arrival_time_local_common_clock_ns); + } + + void process_frame_set(const std::shared_ptr& frame_set, int64_t arrival_time_local_common_clock_ns) + { + if (!frame_set) + return; + for (const auto& stream : streams_) + { + const auto raw_frame = frame_set->getFrame(to_ob_frame(stream.camera)); + if (!raw_frame) + continue; + const auto frame = raw_frame->as(); + if (!frame) + throw std::runtime_error("Ego video frameset contained a non-video frame"); + + const auto profile = frame->getStreamProfile()->as(); + const auto expected = active_profiles_.at(stream.camera); + if (!profile || frame->getWidth() != expected->getWidth() || frame->getHeight() != expected->getHeight() || + frame->getFormat() != expected->getFormat() || profile->getFps() != expected->getFps()) + { + throw std::runtime_error( + "Ego " + std::string(core::EnumNameEgoCameraStream(stream.camera)) + + " delivered a frame outside the selected active profile " + std::to_string(expected->getWidth()) + + "x" + std::to_string(expected->getHeight()) + "@" + std::to_string(expected->getFps()) + " " + + ob::TypeHelper::convertOBFormatTypeToString(expected->getFormat())); + } + CapturedFrame captured; + captured.metadata.stream = stream.camera; + captured.metadata.sequence_number = frame->getIndex(); + captured.metadata.width = frame->getWidth(); + captured.metadata.height = frame->getHeight(); + captured.metadata.fps = profile->getFps(); + captured.metadata.pixel_format = stream.pixel_format; + captured.metadata.encoded_bytes = frame->getDataSize(); + captured.metadata.capture_epoch = capture_epoch_; + for (int type = 0; type < OB_FRAME_METADATA_TYPE_COUNT; ++type) + { + const auto metadata_type = static_cast(type); + if (frame->hasMetadata(metadata_type)) + captured.metadata.sdk_metadata.emplace_back(type, frame->getMetadataValue(metadata_type)); + } + captured.encoded_data.assign(frame->getData(), frame->getData() + frame->getDataSize()); + captured.sample_time_raw_device_clock_ns = sdk_timestamp_ns(frame->getTimeStampUs()); + captured.sample_time_local_common_clock_ns = time_alignment_.sample_time( + stream.camera == core::EgoCameraStream_ColorLeft ? "left_rgb" : "right_rgb", + captured.sample_time_raw_device_clock_ns, arrival_time_local_common_clock_ns); + auto& stats = stats_[stream.camera]; + if (stats.epoch_frame_count > 0 && captured.metadata.sequence_number <= stats.last_sequence) + { + throw std::runtime_error( + "Ego " + std::string(core::EnumNameEgoCameraStream(stream.camera)) + + " delivered a non-increasing sequence: previous=" + std::to_string(stats.last_sequence) + + " current=" + std::to_string(captured.metadata.sequence_number)); + } + if (stats.epoch_frame_count > 0 && captured.sample_time_raw_device_clock_ns <= stats.last_device_timestamp_ns) + { + throw std::runtime_error("Ego " + std::string(core::EnumNameEgoCameraStream(stream.camera)) + + " delivered a non-increasing raw device timestamp: previous=" + + std::to_string(stats.last_device_timestamp_ns) + + " current=" + std::to_string(captured.sample_time_raw_device_clock_ns)); + } + sink_->on_frame(captured, + [this, arrival_time_local_common_clock_ns](const CapturedFrame& accepted) + { + if (accepted.metadata.stream == core::EgoCameraStream_ColorLeft) + time_alignment_.observe_left( + accepted.sample_time_raw_device_clock_ns, arrival_time_local_common_clock_ns); +#if defined(EGO_ENABLE_PREVIEW) + if (preview_) + preview_->submit(accepted); +#endif + }); + + if (stats.epoch_frame_count > 0 && captured.metadata.sequence_number > stats.last_sequence + 1) + stats.sequence_gaps += captured.metadata.sequence_number - stats.last_sequence - 1; + stats.frame_count++; + stats.epoch_frame_count++; + stats.byte_count += captured.encoded_data.size(); + stats.last_sequence = captured.metadata.sequence_number; + stats.last_device_timestamp_ns = captured.sample_time_raw_device_clock_ns; + last_video_arrival_ns_[stream.camera] = arrival_time_local_common_clock_ns; + } + } + + void print_stats() const + { + uint64_t dropped_video_frame_sets = 0; + AuxiliaryStats auxiliary_stats; + auxiliary_stats.accel_samples = auxiliary_stats_.accel_samples; + auxiliary_stats.gyro_samples = auxiliary_stats_.gyro_samples; + auxiliary_stats.audio_samples = auxiliary_stats_.audio_samples; + { + std::lock_guard lock(video_queue_mutex_); + dropped_video_frame_sets = auxiliary_stats_.dropped_video_frame_sets; + } + { + std::lock_guard lock(queue_mutex_); + auxiliary_stats.publish_queue_peak = auxiliary_stats_.publish_queue_peak; + auxiliary_stats.dropped_events = auxiliary_stats_.dropped_events; + } + for (const auto& [stream, stats] : stats_) + { + detail::log_info() + << " " << core::EnumNameEgoCameraStream(stream) << ": " << stats.frame_count << " frames, " + << stats.byte_count << " bytes, " << stats.sequence_gaps << " sequence gaps" << std::endl; + } + detail::log_info() + << " IMU: accel=" << auxiliary_stats.accel_samples << " gyro=" << auxiliary_stats.gyro_samples + << " samples; audio=" << auxiliary_stats.audio_samples + << " samples; queue_peak=" << auxiliary_stats.publish_queue_peak + << " dropped=" << auxiliary_stats.dropped_events << " video_frame_sets_dropped=" << dropped_video_frame_sets + << "; capture_epoch=" << auxiliary_stats_.capture_epoch + << " reconnect_attempts=" << auxiliary_stats_.reconnect_attempts + << " successful_reconnects=" << auxiliary_stats_.successful_reconnects << std::endl; + } + + const std::map& stats() const + { + return stats_; + } + + const AuxiliaryStats& auxiliary_stats() const + { + return auxiliary_stats_; + } + bool preview_closed() const + { +#if defined(EGO_ENABLE_PREVIEW) + return preview_ && preview_->closed(); +#else + return false; +#endif + } + + struct ImuEvent + { + core::EgoImuBatchT batch; + int64_t local_ns = 0; + int64_t device_ns = 0; + }; + struct AudioEvent + { + std::vector bytes; + int64_t local_ns = 0; + int64_t device_ns = 0; + }; + struct CalibrationEvent + { + core::EgoCalibrationT calibration; + int64_t local_ns = 0; + }; + struct DeviceStateEvent + { + core::EgoDeviceStateT state; + int64_t local_ns = 0; + }; + using PublishEvent = std::variant; + + template + void enqueue(Event&& event) + { + std::lock_guard lock(queue_mutex_); + if (events_.size() >= kMaxQueuedEvents) + { + ++auxiliary_stats_.dropped_events; + if (async_error_.empty()) + async_error_ = "Ego metadata queue is full; capture stopped to avoid silent loss"; + return; + } + events_.emplace_back(std::forward(event)); + auxiliary_stats_.publish_queue_peak = std::max(auxiliary_stats_.publish_queue_peak, events_.size()); + } + + void set_async_error(const std::string& message) + { + std::lock_guard lock(queue_mutex_); + if (async_error_.empty()) + async_error_ = message; + } + + void drain_events() + { + if (last_calibration_ && time_alignment_.has_update()) + { + auto value = *last_calibration_; + attach_time_alignment(value); + enqueue(CalibrationEvent{ std::move(value), core::os_monotonic_now_ns() }); + } + std::deque pending; + { + std::lock_guard lock(queue_mutex_); + pending.swap(events_); + } + auto* metadata = sink_->metadata_sink(); + for (auto& event : pending) + { + std::visit( + [this, metadata](auto& value) + { + using Event = std::decay_t; + if constexpr (std::is_same_v) + { + if (value.batch.sensor == core::EgoImuSensor_Accel) + auxiliary_stats_.accel_samples += value.batch.samples.size(); + else + auxiliary_stats_.gyro_samples += value.batch.samples.size(); + if (metadata) + metadata->on_imu_batch(value.batch, value.local_ns, value.device_ns); + } + else if constexpr (std::is_same_v) + { + core::EgoAudioChunkT chunk; + chunk.sequence_number = audio_sequence_++; + chunk.sample_rate_hz = audio_rate_; + chunk.channel_count = audio_channels_; + chunk.bits_per_sample = audio_bits_; + chunk.sample_format = core::EgoAudioSampleFormat_S16LE; + chunk.byte_count = static_cast(value.bytes.size()); + chunk.capture_epoch = capture_epoch_; + const uint32_t bytes_per_sample = audio_channels_ * audio_bits_ / 8; + chunk.sample_count = bytes_per_sample == 0 ? 0 : chunk.byte_count / bytes_per_sample; + if (bytes_per_sample == 0 || chunk.byte_count % bytes_per_sample != 0) + throw std::runtime_error("EGO audio payload is not a whole number of PCM samples"); + chunk.timing = std::make_shared( + time_alignment_.audio_timing(value.device_ns, chunk.sample_count, chunk.sample_rate_hz)); + if (!config_.audio_output.empty()) + chunk.wav_data_offset = wav_writer_.write(value.bytes); + auxiliary_stats_.audio_samples += chunk.sample_count; + if (metadata) + { + metadata->on_audio_chunk(chunk, value.local_ns, value.device_ns); + core::EgoPcmAudioChunkT pcm; + pcm.sequence_number = chunk.sequence_number; + pcm.sample_rate_hz = chunk.sample_rate_hz; + pcm.channel_count = chunk.channel_count; + pcm.bits_per_sample = chunk.bits_per_sample; + pcm.sample_format = chunk.sample_format; + pcm.sample_count = chunk.sample_count; + pcm.pcm_data = std::move(value.bytes); + pcm.capture_epoch = capture_epoch_; + pcm.timing = chunk.timing; + metadata->on_pcm_audio_chunk(pcm, value.local_ns, value.device_ns); + } + } + else if constexpr (std::is_same_v) + { + if (metadata) + metadata->on_calibration(value.calibration, value.local_ns, 0); + } + else if constexpr (std::is_same_v) + { + if (metadata) + metadata->on_device_state(value.state, value.local_ns, 0); + } + }, + event); + } + } + + struct PendingImu + { + std::vector samples; + uint64_t sequence = 0; + int64_t first_local_ns = 0; + }; + + void add_imu_sample(core::EgoImuSensor sensor, const OBFloat3D& value, float temperature, uint64_t timestamp_us) + { + auto& pending = sensor == core::EgoImuSensor_Accel ? accel_pending_ : gyro_pending_; + const int64_t arrival_ns = core::os_monotonic_now_ns(); + const int64_t device_ns = sdk_timestamp_ns(timestamp_us); + const int64_t local_ns = + time_alignment_.sample_time(sensor == core::EgoImuSensor_Accel ? "accel" : "gyro", device_ns, arrival_ns); + if (pending.samples.empty()) + pending.first_local_ns = arrival_ns; + pending.samples.emplace_back(value.x, value.y, value.z, temperature, local_ns, device_ns); + if (pending.samples.size() < 32 && arrival_ns - pending.first_local_ns < 20'000'000) + return; + flush_imu(sensor); + } + + void flush_imu(core::EgoImuSensor sensor) + { + auto& pending = sensor == core::EgoImuSensor_Accel ? accel_pending_ : gyro_pending_; + if (pending.samples.empty()) + return; + const int64_t local_ns = pending.samples.back().sample_time_local_common_clock_ns(); + const int64_t device_ns = pending.samples.back().sample_time_raw_device_clock_ns(); + core::EgoImuBatchT batch; + batch.sensor = sensor; + batch.sequence_number = pending.sequence++; + batch.sample_rate_hz = config_.imu_rate; + batch.full_scale = sensor == core::EgoImuSensor_Accel ? config_.accel_full_scale_g : config_.gyro_full_scale_dps; + batch.capture_epoch = capture_epoch_; + batch.samples.swap(pending.samples); + enqueue(ImuEvent{ std::move(batch), local_ns, device_ns }); + } + + void start_imu() + { + check_cancelled(); + auto imu_config = std::make_shared(); + imu_config->enableAccelStream(accel_scale(config_.accel_full_scale_g), imu_rate(config_.imu_rate)); + imu_config->enableGyroStream(gyro_scale(config_.gyro_full_scale_dps), imu_rate(config_.imu_rate)); + imu_pipeline_ = std::make_unique(device_); + imu_pipeline_->start( + imu_config, + [this](std::shared_ptr frame_set) + { + if (!accepting_callbacks_.load(std::memory_order_acquire)) + return; + try + { + if (const auto raw = frame_set ? frame_set->getFrame(OB_FRAME_ACCEL) : nullptr) + { + last_accel_arrival_ns_.store(core::os_monotonic_now_ns(), std::memory_order_release); + if (!accel_ready_.exchange(true, std::memory_order_acq_rel)) + auxiliary_readiness_wake_.notify_all(); + if (capture_active_.load(std::memory_order_acquire)) + { + const auto frame = raw->as(); + add_imu_sample(core::EgoImuSensor_Accel, frame->getValue(), frame->getTemperature(), + frame->getTimeStampUs()); + if (!active_accel_ready_.exchange(true, std::memory_order_acq_rel)) + auxiliary_readiness_wake_.notify_all(); + } + } + if (const auto raw = frame_set ? frame_set->getFrame(OB_FRAME_GYRO) : nullptr) + { + last_gyro_arrival_ns_.store(core::os_monotonic_now_ns(), std::memory_order_release); + if (!gyro_ready_.exchange(true, std::memory_order_acq_rel)) + auxiliary_readiness_wake_.notify_all(); + if (capture_active_.load(std::memory_order_acquire)) + { + const auto frame = raw->as(); + add_imu_sample(core::EgoImuSensor_Gyro, frame->getValue(), frame->getTemperature(), + frame->getTimeStampUs()); + if (!active_gyro_ready_.exchange(true, std::memory_order_acq_rel)) + auxiliary_readiness_wake_.notify_all(); + } + } + } + catch (const std::exception& error) + { + set_async_error(std::string("IMU callback failed: ") + error.what()); + } + }); + imu_pipeline_started_ = true; + detail::log_info() << "Ego IMU started at " << config_.imu_rate << " Hz" << std::endl; + } + + void start_audio() + { + check_cancelled(); + audio_sensor_ = device_->getSensor(OB_SENSOR_AUDIO); + const auto profiles = audio_sensor_->getStreamProfileList(); + if (profiles->getCount() == 0) + throw std::runtime_error("EGO audio sensor has no profiles"); + const auto profile = profiles->getProfile(0)->as(); + audio_rate_ = profile->getSampleRate(); + audio_channels_ = static_cast(profile->getChannelCount()); + audio_bits_ = static_cast(profile->getBitsPerSample()); + if (audio_rate_ != 48000 || audio_channels_ != 1 || audio_bits_ != 16) + throw std::runtime_error("Unsupported Ego audio profile; expected PCM 48000 Hz mono S16_LE"); + if (!config_.audio_output.empty() && !wav_writer_.is_open()) + wav_writer_.open(config_.audio_output, audio_rate_, audio_channels_, audio_bits_); + audio_sensor_->start(profile, + [this](std::shared_ptr frame) + { + try + { + if (!frame || !accepting_callbacks_.load(std::memory_order_acquire)) + return; + if (!audio_ready_.exchange(true, std::memory_order_acq_rel)) + auxiliary_readiness_wake_.notify_all(); + const int64_t arrival_ns = core::os_monotonic_now_ns(); + last_audio_arrival_ns_.store(arrival_ns, std::memory_order_release); + if (!capture_active_.load(std::memory_order_acquire)) + return; + AudioEvent event; + event.bytes.assign(frame->getData(), frame->getData() + frame->getDataSize()); + event.local_ns = arrival_ns; + event.device_ns = sdk_timestamp_ns(frame->getTimeStampUs()); + enqueue(std::move(event)); + if (!active_audio_ready_.exchange(true, std::memory_order_acq_rel)) + auxiliary_readiness_wake_.notify_all(); + } + catch (const std::exception& error) + { + set_async_error(std::string("Audio callback failed: ") + error.what()); + } + }); + audio_started_ = true; + std::ostringstream audio_message; + audio_message << "Audio capture started"; + if (!config_.audio_output.empty()) + audio_message << "; sidecar WAV: " << config_.audio_output; + detail::log_info() << audio_message.str(); + } + + void wait_for_auxiliary_readiness() + { + const bool expect_audio = config_.enable_audio || !config_.audio_output.empty(); + const auto ready = [this, expect_audio] + { + return (!config_.enable_imu || + (accel_ready_.load(std::memory_order_acquire) && gyro_ready_.load(std::memory_order_acquire))) && + (!expect_audio || audio_ready_.load(std::memory_order_acquire)); + }; + std::unique_lock lock(auxiliary_readiness_mutex_); + if (wait_capture_ready(auxiliary_readiness_wake_, lock, std::chrono::seconds(10), ready, config_.stop_requested)) + return; + std::string missing; + if (config_.enable_imu && !accel_ready_.load(std::memory_order_relaxed)) + missing += " Accel"; + if (config_.enable_imu && !gyro_ready_.load(std::memory_order_relaxed)) + missing += " Gyro"; + if (expect_audio && !audio_ready_.load(std::memory_order_relaxed)) + missing += " Audio"; + throw std::runtime_error("Ego uid=" + selected_device_uid_ + " firmware=" + selected_firmware_ + + " active profiles [" + describe_profiles(active_profiles_) + + "] timed out waiting for auxiliary streams:" + missing); + } + + void wait_for_active_auxiliary_readiness() + { + const bool expect_audio = config_.enable_audio || !config_.audio_output.empty(); + const auto ready = [this, expect_audio] + { + return (!config_.enable_imu || (active_accel_ready_.load(std::memory_order_acquire) && + active_gyro_ready_.load(std::memory_order_acquire))) && + (!expect_audio || active_audio_ready_.load(std::memory_order_acquire)); + }; + const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(10); + while (!ready() && std::chrono::steady_clock::now() < deadline) + { + check_cancelled(); + drain_video_frames(); + drain_events(); + if (const auto failure = sink_->metadata_error(); !failure.empty()) + throw std::runtime_error("Ego metadata publication failed: " + failure); + { + std::lock_guard lock(queue_mutex_); + if (!async_error_.empty()) + throw std::runtime_error(async_error_); + } + std::unique_lock lock(auxiliary_readiness_mutex_); + auxiliary_readiness_wake_.wait_for(lock, std::chrono::milliseconds(20), ready); + } + if (ready()) + return; + std::string missing; + if (config_.enable_imu && !active_accel_ready_.load(std::memory_order_relaxed)) + missing += " Accel"; + if (config_.enable_imu && !active_gyro_ready_.load(std::memory_order_relaxed)) + missing += " Gyro"; + if (expect_audio && !active_audio_ready_.load(std::memory_order_relaxed)) + missing += " Audio"; + throw std::runtime_error("Ego uid=" + selected_device_uid_ + " firmware=" + selected_firmware_ + + " active profiles [" + describe_profiles(active_profiles_) + + "] timed out waiting for auxiliary streams after video became active:" + missing); + } + + static std::shared_ptr camera_intrinsics(const OBCalibrationParam& param, + OBSensorType sensor) + { + const auto& source = param.intrinsics[sensor]; + const auto& distortion = param.distortion[sensor]; + auto result = std::make_shared(); + result->width = source.width; + result->height = source.height; + result->fx = source.fx; + result->fy = source.fy; + result->cx = source.cx; + result->cy = source.cy; + result->distortion_model = distortion.model; + result->distortion = { distortion.k1, distortion.k2, distortion.k3, distortion.k4, + distortion.k5, distortion.k6, distortion.p1, distortion.p2 }; + return result; + } + + static std::shared_ptr extrinsics(const OBCalibrationParam& param, + OBSensorType from, + OBSensorType to) + { + const auto& source = param.extrinsics[from][to]; + auto result = std::make_shared(); + result->rotation.assign(std::begin(source.rot), std::end(source.rot)); + result->translation_mm.assign(std::begin(source.trans), std::end(source.trans)); + return result; + } + + void configure_time_alignment() + { + if (active_profiles_.empty()) + return; + const auto first = active_profiles_.begin()->second; + if (!config_.time_alignment.empty()) + { + if (!active_profiles_.count(core::EgoCameraStream_ColorLeft)) + throw std::invalid_argument("EGO capture timing profile requires the left reference camera"); + for (const auto& [_, profile] : active_profiles_) + if (profile->getWidth() != first->getWidth() || profile->getHeight() != first->getHeight() || + profile->getFps() != first->getFps() || profile->getFormat() != first->getFormat()) + throw std::invalid_argument("EGO capture timing profile requires matching stereo profiles"); + } + const std::string format = first->getFormat() == OB_FORMAT_H264 ? "h264" : + first->getFormat() == OB_FORMAT_H265 ? "h265" : + "mjpg"; + const bool audio = config_.enable_audio || !config_.audio_output.empty(); + time_alignment_.configure( + { selected_device_serial_, selected_firmware_, + std::to_string(ob::Version::getMajor()) + "." + std::to_string(ob::Version::getMinor()) + "." + + std::to_string(ob::Version::getPatch()), + format, first->getWidth(), first->getHeight(), first->getFps(), config_.enable_imu ? config_.imu_rate : 0U, + audio ? 48000U : 0U, static_cast(audio ? 1 : 0), static_cast(audio ? 16 : 0), + active_profiles_.count(core::EgoCameraStream_ColorRight) != 0, loaded_sdk_binary_sha256() }); + } + + void attach_time_alignment(core::EgoCalibrationT& value) + { + value.time_alignment = std::make_shared(); + value.time_alignment->json = time_alignment_.snapshot(); + if (core::pack(value).buffer().size() > core::EGO_MAX_FLATBUFFER_SIZE) + throw std::runtime_error("EGO calibration with timing provenance exceeds the 32 KiB transport limit"); + } + + void publish_calibration(const std::shared_ptr& pipeline_config) + { + core::EgoCalibrationT value; + value.device_uid = device_->getDeviceInfo()->getUid(); + value.capture_epoch = capture_epoch_ + (recovering_ ? 1U : 0U); + time_alignment_.begin_epoch(value.capture_epoch); + std::string sdk_calibration_error; + try + { + const auto fill_structured_calibration = [this, &value](const std::shared_ptr& config) + { + const auto param = pipeline_->getCalibrationParam(config); + value.color_left = camera_intrinsics(param, OB_SENSOR_COLOR_LEFT); + value.color_right = camera_intrinsics(param, OB_SENSOR_COLOR_RIGHT); + value.left_to_right = extrinsics(param, OB_SENSOR_COLOR_LEFT, OB_SENSOR_COLOR_RIGHT); + value.accel_to_left = extrinsics(param, OB_SENSOR_ACCEL, OB_SENSOR_COLOR_LEFT); + value.gyro_to_left = extrinsics(param, OB_SENSOR_GYRO, OB_SENSOR_COLOR_LEFT); + }; + try + { + fill_structured_calibration(pipeline_config); + } + catch (const ob::Error&) + { + // Firmware calibration is indexed by MJPEG profiles even when the active stream is H.264/H.265. + auto calibration_config = std::make_shared(); + for (const auto& [stream, profile] : active_profiles_) + { + const auto fallback = + device_->getSensor(to_ob_sensor(stream)) + ->getStreamProfileList() + ->getVideoStreamProfile(profile->getWidth(), profile->getHeight(), OB_FORMAT_MJPG, 0); + calibration_config->enableStream(fallback); + } + fill_structured_calibration(calibration_config); + } + } + catch (const ob::Error& error) + { + sdk_calibration_error = error.what(); + } + try + { + value.raw_alignment_yaml = raw_data(device_, OB_RAW_DATA_ALIGN_CALIB_YAML); + value.raw_imu_yaml = raw_data(device_, OB_RAW_DATA_IMU_CALIB_YAML); + if ((!value.color_left || !value.color_right) && + !populate_stereo_calibration_from_yaml(value.raw_alignment_yaml, value)) + detail::log_error() << "Raw alignment calibration does not contain a usable stereo pair." << std::endl; + } + catch (const std::exception& error) + { + detail::log_error() << "Raw calibration unavailable: " << error.what() << std::endl; + } + if (!sdk_calibration_error.empty()) + { + if (value.color_left && value.color_right) + detail::log_info() << "Using raw alignment calibration because SDK profile calibration is unavailable: " + << sdk_calibration_error << std::endl; + else + detail::log_error() << "Structured calibration unavailable: " << sdk_calibration_error << std::endl; + } + const auto now = core::os_monotonic_now_ns(); + attach_time_alignment(value); + last_calibration_ = value; + if (!config_.calibration_output.empty()) + { + const std::filesystem::path output(config_.calibration_output); + if (!output.parent_path().empty()) + std::filesystem::create_directories(output.parent_path()); + std::ofstream json(config_.calibration_output, std::ios::trunc); + if (!json) + throw std::runtime_error("Unable to open calibration output: " + config_.calibration_output); + const auto write_intrinsics = + [&json](const char* name, const std::shared_ptr& intrinsics) + { + json << " \"" << name << "\": "; + if (!intrinsics) + { + json << "null"; + return; + } + json << "{\"width\":" << intrinsics->width << ",\"height\":" << intrinsics->height + << ",\"fx\":" << intrinsics->fx << ",\"fy\":" << intrinsics->fy << ",\"cx\":" << intrinsics->cx + << ",\"cy\":" << intrinsics->cy << ",\"distortion_model\":" << intrinsics->distortion_model + << ",\"distortion\":["; + for (size_t index = 0; index < intrinsics->distortion.size(); ++index) + json << (index == 0 ? "" : ",") << intrinsics->distortion[index]; + json << "]}"; + }; + const auto write_extrinsics = [&json]( + const char* name, const std::shared_ptr& extrinsics) + { + json << " \"" << name << "\": "; + if (!extrinsics) + { + json << "null"; + return; + } + json << "{\"rotation\":["; + for (size_t index = 0; index < extrinsics->rotation.size(); ++index) + json << (index == 0 ? "" : ",") << extrinsics->rotation[index]; + json << "],\"translation_mm\":["; + for (size_t index = 0; index < extrinsics->translation_mm.size(); ++index) + json << (index == 0 ? "" : ",") << extrinsics->translation_mm[index]; + json << "]}"; + }; + json << "{\n \"device_uid\": \"" << json_escape(value.device_uid) << "\",\n"; + write_intrinsics("color_left", value.color_left); + json << ",\n"; + write_intrinsics("color_right", value.color_right); + json << ",\n"; + write_extrinsics("left_to_right", value.left_to_right); + json << ",\n"; + write_extrinsics("accel_to_left", value.accel_to_left); + json << ",\n"; + write_extrinsics("gyro_to_left", value.gyro_to_left); + json << ",\n \"raw_alignment_yaml\": \"" << json_escape(value.raw_alignment_yaml) + << "\",\n \"raw_imu_yaml\": \"" << json_escape(value.raw_imu_yaml) + << "\",\n \"time_alignment\": " << value.time_alignment->json << "\n}\n"; + if (!json) + throw std::runtime_error("Unable to write calibration output: " + config_.calibration_output); + } + enqueue(CalibrationEvent{ std::move(value), now }); + } + + void start_device_state() + { + check_cancelled(); + device_->setEgoStateCallback( + [this](const OBEgoStateReport& report) + { + if (!accepting_callbacks_.load(std::memory_order_acquire)) + return; + if (!capture_active_.load(std::memory_order_acquire)) + return; + const auto now = core::os_monotonic_now_ns(); + auto state = sdk_report(diagnostic_origin(now), report.sequence, report.work_state, report.state_flags, + report.error_flags, report.storage_free_bytes); + enqueue(DeviceStateEvent{ std::move(state), now }); + }); + } + + void publish_periodic_device_state(const std::string& final_error = {}, bool direct = false) + { + if (selected_device_uid_.empty()) + return; + // Disconnect detection belongs to removal callbacks and existing stream liveness checks. + core::EgoCaptureSummaryT summary; + uint64_t dropped_video_frame_sets = 0; + { + std::lock_guard lock(video_queue_mutex_); + dropped_video_frame_sets = auxiliary_stats_.dropped_video_frame_sets; + } + { + std::lock_guard lock(queue_mutex_); + summary.queue_capacity = static_cast(kMaxQueuedEvents); + summary.queue_peak = static_cast(auxiliary_stats_.publish_queue_peak); + summary.dropped_events = auxiliary_stats_.dropped_events; + if (!async_error_.empty() || summary.dropped_events != 0 || dropped_video_frame_sets != 0) + { + summary.capture_health = core::EgoCaptureHealth_Incomplete; + if (!async_error_.empty()) + summary.reason = async_error_; + else if (summary.dropped_events != 0) + summary.reason = "dropped metadata event"; + else + summary.reason = "dropped video frame set"; + } + else if (events_.size() >= kMaxQueuedEvents * 85 / 100) + { + summary.capture_health = core::EgoCaptureHealth_Warning; + summary.reason = "metadata queue reached 85 percent capacity"; + } + else + summary.capture_health = core::EgoCaptureHealth_Healthy; + } + if (!final_error.empty()) + { + summary.capture_health = core::EgoCaptureHealth_Incomplete; + summary.reason = final_error; + } + const auto now = core::os_monotonic_now_ns(); + summary.dropped_video_frame_sets = dropped_video_frame_sets; + summary.is_final = direct; + auto state = capture_summary(diagnostic_origin(now), std::move(summary)); + // Producers are frozen at shutdown; bypass the stopped producer queue for the final summary. + if (direct) + { + if (auto* metadata = sink_->metadata_sink()) + metadata->on_device_state(state, now, 0); + } + else + enqueue(DeviceStateEvent{ std::move(state), now }); + last_device_poll_ = std::chrono::steady_clock::now(); + } + +private: + CaptureConfig config_; + TimeAlignment time_alignment_; + std::optional last_calibration_; + std::unique_ptr context_; + std::shared_ptr device_; + std::unique_ptr pipeline_; + std::unique_ptr imu_pipeline_; + std::shared_ptr audio_sensor_; + std::vector streams_; + ActiveProfiles active_profiles_; + std::unique_ptr sink_; +#if defined(EGO_ENABLE_PREVIEW) + std::unique_ptr preview_; +#endif + std::map stats_; + std::map last_video_arrival_ns_; + std::vector original_properties_; + std::vector requested_controls_; + AuxiliaryStats auxiliary_stats_; + WavWriter wav_writer_; + uint32_t audio_rate_ = 0; + uint16_t audio_channels_ = 0; + uint16_t audio_bits_ = 0; + uint64_t audio_sequence_ = 0; + PendingImu accel_pending_; + PendingImu gyro_pending_; + mutable std::mutex queue_mutex_; + mutable std::mutex video_queue_mutex_; + std::condition_variable video_queue_cv_; + std::deque, int64_t>> video_frame_sets_; + std::deque events_; + std::string async_error_; + std::string selected_device_uid_; + std::string selected_device_serial_; + uint16_t selected_device_vid_ = 0; + uint16_t selected_device_pid_ = 0; + std::string selected_firmware_; + std::string initial_profile_description_; + std::string recovery_reason_; + std::string recovery_last_error_; + std::chrono::steady_clock::time_point last_device_poll_{}; + std::chrono::steady_clock::time_point recovery_deadline_{}; + std::chrono::steady_clock::time_point next_reconnect_attempt_{}; + int64_t capture_epoch_started_ns_ = 0; + uint32_t capture_epoch_ = 0; + OBCallbackId device_changed_callback_id_ = 0; + std::atomic accepting_callbacks_{ true }; + std::atomic capture_active_{ false }; + std::atomic device_removed_{ false }; + std::atomic accel_ready_{ false }; + std::atomic gyro_ready_{ false }; + std::atomic audio_ready_{ false }; + std::atomic active_accel_ready_{ false }; + std::atomic active_gyro_ready_{ false }; + std::atomic active_audio_ready_{ false }; + std::atomic last_accel_arrival_ns_{ 0 }; + std::atomic last_gyro_arrival_ns_{ 0 }; + std::atomic last_audio_arrival_ns_{ 0 }; + std::mutex auxiliary_readiness_mutex_; + std::condition_variable auxiliary_readiness_wake_; + bool video_pipeline_started_ = false; + bool imu_pipeline_started_ = false; + bool audio_started_ = false; + bool controls_applied_ = false; + bool device_changed_callback_registered_ = false; + bool recovering_ = false; + bool recovery_device_present_ = false; + bool shutdown_complete_ = false; + std::string cleanup_error_; +}; + +EgoCamera::EgoCamera(const CaptureConfig& config, const std::vector& streams, std::unique_ptr sink) + : impl_(std::make_unique(config, streams, std::move(sink))) +{ +} + +EgoCamera::~EgoCamera() = default; + +void EgoCamera::update() +{ + impl_->update(); +} + +void EgoCamera::close() +{ + impl_->close(); +} + +void EgoCamera::print_stats() const +{ + impl_->print_stats(); +} + +const std::map& EgoCamera::stats() const +{ + return impl_->stats(); +} + +const AuxiliaryStats& EgoCamera::auxiliary_stats() const +{ + return impl_->auxiliary_stats(); +} + +bool EgoCamera::preview_closed() const +{ + return impl_->preview_closed(); +} + +void EgoCamera::list_capabilities(const CaptureConfig& config) +{ + ob::Context context(prepare_sdk_logging()); +#if defined(__linux__) || defined(__ANDROID__) + context.setUvcBackendType(OB_UVC_BACKEND_TYPE_LIBUVC); +#endif + const auto devices = context.queryDeviceList(); + for (uint32_t index = 0; index < devices->getCount(); ++index) + { + const auto device = devices->getDevice(index); + if (config.device_uid.empty() || device->getDeviceInfo()->getUid() == config.device_uid) + { + print_capabilities(device); + return; + } + } + throw std::runtime_error("No matching Ego device found"); +} + +} // namespace plugins::ego diff --git a/src/plugins/ego/core/frame_sink.cpp b/src/plugins/ego/core/frame_sink.cpp new file mode 100644 index 0000000000..5a14988caf --- /dev/null +++ b/src/plugins/ego/core/frame_sink.cpp @@ -0,0 +1,387 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "diagnostics.hpp" +#include "inc/ego_camera/ego_camera.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace plugins::ego +{ + +void validate_stream_config(const StreamConfig& stream, const CaptureConfig& config) +{ + static_cast(config); + switch (stream.pixel_format) + { + case core::EgoPixelFormat_Mjpg: + case core::EgoPixelFormat_H264: + case core::EgoPixelFormat_H265: + return; + default: + throw std::invalid_argument("Unsupported Ego pixel format"); + } +} + +void validate_resolved_stream_config(const StreamConfig& stream, uint32_t resolved_fps) +{ + const bool encoded = + stream.pixel_format == core::EgoPixelFormat_H264 || stream.pixel_format == core::EgoPixelFormat_H265; + if (encoded && resolved_fps > 30) + { + throw std::invalid_argument("resolved encoded profile is " + std::to_string(resolved_fps) + + " FPS, but this build certifies encoded recording only through 30 FPS; " + "no profile fallback was attempted"); + } +} + +class FrameSink::Impl +{ +public: + Impl(const std::vector& streams, std::unique_ptr metadata_sink, bool write_media_sidecars) + : metadata_sink_(std::move(metadata_sink)) + { + for (const auto& stream : streams) + { + std::unique_ptr file; + if (write_media_sidecars) + { + const std::filesystem::path output(stream.output_path); + if (!output.parent_path().empty()) + std::filesystem::create_directories(output.parent_path()); + file = std::make_unique(stream.output_path, std::ios::binary | std::ios::trunc); + if (!*file) + throw std::runtime_error("Unable to open encoded output: " + stream.output_path); + detail::log_info() << "Add stream: " << core::EnumNameEgoCameraStream(stream.camera) << " -> " + << stream.output_path << std::endl; + } + writers_.emplace(stream.camera, Writer{ std::move(file), stream.pixel_format }); + } + } + + void on_frame(const CapturedFrame& frame, const std::function& accepted_frame) + { + const auto it = writers_.find(frame.metadata.stream); + if (it == writers_.end()) + return; + + const bool sequence_gap = + it->second.has_sequence && frame.metadata.sequence_number > it->second.last_sequence_number + 1; + it->second.has_sequence = true; + it->second.last_sequence_number = frame.metadata.sequence_number; + auto encoded_data = it->second.remove_ego_timestamp_sei(frame.encoded_data); + if (encoded_data.empty() || !it->second.accept(encoded_data, sequence_gap)) + return; + + if (it->second.file) + { + it->second.file->write( + reinterpret_cast(encoded_data.data()), static_cast(encoded_data.size())); + if (!*it->second.file) + throw std::runtime_error("Failed while writing Ego encoded data"); + } + + if (!metadata_sink_ && !accepted_frame) + return; + CapturedFrame recorded{ frame.metadata, std::move(encoded_data), frame.sample_time_local_common_clock_ns, + frame.sample_time_raw_device_clock_ns }; + recorded.metadata.encoded_bytes = recorded.encoded_data.size(); + if (metadata_sink_) + { + metadata_sink_->on_frame_metadata(recorded); + metadata_sink_->on_encoded_video_frame(recorded); + } + if (accepted_frame) + accepted_frame(recorded); + } + + void begin_capture_epoch() + { + for (auto& [_, writer] : writers_) + writer.begin_capture_epoch(); + } + + IMetadataSink* metadata_sink() + { + return metadata_sink_.get(); + } + + void close_media() + { + if (media_failed_) + throw std::runtime_error("Ego media sidecar finalization previously failed"); + std::string failures; + for (auto& [stream, writer] : writers_) + { + if (!writer.file || !writer.file->is_open()) + continue; + writer.file->flush(); + bool failed = !*writer.file; + writer.file->close(); + failed = failed || writer.file->fail(); + if (failed) + { + if (!failures.empty()) + failures += "; "; + failures += std::string(core::EnumNameEgoCameraStream(stream)); + } + } + if (!failures.empty()) + { + media_failed_ = true; + throw std::runtime_error("Failed while finalizing Ego media sidecars: " + failures); + } + media_closed_ = true; + } + + void commit_metadata() + { + if (!media_closed_ || media_failed_) + throw std::runtime_error("Cannot commit Ego metadata before media sidecars close successfully"); + if (metadata_sink_) + metadata_sink_->commit(); + } + +private: + struct Writer + { + std::unique_ptr file; + core::EgoPixelFormat format; + bool parameter_sets_ready = false; + bool has_vps = false; + bool has_sps = false; + bool has_pps = false; + bool has_sequence = false; + uint64_t last_sequence_number = 0; + + static size_t start_code_size(const std::vector& bytes, size_t offset) + { + if (offset + 3 <= bytes.size() && bytes[offset] == 0 && bytes[offset + 1] == 0 && bytes[offset + 2] == 1) + return 3; + if (offset + 4 <= bytes.size() && bytes[offset] == 0 && bytes[offset + 1] == 0 && bytes[offset + 2] == 0 && + bytes[offset + 3] == 1) + return 4; + return 0; + } + + static bool is_ego_timestamp_sei(const std::vector& bytes, + size_t payload_begin, + size_t payload_end, + core::EgoPixelFormat format) + { + const auto begin = bytes.begin() + static_cast(payload_begin); + const auto end = bytes.begin() + static_cast(payload_end); + static constexpr std::array kLegacyMarker = { 'O', 'R', 'B', 'B', 'E', 'C', + ',', 'E', 'G', 'O', '_' }; + if (std::search(begin, end, kLegacyMarker.begin(), kLegacyMarker.end()) != end) + return true; + + static constexpr std::string_view kEgo = "EGO"; + static constexpr std::string_view kTimestamp = ",timestamp_us="; + static constexpr std::string_view kH264Frame = ",frameId="; + static constexpr std::string_view kH265Frame = ",frame_seq="; + const auto matches = [&bytes, payload_end](size_t& offset, std::string_view text) + { + if (offset + text.size() > payload_end || + !std::equal(text.begin(), text.end(), bytes.begin() + static_cast(offset))) + { + return false; + } + offset += text.size(); + return true; + }; + const auto consume_decimal = [&bytes, payload_end](size_t& offset) + { + const size_t first = offset; + while (offset < payload_end && bytes[offset] >= '0' && bytes[offset] <= '9') + ++offset; + return offset != first; + }; + + for (auto marker = std::search(begin, end, kEgo.begin(), kEgo.end()); marker != end; + marker = std::search(marker + 1, end, kEgo.begin(), kEgo.end())) + { + size_t offset = static_cast(std::distance(bytes.begin(), marker)); + if (!matches(offset, kEgo)) + continue; + const size_t digits_begin = offset; + if (!consume_decimal(offset) || offset - digits_begin != 10 || offset + 2 > payload_end || + bytes[offset] != '_' || (bytes[offset + 1] != 'L' && bytes[offset + 1] != 'R')) + { + continue; + } + offset += 2; + if (!matches(offset, kTimestamp) || !consume_decimal(offset) || + !matches(offset, format == core::EgoPixelFormat_H264 ? kH264Frame : kH265Frame) || + !consume_decimal(offset)) + { + continue; + } + if (offset + 1 == payload_end && bytes[offset] == 0x80) + return true; + } + return false; + } + + std::vector remove_ego_timestamp_sei(const std::vector& bytes) const + { + if (format == core::EgoPixelFormat_Mjpg) + return bytes; + + std::vector result; + size_t current = 0; + while (current < bytes.size()) + { + const auto code_size = start_code_size(bytes, current); + if (code_size == 0) + return bytes; + const size_t nal_begin = current + code_size; + if (nal_begin >= bytes.size()) + break; + size_t next = nal_begin; + while (next < bytes.size() && start_code_size(bytes, next) == 0) + ++next; + const uint8_t nal_type = + format == core::EgoPixelFormat_H264 ? bytes[nal_begin] & 0x1fU : (bytes[nal_begin] >> 1U) & 0x3fU; + const bool is_sei = + format == core::EgoPixelFormat_H264 ? nal_type == 6 : nal_type == 39 || nal_type == 40; + if (!is_sei || !is_ego_timestamp_sei(bytes, nal_begin + 1, next, format)) + result.insert(result.end(), bytes.begin() + static_cast(current), + bytes.begin() + static_cast(next)); + current = next; + } + return result; + } + + bool accept(const std::vector& bytes, bool sequence_gap) + { + if (format == core::EgoPixelFormat_Mjpg) + return true; + + if (sequence_gap) + { + // P frames after a missing access unit reference data that is no + // longer in this elementary stream. Resume from a parameterized IDR. + parameter_sets_ready = false; + has_vps = false; + has_sps = false; + has_pps = false; + } + + bool keyframe = false; + bool picture = false; + for (size_t offset = 0; offset + 4 < bytes.size();) + { + const auto code_size = start_code_size(bytes, offset); + if (code_size == 0) + { + ++offset; + continue; + } + const size_t nal_offset = offset + code_size; + if (nal_offset >= bytes.size()) + break; + if (format == core::EgoPixelFormat_H264) + { + const uint8_t nal_type = bytes[nal_offset] & 0x1fU; + has_sps = has_sps || nal_type == 7; + has_pps = has_pps || nal_type == 8; + keyframe = keyframe || nal_type == 5; + picture = picture || (nal_type >= 1 && nal_type <= 5); + } + else + { + const uint8_t nal_type = (bytes[nal_offset] >> 1U) & 0x3fU; + has_vps = has_vps || nal_type == 32; + has_sps = has_sps || nal_type == 33; + has_pps = has_pps || nal_type == 34; + keyframe = keyframe || nal_type == 19 || nal_type == 20 || nal_type == 21; + picture = picture || nal_type <= 31; + } + offset = nal_offset + 1; + } + if (!parameter_sets_ready) + parameter_sets_ready = keyframe && has_sps && has_pps && (format == core::EgoPixelFormat_H264 || has_vps); + // Ego emits a separate SEI-only frame that carries a device timestamp. + // Timestamp metadata is recorded independently; a standalone SEI is not a + // decodable elementary-video access unit and must not be appended to media. + return parameter_sets_ready && picture; + } + + void begin_capture_epoch() + { + parameter_sets_ready = false; + has_vps = false; + has_sps = false; + has_pps = false; + has_sequence = false; + } + }; + + std::map writers_; + std::unique_ptr metadata_sink_; + bool media_closed_ = false; + bool media_failed_ = false; +}; + +FrameSink::FrameSink(const std::vector& streams, + std::unique_ptr metadata_sink, + bool write_media_sidecars) + : impl_(std::make_unique(streams, std::move(metadata_sink), write_media_sidecars)) +{ +} + +FrameSink::~FrameSink() = default; + +void FrameSink::on_frame(const CapturedFrame& frame, const std::function& accepted_frame) +{ + impl_->on_frame(frame, accepted_frame); +} + +void FrameSink::begin_capture_epoch() +{ + impl_->begin_capture_epoch(); +} + +IMetadataSink* FrameSink::metadata_sink() +{ + return impl_->metadata_sink(); +} + +void FrameSink::close_media() +{ + impl_->close_media(); +} + +void FrameSink::close_metadata() +{ + if (auto* sink = impl_->metadata_sink()) + sink->close(); +} + +void FrameSink::abort_metadata() +{ + if (auto* sink = impl_->metadata_sink()) + sink->abort(); +} + +std::string FrameSink::metadata_error() const +{ + if (const auto* sink = impl_->metadata_sink()) + return sink->error(); + return {}; +} + +void FrameSink::commit_metadata() +{ + impl_->commit_metadata(); +} + +} // namespace plugins::ego diff --git a/src/plugins/ego/core/inc/ego_camera/cancellation.hpp b/src/plugins/ego/core/inc/ego_camera/cancellation.hpp new file mode 100644 index 0000000000..46faad6456 --- /dev/null +++ b/src/plugins/ego/core/inc/ego_camera/cancellation.hpp @@ -0,0 +1,40 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 ORBBEC INC. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "ego_camera.hpp" + +#include +#include +#include +#include + +namespace plugins::ego +{ +inline void check_capture_cancelled(const std::function& cancelled) +{ + if (cancelled && cancelled()) + throw CaptureCancelled(); +} + +template +bool wait_capture_ready(std::condition_variable& wake, + std::unique_lock& lock, + const std::chrono::duration& timeout, + Predicate ready, + const std::function& cancelled) +{ + const auto deadline = std::chrono::steady_clock::now() + timeout; + while (!ready()) + { + check_capture_cancelled(cancelled); + const auto now = std::chrono::steady_clock::now(); + if (now >= deadline) + return false; + wake.wait_until(lock, std::min(deadline, now + std::chrono::milliseconds(20))); + } + check_capture_cancelled(cancelled); + return true; +} +} // namespace plugins::ego diff --git a/src/plugins/ego/core/inc/ego_camera/ego_camera.hpp b/src/plugins/ego/core/inc/ego_camera/ego_camera.hpp new file mode 100644 index 0000000000..a4763dd37c --- /dev/null +++ b/src/plugins/ego/core/inc/ego_camera/ego_camera.hpp @@ -0,0 +1,242 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace plugins::ego +{ + +struct StreamConfig +{ + core::EgoCameraStream camera; + std::string output_path; + core::EgoPixelFormat pixel_format = core::EgoPixelFormat_Mjpg; + uint32_t width = 0; + uint32_t height = 0; + uint32_t fps = 0; +}; + +enum class McapMediaMode +{ + MetadataOnly, + Embedded, +}; + +struct PropertySetting +{ + std::string name; + double value = 0.0; +}; + +class CaptureCancelled : public std::runtime_error +{ +public: + CaptureCancelled() : std::runtime_error("Ego capture cancelled") + { + } +}; + +struct CaptureConfig +{ + std::function stop_requested; + std::string device_uid; + uint32_t width = 0; + uint32_t height = 0; + uint32_t fps = 0; + uint32_t bitrate = 0; + bool dynamic_bitrate = false; + bool dynamic_bitrate_set = false; + bool preview = false; + bool enable_imu = false; + uint32_t imu_rate = 400; + // These are requests, not a PID contract. start_imu validates them against + // the profiles enumerated from the selected device. + float accel_full_scale_g = 24.0f; + float gyro_full_scale_dps = 2000.0f; + bool enable_audio = false; + std::string audio_output; + std::string collection_prefix; + std::string plugin_root_id; + bool record_device_diagnostics = true; + std::string mcap_filename; + std::string mcap_media_spool; + McapMediaMode mcap_media_mode = McapMediaMode::MetadataOnly; + bool keep_media_sidecars = false; + std::string calibration_output; + std::string time_alignment; + std::vector properties; + bool persist_controls = false; + uint32_t reconnect_timeout_seconds = 30; + uint32_t reconnect_interval_milliseconds = 1000; +}; + +// Validates device-independent stream fields. Profile availability and +// simultaneous-stream compatibility are resolved from the selected device. +void validate_stream_config(const StreamConfig& stream, const CaptureConfig& config); +// Applies release certification policy to the SDK-resolved profile, including +// requests where zero selected the SDK default. +void validate_resolved_stream_config(const StreamConfig& stream, uint32_t resolved_fps); + +struct CapturedFrame +{ + core::EgoFrameMetadataT metadata; + std::vector encoded_data; + int64_t sample_time_local_common_clock_ns = 0; + int64_t sample_time_raw_device_clock_ns = 0; +}; + +// Facts are refreshed by the capture owner; a publishing thread cannot refresh their age. +enum class CaptureAvailability +{ + Unknown, + Absent, + Starting, + Streaming, + Recovering, + Failed +}; +struct CaptureStatus +{ + CaptureAvailability availability = CaptureAvailability::Unknown; + int64_t observed_at_ns = 0; + std::string error; +}; + +class IMetadataSink +{ +public: + virtual ~IMetadataSink() = default; + // Payload references are borrowed; copy any data retained after the call returns. + // Timestamp pairs are (local common-clock ns, raw device-clock ns). + virtual void on_frame_metadata(const CapturedFrame& frame) = 0; + virtual void on_imu_batch(const core::EgoImuBatchT&, int64_t, int64_t) + { + } + virtual void on_audio_chunk(const core::EgoAudioChunkT&, int64_t, int64_t) + { + } + virtual void on_encoded_video_frame(const CapturedFrame&) + { + } + virtual void on_pcm_audio_chunk(const core::EgoPcmAudioChunkT&, int64_t, int64_t) + { + } + virtual void on_calibration(const core::EgoCalibrationT&, int64_t, int64_t) + { + } + virtual void on_device_state(const core::EgoDeviceStateT&, int64_t, int64_t) + { + } + virtual void on_capture_status(const CaptureStatus&) + { + } + virtual void close() + { + } + // Sealing and promotion are separate so producer cleanup can veto delivery. + virtual void commit() + { + } + virtual void abort() + { + close(); + } + virtual std::string error() const + { + return {}; + } +}; + +std::unique_ptr compose_metadata_sinks(std::vector> sinks); + +class FrameSink +{ +public: + FrameSink(const std::vector& streams, + std::unique_ptr metadata_sink = nullptr, + bool write_media_sidecars = true); + ~FrameSink(); + + FrameSink(const FrameSink&) = delete; + FrameSink& operator=(const FrameSink&) = delete; + + // The observer runs synchronously after the recording gate, with canonical bytes. + void on_frame(const CapturedFrame& frame, const std::function& accepted_frame = {}); + void begin_capture_epoch(); + IMetadataSink* metadata_sink(); + void close_media(); + void close_metadata(); + void commit_metadata(); + void abort_metadata(); + std::string metadata_error() const; + +private: + class Impl; + std::unique_ptr impl_; +}; + +std::unique_ptr create_frame_sink(const std::vector& streams, + const std::string& collection_prefix); +std::unique_ptr create_frame_sink(const std::vector& streams, const CaptureConfig& config); + +struct StreamStats +{ + uint64_t frame_count = 0; + uint64_t byte_count = 0; + uint64_t sequence_gaps = 0; + uint64_t last_sequence = 0; + int64_t last_device_timestamp_ns = 0; + uint64_t epoch_frame_count = 0; +}; + +struct AuxiliaryStats +{ + uint64_t accel_samples = 0; + uint64_t gyro_samples = 0; + uint64_t audio_samples = 0; + uint64_t publish_queue_peak = 0; + uint64_t dropped_events = 0; + uint64_t dropped_video_frame_sets = 0; + uint32_t capture_epoch = 0; + uint32_t reconnect_attempts = 0; + uint32_t successful_reconnects = 0; +}; + +class EgoCamera +{ +public: + EgoCamera(const CaptureConfig& config, const std::vector& streams, std::unique_ptr sink); + ~EgoCamera(); + + EgoCamera(const EgoCamera&) = delete; + EgoCamera& operator=(const EgoCamera&) = delete; + + void update(); + void close(); + void print_stats() const; + const std::map& stats() const; + const AuxiliaryStats& auxiliary_stats() const; + bool preview_closed() const; + + static void list_capabilities(const CaptureConfig& config); + +private: + class Impl; + std::unique_ptr impl_; +}; + +} // namespace plugins::ego diff --git a/src/plugins/ego/core/inc/ego_camera/time_alignment.hpp b/src/plugins/ego/core/inc/ego_camera/time_alignment.hpp new file mode 100644 index 0000000000..18dc03f475 --- /dev/null +++ b/src/plugins/ego/core/inc/ego_camera/time_alignment.hpp @@ -0,0 +1,62 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include + +#include +#include +#include + +namespace plugins::ego +{ + +// Syntax/trust checks require no SDK context and precede output creation. +void validate_time_alignment_profile(const std::string& path); + +// Hash the SDK library that supplies the version symbol, not a configured path. +std::string loaded_sdk_binary_sha256(); + +int64_t sdk_timestamp_ns(uint64_t timestamp_us); + +struct TimingIdentity +{ + std::string serial; + std::string firmware; + std::string sdk_version; + std::string video_format; + uint32_t width = 0; + uint32_t height = 0; + uint32_t fps = 0; + uint32_t imu_rate_hz = 0; + uint32_t audio_rate_hz = 0; + uint16_t audio_channels = 0; + uint16_t audio_bits = 0; + bool right_enabled = true; + std::string sdk_binary_sha256; +}; + +// The SDK audio path has no physical sample-loss counter. Its retained PCM +// index is usable for slicing, but never certifies hardware continuity. +class TimeAlignment +{ +public: + explicit TimeAlignment(const std::string& capture_profile_path = {}); + ~TimeAlignment(); + TimeAlignment(const TimeAlignment&) = delete; + TimeAlignment& operator=(const TimeAlignment&) = delete; + void configure(const TimingIdentity& identity); + void begin_epoch(uint32_t epoch); + bool observe_left(int64_t raw_ns, int64_t arrival_ns); + int64_t sample_time(const std::string& stream, int64_t raw_ns, int64_t arrival_ns); + core::EgoAudioTimingT audio_timing(int64_t raw_ns, uint32_t sample_count, uint32_t sample_rate); + bool has_update() const; + std::string snapshot() const; + +private: + class Impl; + std::unique_ptr impl_; +}; + +} // namespace plugins::ego diff --git a/src/plugins/ego/core/mcap_metadata_sink.cpp b/src/plugins/ego/core/mcap_metadata_sink.cpp new file mode 100644 index 0000000000..29b0a10e24 --- /dev/null +++ b/src/plugins/ego/core/mcap_metadata_sink.cpp @@ -0,0 +1,311 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "diagnostics.hpp" +#include "metadata_sink.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace plugins::ego +{ + +namespace +{ +class McapMetadataSink final : public IMetadataSink +{ +public: + McapMetadataSink(const std::vector& streams, + const std::string& filename, + bool include_media, + bool include_structured, + bool record_device_diagnostics) + : final_filename_(filename), temporary_filename_(filename + ".partial") + { + if (std::filesystem::exists(final_filename_) || std::filesystem::is_symlink(final_filename_) || + std::filesystem::exists(temporary_filename_) || std::filesystem::is_symlink(temporary_filename_)) + throw std::runtime_error("Ego MCAP output already exists: " + filename); + try + { + mcap::McapWriterOptions options("ego"); + options.compression = mcap::Compression::None; + const std::filesystem::path output_path(filename); + if (!output_path.parent_path().empty()) + std::filesystem::create_directories(output_path.parent_path()); + output_.open(temporary_filename_); + writer_.open(output_, options); + std::vector video_names; + for (const auto& stream : streams) + video_names.emplace_back(core::EnumNameEgoCameraStream(stream.camera)); + if (include_structured) + { + video_ = std::make_unique>( + writer_, "ego_metadata", video_names); + } + for (size_t index = 0; index < streams.size(); ++index) + video_indices_.emplace(streams[index].camera, index); + if (include_structured) + { + imu_ = std::make_unique>( + writer_, "ego_imu", std::vector{ "Accel", "Gyro" }); + audio_ = std::make_unique>( + writer_, "ego_audio", std::vector{ "Audio" }); + calibration_ = std::make_unique>( + writer_, "ego_calibration", std::vector{ "Calibration" }); + if (record_device_diagnostics) + device_state_ = std::make_unique>( + writer_, "ego_device", std::vector{ "DeviceState" }); + } + mcap::Metadata policy; + policy.name = "ego.recording"; + policy.metadata = { { "record_device_diagnostics", record_device_diagnostics ? "on" : "off" }, + { "diagnostic_version", "2" } }; + const auto policy_status = writer_.write(policy); + if (!policy_status.ok()) + throw std::runtime_error("Unable to write EGO recording policy: " + policy_status.message); + if (include_media) + { + media_video_ = std::make_unique>( + writer_, "ego_media", video_names); + media_audio_ = std::make_unique>( + writer_, "ego_media", std::vector{ "Audio" }); + } + } + catch (...) + { + writer_.terminate(); + throw; + } + } + + ~McapMetadataSink() override + { + try + { + abort(); + } + catch (const std::exception& error) + { + // Destructors run during capture-error unwinding. Preserve the primary + // failure rather than terminating while attempting to write the footer. + detail::log_error() << "Ego MCAP shutdown failed: " << error.what() << std::endl; + } + } + + void close() override + { + if (closed_) + return; + try + { + writer_.close(); + output_.end(); + closed_ = true; + closed_successfully_ = true; + } + catch (...) + { + // McapWriter retries close from its destructor unless it is reset. + // Terminate after an I/O failure so an error-path footer retry cannot abort. + writer_.terminate(); + closed_ = true; + throw; + } + } + + void commit() override + { + if (committed_) + return; + if (!closed_successfully_ || aborted_) + throw std::runtime_error("Cannot commit unfinished Ego MCAP output"); + if (std::filesystem::exists(final_filename_) || std::filesystem::is_symlink(final_filename_)) + throw std::runtime_error("Ego MCAP final output appeared before commit: " + final_filename_); + std::filesystem::rename(temporary_filename_, final_filename_); + committed_ = true; + } + + void abort() override + { + aborted_ = true; + close(); + } + + void on_frame_metadata(const CapturedFrame& frame) override + { + if (video_) + write_record(*video_, video_indices_.at(frame.metadata.stream), + frame.metadata, frame.sample_time_local_common_clock_ns, + frame.sample_time_raw_device_clock_ns); + } + void on_imu_batch(const core::EgoImuBatchT& batch, int64_t local_ns, int64_t device_ns) override + { + if (imu_) + write_record(*imu_, static_cast(batch.sensor), batch, local_ns, device_ns); + } + void on_audio_chunk(const core::EgoAudioChunkT& chunk, int64_t local_ns, int64_t device_ns) override + { + if (audio_) + write_record(*audio_, 0, chunk, local_ns, device_ns); + } + void on_calibration(const core::EgoCalibrationT& calibration, int64_t local_ns, int64_t device_ns) override + { + if (calibration_) + write_record(*calibration_, 0, calibration, local_ns, device_ns); + } + void on_device_state(const core::EgoDeviceStateT& state, int64_t local_ns, int64_t device_ns) override + { + if (device_state_) + write_record(*device_state_, 0, state, local_ns, device_ns); + } + void on_encoded_video_frame(const CapturedFrame& frame) override + { + if (!media_video_) + return; + core::EgoEncodedVideoFrameT data; + data.stream = frame.metadata.stream; + data.sequence_number = frame.metadata.sequence_number; + data.width = frame.metadata.width; + data.height = frame.metadata.height; + data.fps = frame.metadata.fps; + data.pixel_format = frame.metadata.pixel_format; + data.encoded_data = frame.encoded_data; + data.capture_epoch = frame.metadata.capture_epoch; + write_record(*media_video_, video_indices_.at(data.stream), data, + frame.sample_time_local_common_clock_ns, + frame.sample_time_raw_device_clock_ns); + } + void on_pcm_audio_chunk(const core::EgoPcmAudioChunkT& chunk, int64_t local_ns, int64_t device_ns) override + { + if (media_audio_) + write_record(*media_audio_, 0, chunk, local_ns, device_ns); + } + +private: + template + void write_record(core::McapTrackerChannels& channels, + size_t index, + const core::record_payload_t& value, + int64_t local_ns, + int64_t device_ns) + { + try + { + if (closed_) + throw std::runtime_error("Attempted to write a closed Ego MCAP output"); + ego_mcap::write_checked( + writer_, channels, index, core::pack_record(&value, timestamp(local_ns, device_ns))); + } + catch (...) + { + aborted_ = true; + throw; + } + } + + class CheckedMcapFileWriter final : public mcap::IWritable + { + public: + ~CheckedMcapFileWriter() override + { + try + { + end(); + } + catch (const std::exception&) + { + } + } + + void open(const std::string& filename) + { + end(); + size_ = 0; + // Exclusive creation preserves a prior failed capture, including dangling symlinks. + file_ = std::fopen(filename.c_str(), "wbx"); + if (!file_) + throw std::runtime_error("Unable to open Ego MCAP output: " + filename + ": " + std::strerror(errno)); + } + + void end() override + { + if (!file_) + return; + FILE* const file = std::exchange(file_, nullptr); + const int flush_status = std::fflush(file); + const int close_status = std::fclose(file); + if (flush_status != 0 || close_status != 0) + throw std::runtime_error("Failed while closing Ego MCAP output: " + std::string(std::strerror(errno))); + } + + uint64_t size() const override + { + return size_; + } + + protected: + void handleWrite(const std::byte* data, uint64_t size) override + { + if (!file_) + throw std::runtime_error("Attempted to write a closed Ego MCAP output"); + const size_t written = std::fwrite(data, 1, static_cast(size), file_); + if (written != size) + throw std::runtime_error("Failed while writing Ego MCAP output: " + std::string(std::strerror(errno))); + size_ += size; + } + + private: + FILE* file_ = nullptr; + uint64_t size_ = 0; + }; + + static core::DeviceDataTimestamp timestamp(int64_t local_ns, int64_t device_ns) + { + return core::DeviceDataTimestamp(core::os_monotonic_now_ns(), local_ns, device_ns); + } + + CheckedMcapFileWriter output_; + mcap::McapWriter writer_; + std::string final_filename_; + std::string temporary_filename_; + std::unique_ptr> video_; + std::unique_ptr> imu_; + std::unique_ptr> audio_; + std::unique_ptr> calibration_; + std::unique_ptr> device_state_; + std::unique_ptr> media_video_; + std::unique_ptr> media_audio_; + std::map video_indices_; + bool closed_ = false; + bool closed_successfully_ = false; + bool aborted_ = false; + bool committed_ = false; +}; + +} + +std::unique_ptr make_mcap_metadata_sink(const std::vector& streams, + const std::string& filename, + bool include_media, + bool include_structured, + bool record_device_diagnostics) +{ + return std::make_unique( + streams, filename, include_media, include_structured, record_device_diagnostics); +} + +} // namespace plugins::ego diff --git a/src/plugins/ego/core/metadata_sink.cpp b/src/plugins/ego/core/metadata_sink.cpp new file mode 100644 index 0000000000..288a97c0c2 --- /dev/null +++ b/src/plugins/ego/core/metadata_sink.cpp @@ -0,0 +1,199 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "metadata_sink.hpp" + +#include "diagnostics.hpp" + +#include +#include +#include +#include +#include + +namespace plugins::ego +{ + +namespace +{ +class CompositeMetadataSink final : public IMetadataSink +{ +public: + explicit CompositeMetadataSink(std::vector> sinks) : sinks_(std::move(sinks)) + { + } + void on_frame_metadata(const CapturedFrame& frame) override + { + for_each([&](auto& sink) { sink.on_frame_metadata(frame); }); + } + void on_imu_batch(const core::EgoImuBatchT& value, int64_t local, int64_t device) override + { + for_each([&](auto& sink) { sink.on_imu_batch(value, local, device); }); + } + void on_audio_chunk(const core::EgoAudioChunkT& value, int64_t local, int64_t device) override + { + for_each([&](auto& sink) { sink.on_audio_chunk(value, local, device); }); + } + void on_calibration(const core::EgoCalibrationT& value, int64_t local, int64_t device) override + { + for_each([&](auto& sink) { sink.on_calibration(value, local, device); }); + } + void on_device_state(const core::EgoDeviceStateT& value, int64_t local, int64_t device) override + { + for_each([&](auto& sink) { sink.on_device_state(value, local, device); }); + } + void on_encoded_video_frame(const CapturedFrame& frame) override + { + for_each([&](auto& sink) { sink.on_encoded_video_frame(frame); }); + } + void on_pcm_audio_chunk(const core::EgoPcmAudioChunkT& value, int64_t local, int64_t device) override + { + for_each([&](auto& sink) { sink.on_pcm_audio_chunk(value, local, device); }); + } + void on_capture_status(const CaptureStatus& status) override + { + for_each([&](auto& sink) { sink.on_capture_status(status); }); + } + void close() override + { + closed_successfully_ = false; + finish([](IMetadataSink& sink) { sink.close(); }); + closed_successfully_ = true; + } + void commit() override + { + if (!closed_successfully_) + throw std::runtime_error("Cannot commit Ego metadata before every sink closes successfully"); + for_each([](IMetadataSink& sink) { sink.commit(); }); + } + void abort() override + { + closed_successfully_ = false; + finish([](IMetadataSink& sink) { sink.abort(); }); + } + std::string error() const override + { + for (const auto& sink : sinks_) + if (!sink->error().empty()) + return sink->error(); + return {}; + } + +private: + template + void finish(Finish&& finish_sink) + { + std::string failures; + // Seal local archives first; promotion waits for every sink to close. + for (size_t offset = 0; offset < sinks_.size(); ++offset) + { + const size_t index = sinks_.size() - offset - 1; + try + { + finish_sink(*sinks_[index]); + } + catch (const std::exception& error) + { + if (!failures.empty()) + failures += "; "; + failures += "sink " + std::to_string(index) + ": " + error.what(); + } + catch (...) + { + if (!failures.empty()) + failures += "; "; + failures += "sink " + std::to_string(index) + ": unknown error"; + } + } + if (!failures.empty()) + throw std::runtime_error(failures); + } + template + void for_each(Function&& function) + { + for (auto& sink : sinks_) + function(*sink); + } + bool closed_successfully_ = false; + std::vector> sinks_; +}; + +} + +std::unique_ptr compose_metadata_sinks(std::vector> sinks) +{ + return std::make_unique(std::move(sinks)); +} + +std::unique_ptr create_frame_sink(const std::vector& streams, + const std::string& collection_prefix) +{ + std::unique_ptr metadata_sink; + if (!collection_prefix.empty()) + metadata_sink = make_schema_metadata_sink(streams, collection_prefix); + return std::make_unique(streams, std::move(metadata_sink)); +} + +std::unique_ptr create_frame_sink(const std::vector& streams, const CaptureConfig& config) +{ + if (!config.collection_prefix.empty() && !config.mcap_filename.empty()) + throw std::invalid_argument("--collection-prefix and --mcap-filename are mutually exclusive"); + if (config.mcap_media_mode == McapMediaMode::Embedded && config.collection_prefix.empty() && + config.mcap_filename.empty()) + throw std::invalid_argument("--mcap-media=embedded requires --mcap-filename or --collection-prefix"); + if (config.mcap_media_mode == McapMediaMode::Embedded && !config.collection_prefix.empty() && + config.mcap_media_spool.empty()) + { + throw std::invalid_argument( + "--mcap-media=embedded with --collection-prefix requires --mcap-media-spool=PATH; " + "the TeleopSession merger consumes this media fragment after the session closes"); + } + std::set outputs; + const auto require_new_output = [&outputs](const std::string& path) + { + if (path.empty()) + return; + if (std::filesystem::exists(path)) + throw std::invalid_argument("Ego output already exists; refusing to overwrite: " + path); + if (!outputs.insert(std::filesystem::weakly_canonical(std::filesystem::absolute(path))).second) + throw std::invalid_argument("Ego outputs refer to the same path: " + path); + }; + if (config.mcap_media_mode == McapMediaMode::MetadataOnly || config.keep_media_sidecars) + { + for (const auto& stream : streams) + require_new_output(stream.output_path); + require_new_output(config.audio_output); + } + require_new_output(config.calibration_output); + require_new_output(config.mcap_filename); + if (config.mcap_media_mode == McapMediaMode::Embedded) + require_new_output(config.mcap_media_spool); + std::vector> sinks; + if (!config.collection_prefix.empty()) + sinks.push_back(make_schema_metadata_sink(streams, config.collection_prefix, config.plugin_root_id)); + else if (!config.plugin_root_id.empty()) + { + try + { + sinks.push_back(make_schema_metadata_sink(streams, {}, config.plugin_root_id)); + } + catch (const std::exception& error) + { + // Local recording must not acquire a mandatory OpenXR dependency for monitoring. + detail::log_warning() << "Ego device status session unavailable; local capture continues: " << error.what(); + } + } + if (!config.mcap_filename.empty()) + sinks.push_back(make_mcap_metadata_sink(streams, config.mcap_filename, + config.mcap_media_mode == McapMediaMode::Embedded, true, + config.record_device_diagnostics)); + if (!config.collection_prefix.empty() && config.mcap_media_mode == McapMediaMode::Embedded) + sinks.push_back( + make_mcap_metadata_sink(streams, config.mcap_media_spool, true, true, config.record_device_diagnostics)); + auto metadata_sink = sinks.empty() ? nullptr : compose_metadata_sinks(std::move(sinks)); + return std::make_unique( + streams, std::move(metadata_sink), + config.mcap_media_mode == McapMediaMode::MetadataOnly || config.keep_media_sidecars); +} + +} // namespace plugins::ego diff --git a/src/plugins/ego/core/metadata_sink.hpp b/src/plugins/ego/core/metadata_sink.hpp new file mode 100644 index 0000000000..1e4f090dfd --- /dev/null +++ b/src/plugins/ego/core/metadata_sink.hpp @@ -0,0 +1,18 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "inc/ego_camera/ego_camera.hpp" + +namespace plugins::ego +{ +std::unique_ptr make_schema_metadata_sink(const std::vector& streams, + const std::string& prefix, + const std::string& plugin_root_id = {}); +std::unique_ptr make_mcap_metadata_sink(const std::vector& streams, + const std::string& filename, + bool include_media, + bool include_structured = true, + bool record_device_diagnostics = true); +} // namespace plugins::ego diff --git a/src/plugins/ego/core/preview.cpp b/src/plugins/ego/core/preview.cpp new file mode 100644 index 0000000000..4c50afe1d5 --- /dev/null +++ b/src/plugins/ego/core/preview.cpp @@ -0,0 +1,415 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "preview.hpp" + +#include "diagnostics.hpp" + +#include +extern "C" +{ +#include +#include +#include +} +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace plugins::ego +{ +namespace +{ + +struct RgbFrame +{ + uint32_t width = 0; + uint32_t height = 0; + std::vector pixels; + int64_t device_timestamp_ns = 0; + uint32_t capture_epoch = 0; +}; + +struct Texture +{ + SDL_Texture* handle = nullptr; + uint32_t width = 0; + uint32_t height = 0; + + ~Texture() + { + if (handle) + SDL_DestroyTexture(handle); + } + + void update(SDL_Renderer* renderer, const RgbFrame& image) + { + if (!handle || width != image.width || height != image.height) + { + if (handle) + SDL_DestroyTexture(handle); + handle = SDL_CreateTexture(renderer, SDL_PIXELFORMAT_RGB24, SDL_TEXTUREACCESS_STREAMING, + static_cast(image.width), static_cast(image.height)); + width = image.width; + height = image.height; + } + if (handle) + SDL_UpdateTexture(handle, nullptr, image.pixels.data(), static_cast(image.width * 3)); + } +}; + +class Decoder +{ +public: + explicit Decoder(core::EgoPixelFormat format) : format_(format) + { + if (format_ == core::EgoPixelFormat_Mjpg) + { + converter_ = std::make_unique(); + converter_->setFormatConvertType(FORMAT_MJPG_TO_RGB); + return; + } + const auto codec_id = format_ == core::EgoPixelFormat_H264 ? AV_CODEC_ID_H264 : AV_CODEC_ID_HEVC; + const AVCodec* codec = avcodec_find_decoder(codec_id); + if (!codec) + throw std::runtime_error("FFmpeg decoder unavailable for Ego preview"); + try + { + codec_ = avcodec_alloc_context3(codec); + if (!codec_) + throw std::runtime_error("Unable to allocate FFmpeg Ego preview decoder"); + codec_->pkt_timebase = { 1, 1'000'000'000 }; + if (avcodec_open2(codec_, codec, nullptr) < 0) + throw std::runtime_error("Unable to open FFmpeg Ego preview decoder"); + decoded_ = av_frame_alloc(); + packet_ = av_packet_alloc(); + if (!decoded_ || !packet_) + throw std::runtime_error("Unable to allocate FFmpeg preview frame or packet"); + } + catch (...) + { + av_packet_free(&packet_); + av_frame_free(&decoded_); + avcodec_free_context(&codec_); + throw; + } + } + + ~Decoder() + { + sws_freeContext(sws_); + av_packet_free(&packet_); + av_frame_free(&decoded_); + avcodec_free_context(&codec_); + } + + bool decode(const CapturedFrame& input, RgbFrame& output) + { + if (format_ == core::EgoPixelFormat_Mjpg) + { + auto source = ob::FrameFactory::createVideoFrameFromBuffer( + input.metadata.stream == core::EgoCameraStream_ColorLeft ? OB_FRAME_COLOR_LEFT : OB_FRAME_COLOR_RIGHT, + OB_FORMAT_MJPG, input.metadata.width, input.metadata.height, + const_cast(input.encoded_data.data()), [](uint8_t*) {}, + static_cast(input.encoded_data.size())); + // SDK conversion may reject a frame; keep the last valid preview image. + const auto converted = converter_->process(source); + if (!converted || !converted->is()) + return false; + const auto rgb = converted->as(); + const uint64_t expected_size = static_cast(input.metadata.width) * input.metadata.height * 3; + if (rgb->getFormat() != OB_FORMAT_RGB || rgb->getWidth() != input.metadata.width || + rgb->getHeight() != input.metadata.height || rgb->getDataSize() != expected_size || !rgb->getData()) + return false; + output.width = rgb->getWidth(); + output.height = rgb->getHeight(); + output.pixels.assign(rgb->getData(), rgb->getData() + rgb->getDataSize()); + output.device_timestamp_ns = input.sample_time_raw_device_clock_ns; + return true; + } + + if (input.encoded_data.size() > static_cast(std::numeric_limits::max())) + throw std::runtime_error("Ego preview packet exceeds FFmpeg's size limit"); + av_packet_unref(packet_); + // Refcounted packets provide FFmpeg's required zero padding and retain buffer ownership. + if (av_new_packet(packet_, static_cast(input.encoded_data.size())) < 0) + throw std::runtime_error("Unable to allocate FFmpeg preview packet data"); + std::memcpy(packet_->data, input.encoded_data.data(), input.encoded_data.size()); + packet_->pts = input.sample_time_raw_device_clock_ns; + bool produced = false; + int status = avcodec_send_packet(codec_, packet_); + if (status == AVERROR(EAGAIN)) + { + produced = receive(input, output); + status = avcodec_send_packet(codec_, packet_); + } + if (status < 0) + throw std::runtime_error("FFmpeg rejected an accepted Ego preview access unit"); + av_packet_unref(packet_); + return receive(input, output) || produced; + } + +private: + bool receive(const CapturedFrame& input, RgbFrame& output) + { + bool produced = false; + while (true) + { + const int status = avcodec_receive_frame(codec_, decoded_); + if (status == AVERROR(EAGAIN) || status == AVERROR_EOF) + return produced; + if (status < 0 || decoded_->decode_error_flags != 0 || + decoded_->width != static_cast(input.metadata.width) || + decoded_->height != static_cast(input.metadata.height)) + throw std::runtime_error("FFmpeg could not decode a valid Ego preview image"); + output.width = static_cast(decoded_->width); + output.height = static_cast(decoded_->height); + output.pixels.resize(static_cast(output.width) * output.height * 3); + output.device_timestamp_ns = decoded_->best_effort_timestamp; + sws_ = sws_getCachedContext(sws_, decoded_->width, decoded_->height, + static_cast(decoded_->format), decoded_->width, decoded_->height, + AV_PIX_FMT_RGB24, SWS_FAST_BILINEAR, nullptr, nullptr, nullptr); + if (!sws_) + throw std::runtime_error("Unable to allocate Ego preview RGB converter"); + uint8_t* destinations[] = { output.pixels.data() }; + int strides[] = { decoded_->width * 3 }; + if (sws_scale(sws_, decoded_->data, decoded_->linesize, 0, decoded_->height, destinations, strides) != + decoded_->height) + throw std::runtime_error("Unable to convert Ego preview image to RGB"); + produced = true; + } + } + + core::EgoPixelFormat format_; + std::unique_ptr converter_; + AVCodecContext* codec_ = nullptr; + AVFrame* decoded_ = nullptr; + AVPacket* packet_ = nullptr; + SwsContext* sws_ = nullptr; +}; + +} // namespace + +class Preview::Impl +{ +public: + Impl() : worker_([this] { run(); }) + { + } + ~Impl() + { + stop_.store(true); + wake_.notify_all(); + if (worker_.joinable()) + worker_.join(); + } + + void submit(const CapturedFrame& frame) noexcept + { + try + { + std::unique_lock lock(mutex_); + if (stop_.load()) + return; + if (frame.metadata.pixel_format == core::EgoPixelFormat_Mjpg) + latest_mjpeg_[frame.metadata.stream] = frame; + else + { + if (encoded_.size() >= kMaxQueuedAccessUnits || frame.encoded_data.size() > kMaxQueuedBytes || + queued_bytes_ > kMaxQueuedBytes - frame.encoded_data.size()) + { + lock.unlock(); + disable("encoded queue exceeded 64 access units or 64 MiB"); + return; + } + encoded_.push_back(frame); + queued_bytes_ += frame.encoded_data.size(); + } + wake_.notify_one(); + } + catch (const std::exception& error) + { + disable(error.what()); + } + } + + bool closed() const + { + return closed_.load(); + } + +private: + void disable(const char* reason) noexcept + { + stop_.store(true); + try + { + std::lock_guard lock(mutex_); + encoded_.clear(); + latest_mjpeg_.clear(); + queued_bytes_ = 0; + detail::log_warning() << "Ego preview disabled: " << reason << "; recording continues"; + } + catch (...) + { + } + wake_.notify_all(); + } + + void run() + { + if (SDL_Init(SDL_INIT_VIDEO) != 0) + { + closed_.store(true); + return; + } + SDL_Window* window = SDL_CreateWindow( + "EGO Preview", SDL_WINDOWPOS_CENTERED, SDL_WINDOWPOS_CENTERED, 1280, 520, SDL_WINDOW_RESIZABLE); + SDL_Renderer* renderer = window ? SDL_CreateRenderer(window, -1, SDL_RENDERER_ACCELERATED) : nullptr; + if (!window || !renderer) + { + if (renderer) + SDL_DestroyRenderer(renderer); + if (window) + SDL_DestroyWindow(window); + SDL_QuitSubSystem(SDL_INIT_VIDEO); + closed_.store(true); + return; + } + std::map> decoders; + std::map epochs; + std::map images; + std::map textures; + const auto decode_frame = [&](const CapturedFrame& frame) + { + const auto stream = frame.metadata.stream; + auto it = decoders.find(stream); + if (it == decoders.end() || epochs.at(stream) != frame.metadata.capture_epoch) + { + decoders[stream] = std::make_unique(frame.metadata.pixel_format); + epochs[stream] = frame.metadata.capture_epoch; + images.erase(stream); + it = decoders.find(stream); + } + RgbFrame image; + if (it->second->decode(frame, image)) + { + image.capture_epoch = frame.metadata.capture_epoch; + images[stream] = std::move(image); + } + }; + try + { + while (!stop_.load()) + { + SDL_Event event; + while (SDL_PollEvent(&event)) + { + if (event.type == SDL_QUIT) + { + closed_.store(true); + stop_.store(true); + } + } + std::deque encoded; + std::map mjpeg; + { + std::unique_lock lock(mutex_); + wake_.wait_for(lock, std::chrono::milliseconds(10), + [this] { return stop_.load() || !encoded_.empty() || !latest_mjpeg_.empty(); }); + encoded.swap(encoded_); + mjpeg.swap(latest_mjpeg_); + queued_bytes_ = 0; + } + // Prediction access units must reach the decoder in order; only decoded images may be stale. + for (const auto& frame : encoded) + { + if (stop_.load()) + break; + decode_frame(frame); + } + for (const auto& [_, frame] : mjpeg) + { + if (stop_.load()) + break; + decode_frame(frame); + } + if (stop_.load()) + break; + const auto left = images.find(core::EgoCameraStream_ColorLeft); + const auto right = images.find(core::EgoCameraStream_ColorRight); + if (left != images.end() && right != images.end() && + (left->second.capture_epoch != right->second.capture_epoch || + std::llabs(left->second.device_timestamp_ns - right->second.device_timestamp_ns) > 5'000'000)) + continue; + + SDL_SetRenderDrawColor(renderer, 0, 0, 0, 255); + SDL_RenderClear(renderer); + int window_width = 0; + int window_height = 0; + SDL_GetRendererOutputSize(renderer, &window_width, &window_height); + int index = 0; + for (const auto& [stream, image] : images) + { + auto& texture = textures[stream]; + texture.update(renderer, image); + if (texture.handle) + { + const int count = static_cast(images.size()); + SDL_Rect target{ index * window_width / count, 0, window_width / count, window_height }; + SDL_RenderCopy(renderer, texture.handle, nullptr, &target); + } + ++index; + } + SDL_RenderPresent(renderer); + } + } + catch (const std::exception& error) + { + disable(error.what()); + } + // Destroy owned textures before SDL_DestroyRenderer frees its associated textures. + textures.clear(); + SDL_DestroyRenderer(renderer); + SDL_DestroyWindow(window); + SDL_QuitSubSystem(SDL_INIT_VIDEO); + } + + std::atomic stop_{ false }; + std::atomic closed_{ false }; + mutable std::mutex mutex_; + std::condition_variable wake_; + // The worker owns at most one additional bounded batch while decoding. + static constexpr size_t kMaxQueuedAccessUnits = 64; + static constexpr size_t kMaxQueuedBytes = 64 * 1024 * 1024; + std::deque encoded_; + size_t queued_bytes_ = 0; + std::map latest_mjpeg_; + std::thread worker_; +}; + +Preview::Preview() : impl_(std::make_unique()) +{ +} +Preview::~Preview() = default; +void Preview::submit(const CapturedFrame& frame) noexcept +{ + impl_->submit(frame); +} +bool Preview::closed() const +{ + return impl_->closed(); +} + +} // namespace plugins::ego diff --git a/src/plugins/ego/core/preview.hpp b/src/plugins/ego/core/preview.hpp new file mode 100644 index 0000000000..a3f00175cd --- /dev/null +++ b/src/plugins/ego/core/preview.hpp @@ -0,0 +1,29 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "inc/ego_camera/ego_camera.hpp" + +#include + +namespace plugins::ego +{ + +class Preview +{ +public: + Preview(); + ~Preview(); + Preview(const Preview&) = delete; + Preview& operator=(const Preview&) = delete; + + void submit(const CapturedFrame& frame) noexcept; + bool closed() const; + +private: + class Impl; + std::unique_ptr impl_; +}; + +} // namespace plugins::ego diff --git a/src/plugins/ego/core/schema_metadata_sink.cpp b/src/plugins/ego/core/schema_metadata_sink.cpp new file mode 100644 index 0000000000..41c1b59ce0 --- /dev/null +++ b/src/plugins/ego/core/schema_metadata_sink.cpp @@ -0,0 +1,349 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "diagnostics.hpp" +#include "metadata_sink.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace plugins::ego +{ + +namespace +{ +constexpr size_t kMaxFlatbufferSize = core::EGO_MAX_FLATBUFFER_SIZE; +constexpr size_t kMaxQueuedEvents = 4096; +class SchemaMetadataSink final : public IMetadataSink +{ +public: + SchemaMetadataSink(const std::vector& streams, + const std::string& collection_prefix, + const std::string& plugin_root_id) + // TeleopSession creates DeviceIO before launching plugins; avoid an unbounded HMD wait. + : session_(std::make_shared( + "EgoCameraPlugin", core::SchemaPusher::get_required_extensions(), false)), + plugin_root_id_(plugin_root_id) + { + if (!collection_prefix.empty()) + { + for (const auto& stream : streams) + { + const std::string name = core::EnumNameEgoCameraStream(stream.camera); + const std::string collection_id = collection_prefix + "/" + name; + pushers_.emplace(stream.camera, std::make_unique( + session_->get_handles(), + core::SchemaPusherConfig{ .collection_id = collection_id, + .max_flatbuffer_size = kMaxFlatbufferSize, + .tensor_identifier = "frame_metadata", + .localized_name = "Ego frame metadata", + .app_name = "EgoCameraPlugin" })); + detail::log_info() << " Metadata: " << collection_id << std::endl; + } + imu_pushers_.emplace( + core::EgoImuSensor_Accel, make_pusher(collection_prefix + "/Accel", "imu_batch", "Ego accelerometer")); + imu_pushers_.emplace( + core::EgoImuSensor_Gyro, make_pusher(collection_prefix + "/Gyro", "imu_batch", "Ego gyroscope")); + audio_pusher_ = make_pusher(collection_prefix + "/Audio", "audio_chunk", "Ego audio index"); + calibration_pusher_ = make_pusher(collection_prefix + "/Calibration", "calibration", "Ego calibration"); + device_state_pusher_ = make_pusher(collection_prefix + "/DeviceState", "device_state", "Ego device state"); + } + latest_status_.observed_at_ns = core::os_monotonic_now_ns(); + publisher_ = std::thread([this] { publish_loop(); }); + } + + ~SchemaMetadataSink() override + { + try + { + close(); + } + catch (const std::exception& error) + { + detail::log_error() << "Ego SchemaPusher shutdown failed: " << error.what() << std::endl; + } + catch (...) + { + detail::log_error() << "Ego SchemaPusher shutdown failed: unknown error" << std::endl; + } + } + + void on_frame_metadata(const CapturedFrame& frame) override + { + const auto it = pushers_.find(frame.metadata.stream); + if (it == pushers_.end()) + return; + + enqueue(*it->second, frame.metadata, frame.sample_time_local_common_clock_ns, + frame.sample_time_raw_device_clock_ns); + } + + void on_imu_batch(const core::EgoImuBatchT& batch, int64_t local_ns, int64_t device_ns) override + { + if (const auto found = imu_pushers_.find(batch.sensor); found != imu_pushers_.end()) + enqueue(*found->second, batch, local_ns, device_ns); + } + + void on_audio_chunk(const core::EgoAudioChunkT& chunk, int64_t local_ns, int64_t device_ns) override + { + if (audio_pusher_) + enqueue(*audio_pusher_, chunk, local_ns, device_ns); + } + + void on_calibration(const core::EgoCalibrationT& calibration, int64_t local_ns, int64_t device_ns) override + { + if (calibration_pusher_) + enqueue(*calibration_pusher_, calibration, local_ns, device_ns); + } + + void on_device_state(const core::EgoDeviceStateT& state, int64_t local_ns, int64_t device_ns) override + { + if (device_state_pusher_) + enqueue(*device_state_pusher_, state, local_ns, device_ns); + } + + void on_capture_status(const CaptureStatus& status) override + { + if (plugin_root_id_.empty()) + return; + std::lock_guard lock(publish_mutex_); + if (stopping_ || status.observed_at_ns < latest_status_.observed_at_ns) + return; + status_changed_ = status_changed_ || status.availability != latest_status_.availability || + status.error != latest_status_.error; + latest_status_ = status; + if (status_changed_) + publish_wake_.notify_one(); + } + + void close() override + { + { + std::lock_guard lock(publish_mutex_); + stopping_ = true; + } + publish_wake_.notify_all(); + if (publisher_.joinable()) + publisher_.join(); + pushers_.clear(); + imu_pushers_.clear(); + audio_pusher_.reset(); + calibration_pusher_.reset(); + device_state_pusher_.reset(); + status_publisher_.reset(); + session_.reset(); + const auto failure = error(); + if (!failure.empty()) + throw std::runtime_error("Ego SchemaPusher failed: " + failure); + } + + std::string error() const override + { + std::lock_guard lock(publish_mutex_); + return publish_error_; + } + +private: + std::unique_ptr make_pusher(const std::string& collection, + const std::string& tensor, + const std::string& name) + { + detail::log_info() << " Metadata: " << collection << std::endl; + return std::make_unique( + session_->get_handles(), core::SchemaPusherConfig{ .collection_id = collection, + .max_flatbuffer_size = kMaxFlatbufferSize, + .tensor_identifier = tensor, + .localized_name = name, + .app_name = "EgoCameraPlugin" }); + } + + template + void enqueue(core::SchemaPusher& pusher, + const typename TableT::NativeTableType& value, + int64_t local_ns, + int64_t device_ns) + { + const auto payload = core::pack(value); + enqueue_task( + [&pusher, payload, local_ns, device_ns]() + { + const auto bytes = payload.buffer(); + pusher.push_buffer(bytes.data(), bytes.size(), local_ns, device_ns); + }); + } + + void enqueue(core::SchemaPusher& pusher, const core::EgoImuBatchT& value, int64_t local_ns, int64_t device_ns) + { + enqueue(pusher, value, local_ns, device_ns); + } + void enqueue(core::SchemaPusher& pusher, const core::EgoAudioChunkT& value, int64_t local_ns, int64_t device_ns) + { + enqueue(pusher, value, local_ns, device_ns); + } + void enqueue(core::SchemaPusher& pusher, const core::EgoCalibrationT& value, int64_t local_ns, int64_t device_ns) + { + enqueue(pusher, value, local_ns, device_ns); + } + void enqueue(core::SchemaPusher& pusher, const core::EgoDeviceStateT& value, int64_t local_ns, int64_t device_ns) + { + enqueue(pusher, value, local_ns, device_ns); + } + + void enqueue_task(std::function task) + { + std::lock_guard lock(publish_mutex_); + if (stopping_) + throw std::runtime_error("Ego SchemaPusher is already closed"); + if (!publish_error_.empty()) + throw std::runtime_error("Ego SchemaPusher failed: " + publish_error_); + if (tasks_.size() >= kMaxQueuedEvents) + throw std::runtime_error("Ego SchemaPusher queue is full; capture stopped to avoid silent loss"); + if (tasks_.size() + 1 >= kMaxQueuedEvents * 85 / 100 && !warning_emitted_) + { + warning_emitted_ = true; + detail::log_warning() + << "Warning: Ego SchemaPusher queue reached 85%; capture will stop if it fills." << std::endl; + } + tasks_.push_back(std::move(task)); + publish_wake_.notify_one(); + } + + void publish_status(const CaptureStatus& status) + { + if (plugin_root_id_.empty()) + return; + try + { + if (!status_publisher_) + status_publisher_ = std::make_unique( + session_->get_handles(), plugin_root_id_); + plugin_utils::PluginDeviceStatusEntry entry{ .path = "/camera/ego", + .reason = core::PluginDeviceReason_NO_CURRENT_DATA }; + entry.error = status.error; + switch (status.availability) + { + case CaptureAvailability::Unknown: + entry.state = core::PluginDeviceState_UNKNOWN; + break; + case CaptureAvailability::Absent: + entry.state = core::PluginDeviceState_DISCONNECTED; + entry.reason = core::PluginDeviceReason_NO_HARDWARE_SIGNAL; + break; + case CaptureAvailability::Starting: + case CaptureAvailability::Recovering: + entry.state = core::PluginDeviceState_DEGRADED; + break; + case CaptureAvailability::Streaming: + entry.state = core::PluginDeviceState_CONNECTED; + entry.reason = core::PluginDeviceReason_NONE; + break; + case CaptureAvailability::Failed: + entry.state = core::PluginDeviceState_FAILED; + entry.reason = core::PluginDeviceReason_DEVICE_ERROR; + break; + } + // A blocked capture owner must age out; publishing is not a new observation. + status_publisher_->publish_if_changed({ entry }, status.observed_at_ns); + status_warning_emitted_ = false; + } + catch (const std::exception& error) + { + if (!status_warning_emitted_) + detail::log_warning() << "Ego device status unavailable; capture continues: " << error.what(); + status_warning_emitted_ = true; + } + } + + void publish_loop() + { + auto next_status = std::chrono::steady_clock::now(); + while (true) + { + std::function task; + CaptureStatus status; + bool publish_status_now = false; + { + std::unique_lock lock(publish_mutex_); + const auto ready = [this] { return stopping_ || status_changed_ || !tasks_.empty(); }; + if (plugin_root_id_.empty()) + publish_wake_.wait(lock, ready); + else + publish_wake_.wait_until(lock, next_status, ready); + if (stopping_ && tasks_.empty()) + return; + const auto now = std::chrono::steady_clock::now(); + if (!plugin_root_id_.empty() && (status_changed_ || now >= next_status)) + { + status = latest_status_; + publish_status_now = true; + status_changed_ = false; + // Schedule against wall progress, even when the fact timestamp has stopped. + next_status = now + std::chrono::milliseconds(250); + } + if (!tasks_.empty()) + { + task = std::move(tasks_.front()); + tasks_.pop_front(); + } + } + if (publish_status_now) + publish_status(status); + if (!task) + continue; + try + { + task(); + } + catch (const std::exception& error) + { + std::lock_guard lock(publish_mutex_); + publish_error_ = error.what(); + tasks_.clear(); + detail::log_error() << "Ego SchemaPusher failure: " << publish_error_ << std::endl; + } + } + } + + std::shared_ptr session_; + std::map> pushers_; + std::map> imu_pushers_; + std::unique_ptr audio_pusher_; + std::unique_ptr calibration_pusher_; + std::unique_ptr device_state_pusher_; + std::string plugin_root_id_; + std::unique_ptr status_publisher_; + CaptureStatus latest_status_; + bool status_changed_ = true; + bool status_warning_emitted_ = false; + mutable std::mutex publish_mutex_; + std::condition_variable publish_wake_; + std::deque> tasks_; + std::thread publisher_; + bool stopping_ = false; + bool warning_emitted_ = false; + std::string publish_error_; +}; + +} + +std::unique_ptr make_schema_metadata_sink(const std::vector& streams, + const std::string& prefix, + const std::string& plugin_root_id) +{ + return std::make_unique(streams, prefix, plugin_root_id); +} + +} // namespace plugins::ego diff --git a/src/plugins/ego/core/time_alignment.cpp b/src/plugins/ego/core/time_alignment.cpp new file mode 100644 index 0000000000..ed874913ed --- /dev/null +++ b/src/plugins/ego/core/time_alignment.cpp @@ -0,0 +1,411 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/ego_camera/time_alignment.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace plugins::ego +{ +namespace +{ +using Json = nlohmann::json; +constexpr size_t kMaxModelBytes = 8192; +constexpr std::array kStreams{ "left_rgb", "right_rgb", "accel", "gyro" }; + +int64_t integer(const Json& value, const char* name, bool nonnegative = false) +{ + if (!value.is_number_integer() || + (value.is_number_unsigned() && value.get() > static_cast(INT64_MAX))) + throw std::invalid_argument(std::string("EGO time alignment requires int64 ") + name); + const auto result = value.get(); + if (nonnegative && result < 0) + throw std::invalid_argument(std::string("EGO time alignment requires nonnegative ") + name); + return result; +} + +void text_field(const Json& value, const char* name) +{ + if (!value.is_string() || value.get_ref().empty()) + throw std::invalid_argument(std::string("EGO time alignment requires nonempty ") + name); +} + +Json read_profile(const std::string& path) +{ + if (path.empty()) + return Json(); + std::ifstream input(path, std::ios::binary); + if (!input) + throw std::invalid_argument("Unable to open EGO time alignment: " + path); + std::string bytes(kMaxModelBytes + 1, '\0'); + input.read(bytes.data(), static_cast(bytes.size())); + bytes.resize(static_cast(input.gcount())); + if (bytes.size() > kMaxModelBytes) + throw std::invalid_argument("EGO capture timing profile exceeds 8192 bytes"); + // Reject duplicate keys so the C++ and offline validators interpret the + // same timing provenance; JSON's permissive last-key-wins rule is unsafe here. + std::vector> object_keys; + auto value = Json::parse( + bytes, + [&object_keys](int, Json::parse_event_t event, Json& parsed) + { + if (event == Json::parse_event_t::object_start) + object_keys.emplace_back(); + else if (event == Json::parse_event_t::object_end) + object_keys.pop_back(); + else if (event == Json::parse_event_t::key && !object_keys.back().insert(parsed.get()).second) + throw std::invalid_argument("Duplicate EGO time alignment JSON key"); + return true; + }); + if (integer(value.at("version"), "version") != 1 || value.at("scope") != "capture_profile" || + value.at("status") != "validated" || value.at("reference_stream") != "left_rgb") + throw std::invalid_argument( + "--time-alignment requires a validated version-1 capture_profile, not recording anchors"); + if (value.contains("epochs") || value.contains("host_bridge") || value.contains("audio_segments")) + throw std::invalid_argument("EGO capture profile cannot reuse recording-bound clock anchors"); + text_field(value.at("model_id"), "model_id"); + for (const auto* key : { "serial", "firmware", "sdk_version", "sdk_binary_sha256" }) + text_field(value.at("identity").at(key), key); + const auto digest = value.at("identity").at("sdk_binary_sha256").get(); + if (digest.size() != 64 || digest.find_first_not_of("0123456789abcdef") != std::string::npos) + throw std::invalid_argument("EGO SDK binary SHA-256 must contain 64 lowercase hexadecimal characters"); + const auto& selected_profile = value.at("profile"); + const auto& video = selected_profile.at("video"); + const auto& format = video.at("format"); + if (format != "mjpg" && format != "h264" && format != "h265") + throw std::invalid_argument("EGO timing video format must be mjpg, h264, or h265"); + for (const auto* key : { "width", "height", "fps" }) + { + const auto number = integer(video.at(key), key, true); + if (number == 0 || number > UINT32_MAX) + throw std::invalid_argument(std::string("EGO timing profile requires positive uint32 ") + key); + } + integer(selected_profile.at("imu_rate_hz"), "imu_rate_hz", true); + const auto& audio = selected_profile.at("audio"); + for (const auto* key : { "sample_rate_hz", "channels", "bits_per_sample" }) + integer(audio.at(key), key, true); + const auto& validation = value.at("validation"); + const auto error = integer(validation.at("max_abs_error_ns"), "max_abs_error_ns", true); + const auto uncertainty = integer(validation.at("uncertainty_ns"), "uncertainty_ns", true); + if (static_cast(error) + uncertainty > 500'000'000.0L / integer(video.at("fps"), "fps")) + throw std::invalid_argument("EGO timing validation bound exceeds half a video frame"); + const auto held_out_duration = integer(validation.at("held_out_duration_ns"), "held_out_duration_ns", true); + if (held_out_duration < 900'000'000'000LL) + throw std::invalid_argument("EGO capture profile requires at least 15 minutes of independent held-out validation"); + const auto max_duration = integer(value.at("max_capture_duration_ns"), "max_capture_duration_ns", true); + if (max_duration == 0 || max_duration > held_out_duration) + throw std::invalid_argument( + "EGO capture duration must be positive and within the independently validated duration"); + text_field(validation.at("method"), "method"); + const auto& evidence = validation.at("evidence"); + if (!evidence.is_array() || evidence.empty()) + throw std::invalid_argument("EGO capture profile requires validation evidence"); + for (const auto& entry : evidence) + text_field(entry, "evidence"); + const auto& streams = value.at("streams"); + if (!streams.is_object() || !streams.contains("left_rgb")) + throw std::invalid_argument("EGO capture profile must include the left reference stream"); + for (const auto& [name, mapping] : streams.items()) + { + if (std::find(kStreams.begin(), kStreams.end(), name) == kStreams.end()) + throw std::invalid_argument("EGO capture profile contains an unsupported stream: " + name); + integer(mapping.at("offset_ns"), "offset_ns"); + const auto& scale = mapping.at("scale"); + // Drift anchors are recording-specific; rebasing them to an arbitrary + // future left timestamp would change the calibrated clock relationship. + if (!scale.is_number() || scale.get() != 1) + throw std::invalid_argument("Reusable EGO capture profiles require scale=1; fit drift per recording"); + if (mapping.size() != 2) + throw std::invalid_argument("EGO capture mappings only accept reusable offset_ns and scale"); + } + if (streams.at("left_rgb").at("offset_ns") != 0 || streams.at("left_rgb").at("scale") != 1) + throw std::invalid_argument("EGO left reference mapping must be identity"); + return value; +} +} // namespace + +void validate_time_alignment_profile(const std::string& path) +{ + read_profile(path); +} + +int64_t sdk_timestamp_ns(uint64_t timestamp_us) +{ + // Reject before casting or multiplying; corrupt SDK values must retain + // their original unsigned value in diagnostics instead of overflowing. + constexpr auto maximum_us = static_cast(std::numeric_limits::max() / 1000); + if (timestamp_us > maximum_us) + throw std::overflow_error("EGO SDK timestamp_us=" + std::to_string(timestamp_us) + + " exceeds int64 nanosecond range"); + return static_cast(timestamp_us) * 1000; +} + +std::string loaded_sdk_binary_sha256() +{ + static const std::string digest = [] + { + Dl_info library{}; + if (dladdr(reinterpret_cast(&ob_get_version), &library) == 0 || !library.dli_fname) + throw std::runtime_error("Unable to resolve the loaded EGO SDK library for SHA-256 provenance"); + std::ifstream input(library.dli_fname, std::ios::binary); + if (!input) + throw std::runtime_error("Unable to read the loaded EGO SDK library for SHA-256 provenance"); + std::unique_ptr context(EVP_MD_CTX_new(), EVP_MD_CTX_free); + if (!context || EVP_DigestInit_ex(context.get(), EVP_sha256(), nullptr) != 1) + throw std::runtime_error("Unable to initialize the EGO SDK SHA-256 digest"); + std::array bytes{}; + while (input.read(bytes.data(), static_cast(bytes.size())) || input.gcount() != 0) + if (EVP_DigestUpdate(context.get(), bytes.data(), static_cast(input.gcount())) != 1) + throw std::runtime_error("Unable to update the EGO SDK SHA-256 digest"); + if (!input.eof()) + throw std::runtime_error("Unable to finish reading the loaded EGO SDK library"); + std::array hash{}; + unsigned int hash_size = 0; + if (EVP_DigestFinal_ex(context.get(), hash.data(), &hash_size) != 1 || hash_size != 32) + throw std::runtime_error("Unable to finalize the EGO SDK SHA-256 digest"); + constexpr char hex[] = "0123456789abcdef"; + std::string result; + result.reserve(hash_size * 2); + for (unsigned int index = 0; index < hash_size; ++index) + { + result.push_back(hex[hash[index] >> 4]); + result.push_back(hex[hash[index] & 15]); + } + return result; + }(); + return digest; +} + +class TimeAlignment::Impl +{ +public: + explicit Impl(const std::string& path) : profile(read_profile(path)) + { + } + mutable std::mutex mutex; + Json profile; + Json provenance; + uint32_t epoch = 0; + std::optional reference_anchor; + int64_t local_anchor = 0; + int64_t valid_to = 0; + bool expired = false; + std::optional expired_at_reference; + std::map first_mapped_raw; + std::map last_mapped_local; + uint64_t audio_segment = 0; + uint64_t audio_index = 0; + std::optional audio_previous; + long double audio_previous_duration = 0; + bool audio_restart = false; + bool updated = true; +}; + +TimeAlignment::TimeAlignment(const std::string& path) : impl_(std::make_unique(path)) +{ +} +TimeAlignment::~TimeAlignment() = default; + +void TimeAlignment::configure(const TimingIdentity& value) +{ + std::lock_guard lock(impl_->mutex); + const Json identity{ { "serial", value.serial }, + { "firmware", value.firmware }, + { "sdk_version", value.sdk_version }, + { "sdk_binary_sha256", value.sdk_binary_sha256 } }; + const Json profile{ + { "video", + { { "format", value.video_format }, { "width", value.width }, { "height", value.height }, { "fps", value.fps } } }, + { "imu_rate_hz", value.imu_rate_hz }, + { "audio", + { { "sample_rate_hz", value.audio_rate_hz }, + { "channels", value.audio_channels }, + { "bits_per_sample", value.audio_bits } } } + }; + if (!impl_->profile.is_null()) + { + const auto& mappings = impl_->profile.at("streams"); + if ((value.right_enabled && !mappings.contains("right_rgb")) || + (value.imu_rate_hz != 0 && (!mappings.contains("accel") || !mappings.contains("gyro")))) + throw std::invalid_argument("EGO capture timing profile is missing an enabled stream"); + if (impl_->profile.at("identity") != identity || impl_->profile.at("profile") != profile) + throw std::invalid_argument( + "EGO time alignment identity/profile does not match the selected device and streams"); + const auto& validation = impl_->profile.at("validation"); + const long double bound = + static_cast(integer(validation.at("max_abs_error_ns"), "max_abs_error_ns")) + + integer(validation.at("uncertainty_ns"), "uncertainty_ns"); + if (value.fps == 0 || bound > 500'000'000.0L / value.fps) + throw std::invalid_argument("EGO timing validation bound exceeds half a video frame"); + } + // Audio getTimeStampUs() semantics depend on the SDK transport; the source + // label identifies the API, without assuming a USB host-estimated block start. + impl_->provenance = { { "version", 1 }, + { "scope", "recording" }, + { "status", "unverified" }, + { "model_id", "capture-provenance" }, + { "reference_stream", "left_rgb" }, + { "identity", identity }, + { "profile", profile }, + { "epochs", Json::array() }, + { "sources", + { { "left_rgb", "sdk_uvc_raw" }, + { "right_rgb", "sdk_uvc_raw" }, + { "accel", "sdk_imu_raw" }, + { "gyro", "sdk_imu_raw" }, + { "audio", "sdk_audio_raw" } } }, + { "sample_time_policy", "host_arrival_fallback" } }; + if (!impl_->profile.is_null()) + impl_->provenance["capture_profile"] = impl_->profile; + impl_->updated = true; +} + +void TimeAlignment::begin_epoch(uint32_t epoch) +{ + std::lock_guard lock(impl_->mutex); + impl_->epoch = epoch; + impl_->reference_anchor.reset(); + impl_->first_mapped_raw.clear(); + impl_->last_mapped_local.clear(); + impl_->expired = false; + impl_->expired_at_reference.reset(); + if (impl_->audio_previous || impl_->audio_index != 0) + ++impl_->audio_segment; + impl_->audio_index = 0; + impl_->audio_previous.reset(); + impl_->audio_restart = epoch != 0; + impl_->updated = true; +} + +bool TimeAlignment::observe_left(int64_t raw_ns, int64_t arrival_ns) +{ + std::lock_guard lock(impl_->mutex); + if (impl_->profile.is_null() || impl_->reference_anchor) + return false; + if (raw_ns < 0 || arrival_ns < 0) + throw std::invalid_argument("EGO time alignment received a negative anchor"); + impl_->reference_anchor = raw_ns; + impl_->local_anchor = arrival_ns; + const auto duration = integer(impl_->profile.at("max_capture_duration_ns"), "max_capture_duration_ns"); + impl_->valid_to = raw_ns > INT64_MAX - duration ? INT64_MAX : raw_ns + duration; + impl_->updated = true; + return true; +} + +int64_t TimeAlignment::sample_time(const std::string& stream, int64_t raw_ns, int64_t arrival_ns) +{ + std::lock_guard lock(impl_->mutex); + if (!impl_->reference_anchor || impl_->profile.is_null() || !impl_->profile.at("streams").contains(stream)) + return arrival_ns; + const auto& mapping = impl_->profile.at("streams").at(stream); + const long double reference_time = static_cast(raw_ns) + integer(mapping.at("offset_ns"), "offset_ns"); + if (stream == "left_rgb" && raw_ns >= impl_->valid_to && !impl_->expired) + { + impl_->expired = true; + impl_->expired_at_reference = raw_ns; + impl_->updated = true; + } + if (impl_->expired || reference_time < *impl_->reference_anchor || reference_time >= impl_->valid_to) + return arrival_ns; + const long double result = static_cast(impl_->local_anchor) + reference_time - *impl_->reference_anchor; + const long double rounded = std::round(result); + if (!std::isfinite(rounded) || rounded < 0 || rounded > std::numeric_limits::max()) + throw std::runtime_error("EGO mapped sample time is outside int64 range"); + const auto mapped = static_cast(rounded); + // Arrival fallback is a different time source. Comparing it with the first + // mapped sample can permanently suppress a valid negative-offset mapping. + const auto previous = impl_->last_mapped_local.find(stream); + if (previous != impl_->last_mapped_local.end() && mapped <= previous->second) + throw std::runtime_error("EGO calibrated sample clock regressed within a capture epoch"); + if (impl_->first_mapped_raw.emplace(stream, raw_ns).second) + impl_->updated = true; + impl_->last_mapped_local[stream] = mapped; + return mapped; +} + +core::EgoAudioTimingT TimeAlignment::audio_timing(int64_t raw_ns, uint32_t sample_count, uint32_t sample_rate) +{ + std::lock_guard lock(impl_->mutex); + if (sample_count == 0 || sample_rate == 0) + throw std::invalid_argument("EGO audio timing requires nonempty PCM and a sample rate"); + bool discontinuity = impl_->audio_restart; + if (impl_->audio_previous) + { + const long double delta = static_cast(raw_ns) - *impl_->audio_previous; + // SDK audio time can jitter; only observable large gaps/reversals split a segment. + if (delta <= 0 || std::abs(delta - impl_->audio_previous_duration) > + std::max(5'000'000.0L, 4 * impl_->audio_previous_duration)) + { + ++impl_->audio_segment; + impl_->audio_index = 0; + discontinuity = true; + impl_->updated = true; + } + } + core::EgoAudioTimingT result; + result.segment_id = impl_->audio_segment; + result.first_sample_index = impl_->audio_index; + result.continuity_known = false; + result.discontinuity = discontinuity; + if (sample_count > UINT64_MAX - impl_->audio_index) + throw std::overflow_error("EGO retained audio sample index overflow"); + impl_->audio_index += sample_count; + impl_->audio_previous = raw_ns; + impl_->audio_previous_duration = 1'000'000'000.0L * sample_count / sample_rate; + impl_->audio_restart = false; + return result; +} + +bool TimeAlignment::has_update() const +{ + std::lock_guard lock(impl_->mutex); + return impl_->updated; +} + +std::string TimeAlignment::snapshot() const +{ + std::lock_guard lock(impl_->mutex); + auto result = impl_->provenance; + result["capture_epoch"] = impl_->epoch; + if (impl_->reference_anchor) + { + result["sample_time_policy"] = impl_->expired ? "expired_capture_profile_host_arrival_fallback" : + "validated_relative_video_imu_with_estimated_host_origin"; + result["host_bridge"] = { { "epoch", impl_->epoch }, + { "reference_anchor_ns", *impl_->reference_anchor }, + { "local_anchor_ns", impl_->local_anchor }, + { "host_anchor_method", "left_arrival_estimate" }, + { "host_absolute_accuracy", "unverified" }, + { "first_mapped_raw_ns", impl_->first_mapped_raw }, + { "valid_from_ns", *impl_->reference_anchor }, + { "valid_to_ns", impl_->valid_to }, + { "status", impl_->expired ? "expired" : "active" } }; + if (impl_->expired_at_reference) + result["host_bridge"]["expired_at_reference_ns"] = *impl_->expired_at_reference; + } + result["audio_sample_time_policy"] = "host_arrival_fallback"; + result["audio_segment_id"] = impl_->audio_segment; + const auto json = result.dump(); + if (json.size() > 16 * 1024) + throw std::runtime_error("EGO timing provenance exceeds the calibration payload budget"); + impl_->updated = false; + return json; +} + +} // namespace plugins::ego diff --git a/src/plugins/ego/core/wav_writer.hpp b/src/plugins/ego/core/wav_writer.hpp new file mode 100644 index 0000000000..423cbf9cbe --- /dev/null +++ b/src/plugins/ego/core/wav_writer.hpp @@ -0,0 +1,137 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "diagnostics.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace plugins::ego +{ + +class WavWriter +{ +public: + WavWriter() = default; + ~WavWriter() + { + try + { + close(); + } + catch (const std::exception& error) + { + detail::log_error() << "Ego WAV shutdown failed: " << error.what() << std::endl; + } + } + WavWriter(const WavWriter&) = delete; + WavWriter& operator=(const WavWriter&) = delete; + + void open(const std::string& path, uint32_t rate, uint16_t channels, uint16_t bits) + { + const std::filesystem::path output(path); + if (!output.parent_path().empty()) + std::filesystem::create_directories(output.parent_path()); + file_.open(path, std::ios::binary | std::ios::trunc); + if (!file_) + throw std::runtime_error("Unable to open WAV output: " + path); + data_bytes_ = 0; + rate_ = rate; + channels_ = channels; + bits_ = bits; + write_header(0); + if (!file_) + throw std::runtime_error("Failed while writing Ego WAV header"); + } + + uint64_t write(const std::vector& bytes) + { + check_capacity(bytes.size()); + const uint64_t offset = 44 + data_bytes_; + file_.write(reinterpret_cast(bytes.data()), static_cast(bytes.size())); + if (!file_) + throw std::runtime_error("Failed while writing Ego WAV data"); + data_bytes_ += bytes.size(); + return offset; + } + + bool is_open() const + { + return file_.is_open(); + } + + void close() + { + if (!file_.is_open()) + return; + try + { + check_capacity(0); + } + catch (...) + { + file_.close(); + throw; + } + file_.seekp(0); + write_header(static_cast(data_bytes_)); + file_.flush(); + bool failed = !file_; + file_.close(); + failed = failed || file_.fail(); + if (failed) + throw std::runtime_error("Failed while finalizing Ego WAV output"); + } + +private: + void check_capacity(uint64_t additional_bytes) const + { + // RIFF's size includes 36 header bytes; reject before writing so archive cleanup can abort. + constexpr uint64_t max_data_bytes = std::numeric_limits::max() - 36U; + if (data_bytes_ > max_data_bytes || additional_bytes > max_data_bytes - data_bytes_) + throw std::runtime_error("Ego WAV exceeds RIFF 32-bit size limit"); + } + + void le16(uint16_t value) + { + const uint8_t bytes[] = { static_cast(value), static_cast(value >> 8) }; + file_.write(reinterpret_cast(bytes), sizeof(bytes)); + } + void le32(uint32_t value) + { + const uint8_t bytes[] = { static_cast(value), static_cast(value >> 8), + static_cast(value >> 16), static_cast(value >> 24) }; + file_.write(reinterpret_cast(bytes), sizeof(bytes)); + } + void write_header(uint32_t data_bytes) + { + file_.write("RIFF", 4); + le32(36 + data_bytes); + file_.write("WAVEfmt ", 8); + le32(16); + le16(1); + le16(channels_); + le32(rate_); + const auto block_align = static_cast(channels_ * bits_ / 8); + le32(rate_ * block_align); + le16(block_align); + le16(bits_); + file_.write("data", 4); + le32(data_bytes); + } + + std::ofstream file_; + uint32_t rate_ = 0; + uint16_t channels_ = 0; + uint16_t bits_ = 0; + uint64_t data_bytes_ = 0; +}; + +} // namespace plugins::ego diff --git a/src/plugins/ego/ego.example.toml b/src/plugins/ego/ego.example.toml new file mode 100644 index 0000000000..149d29df1e --- /dev/null +++ b/src/plugins/ego/ego.example.toml @@ -0,0 +1,36 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +[record] +# Omit duration for an unlimited recording stopped with Ctrl-C. +duration = 30 + +# Stereo ColorLeft and ColorRight are both required by this workflow. +format = "h264" +width = 1600 +height = 1300 +fps = 30 + +# Optional sensor streams and local MCAP media layout. +imu = true +audio = false # Set true to capture optional audio; cross-stream synchronization is unverified. +preview = false +mcap_media = "metadata-only" # Or "embedded". +record_device_diagnostics = true # Live monitoring remains enabled when false. +keep_media_sidecars = false # Valid only with embedded media. + +# Same-device recovery policy. +reconnect_timeout = 30 +reconnect_interval_ms = 1000 + +# Optional paths and device selection. Relative paths use the command's working directory. +# output = "recordings/ego_configured" +# device_uid = "1-2-23" +# preset = "py3.11" +# plugin = "/absolute/path/to/ego_camera_plugin" + +# Native plugin controls are passed through unchanged. +plugin_options = [ + "--bitrate=8", + "--dynamic-bitrate=on", +] diff --git a/src/plugins/ego/ego.sh b/src/plugins/ego/ego.sh new file mode 100755 index 0000000000..a4bad5fa3a --- /dev/null +++ b/src/plugins/ego/ego.sh @@ -0,0 +1,986 @@ +#!/usr/bin/env bash +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# One entry point for building, inspecting, recording, and verifying the +# capability-compatible EGO camera plugin. It intentionally never +# installs packages, SDKs, or udev rules: those operations need user approval. + +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +SOURCE_ROOT="$(cd "$SCRIPT_DIR/../../.." && pwd)" +DEFAULT_PRESET="py3.11" + +die() { + echo "ego.sh: $*" >&2 + exit 1 +} + +warn() { + echo "ego.sh: warning: $*" >&2 +} + +usage() { + cat <<'EOF' +Usage: + src/plugins/ego/ego.sh doctor [--sdk-root PATH] [--preset py3.11] + src/plugins/ego/ego.sh build --sdk-root PATH [--preview] [--preset py3.11] [--jobs N] [--clean] [--install-prefix PATH] + src/plugins/ego/ego.sh capabilities [--preset py3.11] [--plugin PATH] [--device-uid UID] + src/plugins/ego/ego.sh record [--config FILE.toml] [options] [-- PLUGIN_OPTIONS...] + src/plugins/ego/ego.sh verify RUN_DIRECTORY [--preset py3.11] [--plugin PATH] + src/plugins/ego/ego.sh export-media RUN_DIRECTORY [--output DIRECTORY] [--preset py3.11] + +Common record options: + --config FILE.toml Load reusable record settings; later CLI options override them. + --duration SECONDS Stop cleanly after SECONDS; omit for Ctrl-C. + --output DIRECTORY Default: recordings/ego_. + --format mjpg|h264|h265 Default: h264. + --width N --height N --fps N Default: 1600 1300 30. + --device-uid UID Select one enumerated device. + --reconnect-timeout SECONDS Wait for the same physical device after disconnect (default: 30; 0 disables). + --reconnect-interval-ms MS Device re-enumeration interval (default: 1000). + --no-imu Disable IMU (requested by default when available). + --audio Request optional audio (default: off; synchronization remains unverified). + --no-audio Disable audio, overriding an audio-enabled configuration. + --preview Enable the plugin's SDL preview (requires a --preview build). + --mcap-media MODE metadata-only (default) or embedded. + --record-device-diagnostics=on|off Save detailed diagnostics in MCAP (default on). + --keep-media-sidecars Retain H.264/H.265/MJPEG and WAV beside embedded MCAP. + --preset NAME --plugin PATH Select a build tree or override the executable. + +Examples: + ./src/plugins/ego/ego.sh doctor --sdk-root /opt/OrbbecSDK + ./src/plugins/ego/ego.sh build --sdk-root /opt/OrbbecSDK --jobs 8 + ./src/plugins/ego/ego.sh build --sdk-root /opt/OrbbecSDK --preview --jobs 8 + ./src/plugins/ego/ego.sh capabilities + ./src/plugins/ego/ego.sh record --duration 30 + # Capabilities also reports advertised profiles that remain uncertified. + ./src/plugins/ego/ego.sh verify recordings/ego_20260810_120000 + ./src/plugins/ego/ego.sh export-media recordings/embedded_demo + +All options after -- are passed unchanged to ego_camera_plugin. For example: + ... record --duration 30 -- --bitrate=8 --dynamic-bitrate=on + +The plugin validates the exact requested stream combination against the SDK; +unsupported combinations fail instead of falling back to another profile. +Encoded profiles above 30 FPS are currently rejected after SDK resolution +until sustained recording and strict-decode certification passes. +EOF +} + +preset_python_version() { + case "$1" in + py3.11) echo "3.11" ;; + py3.12) echo "3.12" ;; + py3.13) echo "3.13" ;; + *) die "Unknown preset '$1'. Expected py3.11, py3.12, or py3.13." ;; + esac +} + +build_dir_for_preset() { + local version + version="$(preset_python_version "$1")" + echo "$SOURCE_ROOT/build-ego-py$version" +} + +validate_sdk_root() { + local sdk_root="$1" + [[ -n "$sdk_root" ]] || die "--sdk-root PATH (or ORBBEC_SDK_ROOT) is required." + [[ -f "$sdk_root/include/libobsensor/ObSensor.hpp" ]] || die "Invalid SDK root '$sdk_root': missing include/libobsensor/ObSensor.hpp" + [[ -f "$sdk_root/lib/OrbbecSDKConfig.cmake" ]] || die "Invalid SDK root '$sdk_root': missing lib/OrbbecSDKConfig.cmake" + [[ -f "$sdk_root/lib/libOrbbecSDK.so.2" ]] || die "Invalid SDK root '$sdk_root': missing lib/libOrbbecSDK.so.2" + [[ -d "$sdk_root/lib/extensions" ]] || die "Invalid SDK root '$sdk_root': missing lib/extensions" +} + +resolve_plugin() { + local preset="$1" + local requested="${2:-}" + local candidate + if [[ -n "$requested" ]]; then + candidate="$requested" + elif [[ -n "${EGO_PLUGIN:-}" ]]; then + candidate="$EGO_PLUGIN" + elif [[ -x "$SCRIPT_DIR/ego_camera_plugin" ]]; then + candidate="$SCRIPT_DIR/ego_camera_plugin" + else + candidate="$(build_dir_for_preset "$preset")/src/plugins/ego_camera/ego_camera_plugin" + fi + [[ -x "$candidate" ]] || die "Plugin is not executable: $candidate. Run 'build' first or pass --plugin PATH." + echo "$candidate" +} + +resolve_exporter() { + local preset="$1" + local requested_plugin="${2:-}" + local candidate + # A verify run must never mix a camera plugin and exporter from different build trees. + if [[ -n "$requested_plugin" ]]; then + [[ -x "$requested_plugin" ]] || die "Plugin is not executable: $requested_plugin" + local plugin_dir + plugin_dir="$(cd "$(dirname "$requested_plugin")" && pwd -P)" \ + || die "Cannot resolve plugin directory: $requested_plugin" + for candidate in \ + "$plugin_dir/ego_mcap_export_media" \ + "$plugin_dir/../mcap/export_media_tool/ego_mcap_export_media"; do + if [[ -x "$candidate" ]]; then + echo "$(cd "$(dirname "$candidate")" && pwd -P)/$(basename "$candidate")" + return 0 + fi + done + die "No embedded-media exporter exists beside plugin or in its build tree: $requested_plugin" + fi + if [[ -x "$SCRIPT_DIR/ego_mcap_export_media" ]]; then + candidate="$SCRIPT_DIR/ego_mcap_export_media" + else + candidate="$(build_dir_for_preset "$preset")/src/plugins/ego_camera/ego_mcap_export_media" + fi + [[ -x "$candidate" ]] || die "Embedded-media exporter is not executable: $candidate. Run 'build' first." + echo "$candidate" +} + +require_command() { + local command_name="$1" + local remediation="$2" + if ! command -v "$command_name" >/dev/null 2>&1; then + warn "Missing command '$command_name'. $remediation" + return 1 + fi +} + +require_cmake_version() { + local version + require_command cmake "Install CMake 3.24 or newer." || return 1 + version="$(cmake --version | awk 'NR == 1 { print $3 }')" + if [[ "$(printf '%s\n%s\n' "3.24" "$version" | sort -V | head -n 1)" != "3.24" ]]; then + warn "CMake $version is too old; Isaac Teleop requires CMake 3.24 or newer." + return 1 + fi +} + +command_doctor() { + local sdk_root="${ORBBEC_SDK_ROOT:-}" + local preset="$DEFAULT_PRESET" + local arg + while (( $# )); do + arg="$1" + case "$arg" in + --sdk-root) sdk_root="${2:-}"; shift 2 ;; + --preset) preset="${2:-}"; shift 2 ;; + --help|-h) usage; return 0 ;; + *) die "Unknown doctor option: $arg" ;; + esac + done + preset_python_version "$preset" >/dev/null + + local failed=0 + echo "== EGO environment diagnosis ==" + echo "Architecture: $(uname -m)" + if [[ "$(uname -m)" != "x86_64" ]]; then + warn "The validated host architecture is x86_64." + fi + if [[ -r /etc/os-release ]]; then + . /etc/os-release + echo "OS: ${PRETTY_NAME:-unknown}" + fi + require_command python3 "Install it with: sudo apt update && sudo apt install -y python3" || failed=1 + require_command ffmpeg "Install it with: sudo apt update && sudo apt install -y ffmpeg" || failed=1 + require_command ffprobe "Install it with: sudo apt update && sudo apt install -y ffmpeg" || failed=1 + if command -v python3 >/dev/null 2>&1 \ + && ! python3 -c 'from mcap.reader import make_reader' >/dev/null 2>&1; then + warn "Python MCAP reader is missing. Install it with: python3 -m pip install --user mcap" + failed=1 + fi + + local source_checkout=0 + [[ -f "$SOURCE_ROOT/CMakeLists.txt" ]] && source_checkout=1 + if (( source_checkout )); then + require_cmake_version || failed=1 + require_command c++ "Install it with: sudo apt update && sudo apt install -y build-essential" || failed=1 + require_command uv "Install uv before configuring Isaac Teleop." || failed=1 + if [[ -z "$sdk_root" ]]; then + warn "No SDK root supplied. Source builds require --sdk-root PATH or ORBBEC_SDK_ROOT." + failed=1 + fi + fi + + if [[ -n "$sdk_root" ]]; then + if validate_sdk_root "$sdk_root"; then + echo "OrbbecSDK: $sdk_root" + if [[ -x "$sdk_root/shared/install_udev_rules.sh" ]]; then + echo "udev rules installer: sudo $sdk_root/shared/install_udev_rules.sh" + else + warn "Could not find this SDK release's shared/install_udev_rules.sh. Consult its package documentation." + fi + fi + fi + + local plugin="" + if plugin="$(resolve_plugin "$preset" "" 2>/dev/null)"; then + echo "Plugin: $plugin" + else + if (( source_checkout )); then + warn "Plugin is not built yet. Run: $SCRIPT_DIR/ego.sh build --sdk-root PATH" + else + warn "Plugin is unavailable. Install a complete prebuilt package." + failed=1 + fi + fi + + if (( !source_checkout )); then + local package_file + for package_file in "$SCRIPT_DIR/ego_mcap_export_media" "$SCRIPT_DIR/libOrbbecSDK.so.2" \ + "$SCRIPT_DIR/OrbbecSDKConfig.xml" "$SCRIPT_DIR/extensions"; do + if [[ ! -e "$package_file" ]]; then + warn "Prebuilt package is incomplete: missing $package_file" + failed=1 + fi + done + if [[ -n "$plugin" ]] && ! "$plugin" --help >/dev/null 2>&1; then + warn "Plugin cannot start. Check the packaged SDK libraries and host runtime dependencies." + failed=1 + fi + fi + + if command -v lsusb >/dev/null 2>&1; then + local usb_line bus device node + usb_line="$(lsusb -d 2bc5: 2>/dev/null || true)" + if [[ -z "$usb_line" ]]; then + warn "No Orbbec USB device found. Connect the camera before capabilities or record." + else + echo "Detected Orbbec USB device(s):" + echo "$usb_line" + while IFS= read -r usb_line; do + bus="$(awk '{print $2}' <<< "$usb_line")" + device="$(awk '{gsub(/:/, "", $4); print $4}' <<< "$usb_line")" + node="/dev/bus/usb/$bus/$device" + if [[ -r "$node" && -w "$node" ]]; then + echo "USB access: OK ($node)" + else + warn "USB access is unavailable for $node. Install this SDK's udev rules, reconnect the camera, then run doctor again." + fi + done <<< "$usb_line" + fi + else + warn "lsusb is unavailable; install usbutils to check camera discovery." + fi + + if (( failed )); then + return 1 + fi + echo "Doctor completed. Connect the camera and run capabilities before recording." +} + +command_build() { + local sdk_root="${ORBBEC_SDK_ROOT:-}" + local preset="$DEFAULT_PRESET" + local jobs="" + local clean=0 + local preview=0 + local install_prefix="" + local -a cmake_args=() + while (( $# )); do + case "$1" in + --sdk-root) sdk_root="${2:-}"; shift 2 ;; + --preset) preset="${2:-}"; shift 2 ;; + --jobs) jobs="${2:-}"; shift 2 ;; + --clean) clean=1; shift ;; + --preview) preview=1; shift ;; + --install-prefix) install_prefix="${2:-}"; shift 2 ;; + --help|-h) usage; return 0 ;; + *) die "Unknown build option: $1" ;; + esac + done + [[ -f "$SOURCE_ROOT/CMakeLists.txt" ]] || die "The build command is only available from a source checkout." + preset_python_version "$preset" >/dev/null + validate_sdk_root "$sdk_root" + require_cmake_version || die "Install CMake 3.24 or newer, then rerun build." + local build_dir + build_dir="$(build_dir_for_preset "$preset")" + if (( clean )); then + echo "Removing generated build directory: $build_dir" + cmake -E rm -rf "$build_dir" + fi + local preview_option=OFF + if (( preview )); then + preview_option=ON + fi + ( + cd "$SOURCE_ROOT" + cmake -S "$SOURCE_ROOT" -B "$build_dir" \ + -DISAAC_TELEOP_PYTHON_VERSION="$(preset_python_version "$preset")" \ + -DBUILD_VIZ=OFF \ + -DBUILD_PLUGIN_EGO_CAMERA=ON \ + -DORBBEC_SDK_ROOT="$sdk_root" \ + -DEGO_ENABLE_PREVIEW="$preview_option" + local build_args=(--build "$build_dir" --target ego_camera_plugin ego_mcap_export_media ego_mcap_merge) + if [[ -n "$jobs" ]]; then + [[ "$jobs" =~ ^[1-9][0-9]*$ ]] || die "--jobs must be a positive integer." + build_args+=(--parallel "$jobs") + else + build_args+=(--parallel) + fi + cmake "${build_args[@]}" + if [[ -n "$install_prefix" ]]; then + cmake --install "$build_dir" --prefix "$install_prefix" --component ego + fi + ) + echo "Build completed." + echo "Plugin: $build_dir/src/plugins/ego_camera/ego_camera_plugin" + echo "Next: $SCRIPT_DIR/ego.sh capabilities --preset $preset" +} + +print_build_provenance() { + local plugin="$1" + local sdk_library="" + echo "== Capture software provenance ==" + echo "Plugin path: $plugin" + if command -v sha256sum >/dev/null 2>&1; then + echo "Plugin SHA-256: $(sha256sum "$plugin" | awk '{print $1}')" + else + warn "sha256sum is unavailable; the plugin build hash was not recorded." + fi + if command -v ldd >/dev/null 2>&1; then + sdk_library="$({ ldd "$plugin" 2>/dev/null || true; } \ + | awk '$1 ~ /^libOrbbecSDK[.]so/ && $2 == "=>" {print $3; exit}')" + fi + if [[ -n "$sdk_library" && -f "$sdk_library" ]]; then + echo "Orbbec SDK library: $sdk_library" + if command -v sha256sum >/dev/null 2>&1; then + echo "Orbbec SDK library SHA-256: $(sha256sum "$sdk_library" | awk '{print $1}')" + fi + else + echo "Orbbec SDK library: unresolved" + warn "The runtime Orbbec SDK library path could not be resolved with ldd." + fi + echo "== Device-advertised capabilities ==" +} + +emit_capabilities() { + local plugin="$1" + local device_uid="${2:-}" + local -a args=(--list-capabilities) + [[ -n "$device_uid" ]] && args+=("--device-uid=$device_uid") + print_build_provenance "$plugin" + "$plugin" "${args[@]}" +} + +command_capabilities() { + local preset="$DEFAULT_PRESET" + local plugin_path="" + local device_uid="" + while (( $# )); do + case "$1" in + --preset) preset="${2:-}"; shift 2 ;; + --plugin) plugin_path="${2:-}"; shift 2 ;; + --device-uid) device_uid="${2:-}"; shift 2 ;; + --help|-h) usage; return 0 ;; + *) die "Unknown capabilities option: $1" ;; + esac + done + preset_python_version "$preset" >/dev/null + local plugin + plugin="$(resolve_plugin "$preset" "$plugin_path")" + emit_capabilities "$plugin" "$device_uid" +} + +record_usage_error() { + die "record option error: $*. Run '$SCRIPT_DIR/ego.sh record --help' for usage." +} + +stream_extension() { + case "$1" in + mjpg) echo "mjpg" ;; + h264) echo "h264" ;; + h265) echo "h265" ;; + *) die "Unsupported format '$1'." ;; + esac +} + +write_requested_profile_manifest() { + local output="$1" + local format="$2" + local width="$3" + local height="$4" + local fps="$5" + local diagnostics="$6" + printf '%s\n' \ + '{' \ + ' "version": 1,' \ + " \"record_device_diagnostics\": \"$diagnostics\"," \ + ' "streams": {' \ + " \"ColorLeft\": {\"format\": \"$format\", \"width\": $width, \"height\": $height, \"fps\": $fps}," \ + " \"ColorRight\": {\"format\": \"$format\", \"width\": $width, \"height\": $height, \"fps\": $fps}" \ + ' }' \ + '}' > "$output" +} + +record_config_python() { + local candidate + for candidate in python3 python3.13 python3.12 python3.11; do + if command -v "$candidate" >/dev/null 2>&1 \ + && "$candidate" -c 'import tomllib' >/dev/null 2>&1; then + command -v "$candidate" + return 0 + fi + done + die "--config requires Python 3.11 or newer for TOML support." +} + +read_record_config() { + local config_file="$1" + [[ -f "$config_file" ]] || record_usage_error "configuration file does not exist: $config_file" + local config_python + config_python="$(record_config_python)" + "$config_python" "$SCRIPT_DIR/tools/record_config.py" "$config_file" +} + +command_record() { + local preset="$DEFAULT_PRESET" + local plugin_path="" + local duration="" + local run_dir="" + local format="h264" + local width=1600 + local height=1300 + local fps=30 + local device_uid="" + local reconnect_timeout=30 + local reconnect_interval_ms=1000 + local request_imu=1 + # Optional audio must be requested explicitly, not enabled by sensor discovery. + local request_audio=0 + local preview=0 + local media_mode="metadata-only" + local keep_sidecars=0 + local record_diagnostics=on + local -a passthrough=() + local -a original_args=("$@") + local config_file="" + local index + for (( index = 0; index < ${#original_args[@]}; ++index )); do + [[ "${original_args[index]}" != "--" ]] || break + if [[ "${original_args[index]}" == "--config" ]]; then + (( index + 1 < ${#original_args[@]} )) || record_usage_error "--config requires a FILE.toml" + [[ -z "$config_file" ]] || record_usage_error "--config may be specified only once" + config_file="${original_args[index + 1]}" + (( ++index )) + fi + done + local -a config_args=() + if [[ -n "$config_file" ]]; then + local config_args_file + config_args_file="$(mktemp)" + if ! read_record_config "$config_file" > "$config_args_file"; then + rm -f -- "$config_args_file" + return 1 + fi + mapfile -d '' -t config_args < "$config_args_file" + rm -f -- "$config_args_file" + fi + set -- "${config_args[@]}" "${original_args[@]}" + while (( $# )); do + case "$1" in + --config|--duration|--output|--format|--width|--height|--fps|--device-uid|--reconnect-timeout|--reconnect-interval-ms|--record-device-diagnostics|--mcap-media|--preset|--plugin|--plugin-option) + (( $# >= 2 )) && [[ -n "$2" ]] || record_usage_error "$1 requires a value" + ;; + esac + case "$1" in + --) shift; passthrough+=("$@"); break ;; + --config) shift 2 ;; + --duration) duration="${2:-}"; shift 2 ;; + --output) run_dir="${2:-}"; shift 2 ;; + --format) format="${2:-}"; shift 2 ;; + --width) width="${2:-}"; shift 2 ;; + --height) height="${2:-}"; shift 2 ;; + --fps) fps="${2:-}"; shift 2 ;; + --device-uid) device_uid="${2:-}"; shift 2 ;; + --reconnect-timeout) reconnect_timeout="${2:-}"; shift 2 ;; + --reconnect-interval-ms) reconnect_interval_ms="${2:-}"; shift 2 ;; + --imu) request_imu=1; shift ;; + --no-imu) request_imu=0; shift ;; + --audio) request_audio=1; shift ;; + --no-audio) request_audio=0; shift ;; + --preview) preview=1; shift ;; + --no-preview) preview=0; shift ;; + --record-device-diagnostics=*) record_diagnostics="${1#*=}"; shift ;; + --record-device-diagnostics) record_diagnostics="${2:-}"; shift 2 ;; + --mcap-media) media_mode="${2:-}"; shift 2 ;; + --keep-media-sidecars) keep_sidecars=1; shift ;; + --no-keep-media-sidecars) keep_sidecars=0; shift ;; + --preset) preset="${2:-}"; shift 2 ;; + --plugin) plugin_path="${2:-}"; shift 2 ;; + --plugin-option) passthrough+=("${2:-}"); shift 2 ;; + --help|-h) usage; return 0 ;; + *) record_usage_error "unknown option '$1'" ;; + esac + done + preset_python_version "$preset" >/dev/null + [[ "$format" == "mjpg" || "$format" == "h264" || "$format" == "h265" ]] || record_usage_error "--format must be mjpg, h264, or h265" + [[ "$media_mode" == "metadata-only" || "$media_mode" == "embedded" ]] || record_usage_error "--mcap-media must be metadata-only or embedded" + [[ "$width" =~ ^[1-9][0-9]*$ && "$height" =~ ^[1-9][0-9]*$ && "$fps" =~ ^[1-9][0-9]*$ ]] || record_usage_error "width, height, and fps must be positive integers" + [[ "$reconnect_timeout" =~ ^[0-9]+$ ]] || record_usage_error "--reconnect-timeout must be a non-negative integer" + [[ "$reconnect_interval_ms" =~ ^[1-9][0-9]*$ ]] || record_usage_error "--reconnect-interval-ms must be a positive integer" + if [[ -n "$duration" ]]; then + [[ "$duration" =~ ^[1-9][0-9]*$ ]] || record_usage_error "--duration must be a positive integer" + fi + if (( keep_sidecars )) && [[ "$media_mode" == "metadata-only" ]]; then + record_usage_error "--keep-media-sidecars is only valid with embedded media" + fi + + # Native passthrough arguments win; record the effective policy, not the wrapper default. + local option + for option in "${passthrough[@]}"; do + [[ "$option" != --record-device-diagnostics=* ]] || record_diagnostics="${option#*=}" + done + case "$record_diagnostics" in + on|true|1) record_diagnostics=on ;; + off|false|0) record_diagnostics=off ;; + *) record_usage_error "--record-device-diagnostics must be on or off" ;; + esac + + local plugin + plugin="$(resolve_plugin "$preset" "$plugin_path")" + local output_base="$SOURCE_ROOT" + [[ -f "$SOURCE_ROOT/CMakeLists.txt" ]] || output_base="$PWD" + if [[ -z "$run_dir" ]]; then + run_dir="$output_base/recordings/ego_$(date +%Y%m%d_%H%M%S)" + elif [[ "$run_dir" != /* ]]; then + run_dir="$PWD/$run_dir" + fi + [[ ! -e "$run_dir" ]] || die "Output directory already exists: $run_dir" + mkdir -p "$run_dir/logs" + if [[ "$media_mode" == "metadata-only" ]] || (( keep_sidecars )); then + mkdir -p "$run_dir/raw" + fi + write_requested_profile_manifest "$run_dir/requested_profile.json" "$format" "$width" "$height" "$fps" "$record_diagnostics" + + local capabilities_file="$run_dir/capabilities.txt" + echo "Inspecting connected device before recording..." + emit_capabilities "$plugin" "$device_uid" | tee "$capabilities_file" + grep -q '^Sensor LeftColor$' "$capabilities_file" || die "Connected device has no ColorLeft sensor." + grep -q '^Sensor RightColor$' "$capabilities_file" || die "Connected device has no ColorRight sensor." + + local enable_imu=0 + local enable_audio=0 + if (( request_imu )); then + if grep -q '^Sensor Accel$' "$capabilities_file" && grep -q '^Sensor Gyro$' "$capabilities_file"; then + enable_imu=1 + else + warn "Accel/Gyro are unavailable; continuing without IMU. Use --no-imu to silence this warning." + fi + fi + if (( request_audio )); then + if grep -q '^Sensor Audio$' "$capabilities_file"; then + enable_audio=1 + else + warn "Audio is unavailable; continuing without audio. Use --no-audio to silence this warning." + fi + fi + + local extension + extension="$(stream_extension "$format")" + local -a plugin_args=( + "--mcap-filename=$run_dir/metadata.mcap" + "--mcap-media=$media_mode" + "--record-device-diagnostics=$record_diagnostics" + "--calibration-output=$run_dir/calibration.json" + "--reconnect-timeout=$reconnect_timeout" + "--reconnect-interval-ms=$reconnect_interval_ms" + ) + if [[ "$media_mode" == "metadata-only" ]] || (( keep_sidecars )); then + plugin_args+=( + "--add-stream=camera=ColorLeft,output=$run_dir/raw/ColorLeft.$extension,format=$format,width=$width,height=$height,fps=$fps" + "--add-stream=camera=ColorRight,output=$run_dir/raw/ColorRight.$extension,format=$format,width=$width,height=$height,fps=$fps" + ) + else + plugin_args+=( + "--add-stream=camera=ColorLeft,format=$format,width=$width,height=$height,fps=$fps" + "--add-stream=camera=ColorRight,format=$format,width=$width,height=$height,fps=$fps" + ) + fi + [[ -n "$device_uid" ]] && plugin_args+=("--device-uid=$device_uid") + (( preview )) && plugin_args+=(--preview) + (( enable_imu )) && plugin_args+=(--enable-imu --imu-rate=1000) + if (( enable_audio )); then + if [[ "$media_mode" == "metadata-only" ]] || (( keep_sidecars )); then + plugin_args+=("--audio-output=$run_dir/Audio.wav") + else + plugin_args+=(--enable-audio) + fi + fi + (( keep_sidecars )) && plugin_args+=(--keep-media-sidecars) + plugin_args+=("${passthrough[@]}") + + echo "Recording directory: $run_dir" + echo "Requested exact stereo profile: ${width}x${height}@${fps} ${format}; no profile fallback will be attempted." + echo "Stop with Ctrl-C once; the plugin will finalize media and MCAP files." + local capture_status + local -a pipeline_statuses=() + set +e + if [[ -n "$duration" ]]; then + timeout --preserve-status --signal=INT --kill-after=30s "$duration" \ + "$plugin" "${plugin_args[@]}" 2>&1 | tee "$run_dir/logs/capture.log" + else + "$plugin" "${plugin_args[@]}" 2>&1 | tee "$run_dir/logs/capture.log" + fi + pipeline_statuses=("${PIPESTATUS[@]}") + capture_status=${pipeline_statuses[0]} + [[ "${pipeline_statuses[1]}" -eq 0 ]] || die "Unable to write capture log: $run_dir/logs/capture.log" + set -e + if [[ "$capture_status" -ne 0 ]]; then + die "Capture failed with status $capture_status. See $run_dir/logs/capture.log" + fi + if [[ -n "$duration" ]]; then + echo "Timed capture command completed; SIGINT was scheduled after $duration seconds." + fi + local -a verify_args=("$run_dir" --preset "$preset" --plugin "$plugin" --media-mode "$media_mode") + (( enable_imu )) && verify_args+=(--expect-imu) + (( enable_audio )) && verify_args+=(--expect-audio) + command_verify "${verify_args[@]}" +} + +find_media_file() { + local directory="$1" + local stream="$2" + local candidate + for candidate in "$directory/$stream.h264" "$directory/$stream.h265" "$directory/$stream.mjpg"; do + if [[ -s "$candidate" ]]; then + echo "$candidate" + return 0 + fi + done + return 1 +} + +check_capture_health_log() { + local capture_log="$1" + if [[ ! -s "$capture_log" ]]; then + warn "No capture log is available; sequence-gap and queue-drop checks were skipped." + return 0 + fi + if grep -Eq '[[:space:]][1-9][0-9]* sequence gaps' "$capture_log"; then + die "Capture log reports a video sequence gap: $capture_log" + fi + if grep -Eq '(^|[[:space:]])dropped=[1-9][0-9]*([[:space:]]|$)' "$capture_log"; then + die "Capture log reports dropped metadata events: $capture_log" + fi + if grep -Eq 'video_frame_sets_dropped=[1-9][0-9]*([[:space:]]|$)' "$capture_log"; then + die "Capture log reports dropped video frame sets: $capture_log" + fi + if grep -Eq 'sequence gaps|video_frame_sets_dropped=' "$capture_log"; then + echo "Capture log sequence-gap and queue-drop counters: OK" + else + warn "Capture log contains no statistics counters; inspect it before accepting the run." + fi +} + +checked_media_command() { + local errors + errors="$(mktemp "$run_dir/.verify_media.XXXXXX")" || die "Could not create media check log" + if "$@" 2>"$errors"; then + if [[ ! -s "$errors" ]]; then + rm -f -- "$errors" + return 0 + fi + fi + # FFmpeg can emit a decoder error yet exit zero; its stderr is part of the result. + cat "$errors" >&2 + rm -f -- "$errors" + die "Media check failed: $1" +} + +video_signature() { + local media_file="$1" + local -a input_args=() + [[ "$media_file" == *.mjpg ]] && input_args=(-f mjpeg) + checked_media_command ffprobe -v error "${input_args[@]}" -select_streams v:0 \ + -show_entries stream=codec_name,width,height,r_frame_rate \ + -of csv=p=0 "$media_file" +} + +video_frame_count() { + local media_file="$1" + local -a input_args=() + [[ "$media_file" == *.mjpg ]] && input_args=(-f mjpeg) + checked_media_command ffprobe -v error "${input_args[@]}" -count_frames -select_streams v:0 \ + -show_entries stream=nb_read_frames -of default=noprint_wrappers=1:nokey=1 "$media_file" +} + +decode_video() { + local stream_name="$1" + local media_file="$2" + local progress + local -a input_args=() + [[ "$media_file" == *.mjpg ]] && input_args=(-f mjpeg) + progress="$(checked_media_command ffmpeg -v error -xerror -err_detect explode \ + "${input_args[@]}" -i "$media_file" -progress pipe:1 -nostats -f null -)" || return 1 + decoded_video_frame_count="$(awk -F= '$1 == "frame" { count = $2 } END { print count }' <<< "$progress")" + [[ "$decoded_video_frame_count" =~ ^[1-9][0-9]*$ ]] || die "No complete decoded $stream_name frames" + grep -Fx 'progress=end' <<< "$progress" >/dev/null || die "Incomplete $stream_name decode" + echo "$stream_name complete decode: OK" +} + +verify_requested_profile_manifest() { + local manifest="$1" + local left_signature="$2" + local right_signature="$3" + python3 "$SCRIPT_DIR/tools/verify_profile.py" "$manifest" "$left_signature" "$right_signature" +} + +verify_mcap_video_timing() { + local mcap="$1" + local mode="$2" + local left_frames="$3" + local right_frames="$4" + local manifest="$5" + python3 "$SCRIPT_DIR/tools/verify_video_timing.py" "$mcap" "$mode" "$left_frames" "$right_frames" "$manifest" +} + +print_fingerprint() { + local label="$1" media_file="$2" checksum + checksum="$(sha256sum -- "$media_file")" || die "Could not hash $media_file" + checksum="${checksum%% *}" + [[ "$checksum" =~ ^[0-9a-f]{64}$ ]] || die "Invalid SHA-256 fingerprint for $media_file" + echo "$label SHA-256: $checksum" +} + +command_verify_impl() { + local run_dir="${1:-}" + shift || true + [[ -n "$run_dir" ]] || die "verify requires a RUN_DIRECTORY" + [[ -d "$run_dir" ]] || die "Run directory does not exist: $run_dir" + local preset="$DEFAULT_PRESET" + local plugin_path="" + local expected_mode="" + local expect_imu=0 + local expect_audio=0 + while (( $# )); do + case "$1" in + --preset) preset="${2:-}"; shift 2 ;; + --plugin) plugin_path="${2:-}"; shift 2 ;; + --media-mode) expected_mode="${2:-}"; shift 2 ;; + --expect-imu) expect_imu=1; shift ;; + --expect-audio) expect_audio=1; shift ;; + --help|-h) usage; return 0 ;; + *) die "Unknown verify option: $1" ;; + esac + done + preset_python_version "$preset" >/dev/null + run_dir="$(cd "$run_dir" && pwd)" + local mcap="$run_dir/metadata.mcap" + [[ -s "$mcap" ]] || die "Missing or empty MCAP: $mcap" + local partial_file + partial_file="$(find "$run_dir" -name '*.partial' -print -quit)" + [[ -z "$partial_file" ]] || die "Incomplete capture: found $partial_file" + require_command ffmpeg "Install it with: sudo apt update && sudo apt install -y ffmpeg" || exit 1 + require_command ffprobe "Install it with: sudo apt update && sudo apt install -y ffmpeg" || exit 1 + require_command python3 "Install Python 3 and the standard MCAP reader: python3 -m pip install --user mcap" || exit 1 + python3 -c 'from mcap.reader import make_reader' 2>/dev/null || die "Python MCAP reader is missing. Install it with: python3 -m pip install --user mcap" + + local mode="$expected_mode" + if [[ -z "$mode" ]]; then + mode="$(python3 "$SCRIPT_DIR/tools/detect_media_mode.py" "$mcap")" + fi + [[ "$mode" == "metadata-only" || "$mode" == "embedded" ]] || die "Unknown MCAP media mode '$mode'" + + local -a topic_check_args=("$mcap" "$mode" "--requested-profile=$run_dir/requested_profile.json") + (( expect_imu )) && topic_check_args+=(--expect-imu) + (( expect_audio )) && topic_check_args+=(--expect-audio) + python3 "$SCRIPT_DIR/tools/verify_sensor_topics.py" "${topic_check_args[@]}" + + check_capture_health_log "$run_dir/logs/capture.log" + + local media_dir="$run_dir/raw" + local retained_media_dir="" + local temporary_export="" + if [[ "$mode" == "embedded" ]]; then + [[ -d "$media_dir" ]] && retained_media_dir="$media_dir" + temporary_export="$(mktemp -d "$run_dir/.verify_export.XXXXXX")" + # Verification failures exit from deep helper calls; EXIT cleanup prevents a + # multi-gigabyte embedded export from being stranded beside the recording. + local cleanup_command + printf -v cleanup_command 'rm -rf -- %q' "$temporary_export" + trap "$cleanup_command" EXIT + local exporter + exporter="$(resolve_exporter "$preset" "$plugin_path")" || return 1 + "$exporter" "$mcap" "$temporary_export" + media_dir="$temporary_export" + fi + local left right + left="$(find_media_file "$media_dir" ColorLeft)" || die "Could not find exported ColorLeft media in $media_dir" + right="$(find_media_file "$media_dir" ColorRight)" || die "Could not find exported ColorRight media in $media_dir" + if [[ -n "$retained_media_dir" ]]; then + local retained_left retained_right + retained_left="$(find_media_file "$retained_media_dir" ColorLeft)" \ + || die "Could not find retained ColorLeft media in $retained_media_dir" + retained_right="$(find_media_file "$retained_media_dir" ColorRight)" \ + || die "Could not find retained ColorRight media in $retained_media_dir" + cmp -s "$left" "$retained_left" || die "Embedded and retained ColorLeft media differ" + cmp -s "$right" "$retained_right" || die "Embedded and retained ColorRight media differ" + echo "Embedded video matches retained media sidecars: OK" + fi + local decoded_video_frame_count left_frames right_frames + decode_video ColorLeft "$left" + left_frames="$decoded_video_frame_count" + decode_video ColorRight "$right" + right_frames="$decoded_video_frame_count" + local left_signature right_signature left_probed_frames right_probed_frames + left_signature="$(video_signature "$left")" + right_signature="$(video_signature "$right")" + [[ -n "$left_signature" ]] || die "ffprobe found no ColorLeft video stream: $left" + [[ -n "$right_signature" ]] || die "ffprobe found no ColorRight video stream: $right" + [[ "$left_signature" == "$right_signature" ]] \ + || die "Stereo media profiles differ: ColorLeft=$left_signature ColorRight=$right_signature" + local requested_profile_manifest="$run_dir/requested_profile.json" + if [[ -e "$requested_profile_manifest" ]]; then + verify_requested_profile_manifest "$requested_profile_manifest" "$left_signature" "$right_signature" + else + warn "No requested_profile.json exists; exact requested-versus-recorded profile checks were skipped for this legacy run." + fi + left_probed_frames="$(video_frame_count "$left")" + right_probed_frames="$(video_frame_count "$right")" + [[ "$left_probed_frames" == "$left_frames" ]] || die "ColorLeft FFprobe count differs from complete decode: $left_probed_frames versus $left_frames" + [[ "$right_probed_frames" == "$right_frames" ]] || die "ColorRight FFprobe count differs from complete decode: $right_probed_frames versus $right_frames" + echo "Stereo elementary-stream profile (codec,width,height,ffprobe_rate): $left_signature" + echo "Decoded frames: ColorLeft=$left_frames ColorRight=$right_frames" + if [[ "$left_frames" != "$right_frames" ]]; then + warn "Stereo decoded counts differ at their initial IDR boundaries; per-stream MCAP counts and cadence must still pass." + fi + verify_mcap_video_timing \ + "$mcap" "$mode" "$left_frames" "$right_frames" "$requested_profile_manifest" + local wav="" + if [[ "$mode" == "embedded" ]]; then + wav="$media_dir/Audio.wav" + if [[ -s "$run_dir/Audio.wav" ]]; then + [[ -s "$wav" ]] || die "Embedded MCAP is missing audio retained as $run_dir/Audio.wav" + cmp -s "$wav" "$run_dir/Audio.wav" || die "Embedded and retained PCM audio differ" + echo "Embedded audio matches retained WAV sidecar: OK" + fi + else + wav="$run_dir/Audio.wav" + fi + if [[ -s "$wav" ]]; then + local wav_signature + wav_signature="$(checked_media_command ffprobe -v error \ + -show_entries stream=codec_name,sample_rate,channels,bits_per_sample \ + -of default=noprint_wrappers=1 "$wav")" + echo "$wav_signature" + if (( expect_audio )); then + grep -Fx 'codec_name=pcm_s16le' <<< "$wav_signature" >/dev/null \ + || die "Expected PCM S16_LE audio in $wav" + grep -Fx 'sample_rate=48000' <<< "$wav_signature" >/dev/null \ + || die "Expected 48000 Hz audio in $wav" + grep -Fx 'channels=1' <<< "$wav_signature" >/dev/null \ + || die "Expected mono audio in $wav" + grep -Fx 'bits_per_sample=16' <<< "$wav_signature" >/dev/null \ + || die "Expected 16-bit audio in $wav" + fi + elif (( expect_audio )); then + die "Audio was requested but no WAV sidecar or exported embedded audio exists." + else + warn "No WAV sidecar exists. This is expected only when audio was unavailable, disabled, or embedded without sidecars." + fi + echo "Delivery file: $mcap" + if command -v sha256sum >/dev/null 2>&1; then + # Hash while exported media still exists, and reject read failures. + print_fingerprint Delivery "$mcap" + print_fingerprint ColorLeft "$left" + print_fingerprint ColorRight "$right" + [[ ! -s "$wav" ]] || print_fingerprint Audio "$wav" + else + warn "sha256sum is unavailable; media fingerprints were not added to the report." + fi + if [[ -n "$temporary_export" ]]; then + rm -rf -- "$temporary_export" + temporary_export="" + trap - EXIT + fi + echo "Verification passed: $run_dir" +} + +command_verify() { + local run_dir="${1:-}" + if [[ ! -d "$run_dir" ]]; then + command_verify_impl "$@" + return + fi + + run_dir="$(cd "$run_dir" && pwd)" + local report="$run_dir/verification_report.txt" + local failed_report="$run_dir/verification_report.failed.txt" + local working_report="$run_dir/.verification_report.txt.inprogress" + local -a pipeline_status + + rm -f -- "$report" || die "Could not invalidate the previous success report: $report" + set +e + { + set -e + echo "Ego MCAP consistency verification report" + echo "Generated (UTC): $(date -u +%Y-%m-%dT%H:%M:%SZ)" + echo "Run directory: $run_dir" + echo + command_verify_impl "$run_dir" "${@:2}" + } 2>&1 | tee "$working_report" + pipeline_status=("${PIPESTATUS[@]}") + set -e + + if (( pipeline_status[1] != 0 )); then + die "Could not write consistency report: $working_report" + fi + if (( pipeline_status[0] != 0 )); then + mv -f -- "$working_report" "$failed_report" + echo "Failed consistency report: $failed_report" >&2 + return "${pipeline_status[0]}" + fi + + mv -f -- "$working_report" "$report" + echo "Consistency report: $report" +} + +command_export_media() { + local run_dir="${1:-}" + shift || true + [[ -n "$run_dir" ]] || die "export-media requires a RUN_DIRECTORY" + [[ -d "$run_dir" ]] || die "Run directory does not exist: $run_dir" + run_dir="$(cd "$run_dir" && pwd)" + local preset="$DEFAULT_PRESET" + local output_dir="$run_dir/exported" + while (( $# )); do + case "$1" in + --output) output_dir="${2:-}"; shift 2 ;; + --preset) preset="${2:-}"; shift 2 ;; + --help|-h) usage; return 0 ;; + *) die "Unknown export-media option: $1" ;; + esac + done + preset_python_version "$preset" >/dev/null + [[ -n "$output_dir" ]] || die "--output requires a DIRECTORY" + [[ "$output_dir" = /* ]] || output_dir="$PWD/$output_dir" + local mcap="$run_dir/metadata.mcap" + [[ -s "$mcap" ]] || die "Missing or empty MCAP: $mcap" + [[ ! -e "$mcap.partial" ]] || die "Incomplete capture: found $mcap.partial" + [[ ! -e "$output_dir" ]] || die "Export output already exists: $output_dir" + local exporter + exporter="$(resolve_exporter "$preset")" || return 1 + "$exporter" "$mcap" "$output_dir" + echo "Export completed: $output_dir" +} + +main() { + local command="${1:-}" + [[ -n "$command" ]] || { usage; return 1; } + shift + case "$command" in + doctor) command_doctor "$@" ;; + build) command_build "$@" ;; + capabilities) command_capabilities "$@" ;; + record) command_record "$@" ;; + verify) command_verify "$@" ;; + export-media) command_export_media "$@" ;; + --help|-h|help) usage ;; + *) die "Unknown command '$command'." ;; + esac +} + +main "$@" diff --git a/src/plugins/ego/mcap/CMakeLists.txt b/src/plugins/ego/mcap/CMakeLists.txt new file mode 100644 index 0000000000..27c03186a2 --- /dev/null +++ b/src/plugins/ego/mcap/CMakeLists.txt @@ -0,0 +1,8 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# Name library directories by function and executable directories with a _tool suffix. +add_subdirectory(io) +add_subdirectory(merge) +add_subdirectory(merge_tool) +add_subdirectory(export_media_tool) diff --git a/src/plugins/ego/mcap/export_media_tool/CMakeLists.txt b/src/plugins/ego/mcap/export_media_tool/CMakeLists.txt new file mode 100644 index 0000000000..2b2a42862d --- /dev/null +++ b/src/plugins/ego/mcap/export_media_tool/CMakeLists.txt @@ -0,0 +1,8 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +add_executable(ego_mcap_export_media export_embedded_media.cpp) +target_link_libraries(ego_mcap_export_media PRIVATE ego::mcap_support log_bridge::log_bridge_core) +set_target_properties(ego_mcap_export_media PROPERTIES + RUNTIME_OUTPUT_DIRECTORY "${EGO_PLUGIN_OUTPUT_DIR}") +install(TARGETS ego_mcap_export_media RUNTIME DESTINATION plugins/ego COMPONENT ego) diff --git a/src/plugins/ego/mcap/export_media_tool/export_embedded_media.cpp b/src/plugins/ego/mcap/export_media_tool/export_embedded_media.cpp new file mode 100644 index 0000000000..541ea7f92f --- /dev/null +++ b/src/plugins/ego/mcap/export_media_tool/export_embedded_media.cpp @@ -0,0 +1,206 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace +{ + +std::string extension(core::EgoPixelFormat format) +{ + switch (format) + { + case core::EgoPixelFormat_Mjpg: + return "mjpg"; + case core::EgoPixelFormat_H264: + return "h264"; + case core::EgoPixelFormat_H265: + return "h265"; + default: + throw std::runtime_error("unsupported Ego video format in embedded MCAP"); + } +} + +void write_le16(std::ofstream& output, uint16_t value) +{ + const uint8_t bytes[] = { static_cast(value), static_cast(value >> 8) }; + output.write(reinterpret_cast(bytes), sizeof(bytes)); +} + +void write_le32(std::ofstream& output, uint32_t value) +{ + const uint8_t bytes[] = { static_cast(value), static_cast(value >> 8), + static_cast(value >> 16), static_cast(value >> 24) }; + output.write(reinterpret_cast(bytes), sizeof(bytes)); +} + +void write_wav_header(std::ofstream& output, uint32_t bytes, uint32_t rate, uint16_t channels, uint16_t bits) +{ + output.write("RIFF", 4); + write_le32(output, 36 + bytes); + output.write("WAVEfmt ", 8); + write_le32(output, 16); + write_le16(output, 1); + write_le16(output, channels); + write_le32(output, rate); + const auto block_align = static_cast(channels * bits / 8); + write_le32(output, rate * block_align); + write_le16(output, block_align); + write_le16(output, bits); + output.write("data", 4); + write_le32(output, bytes); +} + +std::unique_ptr open_reader(const std::filesystem::path& path) +{ + auto reader = std::make_unique(); + const auto status = reader->open(path.string()); + if (!status.ok()) + throw std::runtime_error("unable to open MCAP: " + status.message); + return reader; +} + +void export_media(const std::filesystem::path& input_path, const std::filesystem::path& output_dir) +{ + auto video_reader = open_reader(input_path); + const auto summary = video_reader->readSummary(mcap::ReadSummaryMethod::NoFallbackScan); + if (!summary.ok() || !video_reader->footer()) + throw std::runtime_error("embedded MCAP has no valid completed summary/footer"); + const core::RecordedSchemas recorded(*video_reader); + video_reader.reset(); + using VideoViewer = core::McapTrackerViewers; + // A reader per eye prevents one eye's drain from buffering the entire other eye. + std::array, 2> video; + for (size_t index = 0; index < video.size(); ++index) + video[index] = std::make_unique( + open_reader(input_path), "ego_media", + std::vector{ core::EnumNameEgoCameraStream(static_cast(index)) }, + recorded); + core::McapTrackerViewers audio( + open_reader(input_path), "ego_media", { "Audio" }, recorded); + + std::filesystem::create_directories(output_dir); + size_t video_count = 0; + for (size_t index = 0; index < 2; ++index) + { + std::ofstream output; + std::filesystem::path path; + core::EgoPixelFormat format{}; + while (const auto record = video[index]->read(0)) + { + const auto* data = record->data(); + if (!data || !data->encoded_data()) + continue; + if (static_cast(data->stream()) != index) + throw std::runtime_error("embedded video stream does not match its MCAP topic"); + if (!output.is_open()) + { + format = data->pixel_format(); + path = + output_dir / (std::string(core::EnumNameEgoCameraStream(data->stream())) + "." + extension(format)); + output.open(path, std::ios::binary | std::ios::trunc); + if (!output) + throw std::runtime_error("unable to create video output: " + path.string()); + } + if (data->pixel_format() != format) + throw std::runtime_error("embedded video format changed during recording"); + output.write(reinterpret_cast(data->encoded_data()->data()), + static_cast(data->encoded_data()->size())); + if (!output) + throw std::runtime_error("failed while exporting embedded video"); + } + if (output.is_open()) + { + output.close(); + if (!output) + throw std::runtime_error("failed while finalizing embedded video output"); + ++video_count; + std::cout << core::EnumNameEgoCameraStream(static_cast(index)) << ": " << path + << std::endl; + } + } + + std::ofstream wav; + const auto wav_path = output_dir / "Audio.wav"; + uint64_t pcm_bytes = 0; + uint32_t audio_rate = 0; + uint16_t audio_channels = 0; + uint16_t audio_bits = 0; + while (const auto record = audio.read(0)) + { + const auto* data = record->data(); + if (!data || !data->pcm_data()) + continue; + if (data->sample_rate_hz() == 0 || data->channel_count() == 0 || data->bits_per_sample() != 16 || + data->sample_format() != core::EgoAudioSampleFormat_S16LE || + static_cast(data->sample_count()) * data->channel_count() * 2 != data->pcm_data()->size()) + throw std::runtime_error("embedded PCM profile or sample count is invalid"); + if (!wav.is_open()) + { + audio_rate = data->sample_rate_hz(); + audio_channels = data->channel_count(); + audio_bits = data->bits_per_sample(); + wav.open(wav_path, std::ios::binary | std::ios::trunc); + if (!wav) + throw std::runtime_error("unable to create WAV output"); + // The final RIFF sizes are known only after all PCM chunks are copied. + write_wav_header(wav, 0, audio_rate, audio_channels, audio_bits); + } + if (audio_rate != data->sample_rate_hz() || audio_channels != data->channel_count() || + audio_bits != data->bits_per_sample()) + throw std::runtime_error("embedded audio profile changed during recording"); + pcm_bytes += data->pcm_data()->size(); + if (pcm_bytes > UINT32_MAX - 36U) + throw std::runtime_error("embedded PCM exceeds the RIFF/WAV 4 GiB limit"); + wav.write(reinterpret_cast(data->pcm_data()->data()), + static_cast(data->pcm_data()->size())); + if (!wav) + throw std::runtime_error("failed while exporting embedded audio"); + } + if (wav.is_open()) + { + wav.seekp(0); + write_wav_header(wav, static_cast(pcm_bytes), audio_rate, audio_channels, audio_bits); + wav.close(); + if (!wav) + throw std::runtime_error("failed while finalizing embedded WAV output"); + std::cout << "Audio: " << wav_path << std::endl; + } + if (video_count == 0 && pcm_bytes == 0) + throw std::runtime_error("MCAP contains no embedded Ego media channels"); +} + +} // namespace + +int main(int argc, char** argv) +try +{ + if (argc != 3) + { + std::cerr << "Usage: " << argv[0] << " INPUT.mcap OUTPUT_DIRECTORY" << std::endl; + return 1; + } + export_media(argv[1], argv[2]); + return 0; +} +catch (const std::exception& error) +{ + isaaccapture::Logger::get("isaaccapture.plugins.ego.export_media")->error("{}: {}", argv[0], error.what()); + return 1; +} diff --git a/src/plugins/ego/mcap/io/CMakeLists.txt b/src/plugins/ego/mcap/io/CMakeLists.txt new file mode 100644 index 0000000000..42ee65e464 --- /dev/null +++ b/src/plugins/ego/mcap/io/CMakeLists.txt @@ -0,0 +1,9 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 ORBBEC INC. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +add_library(ego_mcap_support STATIC mcap_impl.cpp) +add_library(ego::mcap_support ALIAS ego_mcap_support) +target_link_libraries(ego_mcap_support PUBLIC mcap::mcap_core isaacteleop_schema) + +target_include_directories(ego_mcap_support INTERFACE + $) diff --git a/src/plugins/ego/mcap/io/inc/ego_mcap/checked_write.hpp b/src/plugins/ego/mcap/io/inc/ego_mcap/checked_write.hpp new file mode 100644 index 0000000000..b54a1baebc --- /dev/null +++ b/src/plugins/ego/mcap/io/inc/ego_mcap/checked_write.hpp @@ -0,0 +1,24 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 ORBBEC INC. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include + +#include + +namespace ego_mcap +{ +template +void write_checked(mcap::McapWriter& writer, + core::McapTrackerChannels& channels, + size_t index, + const core::Serialized& record) +{ + const auto count = writer.statistics().messageCount; + channels.write(index, record); + // The official helper logs status errors; capture must stop rather than lose a record. + if (writer.statistics().messageCount != count + 1) + throw std::runtime_error("Ego MCAP record was not accepted by the writer"); +} +} // namespace ego_mcap diff --git a/src/plugins/ego/mcap/io/mcap_impl.cpp b/src/plugins/ego/mcap/io/mcap_impl.cpp new file mode 100644 index 0000000000..e8765b3e13 --- /dev/null +++ b/src/plugins/ego/mcap/io/mcap_impl.cpp @@ -0,0 +1,6 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 ORBBEC INC. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#define MCAP_IMPLEMENTATION +#include +#include diff --git a/src/plugins/ego/mcap/merge/CMakeLists.txt b/src/plugins/ego/mcap/merge/CMakeLists.txt new file mode 100644 index 0000000000..5379327a41 --- /dev/null +++ b/src/plugins/ego/mcap/merge/CMakeLists.txt @@ -0,0 +1,8 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 ORBBEC INC. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +add_library(ego_mcap_merge_core STATIC merge.cpp) +add_library(ego::mcap_merge ALIAS ego_mcap_merge_core) +target_include_directories(ego_mcap_merge_core INTERFACE + $) +target_link_libraries(ego_mcap_merge_core PUBLIC ego::mcap_support) diff --git a/src/plugins/ego/mcap/merge/inc/ego_mcap/merge.hpp b/src/plugins/ego/mcap/merge/inc/ego_mcap/merge.hpp new file mode 100644 index 0000000000..c604b4984f --- /dev/null +++ b/src/plugins/ego/mcap/merge/inc/ego_mcap/merge.hpp @@ -0,0 +1,14 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 ORBBEC INC. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include + +namespace plugins::ego +{ +// Inputs remain available after success or failure; existing output files are rejected. +void merge_recording(const std::filesystem::path& recording, + const std::filesystem::path& media, + const std::filesystem::path& output); +} diff --git a/src/plugins/ego/mcap/merge/merge.cpp b/src/plugins/ego/mcap/merge/merge.cpp new file mode 100644 index 0000000000..adc67b88fc --- /dev/null +++ b/src/plugins/ego/mcap/merge/merge.cpp @@ -0,0 +1,333 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 ORBBEC INC. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/ego_mcap/merge.hpp" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace plugins::ego +{ +namespace +{ +constexpr std::string_view observed_prefix = "session_observed/"; +constexpr std::array structured_names = { "core.EgoFrameMetadataRecord", "core.EgoImuBatchRecord", + "core.EgoAudioChunkRecord", "core.EgoCalibrationRecord", + "core.EgoDeviceStateRecord" }; + +void check(const mcap::Status& status) +{ + if (!status.ok()) + throw std::runtime_error("MCAP merge: " + status.message); +} + +struct Counts +{ + uint64_t messages = 0; + uint64_t metadata = 0; + uint64_t attachments = 0; +}; + +struct Source +{ + mcap::McapReader reader; + std::unordered_map schemas; + std::unordered_map channels; + std::unordered_map output_channels; + Counts counts; + std::optional record_device_diagnostics; + + explicit Source(const std::filesystem::path& path, bool reject_observed = true) + { + check(reader.open(path.string())); + check(reader.readSummary(mcap::ReadSummaryMethod::NoFallbackScan)); + if (!reader.header() || !reader.footer() || !reader.statistics()) + throw std::runtime_error("MCAP merge requires a complete file with a summary: " + path.string()); + schemas = reader.schemas(); + channels = reader.channels(); + for (const auto& [id, channel] : channels) + { + if (id == 0 || (channel->schemaId != 0 && !schemas.contains(channel->schemaId))) + throw std::runtime_error("MCAP channel references a missing schema"); + if (reject_observed && channel->topic.starts_with(observed_prefix)) + throw std::runtime_error("Reserved MCAP topic prefix already exists: " + channel->topic); + } + } + + mcap::SchemaPtr schema(const mcap::Channel& channel) const + { + return channel.schemaId == 0 ? nullptr : schemas.at(channel.schemaId); + } +}; + +bool same_schema(const Source& left, const mcap::Channel& a, const Source& right, const mcap::Channel& b) +{ + if (a.messageEncoding != b.messageEncoding) + return false; + const auto sa = left.schema(a); + const auto sb = right.schema(b); + if (!sa || !sb) + return !sa && !sb; + return sa->name == sb->name && sa->encoding == sb->encoding && sa->data == sb->data; +} + +bool structured(const Source& source, const mcap::Channel& channel) +{ + const auto schema = source.schema(channel); + if (!schema) + return false; + for (const auto name : structured_names) + if (schema->name == name) + return true; + return false; +} + +Counts scan(Source& source, mcap::McapWriter* output = nullptr) +{ + Counts counts; + unsigned headers = 0; + unsigned footers = 0; + unsigned ends = 0; + auto& data = *source.reader.dataSource(); + mcap::TypedRecordReader records(data, sizeof(mcap::Magic), data.size() - sizeof(mcap::Magic)); + records.onHeader = [&](const auto&, auto) { ++headers; }; + records.onFooter = [&](const auto&, auto) { ++footers; }; + records.onDataEnd = [&](const auto&, auto) { ++ends; }; + records.onSchema = [&](const mcap::SchemaPtr schema, auto, auto) + { + const auto found = source.schemas.find(schema->id); + if (found == source.schemas.end() || found->second->name != schema->name || + found->second->encoding != schema->encoding || found->second->data != schema->data) + throw std::runtime_error("MCAP data and summary schemas disagree"); + }; + records.onChannel = [&](const mcap::ChannelPtr channel, auto, auto) + { + const auto found = source.channels.find(channel->id); + if (found == source.channels.end() || found->second->topic != channel->topic || + found->second->schemaId != channel->schemaId || + found->second->messageEncoding != channel->messageEncoding || found->second->metadata != channel->metadata) + throw std::runtime_error("MCAP data and summary channels disagree"); + }; + records.onMessage = [&](const mcap::Message& message, auto, auto) + { + if (!source.channels.contains(message.channelId)) + throw std::runtime_error("MCAP message references an unknown channel"); + if (message.logTime == mcap::MaxTime) + throw std::runtime_error("MCAP logTime exceeds the indexed reader's range"); + ++counts.messages; + }; + records.onMetadata = [&](const mcap::Metadata& metadata, auto) + { + ++counts.metadata; + if (metadata.name == "ego.recording") + { + const auto flag = metadata.metadata.find("record_device_diagnostics"); + const auto version = metadata.metadata.find("diagnostic_version"); + if (flag == metadata.metadata.end() || (flag->second != "on" && flag->second != "off") || + version == metadata.metadata.end() || version->second != "2") + throw std::runtime_error("MCAP contains an unsupported EGO recording policy"); + const bool enabled = flag->second == "on"; + if (source.record_device_diagnostics && *source.record_device_diagnostics != enabled) + throw std::runtime_error("MCAP contains conflicting EGO recording policies"); + source.record_device_diagnostics = enabled; + } + if (output) + check(output->write(metadata)); + }; + records.onAttachment = [&](const mcap::Attachment& attachment, auto) + { + ++counts.attachments; + if (output) + { + auto copy = attachment; + check(output->write(copy)); + } + }; + // Source indexes contain file offsets; rebuild them for the merged layout. + records.onMessageIndex = [](const auto&, auto) {}; + records.onChunkIndex = [](const auto&, auto) {}; + records.onMetadataIndex = [](const auto&, auto) {}; + records.onAttachmentIndex = [](const auto&, auto) {}; + records.onStatistics = [](const auto&, auto) {}; + records.onSummaryOffset = [](const auto&, auto) {}; + records.onUnknownRecord = [](const auto&, auto, auto) + { throw std::runtime_error("MCAP merge cannot preserve an unknown record type"); }; + while (records.next()) + check(records.status()); + check(records.status()); + const auto& stats = *source.reader.statistics(); + if (headers != 1 || footers != 1 || ends != 1 || counts.messages != stats.messageCount || + counts.metadata != stats.metadataCount || counts.attachments != stats.attachmentCount) + throw std::runtime_error("MCAP record inventory does not match the complete file summary"); + return counts; +} + +std::unordered_set conflicting_topics(const Source& recording, const Source& media) +{ + std::unordered_set conflicts; + for (const Source* source : { &recording, &media }) + { + std::unordered_map topics; + for (const auto& [id, channel] : source->channels) + { + const auto [it, inserted] = topics.emplace(channel->topic, channel); + if (!inserted && !same_schema(*source, *it->second, *source, *channel)) + throw std::runtime_error("Conflicting schemas for MCAP topic: " + channel->topic); + } + } + for (const auto& [id, host] : recording.channels) + for (const auto& [other_id, capture] : media.channels) + if (host->topic == capture->topic) + { + if (!same_schema(recording, *host, media, *capture) || !structured(media, *capture)) + throw std::runtime_error("Conflicting MCAP topic: " + host->topic); + conflicts.insert(host->topic); + } + return conflicts; +} + +void register_channels(Source& source, mcap::McapWriter& output, const std::unordered_set& observed) +{ + std::unordered_map ids; + for (const auto& [id, original] : source.schemas) + { + mcap::Schema schema(original->name, original->encoding, original->data); + output.addSchema(schema); + ids.emplace(id, schema.id); + } + for (const auto& [id, original] : source.channels) + { + auto topic = original->topic; + if (observed.contains(topic)) + topic = std::string(observed_prefix) + topic; + mcap::Channel channel(topic, original->messageEncoding, + original->schemaId == 0 ? 0 : ids.at(original->schemaId), original->metadata); + output.addChannel(channel); + source.output_channels.emplace(id, channel.id); + } +} + +uint64_t merge_messages(Source& recording, Source& media, mcap::McapWriter& output) +{ + mcap::ReadMessageOptions options; + options.readOrder = mcap::ReadMessageOptions::ReadOrder::LogTimeOrder; + const auto problem = [](const mcap::Status& status) { check(status); }; + auto host_messages = recording.reader.readMessages(problem, options); + auto capture_messages = media.reader.readMessages(problem, options); + auto host = host_messages.begin(); + auto capture = capture_messages.begin(); + uint64_t count = 0; + while (host != host_messages.end() || capture != capture_messages.end()) + { + const bool take_host = capture == capture_messages.end() || + (host != host_messages.end() && host->message.logTime <= capture->message.logTime); + auto& iterator = take_host ? host : capture; + auto& source = take_host ? recording : media; + auto message = iterator->message; + message.channelId = source.output_channels.at(message.channelId); + check(output.write(message)); + ++count; + ++iterator; + } + return count; +} +} // namespace + +void merge_recording(const std::filesystem::path& recording_path, + const std::filesystem::path& media_path, + const std::filesystem::path& output_path) +{ + const auto recording_file = std::filesystem::weakly_canonical(recording_path); + const auto media_file = std::filesystem::weakly_canonical(media_path); + const auto final_file = std::filesystem::weakly_canonical(output_path); + const auto partial_file = std::filesystem::path(final_file.string() + ".partial"); + if (recording_file == media_file || recording_file == final_file || media_file == final_file) + throw std::invalid_argument("MCAP merge inputs and output must be distinct"); + for (const auto& path : { final_file, partial_file }) + if (std::filesystem::exists(path) || std::filesystem::is_symlink(path)) + throw std::runtime_error("Refusing to overwrite MCAP output: " + path.string()); + if (!std::filesystem::is_directory(final_file.parent_path())) + throw std::runtime_error("Create the MCAP output directory before merging"); + + Source recording(recording_file); + Source media(media_file); + recording.counts = scan(recording); + media.counts = scan(media); + if (recording.record_device_diagnostics && media.record_device_diagnostics && + recording.record_device_diagnostics != media.record_device_diagnostics) + throw std::runtime_error("Parent and fragment EGO recording policies disagree"); + const auto policy = + media.record_device_diagnostics ? media.record_device_diagnostics : recording.record_device_diagnostics; + if (policy && !*policy) + { + // Match schema identity, including custom topic names and empty registered channels. + for (const Source* source : { &recording, &media }) + for (const auto& [id, channel] : source->channels) + if (const auto schema = source->schema(*channel); schema && schema->name == "core.EgoDeviceStateRecord") + throw std::runtime_error("Diagnostic recording is off but an input contains EgoDeviceState: " + + channel->topic); + } + const auto conflicts = conflicting_topics(recording, media); + if (recording.schemas.size() + media.schemas.size() > std::numeric_limits::max() || + recording.channels.size() + media.channels.size() > std::numeric_limits::max()) + throw std::runtime_error("Merged MCAP exceeds schema or channel ID capacity"); + + std::ofstream stream; + stream.exceptions(std::ios::badbit | std::ios::failbit); + stream.open(partial_file, std::ios::binary | std::ios::out); + mcap::McapWriter output; + try + { + mcap::McapWriterOptions options(recording.reader.header()->profile); + options.compression = mcap::Compression::None; + options.library = "isaaccapture.ego.merge"; + output.open(stream, options); + // The fragment includes capture-tail samples that the host may not poll. + // Preserve overlapping host observations under session_observed/. + register_channels(recording, output, conflicts); + register_channels(media, output, {}); + scan(recording, &output); + scan(media, &output); + mcap::Metadata provenance; + provenance.name = "ego.merge"; + provenance.metadata = { { "recording", recording_file.string() }, + { "media", media_file.string() }, + { "recording.library", recording.reader.header()->library }, + { "media.library", media.reader.header()->library }, + { "media.profile", media.reader.header()->profile }, + { "capture.channels", "complete plugin capture" }, + { "session_observed.channels", "host session observations" } }; + check(output.write(provenance)); + const auto expected_messages = recording.counts.messages + media.counts.messages; + if (merge_messages(recording, media, output) != expected_messages) + throw std::runtime_error("Indexed MCAP merge did not preserve every message"); + output.close(); + stream.close(); + Source validation(partial_file, false); + const auto counts = scan(validation); + if (counts.messages != expected_messages || + counts.metadata != recording.counts.metadata + media.counts.metadata + 1 || + counts.attachments != recording.counts.attachments + media.counts.attachments) + throw std::runtime_error("Merged MCAP failed the final inventory check"); + validation.reader.close(); + if (std::filesystem::exists(final_file) || std::filesystem::is_symlink(final_file)) + throw std::runtime_error("MCAP output appeared during merge: " + final_file.string()); + std::filesystem::rename(partial_file, final_file); + } + catch (...) + { + output.terminate(); + throw; + } +} +} // namespace plugins::ego diff --git a/src/plugins/ego/mcap/merge_tool/CMakeLists.txt b/src/plugins/ego/mcap/merge_tool/CMakeLists.txt new file mode 100644 index 0000000000..c69b0ca2e0 --- /dev/null +++ b/src/plugins/ego/mcap/merge_tool/CMakeLists.txt @@ -0,0 +1,9 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 ORBBEC INC. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +add_executable(ego_mcap_merge main.cpp) +target_link_libraries(ego_mcap_merge PRIVATE ego::mcap_merge log_bridge::log_bridge_core) +set_target_properties(ego_mcap_merge PROPERTIES + RUNTIME_OUTPUT_DIRECTORY "${EGO_PLUGIN_OUTPUT_DIR}") +# The recording helper resolves this tool beside the component-installed plugin. +install(TARGETS ego_mcap_merge RUNTIME DESTINATION plugins/ego COMPONENT ego) diff --git a/src/plugins/ego/mcap/merge_tool/main.cpp b/src/plugins/ego/mcap/merge_tool/main.cpp new file mode 100644 index 0000000000..3c7e8cf13f --- /dev/null +++ b/src/plugins/ego/mcap/merge_tool/main.cpp @@ -0,0 +1,47 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 ORBBEC INC. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include + +#include +#include +#include + +int main(int argc, char** argv) +{ + const auto usage = [] + { std::cout << "Usage: ego_mcap_merge --recording SESSION.mcap --media FRAGMENT.mcap --output FINAL.mcap\n"; }; + if (argc == 2 && std::string(argv[1]) == "--help") + { + usage(); + return 0; + } + std::map options; + for (int i = 1; i < argc; i += 2) + { + const std::string argument = argv[i]; + if (i + 1 >= argc || (argument != "--recording" && argument != "--media" && argument != "--output") || + !options.emplace(argument, argv[i + 1]).second) + { + std::cout << "Invalid or duplicate option: " << argument << '\n'; + usage(); + return 2; + } + } + if (options.size() != 3) + { + usage(); + return 2; + } + try + { + plugins::ego::merge_recording(options.at("--recording"), options.at("--media"), options.at("--output")); + return 0; + } + catch (const std::exception& error) + { + isaaccapture::Logger::get("isaaccapture.ego.merge")->error("{}", error.what()); + return 1; + } +} diff --git a/src/plugins/ego/plugin.yaml b/src/plugins/ego/plugin.yaml new file mode 100644 index 0000000000..cec3f90468 --- /dev/null +++ b/src/plugins/ego/plugin.yaml @@ -0,0 +1,11 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +name: ego_camera +description: "EGO stereo video, IMU, audio, calibration, and state plugin" +command: "./ego_camera_plugin" +version: "1.0.0" +devices: + - path: "/camera/ego" + type: "camera" + description: "EGO ColorLeft/ColorRight, IMU, audio, calibration, and state" diff --git a/src/plugins/ego/tools/detect_media_mode.py b/src/plugins/ego/tools/detect_media_mode.py new file mode 100644 index 0000000000..0a437df21e --- /dev/null +++ b/src/plugins/ego/tools/detect_media_mode.py @@ -0,0 +1,15 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +import sys +from mcap.reader import make_reader + +topics = set() +with open(sys.argv[1], "rb") as stream: + for _, channel, _ in make_reader(stream).iter_messages(log_time_order=False): + topics.add(channel.topic) +print( + "embedded" + if any(topic.startswith("ego_media/") for topic in topics) + else "metadata-only" +) diff --git a/src/plugins/ego/tools/record_config.py b/src/plugins/ego/tools/record_config.py new file mode 100644 index 0000000000..d1c44e21fa --- /dev/null +++ b/src/plugins/ego/tools/record_config.py @@ -0,0 +1,82 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +import sys +import tomllib +from pathlib import Path + +path = Path(sys.argv[1]) +try: + with path.open("rb") as stream: + document = tomllib.load(stream) +except (OSError, tomllib.TOMLDecodeError) as error: + raise SystemExit(f"Invalid record configuration {path}: {error}") from error + +if set(document) != {"record"} or not isinstance(document["record"], dict): + raise SystemExit(f"{path}: the only top-level entry must be a [record] table") +record = document["record"] +scalar_options = { + "duration": (int, "--duration"), + "output": (str, "--output"), + "format": (str, "--format"), + "width": (int, "--width"), + "height": (int, "--height"), + "fps": (int, "--fps"), + "device_uid": (str, "--device-uid"), + "reconnect_timeout": (int, "--reconnect-timeout"), + "reconnect_interval_ms": (int, "--reconnect-interval-ms"), + "mcap_media": (str, "--mcap-media"), + "preset": (str, "--preset"), + "plugin": (str, "--plugin"), +} +boolean_options = { + "record_device_diagnostics": ( + "--record-device-diagnostics=on", + "--record-device-diagnostics=off", + ), + "imu": ("--imu", "--no-imu"), + "audio": ("--audio", "--no-audio"), + "preview": ("--preview", "--no-preview"), + "keep_media_sidecars": ("--keep-media-sidecars", "--no-keep-media-sidecars"), +} +allowed = set(scalar_options) | set(boolean_options) | {"plugin_options"} +unknown = sorted(set(record) - allowed) +if unknown: + raise SystemExit(f"{path}: unknown [record] option(s): {', '.join(unknown)}") + +arguments = [] +for name, (expected_type, option) in scalar_options.items(): + if name not in record: + continue + value = record[name] + if type(value) is not expected_type: + raise SystemExit(f"{path}: record.{name} must be {expected_type.__name__}") + if isinstance(value, str) and (not value or "\0" in value or "\n" in value): + raise SystemExit( + f"{path}: record.{name} must be a non-empty single-line string" + ) + arguments.extend((option, str(value))) + +for name, (enabled, disabled) in boolean_options.items(): + if name not in record: + continue + value = record[name] + if type(value) is not bool: + raise SystemExit(f"{path}: record.{name} must be true or false") + arguments.append(enabled if value else disabled) + +plugin_options = record.get("plugin_options", []) +if not isinstance(plugin_options, list) or any( + type(value) is not str for value in plugin_options +): + raise SystemExit(f"{path}: record.plugin_options must be an array of strings") +for value in plugin_options: + if not value.startswith("--") or "\0" in value or "\n" in value: + raise SystemExit( + f"{path}: each record.plugin_options entry must be a single-line --option" + ) + arguments.extend(("--plugin-option", value)) + +sys.stdout.buffer.write(b"\0".join(value.encode() for value in arguments)) +if arguments: + sys.stdout.buffer.write(b"\0") diff --git a/src/plugins/ego/tools/verify_profile.py b/src/plugins/ego/tools/verify_profile.py new file mode 100644 index 0000000000..fd861ea169 --- /dev/null +++ b/src/plugins/ego/tools/verify_profile.py @@ -0,0 +1,77 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +from fractions import Fraction +import json +from pathlib import Path +import sys + +manifest_path = Path(sys.argv[1]) +actual_signatures = { + "ColorLeft": sys.argv[2], + "ColorRight": sys.argv[3], +} +try: + manifest = json.loads(manifest_path.read_text(encoding="utf-8")) +except (OSError, json.JSONDecodeError) as error: + raise SystemExit( + f"Invalid requested profile manifest {manifest_path}: {error}" + ) from error + +if manifest.get("version") != 1: + raise SystemExit( + f"Unsupported requested profile manifest version in {manifest_path}" + ) +streams = manifest.get("streams") +expected_names = set(actual_signatures) +if not isinstance(streams, dict) or set(streams) != expected_names: + raise SystemExit( + f"Requested profile manifest must contain exactly {sorted(expected_names)}" + ) + +codec_names = {"mjpg": "mjpeg", "h264": "h264", "h265": "hevc"} +for stream_name, signature in actual_signatures.items(): + requested = streams[stream_name] + if not isinstance(requested, dict): + raise SystemExit(f"Invalid requested profile for {stream_name}") + requested_format = requested.get("format") + if requested_format not in codec_names: + raise SystemExit( + f"Invalid requested format for {stream_name}: {requested_format!r}" + ) + for field in ("width", "height", "fps"): + if type(requested.get(field)) is not int or requested[field] <= 0: + raise SystemExit( + f"Invalid requested {field} for {stream_name}: {requested.get(field)!r}" + ) + parts = signature.split(",") + if len(parts) != 4: + raise SystemExit(f"Invalid ffprobe signature for {stream_name}: {signature!r}") + codec, width, height, rate = parts + try: + actual_width = int(width) + actual_height = int(height) + actual_rate = Fraction(rate) + except (ValueError, ZeroDivisionError) as error: + raise SystemExit( + f"Invalid ffprobe signature for {stream_name}: {signature!r}" + ) from error + if actual_rate <= 0: + raise SystemExit(f"Invalid ffprobe rate for {stream_name}: {rate!r}") + expected_media = ( + codec_names[requested_format], + requested["width"], + requested["height"], + ) + actual_media = (codec, actual_width, actual_height) + if actual_media != expected_media: + raise SystemExit( + f"{stream_name} recorded profile {signature!r} does not match request " + f"{requested_format},{requested['width']},{requested['height']},{requested['fps']}/1" + ) + # Elementary-stream VUI clocks may use multiple ticks per frame. MCAP's + # declared profile and raw device timestamps determine capture FPS. +print( + "Requested codec/resolution match ColorLeft and ColorRight: OK; " + "capture FPS is checked from MCAP metadata and device timestamps" +) diff --git a/src/plugins/ego/tools/verify_sensor_topics.py b/src/plugins/ego/tools/verify_sensor_topics.py new file mode 100644 index 0000000000..3815e1c06b --- /dev/null +++ b/src/plugins/ego/tools/verify_sensor_topics.py @@ -0,0 +1,471 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +from collections import Counter +import json +import math +from pathlib import Path +import struct +import sys +from mcap.reader import make_reader + + +def unpack_from(fmt, data, offset, label): + size = struct.calcsize(fmt) + if offset < 0 or offset + size > len(data): + raise SystemExit(f"Invalid FlatBuffer while reading {label} at byte {offset}") + return struct.unpack_from(fmt, data, offset) + + +def field_position(data, table, field_id): + (vtable_delta,) = unpack_from(" vtable + vtable_size: + return None + (field_offset,) = unpack_from(" len(data): + raise SystemExit(f"Invalid FlatBuffer while reading {label} elements") + return count, begin + + +def diagnostic_record(data, label): + table, _ = record_data(data, label) + version = scalar_field(data, table, 3, " tolerance: + raise SystemExit( + f"{topic} epoch {capture_epoch} raw-device cadence is {observed_rate:.3f} Hz; " + f"batches declare {state['rate']} Hz" + ) + coverage = sample_span + 1_000_000_000 / state["rate"] + if coverage < video_span * 0.90: + raise SystemExit( + f"{topic} epoch {capture_epoch} covers only {coverage / 1_000_000_000:.3f}s " + f"of the {video_span / 1_000_000_000:.3f}s video timeline" + ) + print( + f" {topic}: epoch={capture_epoch} batches={epoch['batches']} samples={epoch['samples']} " + f"declared_hz={state['rate']} raw_device_hz={observed_rate:.3f}" + ) + if imu["ego_imu/Accel"]["rate"] != imu["ego_imu/Gyro"]["rate"]: + raise SystemExit("Accel and Gyro declare different sample rates") + +if expect_audio: + for topic, state in audio.items(): + sample_rate = state["profile"][0] + if set(state["epochs"]) != set(video_epochs): + raise SystemExit( + f"{topic} capture epochs differ from video: {sorted(state['epochs'])}" + ) + for capture_epoch, epoch in sorted(state["epochs"].items()): + if epoch["chunks"] < 2 or epoch["last"] <= epoch["first"]: + raise SystemExit(f"{topic} epoch {capture_epoch} has too few chunks") + video_span = ( + video_epochs[capture_epoch]["last"] + - video_epochs[capture_epoch]["first"] + ) + timestamp_span = epoch["last"] - epoch["first"] + timestamped_counts = ( + epoch["samples"] - epoch["first_sample_count"], + epoch["samples"] - epoch["last_sample_count"], + ) + observed_rates = [ + count * 1_000_000_000 / timestamp_span for count in timestamped_counts + ] + observed_rate = min( + observed_rates, key=lambda rate: abs(rate - sample_rate) + ) + tolerance = max(5.0, sample_rate * 0.05) + if abs(observed_rate - sample_rate) > tolerance: + raise SystemExit( + f"{topic} epoch {capture_epoch} raw-device cadence is {observed_rate:.3f} Hz; " + f"chunks declare {sample_rate} Hz" + ) + duration = epoch["samples"] * 1_000_000_000 / sample_rate + if duration < video_span * 0.90: + raise SystemExit( + f"{topic} epoch {capture_epoch} contains only {duration / 1_000_000_000:.3f}s " + f"of PCM for the {video_span / 1_000_000_000:.3f}s video timeline" + ) + print( + f" {topic}: epoch={capture_epoch} chunks={epoch['chunks']} samples={epoch['samples']} " + f"duration_s={duration / 1_000_000_000:.3f} raw_device_hz={observed_rate:.3f}" + ) + if mode == "embedded": + metadata = audio["ego_audio/Audio"] + media = audio["ego_media/Audio"] + if metadata["timeline"] != media["timeline"]: + raise SystemExit("Embedded PCM and audio-metadata timelines differ") +print("MCAP Footer, required topics, and per-topic log-time monotonicity: OK") diff --git a/src/plugins/ego/tools/verify_video_timing.py b/src/plugins/ego/tools/verify_video_timing.py new file mode 100644 index 0000000000..26c4adb1af --- /dev/null +++ b/src/plugins/ego/tools/verify_video_timing.py @@ -0,0 +1,191 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +import json +from pathlib import Path +import struct +import sys + +from mcap.reader import make_reader + + +def unpack_from(fmt, data, offset, label): + size = struct.calcsize(fmt) + if offset < 0 or offset + size > len(data): + raise SystemExit(f"Invalid FlatBuffer while reading {label} at byte {offset}") + return struct.unpack_from(fmt, data, offset) + + +def field_position(data, table, field_id): + (vtable_delta,) = unpack_from(" vtable + vtable_size: + return None + (field_offset,) = unpack_from(" {profile}" + ) + state["last"] = timestamp + state["last_sequence"] = sequence + state["profile"] = profile + state["count"] += 1 + state["timeline"].append((capture_epoch, sequence, timestamp, profile)) + epoch["last"] = timestamp + epoch["last_sequence"] = sequence + epoch["count"] += 1 + +for topic, state in topics.items(): + stream = state["stream"] + if state["count"] != expected_counts[stream]: + raise SystemExit( + f"{topic} count {state['count']} differs from decoded {stream} count {expected_counts[stream]}" + ) + if stream in requested_profiles and state["profile"] != requested_profiles[stream]: + raise SystemExit( + f"{topic} MCAP profile {state['profile']} differs from requested {requested_profiles[stream]}" + ) + for capture_epoch, epoch in sorted(state["epochs"].items()): + if epoch["count"] < 2 or epoch["last"] <= epoch["first"]: + raise SystemExit( + f"{topic} epoch {capture_epoch} has too few valid raw device timestamps" + ) + declared_fps = state["profile"][3] + if declared_fps <= 0: + raise SystemExit(f"{topic} has invalid declared FPS {declared_fps}") + observed_fps = ( + (epoch["count"] - 1) * 1_000_000_000 / (epoch["last"] - epoch["first"]) + ) + tolerance = max(0.5, declared_fps * 0.05) + if abs(observed_fps - declared_fps) > tolerance: + raise SystemExit( + f"{topic} epoch {capture_epoch} raw-device cadence is {observed_fps:.3f} FPS; " + f"the MCAP profile declares {declared_fps} FPS" + ) + print( + f" {topic}: epoch={capture_epoch} count={epoch['count']} profile={state['profile']} " + f"raw_device_fps={observed_fps:.3f} span_ns={epoch['last'] - epoch['first']}" + ) +if mode == "embedded": + for stream in expected_counts: + metadata = topics[f"ego_metadata/{stream}"] + media = topics[f"ego_media/{stream}"] + comparable = ("count", "profile", "timeline") + if any(metadata[field] != media[field] for field in comparable): + raise SystemExit(f"Embedded and metadata timelines differ for {stream}") +print("MCAP video counts and raw device timestamp cadence: OK") diff --git a/src/python/isaaccapture/deviceio_trackers/__init__.py b/src/python/isaaccapture/deviceio_trackers/__init__.py index a46a1df931..770a72e2f0 100644 --- a/src/python/isaaccapture/deviceio_trackers/__init__.py +++ b/src/python/isaaccapture/deviceio_trackers/__init__.py @@ -21,6 +21,8 @@ HapticCommandReaderTracker, TensorPushTracker, FullBodyTracker, + EgoFrameMetadataTracker, + EgoImuTracker, ITrackerSession, NUM_JOINTS, JOINT_PALM, @@ -59,6 +61,8 @@ def __getattr__(name: str): "MessageChannelStatus", "MessageChannelTracker", "FullBodyTracker", + "EgoFrameMetadataTracker", + "EgoImuTracker", "HapticCommandReaderTracker", "TensorPushTracker", "HandTracker", diff --git a/src/python/isaaccapture/ego/__init__.py b/src/python/isaaccapture/ego/__init__.py new file mode 100644 index 0000000000..a71ef590c0 --- /dev/null +++ b/src/python/isaaccapture/ego/__init__.py @@ -0,0 +1,64 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 ORBBEC INC. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""EGO recording, clock calibration, and offline sample association.""" + +from __future__ import annotations + +from importlib import import_module +from typing import TYPE_CHECKING + +from .recording import configure_recording, embedded_recording + +if TYPE_CHECKING: + from typing import Any + +_SYNC_EXPORTS = ( + "RecordReference", + "MediaReference", + "VideoFrame", + "ImuSample", + "AudioSlice", + "SyncWindow", + "iter_sync_windows", + "analyze_recording_sync", + "recording_fingerprint", +) +_CALIBRATION_EXPORTS = ( + "PairedTimeEvent", + "TimeAlignmentFit", + "fit_time_alignment", + "make_recording_alignment", + "evidence_sha256", +) + +__all__ = [ + "configure_recording", + "embedded_recording", + "RecordReference", + "MediaReference", + "VideoFrame", + "ImuSample", + "AudioSlice", + "SyncWindow", + "iter_sync_windows", + "analyze_recording_sync", + "recording_fingerprint", + "PairedTimeEvent", + "TimeAlignmentFit", + "fit_time_alignment", + "make_recording_alignment", + "evidence_sha256", +] + + +def __getattr__(name: str) -> Any: + if name in _SYNC_EXPORTS: + module = import_module(".sync", __name__) + elif name in _CALIBRATION_EXPORTS: + module = import_module(".calibration", __name__) + else: + raise AttributeError(f"module {__name__!r} has no attribute {name!r}") + value = getattr(module, name) + globals()[name] = value + return value diff --git a/src/python/isaaccapture/ego/_records.py b/src/python/isaaccapture/ego/_records.py new file mode 100644 index 0000000000..19803ee557 --- /dev/null +++ b/src/python/isaaccapture/ego/_records.py @@ -0,0 +1,231 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 ORBBEC INC. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Read EGO record wrappers using the BFBS schema stored in their MCAP channel.""" + +from dataclasses import dataclass +import struct + +import numpy as np + + +class _Buffer: + def __init__(self, data): + self.data = data + + def check(self, offset, size): + if offset < 0 or size < 0 or offset + size > len(self.data): + raise ValueError("Truncated or invalid FlatBuffer") + + def scalar(self, offset, fmt): + self.check(offset, struct.calcsize("<" + fmt)) + return struct.unpack_from("<" + fmt, self.data, offset)[0] + + def indirect(self, offset): + target = offset + self.scalar(offset, "I") + self.check(target, 1) + return target + + def field(self, table, field_id): + vtable = table - self.scalar(table, "i") + size = self.scalar(vtable, "H") + self.check(vtable, size) + if size < 4 or size % 2: + raise ValueError("Invalid FlatBuffer vtable") + entry = 4 + 2 * field_id + relative = self.scalar(vtable + entry, "H") if entry < size else 0 + return table + relative if relative else None + + def value(self, table, field_id, fmt, default=0): + offset = self.field(table, field_id) + return default if offset is None else self.scalar(offset, fmt) + + def vector(self, offset, stride): + start = self.indirect(offset) + count = self.scalar(start, "I") + self.check(start + 4, count * stride) + return start + 4, count + + def string(self, offset): + start, size = self.vector(offset, 1) + self.check(start + size, 1) + if self.data[start + size] != 0: + raise ValueError("Invalid FlatBuffer string terminator") + return self.data[start : start + size].decode("utf-8") + + def text(self, table, field_id): + offset = self.field(table, field_id) + return self.string(offset) if offset is not None else "" + + def tables(self, table, field_id): + offset = self.field(table, field_id) + if offset is None: + return [] + start, count = self.vector(offset, 4) + return [self.indirect(start + 4 * i) for i in range(count)] + + +@dataclass(frozen=True) +class ByteVector: + offset: int + size: int + + +_SCALARS = { + 1: "B", + 2: "?", + 3: "b", + 4: "B", + 5: "h", + 6: "H", + 7: "i", + 8: "I", + 9: "q", + 10: "Q", + 11: "f", + 12: "d", +} + + +class RecordDecoder: + """Small reflection reader; media vectors remain references into the message.""" + + def __init__(self, bfbs, root_name): + schema = _Buffer(bfbs) + if len(bfbs) < 8 or bfbs[4:8] != b"BFBS": + raise ValueError("EGO synchronization requires an embedded BFBS schema") + root = schema.indirect(0) + self.objects = [] + for obj in schema.tables(root, 0): + fields = [] + for field in schema.tables(obj, 1): + field_type = schema.field(field, 1) + if field_type is None: + raise ValueError("BFBS field has no type") + field_type = schema.indirect(field_type) + fields.append( + { + "name": schema.text(field, 0), + "base": schema.value(field_type, 0, "b"), + "element": schema.value(field_type, 1, "b"), + "index": schema.value(field_type, 2, "i", -1), + "offset": schema.value(field, 3, "H"), + "integer": schema.value(field, 4, "q"), + "real": schema.value(field, 5, "d"), + "optional": schema.value(field, 11, "?"), + } + ) + self.objects.append( + { + "name": schema.text(obj, 0), + "struct": schema.value(obj, 2, "?"), + "size": schema.value(obj, 4, "i"), + "fields": fields, + } + ) + matches = [i for i, obj in enumerate(self.objects) if obj["name"] == root_name] + if ( + len(matches) != 1 + or not root_name.startswith("core.Ego") + or not root_name.endswith("Record") + ): + raise ValueError(f"Unsupported EGO record schema: {root_name}") + self.root = matches[0] + self.struct_dtypes = {} + if {f["name"] for f in self.objects[self.root]["fields"]} != { + "data", + "timestamp", + }: + raise ValueError("EGO record must contain the full data/timestamp wrapper") + + def decode(self, data): + buf = _Buffer(data) + result = self._object(buf, buf.indirect(0), self.root, 0) + if not result.get("data") or not result.get("timestamp"): + raise ValueError("EGO record is missing data or its capture timestamp") + return result + + def _object(self, buf, start, index, depth): + if depth > 20 or not 0 <= index < len(self.objects): + raise ValueError("Invalid or recursive BFBS object") + obj = self.objects[index] + if obj["struct"]: + buf.check(start, obj["size"]) + result = {} + for field in obj["fields"]: + offset = ( + start + field["offset"] + if obj["struct"] + else buf.field(start, (field["offset"] - 4) // 2) + ) + base = field["base"] + if offset is None: + value = None + if base in _SCALARS and not field["optional"]: + value = field["real"] if base in (11, 12) else field["integer"] + elif base == 13: + value = "" + elif base == 14: + value = [] + else: + value = self._value(buf, offset, base, field, depth + 1) + result[field["name"]] = value + return result + + def _value(self, buf, offset, base, field, depth): + if base in _SCALARS: + return buf.scalar(offset, _SCALARS[base]) + if base == 13: + return buf.string(offset) + if base == 15: + obj = self.objects[field["index"]] + target = offset if obj["struct"] else buf.indirect(offset) + return self._object(buf, target, field["index"], depth) + if base == 14: + element = field["element"] + stride = 4 + if element in _SCALARS: + stride = struct.calcsize(_SCALARS[element]) + elif element == 15 and self.objects[field["index"]]["struct"]: + stride = self.objects[field["index"]]["size"] + if stride <= 0: + raise ValueError("Invalid BFBS vector stride") + start, count = buf.vector(offset, stride) + if element == 4: + return ByteVector(start, count) + if ( + element == 15 + and self.objects[field["index"]]["name"] == "core.EgoImuSample" + ): + index = field["index"] + if index not in self.struct_dtypes: + obj = self.objects[index] + if not obj["struct"] or any( + value["base"] not in _SCALARS + or value["offset"] + struct.calcsize(_SCALARS[value["base"]]) + > stride + for value in obj["fields"] + ): + raise ValueError("Invalid IMU sample struct") + self.struct_dtypes[index] = np.dtype( + { + "names": [value["name"] for value in obj["fields"]], + "formats": [ + "<" + _SCALARS[value["base"]] for value in obj["fields"] + ], + "offsets": [value["offset"] for value in obj["fields"]], + "itemsize": stride, + } + ) + # Keep millions of IMU samples as immutable views of their record + # bytes rather than retaining one Python dictionary per sample. + samples = np.frombuffer( + buf.data, self.struct_dtypes[index], count, start + ) + samples.setflags(write=False) + return samples + return [ + self._value(buf, start + i * stride, element, field, depth) + for i in range(count) + ] + raise ValueError(f"Unsupported BFBS field type {base}") diff --git a/src/python/isaaccapture/ego/calibration.py b/src/python/isaaccapture/ego/calibration.py new file mode 100644 index 0000000000..2f11b5a163 --- /dev/null +++ b/src/python/isaaccapture/ego/calibration.py @@ -0,0 +1,380 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 ORBBEC INC. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Fit recording-bound clocks from annotated events and independent validation.""" + +from __future__ import annotations + +from dataclasses import dataclass +import hashlib +import math +from pathlib import Path +from typing import Mapping, Sequence + + +VALIDATION_DURATION_NS = 15 * 60 * 1_000_000_000 + + +@dataclass(frozen=True) +class PairedTimeEvent: + event_id: str + source_value: int + reference_time_ns: int + uncertainty_ns: int + evidence: str + + +@dataclass(frozen=True) +class TimeAlignmentFit: + source_anchor: int + reference_anchor_ns: int + scale: float + source_unit: str + status: str + max_abs_error_ns: int + uncertainty_ns: int + held_out_duration_ns: int + drift_ppm: float + absolute_error_percentiles_ns: tuple[int, int, int] + evidence: tuple[str, ...] + reasons: tuple[str, ...] + validation_start_ns: int + validation_end_ns: int + + def reference_time(self, source_value: int) -> int: + return self.reference_anchor_ns + round( + (source_value - self.source_anchor) * self.scale + ) + + def validation_report(self) -> dict: + return { + "status": self.status, + "method": "held_out_paired_physical_events", + "max_abs_error_ns": self.max_abs_error_ns, + "uncertainty_ns": self.uncertainty_ns, + "held_out_duration_ns": self.held_out_duration_ns, + "validation_start_ns": self.validation_start_ns, + "validation_end_ns": self.validation_end_ns, + "drift_ppm": self.drift_ppm, + "absolute_error_percentiles_ns": dict( + zip(("p50", "p95", "p99"), self.absolute_error_percentiles_ns) + ), + "evidence": list(self.evidence), + "reasons": list(self.reasons), + } + + +def _events(values: Sequence[PairedTimeEvent], name: str) -> list[PairedTimeEvent]: + if len(values) < 3: + raise ValueError(f"{name} needs at least three independently annotated events") + ids = set() + for value in values: + if not value.event_id or value.event_id in ids or not value.evidence: + raise ValueError(f"{name} needs unique event IDs and evidence references") + ids.add(value.event_id) + for number in ( + value.source_value, + value.reference_time_ns, + value.uncertainty_ns, + ): + if type(number) is not int: + raise ValueError( + "Event coordinates and uncertainty must be integer ns/samples" + ) + if value.uncertainty_ns < 0: + raise ValueError("Measurement uncertainty cannot be negative") + result = sorted(values, key=lambda event: event.source_value) + if any( + left.source_value >= right.source_value + or left.reference_time_ns >= right.reference_time_ns + for left, right in zip(result, result[1:]) + ): + raise ValueError( + f"{name} events must advance within one continuous clock segment" + ) + return result + + +def _percentile(values: list[int], probability: float) -> int: + ordered = sorted(values) + location = probability * (len(ordered) - 1) + lower = math.floor(location) + fraction = location - lower + return round( + ordered[lower] * (1 - fraction) + + ordered[min(lower + 1, len(ordered) - 1)] * fraction + ) + + +def fit_time_alignment( + calibration_events: Sequence[PairedTimeEvent], + validation_events: Sequence[PairedTimeEvent], + *, + fps: float, + validation_start_ns: int, + validation_end_ns: int, + source_unit: str = "ns", + sample_rate_hz: int | None = None, +) -> TimeAlignmentFit: + """Uncertainty includes event detection, exposure and light/sound source delay.""" + training = _events(calibration_events, "Calibration") + held_out = _events(validation_events, "Validation") + if type(fps) not in (int, float) or not math.isfinite(fps) or fps <= 0: + raise ValueError("fps must be finite and positive") + if ( + type(validation_start_ns) is not int + or type(validation_end_ns) is not int + or validation_start_ns >= validation_end_ns + ): + raise ValueError("Validation needs a nonempty integer-ns recording interval") + if source_unit not in ("ns", "samples"): + raise ValueError("source_unit must be ns or samples") + if source_unit == "samples" and ( + type(sample_rate_hz) is not int or sample_rate_hz <= 0 + ): + raise ValueError("Audio fitting needs its nominal sample rate") + if {event.event_id for event in training} & { + event.event_id for event in held_out + } or {(event.source_value, event.reference_time_ns) for event in training} & { + (event.source_value, event.reference_time_ns) for event in held_out + }: + raise ValueError("Calibration and validation events must be disjoint") + if any( + not validation_start_ns <= event.reference_time_ns <= validation_end_ns + for event in held_out + ): + raise ValueError("Validation event is outside its declared recording interval") + + # Subtract integer anchors before floating point; epoch-sized ns lose precision. + source_anchor = training[0].source_value + reference_origin = training[0].reference_time_ns + x = [event.source_value - source_anchor for event in training] + y = [event.reference_time_ns - reference_origin for event in training] + mean_x, mean_y = math.fsum(x) / len(x), math.fsum(y) / len(y) + denominator = math.fsum((value - mean_x) ** 2 for value in x) + scale = math.fsum((a - mean_x) * (b - mean_y) for a, b in zip(x, y)) / denominator + if not math.isfinite(scale) or scale <= 0: + raise ValueError("Fitted clock must be finite and advance") + reference_anchor = reference_origin + round(mean_y - scale * mean_x) + errors = [ + abs( + reference_anchor + + round((event.source_value - source_anchor) * scale) + - event.reference_time_ns + ) + for event in held_out + ] + uncertainty = max(event.uncertainty_ns for event in (*training, *held_out)) + duration = validation_end_ns - validation_start_ns + reasons = [] + if duration < VALIDATION_DURATION_NS: + reasons.append("validation_shorter_than_15_minutes") + relative_times = [ + (event.reference_time_ns - validation_start_ns) / duration for event in held_out + ] + if not ( + any(value <= 0.1 for value in relative_times) + and any(0.4 <= value <= 0.6 for value in relative_times) + and any(value >= 0.9 for value in relative_times) + ): + reasons.append("missing_beginning_middle_or_end_validation") + if max(errors) + uncertainty > 1_000_000_000 / (2 * fps): + reasons.append("physical_error_and_uncertainty_exceed_half_frame") + nominal_period = 1 if source_unit == "ns" else 1_000_000_000 / sample_rate_hz + return TimeAlignmentFit( + source_anchor, + reference_anchor, + scale, + source_unit, + "unverified" if reasons else "validated", + max(errors), + uncertainty, + duration, + (scale / nominal_period - 1) * 1_000_000, + tuple(_percentile(errors, probability) for probability in (0.5, 0.95, 0.99)), + tuple(sorted({event.evidence for event in (*training, *held_out)})), + tuple(reasons), + validation_start_ns, + validation_end_ns, + ) + + +def evidence_sha256(path: str | Path) -> str: + with Path(path).open("rb") as stream: + return hashlib.file_digest(stream, "sha256").hexdigest() + + +def make_recording_alignment( + recording: str | Path, + *, + model_id: str, + identity: Mapping, + profile: Mapping, + epochs: Sequence[Mapping], + media_paths: Sequence[str | Path] = (), +) -> dict: + """Build an explicit MCAP-bound model; each nonreference stream needs a fit.""" + path = Path(recording).resolve(strict=True) + if not path.is_file(): + raise ValueError("recording must name an MCAP file") + if not isinstance(model_id, str) or not model_id.strip() or not epochs: + raise ValueError("A recording model needs an ID and effective epoch intervals") + for field in ("serial", "firmware", "sdk_version"): + if not isinstance(identity.get(field), str) or not identity[field].strip(): + raise ValueError(f"Missing identity.{field}") + fps = profile.get("video", {}).get("fps") + if type(fps) not in (int, float) or not math.isfinite(fps) or fps <= 0: + raise ValueError("profile.video.fps must be finite and positive") + document = { + "version": 1, + "scope": "recording", + "status": "validated", + "reference_stream": "left_rgb", + "model_id": model_id, + "identity": dict(identity), + "profile": dict(profile), + "recording_fingerprint": { + "mcap_sha256": evidence_sha256(path), + "media_sha256": {}, + }, + "epochs": [], + } + for media in media_paths: + media_path = Path(media).resolve(strict=True) + relative = media_path.relative_to(path.parent).as_posix() + document["recording_fingerprint"]["media_sha256"][relative] = evidence_sha256( + media_path + ) + fits = [] + reports = [] + reasons = set() + ranges = {} + for value in epochs: + epoch = value["epoch"] + begin, end = value["valid_from_ns"], value["valid_to_ns"] + if any(type(number) is not int for number in (epoch, begin, end)): + raise ValueError("Epoch and effective interval must be integer values") + if ( + epoch < 0 + or begin >= end + or any( + begin < previous_end and previous_begin < end + for previous_begin, previous_end in ranges.get(epoch, []) + ) + ): + raise ValueError("Invalid epoch or effective interval") + ranges.setdefault(epoch, []).append((begin, end)) + streams = { + "left_rgb": { + "source_anchor_ns": begin, + "reference_anchor_ns": begin, + "scale": 1.0, + } + } + for name, fit in value["streams"].items(): + if name == "left_rgb": + raise ValueError("left_rgb defines the reference and is not fitted") + if name == "audio": + segments = [] + seen = set() + for segment in fit: + result = segment["fit"] + first = segment["first_sample_index"] + count = segment["sample_count"] + segment_id = segment["segment_id"] + if any( + type(number) is not int for number in (first, count, segment_id) + ): + raise ValueError("Audio segment coordinates must be integers") + if first < 0 or count <= 0 or segment_id < 0 or segment_id in seen: + raise ValueError("Invalid or duplicate audio segment") + seen.add(segment_id) + if ( + not isinstance(result, TimeAlignmentFit) + or result.source_unit != "samples" + ): + raise ValueError("Audio segment needs a sample-index fit") + continuity = segment.get("continuity_verified", False) + if type(continuity) is not bool: + raise ValueError("continuity_verified must be a boolean") + if not continuity: + reasons.add("audio_continuity_unverified") + segments.append( + { + "segment_id": segment_id, + "first_sample_index": first, + "sample_count": count, + "reference_anchor_ns": result.reference_time(first), + "period_ns": result.scale, + "continuity_verified": continuity, + "validation_start_ns": result.validation_start_ns, + "validation_end_ns": result.validation_end_ns, + } + ) + fits.append(result) + reports.append( + { + "epoch": epoch, + "stream": name, + "segment_id": segment_id, + **result.validation_report(), + } + ) + if not segments: + raise ValueError( + "Audio model needs at least one continuous segment" + ) + streams[name] = {"segments": segments} + else: + if name not in ("right_rgb", "accel", "gyro") or not isinstance( + fit, TimeAlignmentFit + ): + raise ValueError("Unsupported stream or missing fitted clock") + if fit.source_unit != "ns": + raise ValueError("Video/IMU model must use raw ns") + if not fit.validation_start_ns <= begin < end <= fit.validation_end_ns: + raise ValueError( + "Effective epoch interval exceeds independent validation bounds" + ) + streams[name] = { + "source_anchor_ns": fit.source_anchor, + "reference_anchor_ns": fit.reference_anchor_ns, + "scale": fit.scale, + "validation_start_ns": fit.validation_start_ns, + "validation_end_ns": fit.validation_end_ns, + } + fits.append(fit) + reports.append( + { + "epoch": epoch, + "stream": name, + **fit.validation_report(), + } + ) + document["epochs"].append( + { + "epoch": epoch, + "valid_from_ns": begin, + "valid_to_ns": end, + "streams": streams, + } + ) + if not fits: + raise ValueError("Provide at least one measured nonreference stream") + reasons.update(reason for fit in fits for reason in fit.reasons) + error = max(fit.max_abs_error_ns for fit in fits) + uncertainty = max(fit.uncertainty_ns for fit in fits) + if error + uncertainty > 500_000_000 / fps: + reasons.add("physical_error_and_uncertainty_exceed_half_frame") + if reasons or any(fit.status != "validated" for fit in fits): + document["status"] = "unverified" + document["validation"] = { + "method": "held_out_paired_physical_events", + "max_abs_error_ns": error, + "uncertainty_ns": uncertainty, + "held_out_duration_ns": min(fit.held_out_duration_ns for fit in fits), + "evidence": sorted({evidence for fit in fits for evidence in fit.evidence}), + "reasons": sorted(reasons), + "stream_reports": reports, + } + return document diff --git a/src/python/isaaccapture/ego/recording.py b/src/python/isaaccapture/ego/recording.py new file mode 100644 index 0000000000..20ec7bc0f4 --- /dev/null +++ b/src/python/isaaccapture/ego/recording.py @@ -0,0 +1,274 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 ORBBEC INC. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Finalize EGO media after the official session has released its resources.""" + +from __future__ import annotations + +import logging +import os +from contextlib import contextmanager +from dataclasses import replace +from pathlib import Path +from typing import TYPE_CHECKING +import subprocess + +if TYPE_CHECKING: + from collections.abc import Iterator + + from isaaccapture.teleop_session_manager import TeleopSession, TeleopSessionConfig + +logger = logging.getLogger(__name__) + + +def _option(args: list[str], name: str) -> str | None: + for argument in reversed(args): + if argument.startswith(name + "="): + return argument[len(name) + 1 :] + return None + + +def _fresh_paths(paths: tuple[Path, ...]) -> None: + if len(set(paths)) != len(paths): + raise ValueError("Recording, media fragment, and final output must be distinct") + for path in paths: + if not path.parent.is_dir(): + raise ValueError( + f"Create the output directory before recording: {path.parent}" + ) + for candidate in (path, Path(str(path) + ".partial")): + if candidate.exists() or candidate.is_symlink(): + raise FileExistsError( + f"Refusing to overwrite recording output: {candidate}" + ) + + +def _with_calibration(config: TeleopSessionConfig, arguments: list[str]): + from isaaccapture.deviceio_session import McapRecordingConfig + from isaaccapture.deviceio_trackers import EgoCalibrationTracker + from isaaccapture.retargeting_engine.deviceio_source_nodes import IDeviceIOSource + + if not isinstance(config.mcap_config, McapRecordingConfig): + return config.mcap_config, config.trackers + entries = list(config.mcap_config.get_tracker_names()) + automatic = [] + for graph in (config.pipeline, config.teleop_control_pipeline, *config.sinks): + if graph is not None: + for source in graph.get_leaf_nodes(): + if isinstance(source, IDeviceIOSource): + automatic.append((source.get_tracker(), source.name)) + recorded = [ + tracker + for tracker, _ in entries + automatic + if isinstance(tracker, EgoCalibrationTracker) + ] + if recorded: + calibration = recorded[0] + else: + if any(name == "ego_calibration" for _, name in entries + automatic): + raise ValueError( + "MCAP name 'ego_calibration' is already used by another tracker" + ) + manual = [ + tracker + for tracker in config.trackers + if isinstance(tracker, EgoCalibrationTracker) + ] + if len(manual) > 1: + raise ValueError( + "Choose an explicit MCAP name for each EGO calibration tracker" + ) + if manual: + calibration = manual[0] + else: + prefix = _option(arguments, "--collection-prefix") + if not prefix: + raise ValueError( + "EGO MCAP recording requires --collection-prefix to record calibration" + ) + calibration = EgoCalibrationTracker(prefix + "/Calibration") + entries.append((calibration, "ego_calibration")) + trackers = list(config.trackers) + # McapRecordingConfig holds raw tracker pointers; the session config owns this reference. + if all(tracker is not calibration for tracker in trackers): + trackers.append(calibration) + return McapRecordingConfig(config.mcap_config.filename, entries), trackers + + +def configure_recording( + config: TeleopSessionConfig, + *, + record_device_diagnostics: bool | None = None, +) -> TeleopSessionConfig: + """Copy a live config, retain calibration, and validate diagnostic recording. + + Explicit choice overrides plugin arguments, then the default is on. Off + rejects diagnostic trackers recorded explicitly or through any source graph; + manual live-only trackers remain usable. No session or device is opened. + """ + from isaaccapture.deviceio_session import McapRecordingConfig + from isaaccapture.deviceio_trackers import EgoDeviceStateTracker + from isaaccapture.plugin_manager import PluginManager + from isaaccapture.retargeting_engine.deviceio_source_nodes import IDeviceIOSource + from isaaccapture.teleop_session_manager import SessionMode + + if config.mode != SessionMode.LIVE: + raise ValueError("configure_recording requires a live session") + if ( + record_device_diagnostics is not None + and type(record_device_diagnostics) is not bool + ): + raise TypeError("record_device_diagnostics must be bool or None") + plugins = [ + item + for item in config.plugins + if item.enabled and item.plugin_name == "ego_camera" + ] + if len(plugins) != 1: + raise ValueError( + "configure_recording requires exactly one enabled ego_camera plugin" + ) + plugin = plugins[0] + manager = PluginManager( + [str(path) for path in plugin.search_paths if path.is_dir()] + ) + info = manager.get_plugin_info(plugin.plugin_name) + arguments = list(info.args) + list(plugin.plugin_args) + flag = "--record-device-diagnostics" + value = _option(arguments, flag) + if record_device_diagnostics is None: + if value not in (None, "on", "off", "true", "false", "1", "0"): + raise ValueError(f"{flag} must be on or off") + record_device_diagnostics = value not in ("off", "false", "0") + if not record_device_diagnostics and isinstance( + config.mcap_config, McapRecordingConfig + ): + for tracker, name in config.mcap_config.get_tracker_names(): + if isinstance(tracker, EgoDeviceStateTracker): + raise ValueError( + f"Diagnostic recording is off but MCAP tracker {name!r} records EgoDeviceState" + ) + # Match NV's public source graph roots, without altering session discovery. + roots = [config.pipeline, config.teleop_control_pipeline, *config.sinks] + for graph in roots: + if graph is None: + continue + for source in graph.get_leaf_nodes(): + if isinstance(source, IDeviceIOSource) and isinstance( + source.get_tracker(), EgoDeviceStateTracker + ): + raise ValueError( + f"Diagnostic recording is off but source {source.name!r} is automatically recorded by NV" + ) + args = [arg for arg in plugin.plugin_args if not arg.startswith(flag + "=")] + args.append(f"{flag}={'on' if record_device_diagnostics else 'off'}") + mcap_config, trackers = _with_calibration(config, arguments) + return replace( + config, + mcap_config=mcap_config, + trackers=trackers, + plugins=[ + replace(item, plugin_args=args) if item is plugin else item + for item in config.plugins + ], + ) + + +@contextmanager +def embedded_recording( + config: TeleopSessionConfig, + *, + media_fragment: str | os.PathLike[str], + merger: str | os.PathLike[str] | None = None, + record_device_diagnostics: bool | None = None, +) -> Iterator[TeleopSession]: + """Record through TeleopSession, then atomically merge the EGO capture fragment. + + Configure one EGO plugin with collection-prefix, embedded media, and a matching + mcap-media-spool. Catch KeyboardInterrupt inside the context for a clean stop. + Failures preserve intermediate files and propagate to the caller. + """ + from isaaccapture.deviceio_session import McapRecordingConfig + from isaaccapture.plugin_manager import PluginManager + from isaaccapture.teleop_session_manager import TeleopSession + + config = configure_recording( + config, record_device_diagnostics=record_device_diagnostics + ) + if not isinstance(config.mcap_config, McapRecordingConfig): + raise ValueError("embedded_recording requires McapRecordingConfig") + plugins = [ + plugin + for plugin in config.plugins + if plugin.enabled and plugin.plugin_name == "ego_camera" + ] + if len(plugins) != 1: + raise ValueError( + "embedded_recording requires exactly one enabled ego_camera plugin" + ) + plugin = plugins[0] + manager = PluginManager( + [str(path) for path in plugin.search_paths if path.is_dir()] + ) + info = manager.get_plugin_info(plugin.plugin_name) + working_dir = Path(info.working_dir).resolve() + arguments = list(info.args) + list(plugin.plugin_args) + fragment = Path(media_fragment).resolve() + spool = _option(arguments, "--mcap-media-spool") + if not _option(arguments, "--collection-prefix"): + raise ValueError("The EGO plugin requires --collection-prefix=PREFIX") + if _option(arguments, "--mcap-media") != "embedded" or not spool: + raise ValueError( + "The EGO plugin requires embedded media and --mcap-media-spool=PATH" + ) + if (working_dir / spool).resolve() != fragment: + raise ValueError("media_fragment must match the EGO plugin's mcap-media-spool") + if _option(arguments, "--mcap-filename"): + raise ValueError( + "The EGO plugin cannot use --mcap-filename with a live collection" + ) + executable = ( + Path(merger).resolve() if merger is not None else working_dir / "ego_mcap_merge" + ) + if not executable.is_file() or not os.access(executable, os.X_OK): + raise FileNotFoundError( + f"Build or install the EGO merge tool before capture: {executable}" + ) + + final = Path(config.mcap_config.filename).resolve() + recording = final.with_name(final.stem + ".session" + final.suffix) + _fresh_paths((recording, fragment, final)) + recording_config = McapRecordingConfig( + str(recording), config.mcap_config.get_tracker_names() + ) + session_config = replace( + config, + mcap_config=recording_config, + plugins=[ + replace(item, required=True) if item is plugin else item + for item in config.plugins + ], + ) + with TeleopSession(session_config) as session: + yield session + + # Explicit Plugin.stop() clears exit status, so only a committed fragment proves completion. + if not fragment.is_file() or Path(str(fragment) + ".partial").exists(): + raise RuntimeError( + f"EGO capture did not commit a complete fragment: {fragment}" + ) + subprocess.run( + [ + str(executable), + "--recording", + str(recording), + "--media", + str(fragment), + "--output", + str(final), + ], + check=True, + ) + if not final.is_file() or Path(str(final) + ".partial").exists(): + raise RuntimeError(f"The merge tool did not commit a complete output: {final}") + logger.info("Finalized EGO recording: %s", final) diff --git a/src/python/isaaccapture/ego/sync.py b/src/python/isaaccapture/ego/sync.py new file mode 100644 index 0000000000..57fdda4c92 --- /dev/null +++ b/src/python/isaaccapture/ego/sync.py @@ -0,0 +1,1423 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 ORBBEC INC. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Offline EGO sample association, independent of the NV replay lifecycle.""" + +from __future__ import annotations + +from bisect import bisect_left, bisect_right +from dataclasses import asdict, dataclass, replace +from fractions import Fraction +import hashlib +import json +import math +from pathlib import Path +import re +from typing import TYPE_CHECKING + +from ._records import ByteVector, RecordDecoder + +if TYPE_CHECKING: + from collections.abc import Iterable, Iterator + from typing import Any + +STREAMS = ("left_rgb", "right_rgb", "accel", "gyro", "audio") +_TOPICS = { + "ego_metadata/ColorLeft": ("left_rgb", "core.EgoFrameMetadataRecord"), + "ego_metadata/ColorRight": ("right_rgb", "core.EgoFrameMetadataRecord"), + "ego_imu/Accel": ("accel", "core.EgoImuBatchRecord"), + "ego_imu/Gyro": ("gyro", "core.EgoImuBatchRecord"), + "ego_audio/Audio": ("audio", "core.EgoAudioChunkRecord"), + "ego_media/ColorLeft": ("left_rgb", "core.EgoEncodedVideoFrameRecord"), + "ego_media/ColorRight": ("right_rgb", "core.EgoEncodedVideoFrameRecord"), + "ego_media/Audio": ("audio", "core.EgoPcmAudioChunkRecord"), + "ego_calibration/Calibration": ("calibration", "core.EgoCalibrationRecord"), +} + + +@dataclass(frozen=True, slots=True) +class RecordReference: + topic: str + record_index: int + sequence_number: int + epoch: int + available_time_ns: int + sample_time_local_ns: int + sample_time_raw_ns: int + + +@dataclass(frozen=True, slots=True) +class MediaReference: + storage: str + path: str + topic: str | None + record_index: int | None + byte_offset: int + byte_count: int + + +@dataclass(frozen=True, slots=True) +class VideoFrame: + stream: str + time_ns: int + record: RecordReference + media: MediaReference | None + width: int + height: int + fps: int + pixel_format: str + sdk_metadata: tuple[tuple[int, int], ...] + + +@dataclass(frozen=True, slots=True) +class ImuSample: + stream: str + time_ns: int + record: RecordReference + sample_index: int + sample_time_raw_ns: int + sample_time_local_ns: int + x_si: float + y_si: float + z_si: float + temperature_c: float + + +@dataclass(frozen=True, slots=True) +class AudioSlice: + record: RecordReference + media: MediaReference | None + segment_id: int + first_sample_index: int + sample_offset: int + sample_count: int + start_time_ns: int + end_time_ns: int + period_ns: float + sample_rate_hz: int + channels: int + bits_per_sample: int + continuity_verified: bool + + +@dataclass(frozen=True, slots=True) +class SyncWindow: + epoch: int + model_id: str + model_segment: int + start_time_ns: int + end_time_ns: int + reference_stream: str + required_streams: tuple[str, ...] + left_rgb: VideoFrame | None + right_rgb: VideoFrame | None + accel: tuple[ImuSample, ...] + gyro: tuple[ImuSample, ...] + audio: tuple[AudioSlice, ...] + complete: bool + reasons: tuple[str, ...] + + +def _sha256(path): + digest = hashlib.sha256() + with Path(path).open("rb") as source: + for chunk in iter(lambda: source.read(1024 * 1024), b""): + digest.update(chunk) + return digest.hexdigest() + + +def _path(recording): + path = Path(recording).expanduser().resolve() + path = path / "metadata.mcap" if path.is_dir() else path + if not path.is_file(): + raise ValueError(f"Recording does not exist: {path}") + return path + + +def _load(recording): + try: + from mcap.reader import make_reader + except ImportError as error: + raise ImportError( + "EGO offline synchronization requires isaaccapture[ego]" + ) from error + + path = _path(recording) + records = {stream: [] for stream in STREAMS} + media = {} + models = [] + identities = [] + decoders = {} + topic_counts = {} + ignored = 0 + with path.open("rb") as source: + reader = make_reader(source, validate_crcs=True) + for schema, channel, message in reader.iter_messages(log_time_order=False): + if channel.topic not in _TOPICS: + ignored += 1 + continue + stream, expected = _TOPICS[channel.topic] + if ( + schema is None + or schema.encoding != "flatbuffer" + or schema.name != expected + ): + raise ValueError(f"Unexpected schema for {channel.topic}") + if schema.id not in decoders: + decoders[schema.id] = RecordDecoder(schema.data, schema.name) + record = decoders[schema.id].decode(message.data) + data, timestamp = record["data"], record["timestamp"] + index = topic_counts.get(channel.topic, 0) + topic_counts[channel.topic] = index + 1 + ref = RecordReference( + channel.topic, + index, + data.get("sequence_number", 0), + data.get("capture_epoch", 0), + timestamp["available_time_local_common_clock"], + timestamp["sample_time_local_common_clock"], + timestamp["sample_time_raw_device_clock"], + ) + if stream == "calibration": + timing = data.get("time_alignment") + if timing and timing.get("json"): + model = json.loads(timing["json"]) + model = {**model, "_capture_epoch": ref.epoch} + models.append(model) + if model.get("identity"): + identities.append(model["identity"]) + continue + expected_sensor = { + "left_rgb": 0, + "right_rgb": 1, + "accel": 0, + "gyro": 1, + }.get(stream) + if ( + expected_sensor is not None + and data.get("stream", data.get("sensor")) != expected_sensor + ): + raise ValueError( + f"Sensor discriminator disagrees with topic {channel.topic}" + ) + item = {"data": data, "ref": ref, "media": None} + if channel.topic.startswith("ego_media/"): + vector = data.get("encoded_data", data.get("pcm_data")) + if not isinstance(vector, ByteVector): + raise ValueError(f"Missing embedded media on {channel.topic}") + key = stream, ref.epoch, ref.sequence_number + if key in media: + raise ValueError( + "Duplicate embedded media sequence within an epoch" + ) + item["media"] = MediaReference( + "mcap", str(path), channel.topic, index, vector.offset, vector.size + ) + media[key] = item + else: + records[stream].append(item) + for metadata in reader.iter_metadata(): + if metadata.name == "ego.time_alignment": + encoded = metadata.metadata.get("json") + if encoded: + models.append(json.loads(encoded)) + media_paths = set() + issues = [] + scoped_issues = [] + + def issue(message, streams=()): + issues.append(message) + scoped_issues.append((message, streams)) + + for stream in STREAMS: + byte_offset = 0 + retained_samples = {} + seen = set() + last_raw = {} + raw_regressions = 0 + for item in records[stream]: + data, ref = item["data"], item["ref"] + if stream != "audio": + raw_times = ( + ( + int(sample["sample_time_raw_device_clock_ns"]) + for sample in data["samples"] + ) + if stream in ("accel", "gyro") + else (ref.sample_time_raw_ns,) + ) + for raw in raw_times: + if ref.epoch in last_raw and raw <= last_raw[ref.epoch]: + raw_regressions += 1 + last_raw[ref.epoch] = raw + key = stream, ref.epoch, ref.sequence_number + if key in seen: + raise ValueError( + f"Duplicate {stream} sequence within epoch {ref.epoch}" + ) + seen.add(key) + embedded = media.pop(key, None) + if embedded: + for field in ( + "width", + "height", + "fps", + "pixel_format", + "sample_count", + "sample_rate_hz", + "channel_count", + "bits_per_sample", + "sample_format", + "timing", + ): + if field in data and data[field] != embedded["data"].get(field): + raise ValueError( + f"Embedded {stream} disagrees with its metadata: {field}" + ) + if ref.sample_time_raw_ns != embedded["ref"].sample_time_raw_ns: + raise ValueError( + "Embedded media and metadata have different sample timestamps" + ) + item["media"] = embedded["media"] + elif stream in ("left_rgb", "right_rgb"): + extension = _format(data["pixel_format"]) + media_path = ( + path.parent + / "raw" + / f"{'ColorLeft' if stream == 'left_rgb' else 'ColorRight'}.{extension}" + ) + size = data["encoded_bytes"] + if ( + media_path.is_file() + and byte_offset + size <= media_path.stat().st_size + ): + item["media"] = MediaReference( + "sidecar", str(media_path), None, None, byte_offset, size + ) + media_paths.add(media_path) + byte_offset += size + elif stream == "audio": + media_path = path.parent / "Audio.wav" + size, offset = data["byte_count"], data["wav_data_offset"] + if media_path.is_file() and offset + size <= media_path.stat().st_size: + item["media"] = MediaReference( + "sidecar", str(media_path), None, None, offset, size + ) + media_paths.add(media_path) + if stream == "audio": + if not all( + data[field] > 0 + for field in ("sample_count", "sample_rate_hz", "channel_count") + ): + raise ValueError( + "Audio sample count, rate, and channels must be positive" + ) + expected_bytes = ( + data["sample_count"] + * data["channel_count"] + * data["bits_per_sample"] + // 8 + ) + if ( + data["bits_per_sample"] != 16 + or data["sample_format"] != 0 + or data["byte_count"] != expected_bytes + ): + raise ValueError("Invalid S16LE audio chunk size or format") + if item["media"] and item["media"].byte_count != expected_bytes: + raise ValueError( + "Embedded audio byte length disagrees with sample count" + ) + timing = data.get("timing") + if not timing: + timing = { + "segment_id": 0, + "first_sample_index": retained_samples.get(ref.epoch, 0), + "continuity_known": False, + "discontinuity": False, + } + item["inferred_audio_index"] = True + item["timing"] = timing + retained_samples[ref.epoch] = ( + timing["first_sample_index"] + data["sample_count"] + ) + if raw_regressions: + issue( + f"{stream}: {raw_regressions} nonmonotonic raw sample timestamps within an epoch", + (stream,), + ) + if stream in ("left_rgb", "right_rgb") and records[stream]: + refs = [ + item["media"] + for item in records[stream] + if item["media"] and item["media"].storage == "sidecar" + ] + if refs and Path(refs[0].path).stat().st_size != byte_offset: + issue(f"{stream}: sidecar contains unindexed bytes", (stream,)) + if media: + issue( + f"{len(media)} embedded media records have no canonical metadata", + tuple({key[0] for key in media}), + ) + identity = identities[-1] if identities else {} + if any(value != identity for value in identities): + issue("Device identity changes within recording") + capabilities = path.parent / "capabilities.txt" + if capabilities.is_file(): + text = capabilities.read_text() + for key, pattern in { + "serial": r"\bserial=(\S+)", + "firmware": r"\bfirmware=(\S+)", + "sdk_version": r"SDK version=(\S+)", + }.items(): + match = re.search(pattern, text) + if match: + if key in identity and identity[key] != match[1]: + issue(f"Conflicting recorded {key}") + identity = {**identity, key: match[1]} + return { + "path": path, + "records": records, + "models": models, + "identity": identity, + "media_paths": media_paths, + "issues": issues, + "scoped_issues": scoped_issues, + "ignored_records": ignored, + } + + +def _format(value): + try: + return {0: "mjpg", 1: "h264", 2: "h265"}[value] + except KeyError as error: + raise ValueError(f"Unknown EGO pixel format: {value}") from error + + +def recording_fingerprint(recording: str | Path) -> dict[str, str | dict[str, str]]: + """Hash the MCAP and the media files referenced by its canonical records.""" + return _fingerprint(_load(recording)) + + +def _fingerprint(loaded): + return { + "mcap_sha256": _sha256(loaded["path"]), + "media_sha256": { + str(path.relative_to(loaded["path"].parent)): _sha256(path) + for path in sorted(loaded["media_paths"]) + }, + } + + +def _integer(value, name, minimum=None): + if ( + isinstance(value, bool) + or not isinstance(value, int) + or (minimum is not None and value < minimum) + ): + raise ValueError( + f"{name} must be an integer" + + (f" >= {minimum}" if minimum is not None else "") + ) + return value + + +def _positive(value, name): + if ( + isinstance(value, bool) + or not isinstance(value, (int, float)) + or not math.isfinite(value) + or value <= 0 + ): + raise ValueError(f"{name} must be finite and positive") + return value + + +def _required(required_streams, reference_stream, default=False): + if reference_stream not in ("left_rgb", "right_rgb"): + raise ValueError("reference_stream must be left_rgb or right_rgb") + if required_streams is None and default: + return STREAMS + if isinstance(required_streams, str) or not required_streams: + raise ValueError("required_streams must be an explicit nonempty collection") + values = tuple(dict.fromkeys(required_streams)) + if any(value not in STREAMS for value in values) or reference_stream not in values: + raise ValueError( + "required_streams must name known streams and include reference_stream" + ) + return values + + +def _profile_model(loaded, required, reference): + snapshots = [ + value + for value in loaded["models"] + if value.get("capture_profile", {}).get("status") == "validated" + and value.get("host_bridge") + ] + if not snapshots: + return None + profile = snapshots[-1]["capture_profile"] + if any(value["capture_profile"] != profile for value in snapshots): + raise ValueError("Conflicting embedded capture profiles") + if ( + profile.get("version") != 1 + or profile.get("scope") != "capture_profile" + or profile.get("reference_stream") != "left_rgb" + ): + raise ValueError("Invalid embedded capture profile") + sdk_digest = profile.get("identity", {}).get("sdk_binary_sha256") + if not isinstance(sdk_digest, str) or not re.fullmatch("[0-9a-f]{64}", sdk_digest): + raise ValueError("Embedded capture profile requires sdk_binary_sha256") + duration = _integer( + profile.get("max_capture_duration_ns"), "max_capture_duration_ns", 1 + ) + validated_duration = _integer( + profile.get("validation", {}).get("held_out_duration_ns"), + "held_out_duration_ns", + 900_000_000_000, + ) + if duration > validated_duration: + raise ValueError("Capture profile duration exceeds its independent validation") + for key in ("identity", "profile"): + if any(value.get(key) != profile.get(key) for value in snapshots): + raise ValueError(f"Embedded capture profile {key} mismatch") + if loaded["identity"].get("sdk_binary_sha256") != sdk_digest: + raise ValueError("Embedded capture profile SDK binary digest mismatch") + epochs = {} + for value in snapshots: + bridge = value["host_bridge"] + epoch = _integer(bridge["epoch"], "epoch", 0) + if epoch != value.get("_capture_epoch", epoch): + raise ValueError("Capture bridge epoch disagrees with its wrapper") + anchor = _integer(bridge["reference_anchor_ns"], "reference_anchor_ns") + lo = _integer(bridge.get("valid_from_ns"), "valid_from_ns") + hi = _integer(bridge.get("valid_to_ns"), "valid_to_ns") + if lo != anchor or hi != anchor + duration: + raise ValueError("Capture bridge validity must match its verified duration") + if bridge.get("status") not in ("active", "expired"): + raise ValueError("Capture bridge requires an active or expired status") + expired_at = bridge.get("expired_at_reference_ns") + if expired_at is not None: + if ( + bridge["status"] != "expired" + or _integer(expired_at, "expired_at_reference_ns") < hi + ): + raise ValueError( + "Capture bridge expiry contradicts its validity interval" + ) + first = bridge.get("first_mapped_raw_ns") + if not isinstance(first, dict): + raise ValueError("Capture bridge requires first_mapped_raw_ns boundaries") + for stream, raw in first.items(): + if stream not in profile.get("streams", {}): + raise ValueError("Capture bridge names an uncalibrated stream") + _integer(raw, "first_mapped_raw_ns") + if epoch in epochs and epochs[epoch]["anchor"] != anchor: + raise ValueError("Conflicting capture clock origins within an epoch") + if ( + epochs.get(epoch, {}).get("status") == "expired" + and bridge["status"] != "expired" + ): + raise ValueError( + "An expired capture bridge cannot become active within an epoch" + ) + previous = epochs.get(epoch, {}).get("first", {}) + if any(first.get(stream) != raw for stream, raw in previous.items()): + raise ValueError( + "Conflicting first mapped sample boundaries within an epoch" + ) + epochs[epoch] = { + "anchor": anchor, + "lo": lo, + "hi": hi, + "first": first, + "status": bridge["status"], + } + result = { + key: profile[key] + for key in ( + "version", + "status", + "model_id", + "reference_stream", + "identity", + "profile", + "validation", + ) + } + result.update(scope="recording", epochs=[], source="embedded_capture_profile") + for epoch, value in epochs.items(): + anchor = value["anchor"] + maps = {} + for stream, mapping in profile.get("streams", {}).items(): + if stream == "audio" or stream not in STREAMS: + raise ValueError("Capture profiles may only align RGB/IMU streams") + offset = _integer(mapping.get("offset_ns"), "offset_ns") + scale = _positive(mapping.get("scale"), "scale") + if scale != 1: + raise ValueError("Reusable capture profiles require unit stream scale") + if stream not in value["first"]: + continue + if not value["lo"] <= value["first"][stream] + offset < value["hi"]: + raise ValueError("First mapped sample is outside the verified interval") + maps[stream] = { + "source_anchor_ns": anchor, + "reference_anchor_ns": anchor + offset, + "scale": scale, + "valid_from_raw_ns": value["first"][stream], + "validation_start_ns": value["lo"], + "validation_end_ns": value["hi"], + } + times = [ + item["ref"].sample_time_raw_ns + for item in loaded["records"]["left_rgb"] + if item["ref"].epoch == epoch + ] + if times: + result["epochs"].append( + { + "epoch": epoch, + "valid_from_ns": value["lo"], + "valid_to_ns": value["hi"], + "streams": maps, + } + ) + return result + + +def _select_model(loaded, alignment, required, reference): + external = alignment is not None + if isinstance(alignment, (str, Path)): + alignment = json.loads(Path(alignment).read_text()) + elif alignment is not None: + alignment = json.loads(json.dumps(alignment, allow_nan=False)) + if alignment is not None and not isinstance(alignment, dict): + raise ValueError("Alignment must be a JSON object") + if alignment is None: + candidates = [ + {key: value for key, value in m.items() if not key.startswith("_")} + for m in loaded["models"] + if m.get("status") == "validated" and m.get("scope") == "recording" + ] + if candidates and any(candidate != candidates[-1] for candidate in candidates): + raise ValueError("Conflicting embedded recording alignment models") + alignment = ( + candidates[-1] + if candidates + else _profile_model(loaded, required, reference) + ) + if alignment is None and loaded["models"]: + alignment = loaded["models"][-1] + return alignment, external + + +def _validate_model(loaded, model, external, required, reference): + # The selectable window camera does not change the model's left-camera clock. + if not isinstance(model, dict) or model.get("status") != "validated": + raise ValueError("A validated recording-specific alignment model is required") + if ( + model.get("version") != 1 + or model.get("scope") != "recording" + or model.get("reference_stream") != "left_rgb" + or not model.get("model_id") + ): + raise ValueError( + "Invalid alignment version, scope, reference stream, or model_id" + ) + issues = [ + message + for message, streams in loaded["scoped_issues"] + if not streams or set(required).intersection(streams) + ] + if issues: + raise ValueError("; ".join(issues)) + identity = model.get("identity", {}) + for key in ("serial", "firmware", "sdk_version"): + if not identity.get(key) or identity[key] != loaded["identity"].get(key): + raise ValueError( + f"Alignment {key} does not match independently recorded identity" + ) + if "sdk_binary_sha256" in identity: + digest = identity["sdk_binary_sha256"] + if not isinstance(digest, str) or not re.fullmatch("[0-9a-f]{64}", digest): + raise ValueError( + "Alignment sdk_binary_sha256 must be a lowercase SHA-256 digest" + ) + if digest != loaded["identity"].get("sdk_binary_sha256"): + raise ValueError( + "Alignment SDK binary digest does not match recorded identity" + ) + if external and model.get("recording_fingerprint") != _fingerprint(loaded): + raise ValueError("Alignment recording/media fingerprint mismatch") + profile = model.get("profile", {}) + for stream, items in loaded["records"].items(): + if stream not in required: + continue + for item in items: + data = item["data"] + if stream.endswith("rgb"): + actual = { + "format": _format(data["pixel_format"]), + **{key: data[key] for key in ("width", "height", "fps")}, + } + if actual != profile.get("video"): + raise ValueError(f"Alignment video profile mismatch on {stream}") + elif stream in ("accel", "gyro") and data["sample_rate_hz"] != profile.get( + "imu_rate_hz" + ): + raise ValueError("Alignment IMU profile mismatch") + elif stream == "audio": + actual = { + "sample_rate_hz": data["sample_rate_hz"], + "channels": data["channel_count"], + "bits_per_sample": data["bits_per_sample"], + } + if actual != profile.get("audio"): + raise ValueError("Alignment audio profile mismatch") + validation = model.get("validation", {}) + error = _integer(validation.get("max_abs_error_ns"), "max_abs_error_ns", 0) + uncertainty = _integer(validation.get("uncertainty_ns"), "uncertainty_ns", 0) + _integer( + validation.get("held_out_duration_ns"), + "held_out_duration_ns", + 900_000_000_000, + ) + fps = _positive(profile.get("video", {}).get("fps"), "fps") + if error + uncertainty > 500_000_000 / fps: + raise ValueError("Independent validation exceeds the half-frame error budget") + evidence = validation.get("evidence") + if ( + not isinstance(validation.get("method"), str) + or not validation["method"].strip() + or not isinstance(evidence, list) + or not evidence + or any(not isinstance(value, str) or not value.strip() for value in evidence) + ): + raise ValueError("Independent validation method and evidence are required") + segments = model.get("epochs") + if not isinstance(segments, list) or not segments: + raise ValueError("Alignment must define epoch validity intervals") + ranges = {} + for segment in segments: + epoch = _integer(segment.get("epoch"), "epoch", 0) + lo = _integer(segment.get("valid_from_ns"), "valid_from_ns") + hi = _integer(segment.get("valid_to_ns"), "valid_to_ns") + if hi <= lo or any(lo < b and a < hi for a, b in ranges.get(epoch, [])): + raise ValueError( + "Alignment validity intervals must be nonempty and nonoverlapping within an epoch" + ) + ranges.setdefault(epoch, []).append((lo, hi)) + maps = segment.get("streams", {}) + for stream in required: + if stream not in maps: + raise ValueError(f"Alignment is missing required stream {stream}") + for stream, mapping in maps.items(): + if stream not in STREAMS: + raise ValueError(f"Unknown alignment stream {stream}") + if stream == "audio": + audio_ranges = {} + for audio in mapping.get("segments", []): + sid = _integer(audio.get("segment_id"), "segment_id", 0) + first = _integer( + audio.get("first_sample_index"), "first_sample_index", 0 + ) + count = _integer(audio.get("sample_count"), "sample_count", 1) + _integer(audio.get("reference_anchor_ns"), "reference_anchor_ns") + _positive(audio.get("period_ns"), "period_ns") + if stream in required: + validation_lo = _integer( + audio.get("validation_start_ns"), "validation_start_ns" + ) + validation_hi = _integer( + audio.get("validation_end_ns"), "validation_end_ns" + ) + if validation_hi <= validation_lo: + raise ValueError( + "Audio independent validation bounds are empty" + ) + if type(audio.get("continuity_verified", False)) is not bool: + raise ValueError( + "continuity_verified must be a boolean backed by evidence" + ) + if any( + first < b and a < first + count + for a, b in audio_ranges.get(sid, []) + ): + raise ValueError("Overlapping audio sample ranges") + audio_ranges.setdefault(sid, []).append((first, first + count)) + if stream in required and not audio_ranges: + raise ValueError("Required audio alignment has no sample segments") + else: + _integer(mapping.get("source_anchor_ns"), "source_anchor_ns") + _integer(mapping.get("reference_anchor_ns"), "reference_anchor_ns") + _positive(mapping.get("scale"), "scale") + if stream in required and stream != model["reference_stream"]: + validation_lo = _integer( + mapping.get("validation_start_ns"), "validation_start_ns" + ) + validation_hi = _integer( + mapping.get("validation_end_ns"), "validation_end_ns" + ) + if not validation_lo <= lo < hi <= validation_hi: + raise ValueError( + "Alignment interval exceeds independent validation bounds" + ) + if "valid_from_raw_ns" in mapping: + _integer(mapping["valid_from_raw_ns"], "valid_from_raw_ns") + if stream == model["reference_stream"] and ( + mapping["source_anchor_ns"] != mapping["reference_anchor_ns"] + or mapping["scale"] != 1 + ): + raise ValueError( + "Reference stream must preserve its raw sample clock" + ) + return model + + +def _map_time(raw, mapping): + # Subtract integer anchors before scaling; epoch-sized floats lose sample precision. + return mapping["reference_anchor_ns"] + round( + (raw - mapping["source_anchor_ns"]) * mapping["scale"] + ) + + +def _mapped(loaded, model, required): + mapped = {stream: [] for stream in STREAMS} + occupied = {stream: {} for stream in STREAMS} + for segment_index, segment in enumerate(model["epochs"]): + for stream, mapping in segment["streams"].items(): + if stream == "audio" or stream not in required: + continue + for item in loaded["records"][stream]: + data, ref = item["data"], item["ref"] + if ref.epoch != segment["epoch"]: + continue + previous = occupied[stream].setdefault( + (ref.epoch, ref.record_index), [] + ) + first_index = None + last_index = None + samples = data["samples"] if stream in ("accel", "gyro") else [None] + for index, sample in enumerate(samples): + raw = ( + int(sample["sample_time_raw_device_clock_ns"]) + if sample is not None + else ref.sample_time_raw_ns + ) + if raw < mapping.get("valid_from_raw_ns", raw): + continue + time = _map_time(raw, mapping) + if not segment["valid_from_ns"] <= time < segment["valid_to_ns"]: + continue + if any(lo <= index < hi for lo, hi in previous): + raise ValueError( + "A source sample maps into more than one model interval" + ) + first_index = index if first_index is None else first_index + last_index = index + if sample is not None: + value = ImuSample( + stream, + time, + ref, + index, + raw, + int(sample["sample_time_local_common_clock_ns"]), + float(sample["x_si"]), + float(sample["y_si"]), + float(sample["z_si"]), + float(sample["temperature_c"]), + ) + else: + value = VideoFrame( + stream, + time, + ref, + item["media"], + data["width"], + data["height"], + data["fps"], + _format(data["pixel_format"]), + tuple( + (entry["key"], entry["value"]) + for entry in data.get("sdk_metadata", []) + ), + ) + mapped[stream].append((segment_index, value)) + if first_index is not None: + previous.append((first_index, last_index + 1)) + return mapped + + +def _audio_index(loaded, segment): + spans = [] + for item in loaded["records"]["audio"]: + ref, data, timing = item["ref"], item["data"], item["timing"] + if ref.epoch != segment["epoch"]: + continue + for mapping in segment["streams"].get("audio", {}).get("segments", []): + if timing["segment_id"] != mapping["segment_id"]: + continue + first, anchor = ( + mapping["first_sample_index"], + mapping["reference_anchor_ns"], + ) + period = Fraction(str(mapping["period_ns"])) + begin = max(timing["first_sample_index"], first) + end = min( + timing["first_sample_index"] + data["sample_count"], + first + mapping["sample_count"], + ) + # A retained segment can extend beyond its independently measured + # interval. Its tail has no trusted timing merely because PCM exists. + begin = max( + begin, + first + + math.ceil(Fraction(mapping["validation_start_ns"] - anchor) / period), + ) + end = min( + end, + first + + math.ceil(Fraction(mapping["validation_end_ns"] - anchor) / period), + ) + if end > begin: + spans.append( + ( + anchor + (begin - first) * period, + anchor + (end - first) * period, + item, + mapping, + period, + begin, + end, + ) + ) + spans.sort(key=lambda value: value[0]) + if any(b[0] < a[1] for a, b in zip(spans, spans[1:])): + raise ValueError("Audio chunks overlap within a model interval") + return spans, [value[0] for value in spans], [value[1] for value in spans] + + +def _audio_slices(index, lo, hi): + spans, starts, ends = index + result = [] + # Index once per model interval; a long recording must not rescan all PCM + # chunks for every frame window. + for _, _, item, mapping, period, lower, upper in spans[ + bisect_right(ends, lo) : bisect_left(starts, hi) + ]: + ref, data, timing = item["ref"], item["data"], item["timing"] + first, anchor = mapping["first_sample_index"], mapping["reference_anchor_ns"] + begin = max(lower, first + math.ceil(Fraction(lo - anchor) / period)) + end = min(upper, first + math.ceil(Fraction(hi - anchor) / period)) + if end <= begin: + continue + offset = begin - timing["first_sample_index"] + frame_bytes = data["channel_count"] * data["bits_per_sample"] // 8 + media = item["media"] + if media: + media = replace( + media, + byte_offset=media.byte_offset + offset * frame_bytes, + byte_count=(end - begin) * frame_bytes, + ) + result.append( + AudioSlice( + ref, + media, + timing["segment_id"], + begin, + offset, + end - begin, + anchor + round((begin - first) * period), + anchor + round((end - first) * period), + float(period), + data["sample_rate_hz"], + data["channel_count"], + data["bits_per_sample"], + bool( + timing["continuity_known"] + or mapping.get("continuity_verified", False) + ), + ) + ) + return tuple(sorted(result, key=lambda value: value.start_time_ns)) + + +def _windows(loaded, model, required, reference, epoch, start, end): + mapped = _mapped(loaded, model, required) + used_video = set() + used_audio = {} + for segment_index, segment in enumerate(model["epochs"]): + if epoch is not None and epoch != segment["epoch"]: + continue + audio_index = ( + _audio_index(loaded, segment) if "audio" in required else ([], [], []) + ) + values = { + stream: sorted( + (value for index, value in mapped[stream] if index == segment_index), + key=lambda value: value.time_ns, + ) + for stream in STREAMS + if stream != "audio" + } + times = { + stream: [value.time_ns for value in items] + for stream, items in values.items() + } + refs = values[reference] + for current, following in zip(refs, refs[1:]): + lo, hi = current.time_ns, following.time_ns + reasons = [] + if hi <= lo: + continue + period = 1_000_000_000 / current.fps + gap = ( + following.record.sequence_number != current.record.sequence_number + 1 + or hi - lo > 1.5 * period + ) + if gap: + hi = min(hi, lo + round(period)) + reasons.append("reference_gap") + if (start is not None and lo < start) or (end is not None and hi > end): + continue + frames = {"left_rgb": None, "right_rgb": None, reference: current} + other = "right_rgb" if reference == "left_rgb" else "left_rgb" + # SDK eye sequence counters can start one cycle apart; corrected + # sample times define stereo correspondence, not equal sequence IDs. + position = bisect_left(times[other], lo) + candidates = [ + values[other][index] + for index in (position - 1, position) + if 0 <= index < len(values[other]) + ] + candidates = [ + value + for value in candidates + if abs(value.time_ns - lo) <= 2_000_000 + and (other, value.record.epoch, value.record.record_index) + not in used_video + ] + if candidates: + pair = min(candidates, key=lambda value: abs(value.time_ns - lo)) + frames[other] = pair + used_video.add((other, pair.record.epoch, pair.record.record_index)) + samples = {} + for stream in ("accel", "gyro"): + a, b = bisect_left(times[stream], lo), bisect_left(times[stream], hi) + samples[stream] = tuple(values[stream][a:b]) + if stream in required and samples[stream]: + nominal = 1_000_000_000 / model["profile"]["imu_rate_hz"] + sample_times = [value.time_ns for value in samples[stream]] + if ( + sample_times[0] - lo > 1.5 * nominal + or hi - sample_times[-1] > 1.5 * nominal + or any( + not 0 < y - x <= 1.5 * nominal + for x, y in zip(sample_times, sample_times[1:]) + ) + ): + reasons.append(f"{stream}_gap") + audio = _audio_slices(audio_index, lo, hi) + for value in audio: + key = value.record.epoch, value.record.record_index + previous = used_audio.setdefault(key, []) + begin, stop = ( + value.sample_offset, + value.sample_offset + value.sample_count, + ) + if any(begin < b and a < stop for a, b in previous): + raise ValueError("An audio sample maps into more than one window") + previous.append((begin, stop)) + for stream in required: + present = ( + frames.get(stream) + if stream.endswith("rgb") + else (audio if stream == "audio" else samples[stream]) + ) + if not present: + reasons.append(f"missing_{stream}") + if stream.endswith("rgb") and present and present.media is None: + reasons.append(f"missing_{stream}_media") + if "audio" in required and audio: + if any(value.media is None for value in audio): + reasons.append("missing_audio_media") + if not all(value.continuity_verified for value in audio): + reasons.append("audio_continuity_unverified") + if len({value.segment_id for value in audio}) > 1: + reasons.append("audio_segment_boundary") + if any( + not any( + mapping["segment_id"] == value.segment_id + and mapping["first_sample_index"] <= value.first_sample_index + and value.first_sample_index + value.sample_count + <= mapping["first_sample_index"] + mapping["sample_count"] + and mapping["validation_start_ns"] <= lo + and hi <= mapping["validation_end_ns"] + for mapping in segment["streams"]["audio"]["segments"] + ) + for value in audio + ): + reasons.append("audio_validation_boundary") + tolerance = max(value.period_ns for value in audio) + if ( + audio[0].start_time_ns - lo > tolerance + or hi - audio[-1].end_time_ns > tolerance + or any( + abs(b.start_time_ns - a.end_time_ns) > tolerance + for a, b in zip(audio, audio[1:]) + ) + ): + reasons.append("audio_gap") + yield SyncWindow( + segment["epoch"], + segment.get("model_id", model["model_id"]), + segment_index, + lo, + hi, + reference, + required, + frames["left_rgb"], + frames["right_rgb"], + samples["accel"], + samples["gyro"], + audio, + not reasons, + tuple(reasons), + ) + + +def _arguments( + required_streams, reference_stream, epoch, start_time_ns, end_time_ns, default=False +): + required = _required(required_streams, reference_stream, default) + if epoch is not None: + _integer(epoch, "epoch", 0) + for name, value in (("start_time_ns", start_time_ns), ("end_time_ns", end_time_ns)): + if value is not None: + _integer(value, name) + if ( + start_time_ns is not None + and end_time_ns is not None + and end_time_ns <= start_time_ns + ): + raise ValueError("end_time_ns must exceed start_time_ns") + return required + + +def iter_sync_windows( + recording: str | Path, + *, + required_streams: Iterable[str], + alignment: dict[str, Any] | str | Path | None = None, + reference_stream: str = "left_rgb", + epoch: int | None = None, + start_time_ns: int | None = None, + end_time_ns: int | None = None, +) -> Iterator[SyncWindow]: + """Yield immutable half-open reference-frame windows from a validated model. + + Full windows only are returned within a requested range. Epoch/model tails + and samples outside those windows remain visible in analyze_recording_sync. + """ + required = _arguments( + required_streams, reference_stream, epoch, start_time_ns, end_time_ns + ) + loaded = _load(recording) + for stream in required: + if not any( + epoch is None or item["ref"].epoch == epoch + for item in loaded["records"][stream] + ): + raise ValueError(f"Required stream is absent: {stream}") + model, external = _select_model(loaded, alignment, required, reference_stream) + _validate_model(loaded, model, external, required, reference_stream) + yield from _windows( + loaded, model, required, reference_stream, epoch, start_time_ns, end_time_ns + ) + + +def _sample_spans(samples): + current = None + for sample in samples: + if ( + current is not None + and current[0] == sample.record + and current[1] + current[2] == sample.sample_index + ): + current[2] += 1 + current[4] = sample.time_ns + else: + if current is not None: + yield current + current = [ + sample.record, + sample.sample_index, + 1, + sample.time_ns, + sample.time_ns, + ] + if current is not None: + yield current + + +def _sample_ranges(samples): + return [ + { + "record": asdict(ref), + "first_sample_index": first, + "sample_count": count, + "first_time_ns": lo, + "last_time_ns": hi, + } + for ref, first, count, lo, hi in _sample_spans(samples) + ] + + +def _window_index(window): + # Reports carry batch/sample ranges; copying every XYZ value into JSON makes + # a 30-minute audit several gigabytes. Full values stay in the iterator API. + result = { + key: getattr(window, key) + for key in ( + "epoch", + "model_id", + "model_segment", + "start_time_ns", + "end_time_ns", + "reference_stream", + "required_streams", + "complete", + "reasons", + ) + } + for stream in ("left_rgb", "right_rgb"): + frame = getattr(window, stream) + result[stream] = asdict(frame) if frame else None + result["accel"] = _sample_ranges(window.accel) + result["gyro"] = _sample_ranges(window.gyro) + result["audio"] = [asdict(value) for value in window.audio] + return result + + +def _merge_ranges(ranges): + result = [] + for lo, hi in sorted(ranges): + if result and lo <= result[-1][1]: + result[-1] = (result[-1][0], max(hi, result[-1][1])) + else: + result.append((lo, hi)) + return result + + +def _unassigned(loaded, assigned, assigned_audio, epoch): + result = {stream: [] for stream in ("accel", "gyro", "audio")} + for stream in result: + for item in loaded["records"][stream]: + ref, data = item["ref"], item["data"] + if epoch is not None and ref.epoch != epoch: + continue + if stream == "audio": + count = data["sample_count"] + ranges = _merge_ranges( + assigned_audio.get((ref.epoch, ref.record_index), []) + ) + else: + count = len(data["samples"]) + ranges = _merge_ranges( + assigned[stream].get((ref.epoch, ref.record_index), []) + ) + previous = 0 + for lo, hi in [*ranges, (count, count)]: + if lo > previous: + result[stream].append( + { + "record": asdict(ref), + "first_sample_index": previous, + "sample_count": lo - previous, + } + ) + previous = max(previous, hi) + return result + + +def analyze_recording_sync( + recording: str | Path, + *, + required_streams: Iterable[str] | None = None, + alignment: dict[str, Any] | str | Path | None = None, + reference_stream: str = "left_rgb", + epoch: int | None = None, + start_time_ns: int | None = None, + end_time_ns: int | None = None, +) -> dict[str, Any]: + """Audit all retained records; associations require independently validated timing.""" + required = _arguments( + required_streams, reference_stream, epoch, start_time_ns, end_time_ns, True + ) + loaded = _load(recording) + inventory = {} + for stream, items in loaded["records"].items(): + selected = [ + item for item in items if epoch is None or item["ref"].epoch == epoch + ] + samples = sum( + len(item["data"]["samples"]) + if stream in ("accel", "gyro") + else item["data"]["sample_count"] + if stream == "audio" + else 1 + for item in selected + ) + epochs = sorted({item["ref"].epoch for item in selected}) + sequence_gaps = [] + for a, b in zip(selected, selected[1:]): + if ( + a["ref"].epoch == b["ref"].epoch + and b["ref"].sequence_number != a["ref"].sequence_number + 1 + ): + sequence_gaps.append( + { + "epoch": a["ref"].epoch, + "before": a["ref"].sequence_number, + "after": b["ref"].sequence_number, + } + ) + inventory[stream] = { + "available": bool(selected), + "records": len(selected), + "samples": samples, + "epochs": epochs, + "sequence_gaps": sequence_gaps, + "missing_media_records": sum(item["media"] is None for item in selected) + if stream in ("left_rgb", "right_rgb", "audio") + else 0, + } + model = None + errors = [] + windows = [] + complete_windows = 0 + assigned = {stream: set() for stream in ("left_rgb", "right_rgb")} + assigned.update({stream: {} for stream in ("accel", "gyro")}) + assigned_audio = {} + status = "unverified" + try: + model, external = _select_model(loaded, alignment, required, reference_stream) + if model and model.get("status") == "validated": + _validate_model(loaded, model, external, required, reference_stream) + missing = [ + stream for stream in required if not inventory[stream]["available"] + ] + if missing: + raise ValueError(f"Required streams are absent: {', '.join(missing)}") + for window in _windows( + loaded, + model, + required, + reference_stream, + epoch, + start_time_ns, + end_time_ns, + ): + windows.append(_window_index(window)) + complete_windows += window.complete + for stream in ("left_rgb", "right_rgb"): + frame = getattr(window, stream) + if frame: + assigned[stream].add( + (frame.record.epoch, frame.record.record_index) + ) + for stream in ("accel", "gyro"): + for ref, begin, count, _, _ in _sample_spans( + getattr(window, stream) + ): + assigned[stream].setdefault( + (ref.epoch, ref.record_index), [] + ).append((begin, begin + count)) + for value in window.audio: + key = value.record.epoch, value.record.record_index + assigned_audio.setdefault(key, []).append( + (value.sample_offset, value.sample_offset + value.sample_count) + ) + status = "validated" + except (ValueError, TypeError, KeyError) as error: + status = "invalid" + errors.append(str(error)) + windows.clear() + complete_windows = 0 + for values in assigned.values(): + values.clear() + assigned_audio.clear() + audio_count = sum( + hi - lo + for ranges in assigned_audio.values() + for lo, hi in _merge_ranges(ranges) + ) + assigned_counts = { + stream: len(assigned[stream]) for stream in ("left_rgb", "right_rgb") + } + assigned_counts.update( + { + stream: sum( + hi - lo + for ranges in assigned[stream].values() + for lo, hi in _merge_ranges(ranges) + ) + for stream in ("accel", "gyro") + } + ) + assigned_counts["audio"] = audio_count + coverage = { + stream: { + "assigned_samples": assigned_counts[stream], + "unassigned_samples": inventory[stream]["samples"] + - assigned_counts[stream], + } + for stream in STREAMS + } + unpaired = { + stream: [ + asdict(item["ref"]) + for item in loaded["records"][stream] + if (epoch is None or item["ref"].epoch == epoch) + and (item["ref"].epoch, item["ref"].record_index) not in assigned[stream] + ] + for stream in ("left_rgb", "right_rgb") + } + return { + "version": 1, + "recording": str(loaded["path"]), + "required_streams": list(required), + "reference_stream": reference_stream, + "streams": inventory, + "alignment": { + "status": status, + "model_id": model.get("model_id") if isinstance(model, dict) else None, + "errors": errors, + "declared_validation": model.get("validation") + if isinstance(model, dict) + else None, + }, + "windows": windows, + "association_format": "record_and_sample_ranges_v1", + "five_stream_verified": status == "validated" + and set(required) == set(STREAMS) + and bool(windows) + and complete_windows == len(windows), + "verification_scope": "returned_windows", + "coverage": { + "window_count": len(windows), + "complete_windows": complete_windows, + "incomplete_windows": len(windows) - complete_windows, + "streams": coverage, + "unpaired_video": unpaired, + "unassigned_sample_ranges": _unassigned( + loaded, assigned, assigned_audio, epoch + ), + }, + "issues": loaded["issues"], + "ignored_noncanonical_records": loaded["ignored_records"], + "physical_error_measurements": None, + } diff --git a/src/python/isaaccapture/schema/__init__.py b/src/python/isaaccapture/schema/__init__.py index c7f3ac934d..654a2a1935 100644 --- a/src/python/isaaccapture/schema/__init__.py +++ b/src/python/isaaccapture/schema/__init__.py @@ -85,6 +85,39 @@ StreamType, FrameMetadataOak, FrameMetadataOakRecord, + # EGO payloads and MCAP records. + EgoCameraStream, + EgoPixelFormat, + EgoFrameMetadataEntry, + EgoFrameMetadata, + EgoFrameMetadataRecord, + EgoEncodedVideoFrame, + EgoEncodedVideoFrameRecord, + EgoImuSensor, + EgoImuSample, + EgoImuBatch, + EgoImuBatchRecord, + EgoAudioSampleFormat, + EgoAudioTiming, + EgoAudioChunk, + EgoAudioChunkRecord, + EgoPcmAudioChunk, + EgoPcmAudioChunkRecord, + EgoCameraIntrinsics, + EgoExtrinsics, + EgoTimeAlignment, + EgoCalibration, + EgoCalibrationRecord, + EgoCaptureHealth, + EgoRecoveryStage, + EgoDevicePropertyValue, + EgoDiagnosticEvent, + EgoSdkReport, + EgoCaptureSummary, + EgoRecoveryEvent, + EgoDeviceSnapshot, + EgoDeviceState, + EgoDeviceStateRecord, # Full body-related types. BodyJoint, BodyJointPose, @@ -217,6 +250,39 @@ def __getattr__(name: str): "StreamType", "FrameMetadataOak", "FrameMetadataOakRecord", + # EGO payloads and MCAP records. + "EgoCameraStream", + "EgoPixelFormat", + "EgoFrameMetadataEntry", + "EgoFrameMetadata", + "EgoFrameMetadataRecord", + "EgoEncodedVideoFrame", + "EgoEncodedVideoFrameRecord", + "EgoImuSensor", + "EgoImuSample", + "EgoImuBatch", + "EgoImuBatchRecord", + "EgoAudioSampleFormat", + "EgoAudioTiming", + "EgoAudioChunk", + "EgoAudioChunkRecord", + "EgoPcmAudioChunk", + "EgoPcmAudioChunkRecord", + "EgoCameraIntrinsics", + "EgoExtrinsics", + "EgoTimeAlignment", + "EgoCalibration", + "EgoCalibrationRecord", + "EgoCaptureHealth", + "EgoRecoveryStage", + "EgoDevicePropertyValue", + "EgoDiagnosticEvent", + "EgoSdkReport", + "EgoCaptureSummary", + "EgoRecoveryEvent", + "EgoDeviceSnapshot", + "EgoDeviceState", + "EgoDeviceStateRecord", # Full body types. "BodyJointPose", "BodyJoint", diff --git a/src/python/isaaccapture/teleop_session_manager/config.py b/src/python/isaaccapture/teleop_session_manager/config.py index 8307a43af9..4a25cc5042 100644 --- a/src/python/isaaccapture/teleop_session_manager/config.py +++ b/src/python/isaaccapture/teleop_session_manager/config.py @@ -299,6 +299,7 @@ class PluginConfig: enabled: Whether to load and use this plugin plugin_args: Optional list of arguments passed to the plugin process required: Whether session startup should fail if the plugin cannot be found + shutdown_timeout_seconds: Grace period before forcing the process to stop """ plugin_name: str @@ -307,6 +308,14 @@ class PluginConfig: enabled: bool = True plugin_args: List[str] = field(default_factory=list) required: bool = False + shutdown_timeout_seconds: float = 2.0 + + def __post_init__(self) -> None: + if ( + not math.isfinite(self.shutdown_timeout_seconds) + or self.shutdown_timeout_seconds <= 0.0 + ): + raise ValueError("shutdown_timeout_seconds must be finite and positive") @dataclass diff --git a/src/python/isaaccapture/teleop_session_manager/teleop_session.py b/src/python/isaaccapture/teleop_session_manager/teleop_session.py index f9c019ac7d..82e36b23fd 100644 --- a/src/python/isaaccapture/teleop_session_manager/teleop_session.py +++ b/src/python/isaaccapture/teleop_session_manager/teleop_session.py @@ -62,7 +62,9 @@ logger = logging.getLogger(__name__) # Plugins that publish device status (see PluginDeviceStatusPublisher); others launch unmonitored. -_MONITORED_PLUGIN_NAMES = frozenset({"manus_hand_plugin", "spacemouse", "gamepad"}) +_MONITORED_PLUGIN_NAMES = frozenset( + {"manus_hand_plugin", "spacemouse", "gamepad", "ego_camera"} +) def _resolve_sink(entry: GraphExecutable) -> tuple[GraphExecutable, IDeviceIOSink]: @@ -1306,7 +1308,7 @@ def _destroy_twin(self) -> None: self._twin_teardown_clean = stopped and joined def _resolve_monitored_plugins(self) -> None: - """Resolve the initially supported Manus inventory and status reader.""" + """Resolve supported plugin inventories and their official status readers.""" for plugin_config in self.config.plugins: if ( not plugin_config.enabled @@ -1391,6 +1393,7 @@ def _start_configured_plugins(self, stack: ExitStack) -> None: plugin_config.plugin_name, plugin_config.plugin_root_id, plugin_config.plugin_args, + plugin_config.shutdown_timeout_seconds, ) stack.enter_context(context) self.plugin_contexts.append(context) diff --git a/tests/cpp/core/live_trackers/CMakeLists.txt b/tests/cpp/core/live_trackers/CMakeLists.txt index e9094c08e9..3bdf5a29e3 100644 --- a/tests/cpp/core/live_trackers/CMakeLists.txt +++ b/tests/cpp/core/live_trackers/CMakeLists.txt @@ -5,6 +5,7 @@ cmake_minimum_required(VERSION 3.20) add_executable(live_trackers_tests test_vendor_validation.cpp + test_ego_dispatch.cpp test_keyboard_session.cpp ) diff --git a/tests/cpp/core/live_trackers/test_ego_dispatch.cpp b/tests/cpp/core/live_trackers/test_ego_dispatch.cpp new file mode 100644 index 0000000000..7574e533bc --- /dev/null +++ b/tests/cpp/core/live_trackers/test_ego_dispatch.cpp @@ -0,0 +1,52 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +TEST_CASE("EGO live dispatch resolves only tensor and time extensions", "[unit][live_trackers][ego]") +{ + std::vector> trackers = { + std::make_shared( + "ego_metadata", + std::vector{ core::EgoCameraStream_ColorLeft, core::EgoCameraStream_ColorRight }), + std::make_shared("ego_imu"), + std::make_shared("ego_audio/Audio"), + std::make_shared("ego_calibration/Calibration"), + std::make_shared("ego_device/DeviceState"), + }; + const auto expected = core::SchemaTrackerBase::get_required_extensions(); + const std::set expected_set(expected.begin(), expected.end()); + for (const auto& tracker : trackers) + { + const auto extensions = core::DeviceIOSession::get_required_extensions({ tracker }); + CHECK(std::set(extensions.begin(), extensions.end()) == expected_set); + } + const auto extensions = core::DeviceIOSession::get_required_extensions(trackers); + CHECK(std::set(extensions.begin(), extensions.end()) == expected_set); +} + +TEST_CASE("EGO multi-stream trackers reject ambiguous collection routing", "[unit][live_trackers][ego]") +{ + using Camera = core::EgoFrameMetadataTracker; + using Imu = core::EgoImuTracker; + CHECK_THROWS_AS(Camera("", { core::EgoCameraStream_ColorLeft }), std::invalid_argument); + CHECK_THROWS_AS(Camera("camera", {}), std::invalid_argument); + CHECK_THROWS_AS( + Camera("camera", { core::EgoCameraStream_ColorLeft, core::EgoCameraStream_ColorLeft }), std::invalid_argument); + CHECK_THROWS_AS(Camera("camera", { static_cast(99) }), std::invalid_argument); + CHECK_THROWS_AS(Imu("imu", { core::EgoImuSensor_Accel, core::EgoImuSensor_Accel }), std::invalid_argument); + CHECK_THROWS_AS(Imu("imu", { static_cast(99) }), std::invalid_argument); +} diff --git a/tests/cpp/core/plugin_manager/test_plugin_manager.cpp b/tests/cpp/core/plugin_manager/test_plugin_manager.cpp index 1bb732daa2..7ddf810539 100644 --- a/tests/cpp/core/plugin_manager/test_plugin_manager.cpp +++ b/tests/cpp/core/plugin_manager/test_plugin_manager.cpp @@ -7,6 +7,7 @@ #include #include #include +#include #include #include #include @@ -89,3 +90,54 @@ TEST_CASE("plugin manager returns complete descriptors", "[plugin_manager][metad REQUIRE((manager.query_devices("sample_plugin") == std::vector{ "/hands/left", "/camera" })); REQUIRE_THROWS_AS(manager.get_plugin_info("missing"), std::runtime_error); } + +#ifndef _WIN32 +TEST_CASE("plugin shutdown grace periods apply to each instance", "[plugin_manager][process]") +{ + TemporaryDirectory search_directory; + const auto plugin_directory = search_directory.path() / "sample"; + std::filesystem::create_directory(plugin_directory); + std::ofstream metadata(plugin_directory / "plugin.yaml"); + metadata << "name: sample_plugin\ncommand: " << PLUGIN_MANAGER_TEST_PROCESS << '\n'; + metadata.close(); + core::PluginManager manager({ search_directory.path().string() }); + const auto marker = plugin_directory / "finalized"; + + SECTION("the default still forces cleanup exceeding two seconds to stop") + { + auto plugin = manager.start("sample_plugin", "root", { "finalize", "2500", marker.string() }); + plugin->stop(); + REQUIRE_FALSE(std::filesystem::exists(marker)); + REQUIRE(plugin->get_process_snapshot().state == core::ProcessState::STOPPED); + REQUIRE_FALSE(plugin->get_process_snapshot().term_signal.has_value()); + } + + SECTION("an explicit longer grace period allows cleanup to finish") + { + auto plugin = manager.start("sample_plugin", "root", { "finalize", "2500", marker.string() }, 4.0); + plugin->stop(); + REQUIRE(std::filesystem::exists(marker)); + REQUIRE(plugin->get_process_snapshot().state == core::ProcessState::STOPPED); + } + + SECTION("a shorter grace period does not change the next instance's default") + { + auto fast = manager.start("sample_plugin", "root", { "finalize", "350", marker.string() }, 0.1); + fast->stop(); + REQUIRE_FALSE(std::filesystem::exists(marker)); + auto normal = manager.start("sample_plugin", "root", { "finalize", "350", marker.string() }); + normal->stop(); + REQUIRE(std::filesystem::exists(marker)); + } +} + +TEST_CASE("invalid plugin shutdown grace periods fail before launching", "[plugin_manager][process]") +{ + for (const double timeout : + { 0.0, -1.0, std::numeric_limits::quiet_NaN(), std::numeric_limits::infinity() }) + { + CAPTURE(timeout); + REQUIRE_THROWS_AS(core::Plugin("missing-plugin", "", "root", {}, timeout), std::invalid_argument); + } +} +#endif diff --git a/tests/cpp/core/plugin_manager/test_process/test_process.cpp b/tests/cpp/core/plugin_manager/test_process/test_process.cpp index c553f139b6..f99eb66f46 100644 --- a/tests/cpp/core/plugin_manager/test_process/test_process.cpp +++ b/tests/cpp/core/plugin_manager/test_process/test_process.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include #include @@ -43,13 +44,25 @@ int main(int argc, char** argv) return 2; } - if (arguments[0] == "wait") + if (arguments[0] == "wait" || arguments[0] == "finalize") { std::signal(SIGINT, request_stop); while (!stop_requested) { std::this_thread::sleep_for(std::chrono::milliseconds(10)); } + if (arguments[0] == "finalize") + { + if (arguments.size() != 3) + { + return 2; + } + std::this_thread::sleep_for(std::chrono::milliseconds(std::stoi(arguments[1]))); + std::ofstream marker(arguments[2]); + marker << "finalized\n"; + marker.close(); + return marker.fail() ? 3 : 0; + } return 0; } diff --git a/tests/cpp/core/replay_deviceio_session/CMakeLists.txt b/tests/cpp/core/replay_deviceio_session/CMakeLists.txt index 85e8a727bd..e449c8901a 100644 --- a/tests/cpp/core/replay_deviceio_session/CMakeLists.txt +++ b/tests/cpp/core/replay_deviceio_session/CMakeLists.txt @@ -5,6 +5,7 @@ cmake_minimum_required(VERSION 3.20) add_executable(replay_deviceio_session_tests test_replay_session.cpp + test_ego_replay.cpp ) target_link_libraries(replay_deviceio_session_tests PRIVATE diff --git a/tests/cpp/core/replay_deviceio_session/test_ego_replay.cpp b/tests/cpp/core/replay_deviceio_session/test_ego_replay.cpp new file mode 100644 index 0000000000..d7a40ab0b0 --- /dev/null +++ b/tests/cpp/core/replay_deviceio_session/test_ego_replay.cpp @@ -0,0 +1,246 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace +{ + +struct Recording +{ + std::string path; + Recording() + { + static std::atomic counter{ 0 }; + const auto time = std::chrono::steady_clock::now().time_since_epoch().count(); + path = (std::filesystem::temp_directory_path() / + ("ego_replay_" + std::to_string(time) + "_" + std::to_string(counter++) + ".mcap")) + .string(); + } + ~Recording() + { + std::error_code error; + std::filesystem::remove(path, error); + } +}; + +std::unique_ptr open_reader(const std::string& path) +{ + auto reader = std::make_unique(); + REQUIRE(reader->open(path).ok()); + return reader; +} + +std::vector channels(auto values) +{ + return { values.begin(), values.end() }; +} + +} // namespace + +TEST_CASE("EGO replay reads all seven raw streams and retains snapshots", "[unit][replay][ego]") +{ + bool include_tracked = false; + SECTION("legacy recording has only raw topics") + { + } + SECTION("new recording also has tracked auxiliary topics") + { + include_tracked = true; + } + + Recording recording; + { + mcap::McapWriter writer; + mcap::McapWriterOptions options("ego-test"); + options.compression = mcap::Compression::None; + REQUIRE(writer.open(recording.path, options).ok()); + core::McapTrackerChannels camera( + writer, "ego_metadata", { "ColorLeft", "ColorRight" }); + core::McapTrackerChannels imu(writer, "ego_imu", { "Accel", "Gyro" }); + core::McapTrackerChannels audio( + writer, "ego_audio", + include_tracked ? channels(core::EgoAudioRecordingTraits::recording_channels) : + std::vector{ "Audio" }); + core::McapTrackerChannels calibration( + writer, "ego_calibration", + include_tracked ? channels(core::EgoCalibrationRecordingTraits::recording_channels) : + std::vector{ "Calibration" }); + core::McapTrackerChannels state( + writer, "ego_device", + include_tracked ? channels(core::EgoDeviceStateRecordingTraits::recording_channels) : + std::vector{ "DeviceState" }); + core::McapTrackerChannels media(writer, "ego_media", { "ColorLeft" }); + for (uint64_t sequence = 1; sequence <= 3; ++sequence) + { + const core::DeviceDataTimestamp timestamp(sequence * 100, sequence * 90, sequence * 80); + const bool inactive = sequence == 2; + core::EgoFrameMetadataT frame; + frame.sequence_number = sequence; + frame.capture_epoch = 3; + frame.stream = core::EgoCameraStream_ColorLeft; + camera.write(0, core::pack_record(inactive ? nullptr : &frame, timestamp)); + frame.stream = core::EgoCameraStream_ColorRight; + camera.write(1, core::pack_record(inactive ? nullptr : &frame, timestamp)); + core::EgoImuBatchT batch; + batch.sequence_number = sequence; + batch.sensor = core::EgoImuSensor_Accel; + batch.samples.emplace_back(1, 2, 3, 24, 90, 80); + imu.write(0, core::pack_record(inactive ? nullptr : &batch, timestamp)); + batch.sensor = core::EgoImuSensor_Gyro; + imu.write(1, core::pack_record(inactive ? nullptr : &batch, timestamp)); + core::EgoAudioChunkT chunk; + chunk.sequence_number = sequence; + chunk.wav_data_offset = 44; + const auto audio_record = + core::pack_record(inactive ? nullptr : &chunk, timestamp); + audio.write(0, audio_record); + core::EgoCalibrationT calib; + calib.device_uid = "ego_" + std::to_string(sequence); + const auto calibration_record = + core::pack_record(inactive ? nullptr : &calib, timestamp); + calibration.write(0, calibration_record); + core::EgoDeviceStateT device; + device.device_uid = "ego"; + device.capture_epoch = 3; + device.observed_at_ns = sequence * 90; + device.diagnostic_version = 2; + core::EgoSdkReportT report; + report.sequence_number = sequence; + device.event.Set(report); + const auto state_record = + core::pack_record(inactive ? nullptr : &device, timestamp); + state.write(0, state_record); + if (include_tracked) + { + audio.write(1, audio_record); + calibration.write(1, calibration_record); + state.write(1, state_record); + } + core::EgoEncodedVideoFrameT video; + video.encoded_data = { 0, 0, 0, 1, 0x65 }; + media.write(0, core::pack_record(&video, timestamp)); + } + writer.close(); + } + + core::EgoFrameMetadataTracker camera( + "ego_metadata", { core::EgoCameraStream_ColorLeft, core::EgoCameraStream_ColorRight }); + core::EgoImuTracker imu("ego_imu"); + core::EgoAudioTracker audio("ego_audio/Audio"); + core::EgoCalibrationTracker calibration("ego_calibration/Calibration"); + core::EgoDeviceStateTracker state("ego_device/DeviceState"); + auto session = core::ReplaySession::run({ recording.path, + { + { &camera, "ego_metadata" }, + { &imu, "ego_imu" }, + { &audio, "ego_audio" }, + { &calibration, "ego_calibration" }, + { &state, "ego_device" }, + } }); + CHECK_FALSE(camera.get_stream_data(*session, 0)); + CHECK_FALSE(audio.get_data(*session)); + session->update(); + const auto snapshot = camera.get_stream_data(*session, 0); + REQUIRE(snapshot); + for (uint64_t sequence = 1; sequence <= 3; ++sequence) + { + if (sequence > 1) + session->update(); + if (sequence == 2) + { + CHECK_FALSE(camera.get_stream_data(*session, 0)); + CHECK_FALSE(camera.get_stream_data(*session, 1)); + CHECK_FALSE(imu.get_stream_data(*session, 0)); + CHECK_FALSE(imu.get_stream_data(*session, 1)); + CHECK_FALSE(audio.get_data(*session)); + CHECK_FALSE(calibration.get_data(*session)); + CHECK_FALSE(state.get_data(*session)); + continue; + } + REQUIRE(camera.get_stream_data(*session, 1)); + CHECK(camera.get_stream_data(*session, 0)->sequence_number() == sequence); + CHECK(camera.get_stream_data(*session, 1)->stream() == core::EgoCameraStream_ColorRight); + REQUIRE(imu.get_stream_data(*session, 0)); + REQUIRE(imu.get_stream_data(*session, 1)); + CHECK(imu.get_stream_data(*session, 0)->sensor() == core::EgoImuSensor_Accel); + CHECK(imu.get_stream_data(*session, 1)->sensor() == core::EgoImuSensor_Gyro); + REQUIRE(audio.get_data(*session)); + CHECK(audio.get_data(*session)->sequence_number() == sequence); + REQUIRE(calibration.get_data(*session)); + CHECK(calibration.get_data(*session)->device_uid()->str() == "ego_" + std::to_string(sequence)); + REQUIRE(state.get_data(*session)); + CHECK(state.get_data(*session)->event_as_EgoSdkReport()->sequence_number() == sequence); + } + session->update(); + CHECK_FALSE(camera.get_stream_data(*session, 0)); + CHECK_FALSE(camera.get_stream_data(*session, 1)); + CHECK_FALSE(imu.get_stream_data(*session, 0)); + CHECK_FALSE(imu.get_stream_data(*session, 1)); + CHECK_FALSE(audio.get_data(*session)); + CHECK_FALSE(calibration.get_data(*session)); + CHECK_FALSE(state.get_data(*session)); + CHECK_THROWS_AS(camera.get_stream_data(*session, 2), std::out_of_range); + CHECK_THROWS_AS(imu.get_stream_data(*session, 2), std::out_of_range); + session.reset(); + CHECK(snapshot->sequence_number() == 1); + CHECK(snapshot->capture_epoch() == 3); +} + +TEST_CASE("EGO secondary media records reuse the unchanged file schemas", "[unit][replay][ego][mcap]") +{ + Recording recording; + { + mcap::McapWriter writer; + mcap::McapWriterOptions options("ego-test"); + options.compression = mcap::Compression::None; + REQUIRE(writer.open(recording.path, options).ok()); + core::McapTrackerChannels video(writer, "ego_media", { "ColorLeft" }); + core::McapTrackerChannels audio(writer, "ego_media", { "Audio" }); + core::EgoEncodedVideoFrameT frame; + frame.encoded_data = { 0, 0, 0, 1, 0x65 }; + video.write( + 0, core::pack_record(&frame, core::DeviceDataTimestamp(100, 90, 80))); + core::EgoPcmAudioChunkT chunk; + chunk.pcm_data = { 1, 0, 2, 0 }; + audio.write(0, core::pack_record(&chunk, core::DeviceDataTimestamp(100, 90, 80))); + writer.close(); + } + auto inventory_reader = open_reader(recording.path); + const core::RecordedSchemas recorded(*inventory_reader); + core::McapTrackerViewers video( + open_reader(recording.path), "ego_media", { "ColorLeft" }, recorded); + core::McapTrackerViewers audio( + open_reader(recording.path), "ego_media", { "Audio" }, recorded); + const auto frame = video.read(0); + REQUIRE(frame); + CHECK(frame->data()->encoded_data()->Get(4) == 0x65); + CHECK(frame->timestamp()->sample_time_raw_device_clock() == 80); + const auto chunk = audio.read(0); + REQUIRE(chunk); + CHECK(chunk->data()->pcm_data()->Get(2) == 2); + CHECK_FALSE(video.read(0)); + CHECK_FALSE(audio.read(0)); +} diff --git a/tests/cpp/core/schema/CMakeLists.txt b/tests/cpp/core/schema/CMakeLists.txt index 3fa2548b78..73d24122bc 100644 --- a/tests/cpp/core/schema/CMakeLists.txt +++ b/tests/cpp/core/schema/CMakeLists.txt @@ -5,6 +5,7 @@ cmake_minimum_required(VERSION 3.20) add_executable(schema_tests test_oak.cpp + test_ego_camera.cpp test_pose.cpp test_head.cpp test_hand.cpp diff --git a/tests/cpp/core/schema/test_ego_camera.cpp b/tests/cpp/core/schema/test_ego_camera.cpp new file mode 100644 index 0000000000..e3a2664464 --- /dev/null +++ b/tests/cpp/core/schema/test_ego_camera.cpp @@ -0,0 +1,295 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#define VT(field) (field + 2) * 2 + +static_assert(core::EgoFrameMetadata::VT_STREAM == VT(0)); +static_assert(core::EgoFrameMetadata::VT_SEQUENCE_NUMBER == VT(1)); +static_assert(core::EgoFrameMetadata::VT_WIDTH == VT(2)); +static_assert(core::EgoFrameMetadata::VT_HEIGHT == VT(3)); +static_assert(core::EgoFrameMetadata::VT_FPS == VT(4)); +static_assert(core::EgoFrameMetadata::VT_PIXEL_FORMAT == VT(5)); +static_assert(core::EgoFrameMetadata::VT_ENCODED_BYTES == VT(6)); +static_assert(core::EgoFrameMetadata::VT_SDK_METADATA == VT(7)); +static_assert(core::EgoFrameMetadata::VT_CAPTURE_EPOCH == VT(8)); +static_assert(core::EgoPixelFormat_Mjpg == 0); +static_assert(core::EgoAudioChunk::VT_TIMING == VT(9)); +static_assert(core::EgoPcmAudioChunk::VT_TIMING == VT(8)); +static_assert(core::EgoCalibration::VT_TIME_ALIGNMENT == VT(11)); + +TEST_CASE("EGO timing tables preserve optional provenance through record serialization", "[unit][ego][schema]") +{ + core::EgoCalibrationT calibration; + CHECK(core::pack(calibration)->time_alignment() == nullptr); + calibration.time_alignment = std::make_shared(); + calibration.time_alignment->json = "{\"version\":1,\"status\":\"unverified\"}"; + const auto encoded = core::pack(calibration); + REQUIRE(encoded->time_alignment()); + CHECK(encoded->time_alignment()->json()->str() == calibration.time_alignment->json); + + core::EgoAudioChunkT audio; + CHECK(core::pack(audio)->timing() == nullptr); + audio.timing = std::make_shared(); + audio.timing->segment_id = 42; + audio.timing->first_sample_index = 1ULL << 40; + audio.timing->discontinuity = true; + // Schema tests use native records; pack_record belongs to the separate MCAP module. + core::EgoAudioChunkRecordT native_record; + native_record.data = std::make_shared(audio); + native_record.timestamp = std::make_shared(30, 20, 10); + const auto record = core::pack(native_record); + REQUIRE(record->data()->timing()); + CHECK(record->data()->timing()->segment_id() == 42); + CHECK(record->data()->timing()->first_sample_index() == (1ULL << 40)); + CHECK_FALSE(record->data()->timing()->continuity_known()); + CHECK(record->data()->timing()->discontinuity()); + CHECK(record->timestamp()->sample_time_raw_device_clock() == 10); + + core::EgoPcmAudioChunkT pcm; + pcm.timing = audio.timing; + const auto pcm_encoded = core::pack(pcm); + REQUIRE(pcm_encoded->timing()); + CHECK(pcm_encoded->timing()->first_sample_index() == audio.timing->first_sample_index); +} + +TEST_CASE("EGO camera metadata round trips", "[unit][ego][schema]") +{ + core::EgoFrameMetadataT original; + original.stream = core::EgoCameraStream_ColorRight; + original.sequence_number = UINT64_MAX; + original.width = 1280; + original.height = 720; + original.fps = 30; + original.pixel_format = core::EgoPixelFormat_Mjpg; + original.encoded_bytes = 1234; + original.sdk_metadata.emplace_back(1, 99); + original.capture_epoch = 3; + + flatbuffers::FlatBufferBuilder builder; + builder.Finish(core::EgoFrameMetadata::Pack(builder, &original)); + + core::EgoFrameMetadataT restored; + flatbuffers::GetRoot(builder.GetBufferPointer())->UnPackTo(&restored); + CHECK(restored.stream == core::EgoCameraStream_ColorRight); + CHECK(restored.sequence_number == UINT64_MAX); + CHECK(restored.width == 1280); + CHECK(restored.height == 720); + CHECK(restored.fps == 30); + CHECK(restored.pixel_format == core::EgoPixelFormat_Mjpg); + CHECK(restored.encoded_bytes == 1234); + REQUIRE(restored.sdk_metadata.size() == 1); + CHECK(restored.sdk_metadata[0].value() == 99); + CHECK(restored.capture_epoch == 3); +} + +TEST_CASE("EGO auxiliary schemas round trip", "[unit][ego][schema]") +{ + core::EgoImuBatchT imu; + imu.sensor = core::EgoImuSensor_Gyro; + imu.sequence_number = 5; + imu.sample_rate_hz = 1000; + imu.full_scale = 500; + imu.samples.emplace_back(1, 2, 3, 24, 100, 200); + imu.capture_epoch = 2; + flatbuffers::FlatBufferBuilder imu_builder; + imu_builder.Finish(core::EgoImuBatch::Pack(imu_builder, &imu)); + core::EgoImuBatchT restored_imu; + flatbuffers::GetRoot(imu_builder.GetBufferPointer())->UnPackTo(&restored_imu); + REQUIRE(restored_imu.samples.size() == 1); + CHECK(restored_imu.samples[0].z_si() == 3); + CHECK(restored_imu.capture_epoch == 2); + + core::EgoAudioChunkT audio; + audio.sample_rate_hz = 48000; + audio.channel_count = 1; + audio.bits_per_sample = 16; + audio.sample_count = 480; + audio.wav_data_offset = 44; + audio.capture_epoch = 2; + flatbuffers::FlatBufferBuilder audio_builder; + audio_builder.Finish(core::EgoAudioChunk::Pack(audio_builder, &audio)); + core::EgoAudioChunkT restored_audio; + flatbuffers::GetRoot(audio_builder.GetBufferPointer())->UnPackTo(&restored_audio); + CHECK(restored_audio.wav_data_offset == 44); + CHECK(restored_audio.sample_count == 480); + CHECK(restored_audio.capture_epoch == 2); + + core::EgoCalibrationT calibration; + calibration.device_uid = "ego"; + calibration.raw_alignment_yaml = "camera: left"; + calibration.capture_epoch = 2; + flatbuffers::FlatBufferBuilder calibration_builder; + calibration_builder.Finish(core::EgoCalibration::Pack(calibration_builder, &calibration)); + core::EgoCalibrationT restored_calibration; + flatbuffers::GetRoot(calibration_builder.GetBufferPointer())->UnPackTo(&restored_calibration); + CHECK(restored_calibration.device_uid == "ego"); + CHECK(restored_calibration.capture_epoch == 2); +} + +TEST_CASE("EGO camera record retains timestamp", "[unit][ego][schema]") +{ + auto record = std::make_shared(); + record->data = std::make_shared(); + record->timestamp = std::make_shared(1, 2, 3); + + flatbuffers::FlatBufferBuilder builder; + builder.Finish(core::EgoFrameMetadataRecord::Pack(builder, record.get())); + const auto* restored = flatbuffers::GetRoot(builder.GetBufferPointer()); + REQUIRE(restored->timestamp() != nullptr); + CHECK(restored->timestamp()->available_time_local_common_clock() == 1); + CHECK(restored->timestamp()->sample_time_local_common_clock() == 2); + CHECK(restored->timestamp()->sample_time_raw_device_clock() == 3); +} + +TEST_CASE("EGO embedded media schemas round trip", "[unit][ego][schema]") +{ + core::EgoEncodedVideoFrameT video; + video.stream = core::EgoCameraStream_ColorRight; + video.sequence_number = 42; + video.width = 1600; + video.height = 1300; + video.fps = 30; + video.pixel_format = core::EgoPixelFormat_H264; + video.encoded_data = { 0, 0, 0, 1, 0x65, 1 }; + video.capture_epoch = 2; + flatbuffers::FlatBufferBuilder video_builder; + video_builder.Finish(core::EgoEncodedVideoFrame::Pack(video_builder, &video)); + core::EgoEncodedVideoFrameT restored_video; + flatbuffers::GetRoot(video_builder.GetBufferPointer())->UnPackTo(&restored_video); + CHECK(restored_video.stream == core::EgoCameraStream_ColorRight); + CHECK(restored_video.encoded_data == video.encoded_data); + CHECK(restored_video.capture_epoch == 2); + + core::EgoPcmAudioChunkT audio; + audio.sequence_number = 7; + audio.sample_rate_hz = 48000; + audio.channel_count = 1; + audio.bits_per_sample = 16; + audio.sample_count = 2; + audio.pcm_data = { 1, 0, 2, 0 }; + audio.capture_epoch = 2; + flatbuffers::FlatBufferBuilder audio_builder; + audio_builder.Finish(core::EgoPcmAudioChunk::Pack(audio_builder, &audio)); + core::EgoPcmAudioChunkT restored_audio; + flatbuffers::GetRoot(audio_builder.GetBufferPointer())->UnPackTo(&restored_audio); + CHECK(restored_audio.pcm_data == audio.pcm_data); + CHECK(restored_audio.capture_epoch == 2); +} + +TEST_CASE("EGO diagnostic events preserve typed payloads and optional zero", "[unit][ego][schema]") +{ + core::EgoDeviceStateT native; + native.diagnostic_version = core::EGO_DIAGNOSTIC_VERSION; + native.device_uid = "test-device"; + native.capture_epoch = 2; + native.observed_at_ns = 100; + SECTION("SDK report keeps a real zero error clear") + { + core::EgoSdkReportT report; + report.sequence_number = UINT64_MAX; + report.error_flags = 0; + native.event.Set(report); + const auto state = core::pack(native); + CHECK_NOTHROW(core::validate_ego_diagnostic(*state)); + CHECK(state->event_as_EgoSdkReport()->sequence_number() == UINT64_MAX); + CHECK(state->event_as_EgoSdkReport()->error_flags() == 0); + CHECK(state->event_as_EgoRecoveryEvent() == nullptr); + } + SECTION("final summary keeps cumulative losses") + { + core::EgoCaptureSummaryT summary; + summary.capture_health = core::EgoCaptureHealth_Incomplete; + summary.reason = "dropped video"; + summary.queue_capacity = 4096; + summary.queue_peak = 99; + summary.dropped_video_frame_sets = 3; + summary.is_final = true; + native.event.Set(summary); + auto state = core::pack(native); + CHECK_NOTHROW(core::validate_ego_diagnostic(*state)); + auto retained = state.narrow(state->event_as_EgoCaptureSummary()); + state.reset(); + CHECK(retained->dropped_video_frame_sets() == 3); + CHECK(retained->is_final()); + CHECK(retained->reason()->str() == "dropped video"); + } + SECTION("recovery carries only its transition") + { + core::EgoRecoveryEventT recovery; + recovery.stage = core::EgoRecoveryStage_Recovered; + recovery.reconnect_attempt = 4; + native.event.Set(recovery); + const auto state = core::pack(native); + CHECK_NOTHROW(core::validate_ego_diagnostic(*state)); + CHECK(state->capture_epoch() == 2); + CHECK(state->event_as_EgoRecoveryEvent()->reconnect_attempt() == 4); + } + SECTION("unavailable and zero temperature differ on the wire") + { + core::EgoDeviceSnapshotT snapshot; + snapshot.properties.emplace_back(279, 0); + native.event.Set(snapshot); + const auto missing = core::pack(native); + CHECK_NOTHROW(core::validate_ego_diagnostic(*missing)); + CHECK_FALSE(missing->event_as_EgoDeviceSnapshot()->imu_temperature_c().has_value()); + snapshot.imu_temperature_c = 0.0f; + native.event.Set(snapshot); + const auto measured = core::pack(native); + const auto temperature = measured->event_as_EgoDeviceSnapshot()->imu_temperature_c(); + REQUIRE(temperature.has_value()); + CHECK(*temperature == 0.0f); + CHECK(measured->event_as_EgoDeviceSnapshot()->properties()->Get(0)->value() == 0); + } +} + +TEST_CASE("EGO diagnostics reject missing events and unsupported versions", "[unit][ego][schema]") +{ + core::EgoDeviceStateT native; + native.device_uid = "test-device"; + native.observed_at_ns = 100; + native.diagnostic_version = core::EGO_DIAGNOSTIC_VERSION; + CHECK_THROWS_AS(core::validate_ego_diagnostic(*core::pack(native)), std::invalid_argument); + native.event.Set(core::EgoSdkReportT{}); + native.diagnostic_version = 1; + CHECK_THROWS_AS(core::validate_ego_diagnostic(*core::pack(native)), std::invalid_argument); + native.diagnostic_version = core::EGO_DIAGNOSTIC_VERSION; + auto state = core::pack(native); + auto bytes = std::vector(state.buffer().begin(), state.buffer().end()); + auto* mutable_state = flatbuffers::GetMutableRoot(bytes.data()); + // The generated verifier accepts unknown union tags; the EGO consumer rejects them. + auto* table = reinterpret_cast(mutable_state); + REQUIRE(table->SetField(core::EgoDeviceState::VT_EVENT_TYPE, 99)); + CHECK_THROWS_AS(core::validate_ego_diagnostic(*mutable_state), std::invalid_argument); +} + +TEST_CASE("EGO event protocol intentionally rejects the former diagnostic layout", "[unit][ego][schema]") +{ + flatbuffers::Parser old; + REQUIRE(old.Parse(R"( + namespace core; + table EgoDeviceState { sequence_number: uint64 (id: 0); device_uid: string (id: 1); } + table EgoDeviceStateRecord { data: EgoDeviceState (id: 0); } + root_type EgoDeviceStateRecord; + )")); + old.Serialize(); + const auto current = core::EgoDeviceStateRecordBinarySchema::data(); + const auto result = core::check_schema_compat({ old.builder_.GetBufferPointer(), old.builder_.GetSize() }, + { current, core::EgoDeviceStateRecordBinarySchema::size() }); + CHECK(result.status == core::SchemaCompat::Incompatible); +} diff --git a/tests/cpp/plugins/CMakeLists.txt b/tests/cpp/plugins/CMakeLists.txt index 1fd14aa203..a7141339b6 100644 --- a/tests/cpp/plugins/CMakeLists.txt +++ b/tests/cpp/plugins/CMakeLists.txt @@ -12,3 +12,15 @@ endif() if(BUILD_PLUGIN_OGLO) add_subdirectory(oglo_tactile) endif() + +if(TARGET ego::mcap_merge) + add_subdirectory(ego_mcap) +endif() + +if(TARGET ego_mcap_export_media AND UNIX) + add_subdirectory(ego_media) +endif() + +if(BUILD_PLUGIN_EGO_CAMERA OR BUILD_EGO_MEDIA_TOOLS) + add_subdirectory(ego) +endif() diff --git a/tests/cpp/plugins/ego/CMakeLists.txt b/tests/cpp/plugins/ego/CMakeLists.txt new file mode 100644 index 0000000000..29d51944b9 --- /dev/null +++ b/tests/cpp/plugins/ego/CMakeLists.txt @@ -0,0 +1,24 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +if(TARGET ego::camera_core) + add_executable(ego_camera_tests + test_ego_camera.cpp + test_time_alignment.cpp + ) + + target_link_libraries(ego_camera_tests + PRIVATE + ego::camera_core + ego::mcap_support + Catch2::Catch2WithMain + ) + + catch_discover_tests(ego_camera_tests ADD_TAGS_AS_LABELS) +endif() + +add_test( + NAME ego_script_tests + COMMAND bash "${CMAKE_CURRENT_SOURCE_DIR}/test_ego_script.sh" + "${CMAKE_SOURCE_DIR}/src/plugins/ego/ego.sh" +) diff --git a/tests/cpp/plugins/ego/test_ego_camera.cpp b/tests/cpp/plugins/ego/test_ego_camera.cpp new file mode 100644 index 0000000000..4e2e39e505 --- /dev/null +++ b/tests/cpp/plugins/ego/test_ego_camera.cpp @@ -0,0 +1,900 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if defined(__linux__) +# include + +# include +#endif + +namespace +{ + +class RecordingMetadataSink final : public plugins::ego::IMetadataSink +{ +public: + void on_frame_metadata(const plugins::ego::CapturedFrame& frame) override + { + frames.push_back(frame); + } + std::vector frames; +}; + +class RecordingMediaSink final : public plugins::ego::IMetadataSink +{ +public: + void on_frame_metadata(const plugins::ego::CapturedFrame&) override + { + } + void on_encoded_video_frame(const plugins::ego::CapturedFrame& frame) override + { + frames.push_back(frame); + } + std::vector frames; +}; + +void append_nal(std::vector& bytes, std::initializer_list header, const std::string& payload = {}) +{ + bytes.insert(bytes.end(), { 0, 0, 0, 1 }); + bytes.insert(bytes.end(), header); + bytes.insert(bytes.end(), payload.begin(), payload.end()); +} + +std::vector parameterized_idr(core::EgoPixelFormat format) +{ + std::vector bytes; + if (format == core::EgoPixelFormat_H264) + { + append_nal(bytes, { 0x67, 0x01 }); + append_nal(bytes, { 0x68, 0x01 }); + append_nal(bytes, { 0x65, 0x01 }); + } + else + { + append_nal(bytes, { 0x40, 0x01, 0x01 }); + append_nal(bytes, { 0x42, 0x01, 0x01 }); + append_nal(bytes, { 0x44, 0x01, 0x01 }); + append_nal(bytes, { 0x26, 0x01, 0x01 }); + } + return bytes; +} + +// Firmware payload strings retain vendor bytes; only API names use Ego. +void append_sei(std::vector& bytes, core::EgoPixelFormat format, const std::string& payload, bool suffix = false) +{ + if (format == core::EgoPixelFormat_H264) + append_nal(bytes, { 0x06, 0x05, static_cast(payload.size()) }, payload); + else + append_nal(bytes, + { static_cast(suffix ? 0x50 : 0x4e), 0x01, 0x05, static_cast(payload.size()) }, + payload); + bytes.push_back(0x80); +} + +std::vector read_bytes(const std::filesystem::path& path) +{ + std::ifstream input(path, std::ios::binary); + return { std::istreambuf_iterator(input), std::istreambuf_iterator() }; +} + +} // namespace + +TEST_CASE("EGO FrameSink writes raw MJPEG and forwards metadata", "[ego][writer][metadata]") +{ + const auto output = std::filesystem::temp_directory_path() / "isaacteleop_ego_camera_test.mjpg"; + auto metadata_sink = std::make_unique(); + auto* metadata_sink_ptr = metadata_sink.get(); + { + plugins::ego::FrameSink sink({ { core::EgoCameraStream_ColorLeft, output.string() } }, std::move(metadata_sink)); + plugins::ego::CapturedFrame frame; + frame.metadata.stream = core::EgoCameraStream_ColorLeft; + frame.metadata.sequence_number = 42; + frame.metadata.width = 1280; + frame.metadata.height = 720; + frame.metadata.fps = 30; + frame.metadata.pixel_format = core::EgoPixelFormat_Mjpg; + frame.encoded_data = { 0xff, 0xd8, 0x01, 0x02, 0xff, 0xd9 }; + frame.sample_time_local_common_clock_ns = 100; + frame.sample_time_raw_device_clock_ns = 200; + sink.on_frame(frame); + REQUIRE(metadata_sink_ptr->frames.size() == 1); + REQUIRE(metadata_sink_ptr->frames.front().metadata.sequence_number == 42); + } + + std::ifstream input(output, std::ios::binary); + std::vector bytes((std::istreambuf_iterator(input)), std::istreambuf_iterator()); + REQUIRE(bytes.size() == 6); + REQUIRE(static_cast(bytes.front()) == 0xff); + REQUIRE(static_cast(bytes.back()) == 0xd9); + std::filesystem::remove(output); +} + +TEST_CASE("EGO capture refuses existing sidecars before creating sibling outputs", "[ego][writer]") +{ + const auto existing = std::filesystem::temp_directory_path() / "ego_existing_sidecar_test"; + const auto sibling = std::filesystem::temp_directory_path() / "ego_new_sidecar_test"; + { + std::ofstream file(existing); + file << "preserve"; + } + plugins::ego::CaptureConfig config; + std::vector streams{ { core::EgoCameraStream_ColorLeft, sibling.string() } }; + SECTION("video") + { + streams.push_back({ core::EgoCameraStream_ColorRight, existing.string() }); + } + SECTION("audio") + { + config.audio_output = existing.string(); + } + SECTION("calibration") + { + config.calibration_output = existing.string(); + } + REQUIRE_THROWS_AS(plugins::ego::create_frame_sink(streams, config), std::invalid_argument); + CHECK(read_bytes(existing) == std::vector{ 'p', 'r', 'e', 's', 'e', 'r', 'v', 'e' }); + CHECK_FALSE(std::filesystem::exists(sibling)); + std::filesystem::remove(existing); +} + +TEST_CASE("EGO capture rejects aliased outputs before creating files", "[ego][writer]") +{ + const auto output = std::filesystem::temp_directory_path() / "ego_aliased_output_test"; + plugins::ego::CaptureConfig config; + std::vector streams{ { core::EgoCameraStream_ColorLeft, output.string() } }; + SECTION("two cameras") + { + streams.push_back({ core::EgoCameraStream_ColorRight, output.string() }); + } + SECTION("audio") + { + config.audio_output = (output.parent_path() / "." / output.filename()).string(); + } + SECTION("calibration") + { + config.calibration_output = output.string(); + } + SECTION("archive") + { + config.mcap_filename = output.string(); + } + REQUIRE_THROWS_AS(plugins::ego::create_frame_sink(streams, config), std::invalid_argument); + CHECK_FALSE(std::filesystem::exists(output)); +} + +TEST_CASE("EGO FrameSink preserves H264 and H265 elementary stream bytes", "[ego][writer]") +{ + for (const auto format : { core::EgoPixelFormat_H264, core::EgoPixelFormat_H265 }) + { + const auto suffix = format == core::EgoPixelFormat_H264 ? ".h264" : ".h265"; + const auto output = std::filesystem::temp_directory_path() / (std::string("isaacteleop_ego") + suffix); + size_t expected_size = 0; + { + plugins::ego::StreamConfig stream{ core::EgoCameraStream_ColorLeft, output.string() }; + stream.pixel_format = format; + plugins::ego::FrameSink sink({ stream }); + plugins::ego::CapturedFrame frame; + frame.metadata.stream = stream.camera; + frame.metadata.pixel_format = format; + if (format == core::EgoPixelFormat_H264) + frame.encoded_data = { 0, 0, 0, 1, 0x67, 0x01, 0, 0, 0, 1, 0x68, 0x01, 0, 0, 0, 1, 0x65, 0x01 }; + else + frame.encoded_data = { 0, 0, 0, 1, 0x40, 0x01, 0, 0, 0, 1, 0x42, 0x01, + 0, 0, 0, 1, 0x44, 0x01, 0, 0, 0, 1, 0x26, 0x01 }; + expected_size = frame.encoded_data.size(); + sink.on_frame(frame); + } + std::ifstream input(output, std::ios::binary); + std::vector bytes((std::istreambuf_iterator(input)), std::istreambuf_iterator()); + REQUIRE(bytes.size() == expected_size); + CHECK(bytes[3] == 1); + std::filesystem::remove(output); + } +} + +TEST_CASE("EGO embedded FrameSink does not require sidecar paths", "[ego][writer][mcap]") +{ + auto media_sink = std::make_unique(); + auto* media_sink_ptr = media_sink.get(); + plugins::ego::StreamConfig stream{ core::EgoCameraStream_ColorLeft, "" }; + stream.pixel_format = core::EgoPixelFormat_H264; + plugins::ego::FrameSink sink({ stream }, std::move(media_sink), false); + plugins::ego::CapturedFrame frame; + frame.metadata.stream = stream.camera; + frame.metadata.pixel_format = stream.pixel_format; + frame.metadata.sequence_number = 1; + frame.encoded_data = { 0, 0, 0, 1, 0x67, 1, 0, 0, 0, 1, 0x68, 1, 0, 0, 0, 1, 0x65, 1 }; + sink.on_frame(frame); + REQUIRE(media_sink_ptr->frames.size() == 1); + CHECK(media_sink_ptr->frames.front().metadata.encoded_bytes == frame.encoded_data.size()); +} + +TEST_CASE("EGO stream validation delegates encoded FPS support to SDK profile selection", "[ego][profile]") +{ + plugins::ego::CaptureConfig capture; + plugins::ego::StreamConfig h264{ core::EgoCameraStream_ColorLeft, "unused.h264" }; + h264.pixel_format = core::EgoPixelFormat_H264; + h264.fps = 60; + REQUIRE_NOTHROW(plugins::ego::validate_stream_config(h264, capture)); + + plugins::ego::StreamConfig h265{ core::EgoCameraStream_ColorRight, "unused.h265" }; + h265.pixel_format = core::EgoPixelFormat_H265; + capture.fps = 60; + REQUIRE_NOTHROW(plugins::ego::validate_stream_config(h265, capture)); + + capture.fps = 0; + REQUIRE_NOTHROW(plugins::ego::validate_stream_config(h265, capture)); +} + +TEST_CASE("EGO resolved-profile certification closes automatic FPS bypass", "[ego][profile]") +{ + plugins::ego::StreamConfig stream{ core::EgoCameraStream_ColorLeft, "unused.h264" }; + stream.pixel_format = core::EgoPixelFormat_H264; + stream.fps = 0; + CHECK_NOTHROW(plugins::ego::validate_resolved_stream_config(stream, 30)); + // This target's Catch2 macro subset omits CHECK_THROWS_WITH, so inspect the exception explicitly. + bool rejected = false; + try + { + plugins::ego::validate_resolved_stream_config(stream, 60); + } + catch (const std::invalid_argument& error) + { + rejected = true; + CHECK(std::string(error.what()) == + "resolved encoded profile is 60 FPS, but this build certifies encoded recording only through 30 FPS; " + "no profile fallback was attempted"); + } + CHECK(rejected); + + stream.pixel_format = core::EgoPixelFormat_H265; + CHECK_THROWS(plugins::ego::validate_resolved_stream_config(stream, 60)); + stream.pixel_format = core::EgoPixelFormat_Mjpg; + CHECK_NOTHROW(plugins::ego::validate_resolved_stream_config(stream, 60)); +} + +TEST_CASE("EGO FrameSink resumes compressed recording at an IDR after a sequence gap", "[ego][writer]") +{ + const auto output = std::filesystem::temp_directory_path() / "isaacteleop_ego_gap.h264"; + plugins::ego::StreamConfig stream{ core::EgoCameraStream_ColorLeft, output.string() }; + stream.pixel_format = core::EgoPixelFormat_H264; + const std::vector idr = { 0, 0, 0, 1, 0x67, 0x01, 0, 0, 0, 1, 0x68, 0x01, 0, 0, 0, 1, 0x65, 0x01 }; + const std::vector p_frame = { 0, 0, 0, 1, 0x41, 0x01 }; + { + plugins::ego::FrameSink sink({ stream }); + plugins::ego::CapturedFrame frame; + frame.metadata.stream = stream.camera; + frame.metadata.pixel_format = stream.pixel_format; + frame.metadata.sequence_number = 1; + frame.encoded_data = idr; + sink.on_frame(frame); + + frame.metadata.sequence_number = 3; + frame.encoded_data = p_frame; + sink.on_frame(frame); + + frame.metadata.sequence_number = 4; + sink.on_frame(frame); + + frame.metadata.sequence_number = 5; + frame.encoded_data = idr; + sink.on_frame(frame); + } + std::ifstream input(output, std::ios::binary); + std::vector bytes((std::istreambuf_iterator(input)), std::istreambuf_iterator()); + REQUIRE(bytes.size() == idr.size() * 2); + std::filesystem::remove(output); +} + +TEST_CASE("EGO FrameSink starts every capture epoch at a parameterized IDR", "[ego][writer][reconnect]") +{ + const auto output = std::filesystem::temp_directory_path() / "isaacteleop_ego_epoch.h264"; + plugins::ego::StreamConfig stream{ core::EgoCameraStream_ColorLeft, output.string() }; + stream.pixel_format = core::EgoPixelFormat_H264; + const auto idr = parameterized_idr(stream.pixel_format); + const std::vector p_frame = { 0, 0, 0, 1, 0x41, 0x01 }; + { + plugins::ego::FrameSink sink({ stream }); + plugins::ego::CapturedFrame frame; + frame.metadata.stream = stream.camera; + frame.metadata.pixel_format = stream.pixel_format; + frame.metadata.sequence_number = 100; + frame.encoded_data = idr; + sink.on_frame(frame); + frame.metadata.sequence_number = 101; + frame.encoded_data = p_frame; + sink.on_frame(frame); + + sink.begin_capture_epoch(); + frame.metadata.sequence_number = 0; + sink.on_frame(frame); + frame.metadata.sequence_number = 1; + frame.encoded_data = idr; + sink.on_frame(frame); + } + auto expected = idr; + expected.insert(expected.end(), p_frame.begin(), p_frame.end()); + expected.insert(expected.end(), idr.begin(), idr.end()); + REQUIRE(read_bytes(output) == expected); + std::filesystem::remove(output); +} + +TEST_CASE("EGO FrameSink removes legacy and firmware 1.1.1 timestamp SEI", "[ego][writer]") +{ + for (const auto format : { core::EgoPixelFormat_H264, core::EgoPixelFormat_H265 }) + { + const auto frame_field = format == core::EgoPixelFormat_H264 ? "frameId" : "frame_seq"; + const std::vector timestamp_payloads = { + "ORBBEC,EGO_", + "EGO0000000000_L,timestamp_us=1788430979164168," + std::string(frame_field) + "=1", + "EGO0000000000_R,timestamp_us=1788430979180835," + std::string(frame_field) + "=2", + }; + for (size_t index = 0; index < timestamp_payloads.size(); ++index) + { + const auto codec = format == core::EgoPixelFormat_H264 ? "h264" : "h265"; + const auto output = std::filesystem::temp_directory_path() / + (std::string("isaacteleop_ego_timestamp_") + codec + "_" + std::to_string(index)); + const auto expected = parameterized_idr(format); + auto encoded = expected; + append_sei(encoded, format, timestamp_payloads[index]); + if (format == core::EgoPixelFormat_H265) + append_sei(encoded, format, timestamp_payloads[index], true); + + { + auto media_sink = std::make_unique(); + auto* media_sink_ptr = media_sink.get(); + plugins::ego::StreamConfig stream{ core::EgoCameraStream_ColorLeft, output.string() }; + stream.pixel_format = format; + plugins::ego::FrameSink sink({ stream }, std::move(media_sink)); + plugins::ego::CapturedFrame frame; + frame.metadata.stream = stream.camera; + frame.metadata.pixel_format = stream.pixel_format; + frame.metadata.sequence_number = 1; + frame.encoded_data = std::move(encoded); + sink.on_frame(frame); + REQUIRE(media_sink_ptr->frames.size() == 1); + CHECK(media_sink_ptr->frames.front().encoded_data == expected); + CHECK(media_sink_ptr->frames.front().metadata.encoded_bytes == expected.size()); + } + + REQUIRE(read_bytes(output) == expected); + std::filesystem::remove(output); + } + } +} + +TEST_CASE("EGO FrameSink preserves non-timestamp H264 and H265 SEI", "[ego][writer]") +{ + for (const auto format : { core::EgoPixelFormat_H264, core::EgoPixelFormat_H265 }) + { + const auto frame_field = format == core::EgoPixelFormat_H264 ? "frameId" : "frame_seq"; + const std::vector ordinary_payloads = { + "ORBBEC,CAMERA_INFO", + "EGO0000000000_L,camera_info=stereo", + "timestamp_us=1788430979164168," + std::string(frame_field) + "=1", + "EGO0000000000_L," + std::string(frame_field) + "=1,timestamp_us=1788430979164168", + "EGO0000000000_L,timestamp_us=1788430979164168," + + std::string(format == core::EgoPixelFormat_H264 ? "frame_seq" : "frameId") + "=1", + }; + const auto codec = format == core::EgoPixelFormat_H264 ? "h264" : "h265"; + const auto output = + std::filesystem::temp_directory_path() / (std::string("isaacteleop_ego_ordinary_sei_") + codec); + auto encoded = parameterized_idr(format); + for (const auto& payload : ordinary_payloads) + append_sei(encoded, format, payload); + if (format == core::EgoPixelFormat_H265) + append_sei(encoded, format, ordinary_payloads.front(), true); + + { + auto media_sink = std::make_unique(); + auto* media_sink_ptr = media_sink.get(); + plugins::ego::StreamConfig stream{ core::EgoCameraStream_ColorLeft, output.string() }; + stream.pixel_format = format; + plugins::ego::FrameSink sink({ stream }, std::move(media_sink)); + plugins::ego::CapturedFrame frame; + frame.metadata.stream = stream.camera; + frame.metadata.pixel_format = stream.pixel_format; + frame.metadata.sequence_number = 1; + frame.encoded_data = encoded; + sink.on_frame(frame); + REQUIRE(media_sink_ptr->frames.size() == 1); + CHECK(media_sink_ptr->frames.front().encoded_data == encoded); + } + + REQUIRE(read_bytes(output) == encoded); + std::filesystem::remove(output); + } +} + +TEST_CASE("EGO FrameSink observers share the canonical recording gate", "[ego][writer][preview]") +{ + for (const auto format : { core::EgoPixelFormat_H264, core::EgoPixelFormat_H265 }) + { + plugins::ego::StreamConfig stream{ core::EgoCameraStream_ColorLeft, "" }; + stream.pixel_format = format; + auto media = std::make_unique(); + auto* media_ptr = media.get(); + plugins::ego::FrameSink sink({ stream }, std::move(media), false); + std::vector observed; + const auto observe = [&](const plugins::ego::CapturedFrame& accepted) + { + REQUIRE(!media_ptr->frames.empty()); + CHECK(accepted.encoded_data == media_ptr->frames.back().encoded_data); + CHECK(accepted.metadata.encoded_bytes == accepted.encoded_data.size()); + CHECK(accepted.sample_time_local_common_clock_ns == 100); + CHECK(accepted.sample_time_raw_device_clock_ns == 200); + observed.push_back(accepted); + }; + std::vector predicted; + append_nal(predicted, format == core::EgoPixelFormat_H264 ? std::initializer_list{ 0x41, 0x01 } : + std::initializer_list{ 0x02, 0x01, 0x01 }); + auto canonical_idr = parameterized_idr(format); + append_sei(canonical_idr, format, "ORBBEC,CAMERA_INFO"); + auto raw_idr = canonical_idr; + append_sei(raw_idr, format, "ORBBEC,EGO_"); + std::vector timestamp_only; + append_sei(timestamp_only, format, "ORBBEC,EGO_"); + + plugins::ego::CapturedFrame frame; + frame.metadata.stream = stream.camera; + frame.metadata.pixel_format = format; + frame.metadata.capture_epoch = 7; + frame.sample_time_local_common_clock_ns = 100; + frame.sample_time_raw_device_clock_ns = 200; + frame.encoded_data = predicted; + sink.on_frame(frame, observe); + CHECK(observed.empty()); + + frame.metadata.sequence_number = 1; + frame.encoded_data = raw_idr; + sink.on_frame(frame, observe); + REQUIRE(observed.size() == 1); + CHECK(observed.back().encoded_data == canonical_idr); + CHECK(frame.encoded_data == raw_idr); + + frame.metadata.sequence_number = 2; + frame.encoded_data = timestamp_only; + sink.on_frame(frame, observe); + CHECK(observed.size() == 1); + frame.metadata.sequence_number = 3; + frame.encoded_data = predicted; + sink.on_frame(frame, observe); + REQUIRE(observed.size() == 2); + CHECK(observed.back().encoded_data == predicted); + + frame.metadata.sequence_number = 5; + sink.on_frame(frame, observe); + frame.metadata.sequence_number = 6; + sink.on_frame(frame, observe); + CHECK(observed.size() == 2); + frame.metadata.sequence_number = 7; + frame.encoded_data = raw_idr; + sink.on_frame(frame, observe); + REQUIRE(observed.size() == 3); + CHECK(observed.back().metadata.sequence_number == 7); + + sink.begin_capture_epoch(); + frame.metadata.capture_epoch = 8; + frame.metadata.sequence_number = 0; + frame.encoded_data = predicted; + sink.on_frame(frame, observe); + CHECK(observed.size() == 3); + frame.metadata.sequence_number = 1; + frame.encoded_data = raw_idr; + sink.on_frame(frame, observe); + REQUIRE(observed.size() == 4); + CHECK(observed.back().metadata.capture_epoch == 8); + CHECK(observed.back().encoded_data == canonical_idr); + CHECK(media_ptr->frames.size() == observed.size()); + } +} + +TEST_CASE("EGO FrameSink observers work without a metadata sink", "[ego][writer][preview]") +{ + plugins::ego::FrameSink sink({ { core::EgoCameraStream_ColorLeft, "" } }, nullptr, false); + plugins::ego::CapturedFrame frame; + frame.metadata.stream = core::EgoCameraStream_ColorLeft; + frame.metadata.sequence_number = 42; + frame.encoded_data = { 0xff, 0xd8, 0xff, 0xd9 }; + size_t observations = 0; + sink.on_frame(frame, + [&](const plugins::ego::CapturedFrame& accepted) + { + ++observations; + CHECK(accepted.metadata.sequence_number == 42); + CHECK(accepted.metadata.encoded_bytes == 4); + CHECK(accepted.encoded_data == frame.encoded_data); + }); + CHECK(observations == 1); +} + +TEST_CASE("EGO local MCAP and SchemaPusher modes are mutually exclusive", "[ego][cli][mcap]") +{ + plugins::ego::CaptureConfig config; + config.collection_prefix = "ego"; + config.mcap_filename = "metadata.mcap"; + REQUIRE_THROWS_AS(plugins::ego::create_frame_sink({}, config), std::invalid_argument); +} + +TEST_CASE("EGO local MCAP is promoted only by an explicit successful close", "[ego][mcap][shutdown]") +{ + const auto base = std::filesystem::temp_directory_path() / "isaacteleop_ego_shutdown_test.mcap"; + const auto partial = std::filesystem::path(base.string() + ".partial"); + plugins::ego::CaptureConfig config; + config.mcap_media_mode = plugins::ego::McapMediaMode::Embedded; + plugins::ego::StreamConfig stream{ core::EgoCameraStream_ColorLeft, "" }; + stream.pixel_format = core::EgoPixelFormat_H264; + + std::filesystem::remove(base); + std::filesystem::remove(partial); + config.mcap_filename = base.string(); + { + auto sink = plugins::ego::create_frame_sink({ stream }, config); + sink->abort_metadata(); + } + CHECK_FALSE(std::filesystem::exists(base)); + CHECK(std::filesystem::exists(partial)); + std::filesystem::remove(partial); + + { + auto sink = plugins::ego::create_frame_sink({ stream }, config); + CHECK_THROWS(sink->commit_metadata()); + sink->close_media(); + sink->close_metadata(); + CHECK_FALSE(std::filesystem::exists(base)); + CHECK(std::filesystem::exists(partial)); + sink->commit_metadata(); + } + CHECK(std::filesystem::exists(base)); + CHECK_FALSE(std::filesystem::exists(partial)); + std::filesystem::remove(base); +} + +TEST_CASE("EGO composite metadata rejects promotion after any close failure", "[ego][mcap][shutdown]") +{ + struct Sink final : plugins::ego::IMetadataSink + { + bool fail = false; + bool closed = false; + bool committed = false; + void on_frame_metadata(const plugins::ego::CapturedFrame&) override + { + } + void close() override + { + closed = true; + if (fail) + throw std::runtime_error("publisher close failed"); + } + void commit() override + { + committed = true; + } + }; + auto publisher = std::make_unique(); + auto archive = std::make_unique(); + auto* publisher_ptr = publisher.get(); + auto* archive_ptr = archive.get(); + publisher->fail = true; + std::vector> children; + children.push_back(std::move(publisher)); + children.push_back(std::move(archive)); + auto sink = plugins::ego::compose_metadata_sinks(std::move(children)); + CHECK_THROWS(sink->close()); + CHECK(publisher_ptr->closed); + CHECK(archive_ptr->closed); + CHECK_THROWS(sink->commit()); + CHECK_FALSE(publisher_ptr->committed); + CHECK_FALSE(archive_ptr->committed); +} + +TEST_CASE("EGO checked MCAP writes reject non-I/O writer errors", "[ego][mcap]") +{ + const auto path = std::filesystem::temp_directory_path() / "ego_closed_writer_test.mcap"; + mcap::McapWriter writer; + REQUIRE(writer.open(path.string(), mcap::McapWriterOptions("ego")).ok()); + core::McapTrackerChannels channels(writer, "ego_metadata", { "ColorLeft" }); + writer.close(); + const auto record = core::pack_record(nullptr, core::DeviceDataTimestamp(1, 2, 3)); + CHECK_THROWS_AS(ego_mcap::write_checked(writer, channels, 0, record), std::runtime_error); + std::filesystem::remove(path); +} + +TEST_CASE("EGO startup waits respond to cancellation without waiting for readiness timeout", "[ego][shutdown]") +{ + std::atomic cancelled{ false }; + std::mutex mutex; + std::condition_variable wake; + std::thread stopper( + [&] + { + std::this_thread::sleep_for(std::chrono::milliseconds(30)); + cancelled = true; + }); + const auto start = std::chrono::steady_clock::now(); + std::unique_lock lock(mutex); + CHECK_THROWS_AS(plugins::ego::wait_capture_ready( + wake, lock, std::chrono::seconds(10), [] { return false; }, [&] { return cancelled.load(); }), + plugins::ego::CaptureCancelled); + stopper.join(); + CHECK(std::chrono::steady_clock::now() - start < std::chrono::seconds(1)); +} + +TEST_CASE("EGO abort after sealing cannot promote a readable partial MCAP", "[ego][mcap][shutdown]") +{ + const auto path = std::filesystem::temp_directory_path() / "ego_sealed_abort_test.mcap"; + std::filesystem::remove(path); + plugins::ego::CaptureConfig config; + config.mcap_filename = path.string(); + config.mcap_media_mode = plugins::ego::McapMediaMode::Embedded; + auto sink = plugins::ego::create_frame_sink({ { core::EgoCameraStream_ColorLeft, "" } }, config); + sink->close_media(); + sink->close_metadata(); + sink->abort_metadata(); + CHECK_THROWS(sink->commit_metadata()); + CHECK_FALSE(std::filesystem::exists(path)); + CHECK(std::filesystem::exists(path.string() + ".partial")); + std::filesystem::remove(path.string() + ".partial"); +} + +TEST_CASE("EGO capture preserves existing final and partial archives", "[ego][mcap][shutdown]") +{ + const auto path = std::filesystem::temp_directory_path() / "ego_existing_output_test.mcap"; + const auto partial = std::filesystem::path(path.string() + ".partial"); + std::filesystem::remove(path); + std::filesystem::remove(partial); + plugins::ego::CaptureConfig config; + config.mcap_filename = path.string(); + config.mcap_media_mode = plugins::ego::McapMediaMode::Embedded; + const std::vector expected{ 'k', 'e', 'e', 'p' }; + SECTION("existing final") + { + std::ofstream(path) << "keep"; + CHECK_THROWS(plugins::ego::create_frame_sink({}, config)); + CHECK(read_bytes(path) == expected); + CHECK_FALSE(std::filesystem::exists(partial)); + } + SECTION("existing partial") + { + std::ofstream(partial) << "keep"; + CHECK_THROWS(plugins::ego::create_frame_sink({}, config)); + CHECK(read_bytes(partial) == expected); + CHECK_FALSE(std::filesystem::exists(path)); + } +#if defined(__linux__) + SECTION("dangling partial symlink") + { + std::filesystem::create_symlink(path, partial); + CHECK_THROWS(plugins::ego::create_frame_sink({}, config)); + CHECK(std::filesystem::is_symlink(partial)); + CHECK_FALSE(std::filesystem::exists(path)); + } +#endif + SECTION("final appears before commit") + { + auto sink = plugins::ego::create_frame_sink({}, config); + sink->close_media(); + sink->close_metadata(); + std::ofstream(path) << "keep"; + CHECK_THROWS(sink->commit_metadata()); + CHECK(read_bytes(path) == expected); + CHECK(std::filesystem::exists(partial)); + } + std::filesystem::remove(path); + std::filesystem::remove(partial); +} + +TEST_CASE("EGO embedded MCAP preserves structured and media records through the final drain", "[ego][mcap]") +{ + const auto path = std::filesystem::temp_directory_path() / "ego_complete_fragment_test.mcap"; + std::filesystem::remove(path); + std::filesystem::remove(path.string() + ".partial"); + plugins::ego::CaptureConfig config; + config.mcap_filename = path.string(); + config.mcap_media_mode = plugins::ego::McapMediaMode::Embedded; + auto sink = plugins::ego::create_frame_sink( + { { core::EgoCameraStream_ColorLeft, "" }, { core::EgoCameraStream_ColorRight, "" } }, config); + for (const auto camera : { core::EgoCameraStream_ColorLeft, core::EgoCameraStream_ColorRight }) + { + for (uint64_t sequence = 0; sequence < 2; ++sequence) + { + plugins::ego::CapturedFrame frame; + frame.metadata.stream = camera; + frame.metadata.sequence_number = sequence; + frame.metadata.capture_epoch = 3; + frame.metadata.width = 1600; + frame.metadata.height = 1300; + frame.metadata.fps = 30; + frame.encoded_data = { 0xff, 0xd8, 0xff, 0xd9 }; + frame.sample_time_local_common_clock_ns = 123 + sequence; + frame.sample_time_raw_device_clock_ns = 456 + sequence; + sink->on_frame(frame); + } + } + auto* metadata = sink->metadata_sink(); + core::EgoImuBatchT imu; + imu.capture_epoch = 3; + imu.sensor = core::EgoImuSensor_Accel; + metadata->on_imu_batch(imu, 123, 456); + imu.sensor = core::EgoImuSensor_Gyro; + metadata->on_imu_batch(imu, 123, 456); + core::EgoAudioChunkT audio; + audio.capture_epoch = 3; + metadata->on_audio_chunk(audio, 123, 456); + core::EgoPcmAudioChunkT pcm; + pcm.capture_epoch = 3; + pcm.sample_rate_hz = 48'000; + pcm.channel_count = 1; + pcm.bits_per_sample = 16; + pcm.sample_count = 1; + pcm.pcm_data = { 1, 2 }; + metadata->on_pcm_audio_chunk(pcm, 123, 456); + core::EgoCalibrationT calibration; + calibration.capture_epoch = 3; + metadata->on_calibration(calibration, 123, 456); + core::EgoDeviceStateT state; + state.capture_epoch = 3; + state.device_uid = "fixture"; + state.observed_at_ns = 123; + state.diagnostic_version = 2; + core::EgoCaptureSummaryT summary; + summary.is_final = true; + summary.queue_capacity = 4096; + summary.queue_peak = 7; + state.event.Set(summary); + metadata->on_device_state(state, 123, 0); + sink->close_media(); + sink->close_metadata(); + sink->commit_metadata(); + + mcap::McapReader reader; + REQUIRE(reader.open(path.string()).ok()); + std::map counts; + for (const auto& view : reader.readMessages()) + { + ++counts[view.channel->topic]; + CHECK(view.message.logTime > 0); + if (view.channel->topic == "ego_device/DeviceState") + { + const auto* record = flatbuffers::GetRoot(view.message.data); + const auto* event = record->data()->event_as_EgoCaptureSummary(); + REQUIRE(event); + CHECK(event->is_final()); + CHECK(event->queue_peak() == 7); + CHECK(record->data()->observed_at_ns() == record->timestamp()->sample_time_local_common_clock()); + } + if (view.channel->topic == "ego_metadata/ColorLeft" || view.channel->topic == "ego_metadata/ColorRight") + { + flatbuffers::Verifier verifier(reinterpret_cast(view.message.data), view.message.dataSize); + REQUIRE(verifier.VerifyBuffer()); + const auto* record = flatbuffers::GetRoot(view.message.data); + REQUIRE(record->data()); + REQUIRE(record->timestamp()); + CHECK(record->data()->capture_epoch() == 3); + CHECK(record->timestamp()->sample_time_local_common_clock() == 123 + record->data()->sequence_number()); + CHECK(record->timestamp()->sample_time_raw_device_clock() == 456 + record->data()->sequence_number()); + CHECK(record->timestamp()->available_time_local_common_clock() == static_cast(view.message.logTime)); + } + } + CHECK(counts == std::map{ { "ego_metadata/ColorLeft", 2 }, + { "ego_metadata/ColorRight", 2 }, + { "ego_media/ColorLeft", 2 }, + { "ego_media/ColorRight", 2 }, + { "ego_imu/Accel", 1 }, + { "ego_imu/Gyro", 1 }, + { "ego_audio/Audio", 1 }, + { "ego_media/Audio", 1 }, + { "ego_calibration/Calibration", 1 }, + { "ego_device/DeviceState", 1 } }); + reader.close(); + std::filesystem::remove(path); +} + +#if defined(__linux__) +TEST_CASE("EGO MCAP flush failures prevent final archive promotion", "[ego][mcap][shutdown]") +{ + const auto path = std::filesystem::temp_directory_path() / "ego_full_output_test.mcap"; + const auto partial = std::filesystem::path(path.string() + ".partial"); + std::filesystem::remove(path); + std::filesystem::remove(partial); + plugins::ego::CaptureConfig config; + config.mcap_filename = path.string(); + config.mcap_media_mode = plugins::ego::McapMediaMode::Embedded; + auto sink = plugins::ego::create_frame_sink({ { core::EgoCameraStream_ColorLeft, "" } }, config); + sink->close_media(); + struct FileLimit + { + rlimit previous{}; + using Handler = void (*)(int); + Handler handler; + FileLimit() + { + REQUIRE(getrlimit(RLIMIT_FSIZE, &previous) == 0); + const rlimit limit{ 0, previous.rlim_max }; + REQUIRE(setrlimit(RLIMIT_FSIZE, &limit) == 0); + handler = std::signal(SIGXFSZ, SIG_IGN); + } + ~FileLimit() + { + setrlimit(RLIMIT_FSIZE, &previous); + std::signal(SIGXFSZ, handler); + } + }; + { + FileLimit limit; + CHECK_THROWS(sink->close_metadata()); + } + CHECK_THROWS(sink->commit_metadata()); + CHECK_FALSE(std::filesystem::exists(path)); + sink.reset(); + std::filesystem::remove(partial); +} +#endif + +TEST_CASE("EGO diagnostics off omits schema channel and messages but preserves sensors", "[ego][mcap]") +{ + const auto path = std::filesystem::temp_directory_path() / "ego_diagnostics_off_test.mcap"; + std::filesystem::remove(path); + std::filesystem::remove(path.string() + ".partial"); + plugins::ego::CaptureConfig config; + config.mcap_filename = path.string(); + config.mcap_media_mode = plugins::ego::McapMediaMode::Embedded; + config.record_device_diagnostics = false; + auto sink = plugins::ego::create_frame_sink({ { core::EgoCameraStream_ColorLeft, "" } }, config); + core::EgoDeviceStateT diagnostic; + diagnostic.device_uid = "fixture"; + diagnostic.observed_at_ns = 100; + diagnostic.diagnostic_version = 2; + diagnostic.event.Set(core::EgoSdkReportT{}); + sink->metadata_sink()->on_device_state(diagnostic, 100, 0); + core::EgoCalibrationT calibration; + calibration.device_uid = "test"; + sink->metadata_sink()->on_calibration(calibration, 100, 0); + sink->close_media(); + sink->close_metadata(); + sink->commit_metadata(); + mcap::McapReader reader; + REQUIRE(reader.open(path.string()).ok()); + REQUIRE(reader.readSummary(mcap::ReadSummaryMethod::NoFallbackScan).ok()); + for (const auto& [id, schema] : reader.schemas()) + CHECK(schema->name != "core.EgoDeviceStateRecord"); + CHECK(reader.statistics()->messageCount == 1); + for (const auto& view : reader.readMessages()) + CHECK(view.channel->topic == "ego_calibration/Calibration"); + auto& data = *reader.dataSource(); + mcap::TypedRecordReader records(data, sizeof(mcap::Magic), data.size() - sizeof(mcap::Magic)); + unsigned policies = 0; + records.onMetadata = [&](const mcap::Metadata& metadata, auto) + { + if (metadata.name == "ego.recording") + { + ++policies; + CHECK(metadata.metadata.at("record_device_diagnostics") == "off"); + CHECK(metadata.metadata.at("diagnostic_version") == "2"); + } + }; + while (records.next()) + REQUIRE(records.status().ok()); + CHECK(policies == 1); + reader.close(); + std::filesystem::remove(path); +} diff --git a/tests/cpp/plugins/ego/test_ego_script.sh b/tests/cpp/plugins/ego/test_ego_script.sh new file mode 100755 index 0000000000..5440136e53 --- /dev/null +++ b/tests/cpp/plugins/ego/test_ego_script.sh @@ -0,0 +1,697 @@ +#!/usr/bin/env bash +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +set -euo pipefail + +SCRIPT="${1:?Pass the plugin shell entry point}" +TMPDIR_TEST="$(mktemp -d)" +trap 'rm -rf "$TMPDIR_TEST"' EXIT + +expect_failure() { + if "$@" >/dev/null 2>&1; then + echo "Expected command to fail: $*" >&2 + exit 1 + fi +} + +"$SCRIPT" --help >/dev/null +expect_failure "$SCRIPT" doctor --sdk-root "$TMPDIR_TEST/missing-sdk" +expect_failure "$SCRIPT" build --sdk-root "$TMPDIR_TEST/missing-sdk" --preset invalid +expect_failure "$SCRIPT" record --duration 0 +expect_failure "$SCRIPT" record --format invalid +expect_failure "$SCRIPT" record --reconnect-timeout invalid +expect_failure "$SCRIPT" record --reconnect-interval-ms 0 + +MOCK_PLUGIN="$TMPDIR_TEST/mock_plugin" +MOCK_ARGS="$TMPDIR_TEST/plugin_args" +export MOCK_ARGS +cat > "$MOCK_PLUGIN" <<'EOF' +#!/usr/bin/env bash +if [[ "$1" == "--list-capabilities" ]]; then + printf '%s\n' "$@" > "$MOCK_ARGS" + cat <<'CAPABILITIES' +Sensor Accel +Sensor Gyro +Sensor LeftColor +Sensor RightColor +Sensor Audio +CAPABILITIES + exit 0 +fi +printf '%s\n' "$@" > "$MOCK_ARGS" +exit 7 +EOF +chmod +x "$MOCK_PLUGIN" + +CAPABILITY_OUTPUT="$($SCRIPT capabilities --plugin "$MOCK_PLUGIN" 2>/dev/null)" +grep -F "== Capture software provenance ==" <<< "$CAPABILITY_OUTPUT" >/dev/null +grep -F "== Device-advertised capabilities ==" <<< "$CAPABILITY_OUTPUT" >/dev/null +grep -Fx "Sensor LeftColor" <<< "$CAPABILITY_OUTPUT" >/dev/null +"$SCRIPT" capabilities --plugin "$MOCK_PLUGIN" --device-uid ego-test >/dev/null 2>&1 +grep -Fx -- "--device-uid=ego-test" "$MOCK_ARGS" >/dev/null + +RUN="$TMPDIR_TEST/recording" +expect_failure "$SCRIPT" record --plugin "$MOCK_PLUGIN" --duration 1 --output "$RUN" -- --bitrate=8 +grep -Fx -- "--enable-imu" "$MOCK_ARGS" >/dev/null +if grep -Eq '^--enable-audio$|^--audio-output=' "$MOCK_ARGS"; then + echo "Default capture must not request audio" >&2 + exit 1 +fi +grep -Fx -- "--mcap-filename=$RUN/metadata.mcap" "$MOCK_ARGS" >/dev/null +grep -Fx -- "--reconnect-timeout=30" "$MOCK_ARGS" >/dev/null +grep -Fx -- "--reconnect-interval-ms=1000" "$MOCK_ARGS" >/dev/null +grep -Fx -- "--bitrate=8" "$MOCK_ARGS" >/dev/null +test -f "$RUN/capabilities.txt" +test -f "$RUN/logs/capture.log" +grep -F "Plugin SHA-256:" "$RUN/capabilities.txt" >/dev/null +python3 - "$RUN/requested_profile.json" <<'PY' +import json +import sys + +with open(sys.argv[1], encoding="utf-8") as stream: + manifest = json.load(stream) +assert manifest == { + "version": 1, + "record_device_diagnostics": "on", + "streams": { + "ColorLeft": {"format": "h264", "width": 1600, "height": 1300, "fps": 30}, + "ColorRight": {"format": "h264", "width": 1600, "height": 1300, "fps": 30}, + }, +} +PY + +AUDIO_RUN="$TMPDIR_TEST/audio_recording" +expect_failure "$SCRIPT" record --plugin "$MOCK_PLUGIN" --duration 1 --output "$AUDIO_RUN" --audio +grep -Fx -- "--audio-output=$AUDIO_RUN/Audio.wav" "$MOCK_ARGS" >/dev/null + +CONFIG_RUN="$TMPDIR_TEST/config_recording" +CONFIG_FILE="$TMPDIR_TEST/record.toml" +cat > "$CONFIG_FILE" </dev/null + +expect_failure "$SCRIPT" record --config "$CONFIG_FILE" --duration 1 \ + --output "$CONFIG_RUN" --format h264 --no-audio +grep -Fx -- "--device-uid=ego-config" "$MOCK_ARGS" >/dev/null +grep -Fx -- "--reconnect-timeout=45" "$MOCK_ARGS" >/dev/null +grep -Fx -- "--reconnect-interval-ms=250" "$MOCK_ARGS" >/dev/null +grep -Fx -- "--preview" "$MOCK_ARGS" >/dev/null +grep -Fx -- "--bitrate=8" "$MOCK_ARGS" >/dev/null +grep -Fx -- "--dynamic-bitrate=on" "$MOCK_ARGS" >/dev/null +grep -F "format=h264,width=1600,height=1200,fps=30" "$MOCK_ARGS" >/dev/null +if grep -Eq '^--enable-(imu|audio)$|^--audio-output=' "$MOCK_ARGS"; then + echo "Config/CLI stream overrides were not applied" >&2 + exit 1 +fi + +CONFIG_NO_AUDIO_FILE="$TMPDIR_TEST/record_no_audio.toml" +CONFIG_NO_AUDIO_RUN="$TMPDIR_TEST/config_no_audio_recording" +sed 's/^audio = true$/audio = false/' "$CONFIG_FILE" > "$CONFIG_NO_AUDIO_FILE" +expect_failure "$SCRIPT" record --config "$CONFIG_NO_AUDIO_FILE" --duration 1 \ + --output "$CONFIG_NO_AUDIO_RUN" +grep -Fx -- "--mcap-filename=$CONFIG_NO_AUDIO_RUN/metadata.mcap" "$MOCK_ARGS" >/dev/null +if grep -Eq '^--enable-audio$|^--audio-output=' "$MOCK_ARGS"; then + echo "Config with audio=false must not request audio" >&2 + exit 1 +fi + +CONFIG_AUDIO_OVERRIDE_RUN="$TMPDIR_TEST/config_audio_override_recording" +expect_failure "$SCRIPT" record --config "$CONFIG_NO_AUDIO_FILE" --duration 1 \ + --output "$CONFIG_AUDIO_OVERRIDE_RUN" --audio +grep -Fx -- "--audio-output=$CONFIG_AUDIO_OVERRIDE_RUN/Audio.wav" "$MOCK_ARGS" >/dev/null + +INVALID_CONFIG="$TMPDIR_TEST/invalid.toml" +cat > "$INVALID_CONFIG" <<'EOF' +[record] +unknown_option = true +EOF +expect_failure "$SCRIPT" record --config "$INVALID_CONFIG" +expect_failure "$SCRIPT" record --config "$TMPDIR_TEST/missing.toml" + +SIGNAL_PLUGIN="$TMPDIR_TEST/signal_plugin" +cat > "$SIGNAL_PLUGIN" <<'EOF' +#!/usr/bin/env bash +if [[ "$1" == "--list-capabilities" ]]; then + printf '%s\n' 'Sensor LeftColor' 'Sensor RightColor' + exit 0 +fi +trap 'echo "controlled shutdown failed"; exit 7' INT +while true; do + sleep 0.1 +done +EOF +chmod +x "$SIGNAL_PLUGIN" +set +e +SIGNAL_OUTPUT="$($SCRIPT record --plugin "$SIGNAL_PLUGIN" --duration 1 \ + --output "$TMPDIR_TEST/signal_recording" --no-imu --no-audio 2>&1)" +SIGNAL_STATUS=$? +set -e +[[ "$SIGNAL_STATUS" -ne 0 ]] +grep -F "Capture failed with status 7" <<< "$SIGNAL_OUTPUT" >/dev/null + +RUN_EMBEDDED="$TMPDIR_TEST/embedded" +expect_failure "$SCRIPT" record --plugin "$MOCK_PLUGIN" --duration 1 --output "$RUN_EMBEDDED" --mcap-media embedded +grep -Fx -- "--mcap-media=embedded" "$MOCK_ARGS" >/dev/null +if grep -Eq '^--enable-audio$|^--audio-output=' "$MOCK_ARGS"; then + echo "Default embedded capture must not request audio" >&2 + exit 1 +fi + +RUN_EMBEDDED_AUDIO="$TMPDIR_TEST/embedded_audio" +expect_failure "$SCRIPT" record --plugin "$MOCK_PLUGIN" --duration 1 \ + --output "$RUN_EMBEDDED_AUDIO" --mcap-media embedded --audio +grep -Fx -- "--enable-audio" "$MOCK_ARGS" >/dev/null +if grep -q '^--audio-output=' "$MOCK_ARGS"; then + echo "Embedded capture must not request a WAV sidecar by default" >&2 + exit 1 +fi + +PACKAGE_DIR="$TMPDIR_TEST/package/plugins/ego_camera" +MOCK_BIN="$TMPDIR_TEST/bin" +mkdir -p "$PACKAGE_DIR/extensions" "$MOCK_BIN" +cp "$SCRIPT" "$PACKAGE_DIR/ego.sh" +cp -r "$(dirname "$SCRIPT")/tools" "$PACKAGE_DIR/tools" +chmod +x "$PACKAGE_DIR/ego.sh" +touch "$PACKAGE_DIR/libOrbbecSDK.so.2" "$PACKAGE_DIR/OrbbecSDKConfig.xml" + +cat > "$PACKAGE_DIR/ego_camera_plugin" <<'EOF' +#!/usr/bin/env bash +if [[ "$1" == "--help" ]]; then + exit 0 +fi +exit 1 +EOF +cat > "$PACKAGE_DIR/ego_mcap_export_media" <<'EOF' +#!/usr/bin/env bash +mkdir -p "$2" +printf '\0' > "$2/ColorLeft.h264" +printf '\0' > "$2/ColorRight.h264" +if [[ -n "${MOCK_EXPORTER_USED:-}" ]]; then + printf '%s\n' "$0" > "$MOCK_EXPORTER_USED" +fi +EOF +chmod +x "$PACKAGE_DIR/ego_camera_plugin" "$PACKAGE_DIR/ego_mcap_export_media" + +for command_name in python3 ffmpeg ffprobe; do + cat > "$MOCK_BIN/$command_name" <<'EOF' +#!/usr/bin/env bash +exit 0 +EOF + chmod +x "$MOCK_BIN/$command_name" +done + +PACKAGE_RUN="$TMPDIR_TEST/package_run" +mkdir -p "$PACKAGE_RUN" +printf '\0' > "$PACKAGE_RUN/metadata.mcap" +PATH="$MOCK_BIN:$PATH" "$PACKAGE_DIR/ego.sh" doctor >/dev/null +PATH="$MOCK_BIN:$PATH" "$PACKAGE_DIR/ego.sh" export-media "$PACKAGE_RUN" >/dev/null +test -f "$PACKAGE_RUN/exported/ColorLeft.h264" +expect_failure env PATH="$MOCK_BIN:$PATH" "$PACKAGE_DIR/ego.sh" export-media "$PACKAGE_RUN" + +INSTALLED_VERIFY_RUN="$TMPDIR_TEST/installed_verify" +INSTALLED_EXPORTER_USED="$TMPDIR_TEST/installed_exporter_used" +mkdir -p "$INSTALLED_VERIFY_RUN" +printf '\0' > "$INSTALLED_VERIFY_RUN/metadata.mcap" +expect_failure env PATH="$MOCK_BIN:$PATH" MOCK_EXPORTER_USED="$INSTALLED_EXPORTER_USED" \ + "$SCRIPT" verify "$INSTALLED_VERIFY_RUN" --media-mode embedded \ + --plugin "$PACKAGE_DIR/ego_camera_plugin" +grep -Fx "$PACKAGE_DIR/ego_mcap_export_media" "$INSTALLED_EXPORTER_USED" >/dev/null + +BUILD_PLUGIN_DIR="$TMPDIR_TEST/build/src/plugins/ego/app" +BUILD_EXPORTER_DIR="$TMPDIR_TEST/build/src/plugins/ego/mcap/export_media_tool" +BUILD_VERIFY_RUN="$TMPDIR_TEST/build_verify" +BUILD_EXPORTER_USED="$TMPDIR_TEST/build_exporter_used" +mkdir -p "$BUILD_PLUGIN_DIR" "$BUILD_EXPORTER_DIR" "$BUILD_VERIFY_RUN" +cp "$PACKAGE_DIR/ego_camera_plugin" "$BUILD_PLUGIN_DIR/ego_camera_plugin" +cp "$PACKAGE_DIR/ego_mcap_export_media" "$BUILD_EXPORTER_DIR/ego_mcap_export_media" +printf '\0' > "$BUILD_VERIFY_RUN/metadata.mcap" +expect_failure env PATH="$MOCK_BIN:$PATH" MOCK_EXPORTER_USED="$BUILD_EXPORTER_USED" \ + "$SCRIPT" verify "$BUILD_VERIFY_RUN" --media-mode embedded \ + --plugin "$BUILD_PLUGIN_DIR/ego_camera_plugin" +grep -Fx "$BUILD_EXPORTER_DIR/ego_mcap_export_media" "$BUILD_EXPORTER_USED" >/dev/null + +MISSING_PLUGIN_DIR="$TMPDIR_TEST/missing_build/src/plugins/ego/app" +MISSING_VERIFY_RUN="$TMPDIR_TEST/missing_build_verify" +mkdir -p "$MISSING_PLUGIN_DIR" "$MISSING_VERIFY_RUN" +cp "$PACKAGE_DIR/ego_camera_plugin" "$MISSING_PLUGIN_DIR/ego_camera_plugin" +printf '\0' > "$MISSING_VERIFY_RUN/metadata.mcap" +set +e +MISSING_EXPORTER_OUTPUT="$(PATH="$MOCK_BIN:$PATH" "$SCRIPT" verify "$MISSING_VERIFY_RUN" \ + --media-mode embedded --plugin "$MISSING_PLUGIN_DIR/ego_camera_plugin" 2>&1)" +MISSING_EXPORTER_STATUS=$? +set -e +[[ "$MISSING_EXPORTER_STATUS" -ne 0 && "$MISSING_EXPORTER_STATUS" -ne 127 ]] +grep -F "No embedded-media exporter exists beside plugin or in its build tree" \ + <<< "$MISSING_EXPORTER_OUTPUT" >/dev/null + +PARTIAL_RUN="$TMPDIR_TEST/partial_run" +mkdir -p "$PARTIAL_RUN" +printf '\0' > "$PARTIAL_RUN/metadata.mcap" +touch "$PARTIAL_RUN/ego_media_fragment.mcap.partial" +expect_failure "$SCRIPT" verify "$PARTIAL_RUN" + +VERIFY_BIN="$TMPDIR_TEST/verify_bin" +VERIFY_RUN="$TMPDIR_TEST/verify_with_sidecars" +VERIFY_EXPORTER_USED="$TMPDIR_TEST/verify_sidecar_exporter_used" +VERIFY_FFMPEG_ARGS="$TMPDIR_TEST/verify_ffmpeg_args" +mkdir -p "$VERIFY_BIN" "$VERIFY_RUN/raw" "$VERIFY_RUN/logs" +cp "$MOCK_BIN/python3" "$VERIFY_BIN/" +cat > "$VERIFY_BIN/ffmpeg" <<'EOF' +#!/usr/bin/env bash +printf '%s\n' "$@" >> "$VERIFY_FFMPEG_ARGS" +if [[ "${VERIFY_ZERO_EXIT_ERROR:-0}" == 1 ]]; then + echo 'decoder error with a zero exit status' >&2 +fi +printf '%s\n' 'frame=1' 'progress=end' +exit 0 +EOF +cat > "$VERIFY_BIN/ffprobe" <<'EOF' +#!/usr/bin/env bash +for argument in "$@"; do + if [[ "$argument" == "-count_frames" ]]; then + echo 1 + exit 0 + fi +done +echo 'h264,1600,1300,30/1' +EOF +chmod +x "$VERIFY_BIN/ffmpeg" "$VERIFY_BIN/ffprobe" +printf '\0' > "$VERIFY_RUN/metadata.mcap" +printf '\0' > "$VERIFY_RUN/raw/ColorLeft.h264" +printf '\0' > "$VERIFY_RUN/raw/ColorRight.h264" +printf '%s\n' \ + ' ColorLeft: 1 frames, 1 bytes, 0 sequence gaps' \ + ' ColorRight: 1 frames, 1 bytes, 0 sequence gaps' \ + ' IMU: accel=0 gyro=0 samples; audio=0 samples; queue_peak=0 dropped=0 video_frame_sets_dropped=0' \ + > "$VERIFY_RUN/logs/capture.log" +PATH="$VERIFY_BIN:$PATH" MOCK_EXPORTER_USED="$VERIFY_EXPORTER_USED" \ + VERIFY_FFMPEG_ARGS="$VERIFY_FFMPEG_ARGS" \ + "$SCRIPT" verify "$VERIFY_RUN" --media-mode embedded \ + --plugin "$PACKAGE_DIR/ego_camera_plugin" >/dev/null 2>&1 +grep -Fx "$PACKAGE_DIR/ego_mcap_export_media" "$VERIFY_EXPORTER_USED" >/dev/null +test -z "$(find "$VERIFY_RUN" -maxdepth 1 -name '.verify_export.*' -print -quit)" +VERIFY_ZERO_ERROR_RUN="$TMPDIR_TEST/verify_zero_exit_error" +cp -r "$VERIFY_RUN" "$VERIFY_ZERO_ERROR_RUN" +expect_failure env PATH="$VERIFY_BIN:$PATH" MOCK_EXPORTER_USED="$VERIFY_EXPORTER_USED" \ + VERIFY_FFMPEG_ARGS="$VERIFY_FFMPEG_ARGS" VERIFY_ZERO_EXIT_ERROR=1 \ + "$SCRIPT" verify "$VERIFY_ZERO_ERROR_RUN" --media-mode embedded \ + --plugin "$PACKAGE_DIR/ego_camera_plugin" +[[ ! -e "$VERIFY_ZERO_ERROR_RUN/verification_report.txt" ]] +grep -F 'decoder error with a zero exit status' "$VERIFY_ZERO_ERROR_RUN/verification_report.failed.txt" >/dev/null +[[ -z "$(find "$VERIFY_ZERO_ERROR_RUN" -maxdepth 1 -name '.verify_export.*' -print -quit)" ]] +test -s "$VERIFY_RUN/verification_report.txt" +grep -F "Verification passed:" "$VERIFY_RUN/verification_report.txt" >/dev/null +grep -E '^ColorLeft SHA-256: [0-9a-f]{64}$' "$VERIFY_RUN/verification_report.txt" >/dev/null +grep -E '^ColorRight SHA-256: [0-9a-f]{64}$' "$VERIFY_RUN/verification_report.txt" >/dev/null +grep -Fx -- "-xerror" "$VERIFY_FFMPEG_ARGS" >/dev/null +grep -Fx -- "-err_detect" "$VERIFY_FFMPEG_ARGS" >/dev/null +grep -Fx -- "explode" "$VERIFY_FFMPEG_ARGS" >/dev/null + +VERIFY_FAIL_BIN="$TMPDIR_TEST/verify_fail_bin" +VERIFY_FAIL_RUN="$TMPDIR_TEST/verify_export_cleanup" +mkdir -p "$VERIFY_FAIL_BIN" "$VERIFY_FAIL_RUN/logs" +cp "$MOCK_BIN/python3" "$VERIFY_BIN/ffprobe" "$VERIFY_FAIL_BIN/" +cat > "$VERIFY_FAIL_BIN/ffmpeg" <<'EOF' +#!/usr/bin/env bash +exit 9 +EOF +chmod +x "$VERIFY_FAIL_BIN/ffmpeg" +printf '\0' > "$VERIFY_FAIL_RUN/metadata.mcap" +printf '%s\n' \ + ' ColorLeft: 1 frames, 1 bytes, 0 sequence gaps' \ + ' ColorRight: 1 frames, 1 bytes, 0 sequence gaps' \ + ' IMU: accel=0 gyro=0 samples; audio=0 samples; queue_peak=0 dropped=0 video_frame_sets_dropped=0' \ + > "$VERIFY_FAIL_RUN/logs/capture.log" +expect_failure env PATH="$VERIFY_FAIL_BIN:$PATH" \ + "$SCRIPT" verify "$VERIFY_FAIL_RUN" --media-mode embedded \ + --plugin "$PACKAGE_DIR/ego_camera_plugin" +test -z "$(find "$VERIFY_FAIL_RUN" -maxdepth 1 -name '.verify_export.*' -print -quit)" +test -s "$VERIFY_FAIL_RUN/verification_report.failed.txt" + +VERIFY_HASH_FAIL_BIN="$TMPDIR_TEST/verify_hash_fail_bin" +VERIFY_HASH_FAIL_RUN="$TMPDIR_TEST/verify_hash_failure" +mkdir -p "$VERIFY_HASH_FAIL_BIN" +cp "$VERIFY_BIN/"* "$VERIFY_HASH_FAIL_BIN/" +cp -r "$VERIFY_RUN" "$VERIFY_HASH_FAIL_RUN" +rm "$VERIFY_HASH_FAIL_RUN/verification_report.txt" +cat > "$VERIFY_HASH_FAIL_BIN/sha256sum" <<'EOF' +#!/usr/bin/env bash +exit 7 +EOF +chmod +x "$VERIFY_HASH_FAIL_BIN/sha256sum" +expect_failure env PATH="$VERIFY_HASH_FAIL_BIN:$PATH" VERIFY_FFMPEG_ARGS="$VERIFY_FFMPEG_ARGS" \ + "$SCRIPT" verify "$VERIFY_HASH_FAIL_RUN" --media-mode embedded \ + --plugin "$PACKAGE_DIR/ego_camera_plugin" +grep -F "Could not hash" "$VERIFY_HASH_FAIL_RUN/verification_report.failed.txt" >/dev/null +[[ ! -e "$VERIFY_HASH_FAIL_RUN/verification_report.txt" ]] +[[ -z "$(find "$VERIFY_HASH_FAIL_RUN" -maxdepth 1 -name '.verify_export.*' -print -quit)" ]] + +AUX_MCAP_MODULES="$TMPDIR_TEST/aux_mcap_modules" +AUX_VERIFY_BIN="$TMPDIR_TEST/aux_verify_bin" +mkdir -p "$AUX_MCAP_MODULES/mcap" "$AUX_VERIFY_BIN" +touch "$AUX_MCAP_MODULES/mcap/__init__.py" +cat > "$AUX_MCAP_MODULES/mcap/reader.py" <<'PY' +import json +import struct +from types import SimpleNamespace + + +def _record(field_offsets, object_size, timestamp): + vtable_size = 4 + 2 * len(field_offsets) + vtable = 48 + table = (vtable + vtable_size + 3) & ~3 + data = bytearray(table + object_size) + struct.pack_into("= 15 else 0 + sequence = frame_index - 15 if capture_epoch else frame_index + timestamp = base + sequence * video_period + log_time = base + frame_index * video_period + yield from self._message( + "ego_metadata/ColorLeft", _video(sequence, timestamp, capture_epoch, video_fps), log_time + ) + yield from self._message( + "ego_metadata/ColorRight", _video(sequence, timestamp, capture_epoch, video_fps), log_time + ) + if mode == "embedded": + yield from self._message( + "ego_media/ColorLeft", _video(sequence, timestamp, capture_epoch, video_fps), log_time + ) + yield from self._message( + "ego_media/ColorRight", _video(sequence, timestamp, capture_epoch, video_fps), log_time + ) + yield from self._message("ego_calibration/Calibration", b"fixture", base) + yield from self._message("ego_device/DeviceState", b"fixture", base) + + if scenario in ("valid", "empty_imu", "bad_imu_cadence"): + sample_index = 0 + sample_period = 2_000_000 if scenario == "bad_imu_cadence" else 1_000_000 + for sequence in range(40): + timestamps = [base + (sample_index + offset) * sample_period for offset in range(25)] + sample_index += len(timestamps) + accel_timestamps = [] if scenario == "empty_imu" and sequence == 0 else timestamps + yield from self._message( + "ego_imu/Accel", _imu(0, sequence, 1000, accel_timestamps), timestamps[-1] + ) + yield from self._message("ego_imu/Gyro", _imu(1, sequence, 1000, timestamps), timestamps[-1]) + + if scenario in ( + "valid", + "sparse_audio", + "sparse_pcm", + "empty_pcm", + "bad_audio_cadence", + "bad_audio_format", + ): + chunks = 2 if scenario == "sparse_audio" else 32 + sample_count = 1 if scenario == "sparse_audio" else 1500 + cadence_scale = 2 if scenario == "bad_audio_cadence" else 1 + sample_format = 1 if scenario == "bad_audio_format" else 0 + for sequence in range(chunks): + timestamp = base + cadence_scale * sequence * sample_count * 1_000_000_000 // 48_000 + yield from self._message( + "ego_audio/Audio", _audio(sequence, timestamp, sample_count, sample_format), timestamp + ) + if mode == "embedded" and scenario != "sparse_pcm": + pcm_bytes = 0 if scenario == "empty_pcm" and sequence == 0 else None + yield from self._message( + "ego_media/Audio", + _pcm_audio(sequence, timestamp, sample_count, pcm_bytes, sample_format), + timestamp, + ) + if mode == "embedded" and scenario == "sparse_pcm": + for sequence in range(2): + timestamp = base + sequence * 1_000_000_000 // 48_000 + yield from self._message( + "ego_media/Audio", _pcm_audio(sequence, timestamp, 1), timestamp + ) + + @staticmethod + def _message(topic, data, timestamp): + yield ( + None, + SimpleNamespace(topic=topic), + SimpleNamespace(data=data, log_time=timestamp), + ) + + +def make_reader(stream): + return _Reader(stream) +PY + +cat > "$AUX_VERIFY_BIN/ffmpeg" <<'EOF' +#!/usr/bin/env bash +printf '%s\n' 'frame=31' 'progress=end' +exit 0 +EOF +cat > "$AUX_VERIFY_BIN/ffprobe" <<'EOF' +#!/usr/bin/env bash +for argument in "$@"; do + if [[ "$argument" == "-count_frames" ]]; then + echo 31 + exit 0 + fi + if [[ "$argument" == *.wav ]]; then + printf '%s\n' 'codec_name=pcm_s16le' 'sample_rate=48000' 'channels=1' 'bits_per_sample=16' + exit 0 + fi +done +echo 'h264,1600,1300,60/1' +EOF +chmod +x "$AUX_VERIFY_BIN/ffmpeg" "$AUX_VERIFY_BIN/ffprobe" + +make_aux_verify_run() { + local run_dir="$1" + local scenario="$2" + local mode="$3" + mkdir -p "$run_dir/raw" "$run_dir/logs" + printf '{"scenario":"%s","mode":"%s"}\n' "$scenario" "$mode" > "$run_dir/metadata.mcap" + printf '\0' > "$run_dir/raw/ColorLeft.h264" + printf '\0' > "$run_dir/raw/ColorRight.h264" + printf '\0' > "$run_dir/Audio.wav" + cat > "$run_dir/requested_profile.json" <<'EOF' +{"version":1,"streams":{"ColorLeft":{"format":"h264","width":1600,"height":1300,"fps":30},"ColorRight":{"format":"h264","width":1600,"height":1300,"fps":30}}} +EOF + printf '%s\n' \ + ' ColorLeft: 31 frames, 1 bytes, 0 sequence gaps' \ + ' ColorRight: 31 frames, 1 bytes, 0 sequence gaps' \ + ' IMU: accel=1000 gyro=1000 samples; audio=48000 samples; queue_peak=1 dropped=0 video_frame_sets_dropped=0' \ + > "$run_dir/logs/capture.log" +} + +AUX_VALID_RUN="$TMPDIR_TEST/aux_valid" +make_aux_verify_run "$AUX_VALID_RUN" valid metadata-only +PYTHONPATH="$AUX_MCAP_MODULES" PATH="$AUX_VERIFY_BIN:$PATH" \ + "$SCRIPT" verify "$AUX_VALID_RUN" --media-mode metadata-only --expect-imu --expect-audio \ + >/dev/null 2>&1 +grep -F 'raw_device_fps=30.000' "$AUX_VALID_RUN/verification_report.txt" >/dev/null +grep -F 'ffprobe_rate): h264,1600,1300,60/1' "$AUX_VALID_RUN/verification_report.txt" >/dev/null + +for scenario in bad_video_profile bad_video_cadence; do + run="$TMPDIR_TEST/$scenario" + make_aux_verify_run "$run" "$scenario" metadata-only + expect_failure env PYTHONPATH="$AUX_MCAP_MODULES" PATH="$AUX_VERIFY_BIN:$PATH" \ + "$SCRIPT" verify "$run" --media-mode metadata-only + if [[ "$scenario" == bad_video_profile ]]; then + grep -F 'differs from requested' "$run/verification_report.failed.txt" >/dev/null + else + grep -F 'raw-device cadence is 60.000 FPS' "$run/verification_report.failed.txt" >/dev/null + fi +done +expect_failure python3 "$(dirname "$SCRIPT")/tools/verify_profile.py" \ + "$AUX_VALID_RUN/requested_profile.json" h264,1600,1300,0/1 h264,1600,1300,60/1 +expect_failure python3 "$(dirname "$SCRIPT")/tools/verify_profile.py" \ + "$AUX_VALID_RUN/requested_profile.json" h264,1600,1200,60/1 h264,1600,1300,60/1 +expect_failure python3 "$(dirname "$SCRIPT")/tools/verify_profile.py" \ + "$AUX_VALID_RUN/requested_profile.json" hevc,1600,1300,60/1 h264,1600,1300,60/1 + +AUX_RECONNECT_RUN="$TMPDIR_TEST/aux_reconnect" +make_aux_verify_run "$AUX_RECONNECT_RUN" reconnect metadata-only +PYTHONPATH="$AUX_MCAP_MODULES" PATH="$AUX_VERIFY_BIN:$PATH" \ + "$SCRIPT" verify "$AUX_RECONNECT_RUN" --media-mode metadata-only >/dev/null 2>&1 + +AUX_EMPTY_IMU_RUN="$TMPDIR_TEST/aux_empty_imu" +make_aux_verify_run "$AUX_EMPTY_IMU_RUN" empty_imu metadata-only +set +e +AUX_EMPTY_IMU_OUTPUT="$(PYTHONPATH="$AUX_MCAP_MODULES" PATH="$AUX_VERIFY_BIN:$PATH" \ + "$SCRIPT" verify "$AUX_EMPTY_IMU_RUN" --media-mode metadata-only --expect-imu 2>&1)" +AUX_EMPTY_IMU_STATUS=$? +set -e +[[ "$AUX_EMPTY_IMU_STATUS" -ne 0 ]] +grep -F "contains an empty IMU batch" <<< "$AUX_EMPTY_IMU_OUTPUT" >/dev/null + +AUX_BAD_IMU_CADENCE_RUN="$TMPDIR_TEST/aux_bad_imu_cadence" +make_aux_verify_run "$AUX_BAD_IMU_CADENCE_RUN" bad_imu_cadence metadata-only +set +e +AUX_BAD_IMU_CADENCE_OUTPUT="$(PYTHONPATH="$AUX_MCAP_MODULES" PATH="$AUX_VERIFY_BIN:$PATH" \ + "$SCRIPT" verify "$AUX_BAD_IMU_CADENCE_RUN" --media-mode metadata-only --expect-imu 2>&1)" +AUX_BAD_IMU_CADENCE_STATUS=$? +set -e +[[ "$AUX_BAD_IMU_CADENCE_STATUS" -ne 0 ]] +grep -F "raw-device cadence is 500.000 Hz" <<< "$AUX_BAD_IMU_CADENCE_OUTPUT" >/dev/null + +AUX_SPARSE_AUDIO_RUN="$TMPDIR_TEST/aux_sparse_audio" +make_aux_verify_run "$AUX_SPARSE_AUDIO_RUN" sparse_audio metadata-only +set +e +AUX_SPARSE_AUDIO_OUTPUT="$(PYTHONPATH="$AUX_MCAP_MODULES" PATH="$AUX_VERIFY_BIN:$PATH" \ + "$SCRIPT" verify "$AUX_SPARSE_AUDIO_RUN" --media-mode metadata-only --expect-audio 2>&1)" +AUX_SPARSE_AUDIO_STATUS=$? +set -e +[[ "$AUX_SPARSE_AUDIO_STATUS" -ne 0 ]] +grep -F "contains only" <<< "$AUX_SPARSE_AUDIO_OUTPUT" >/dev/null + +AUX_BAD_AUDIO_CADENCE_RUN="$TMPDIR_TEST/aux_bad_audio_cadence" +make_aux_verify_run "$AUX_BAD_AUDIO_CADENCE_RUN" bad_audio_cadence metadata-only +set +e +AUX_BAD_AUDIO_CADENCE_OUTPUT="$(PYTHONPATH="$AUX_MCAP_MODULES" PATH="$AUX_VERIFY_BIN:$PATH" \ + "$SCRIPT" verify "$AUX_BAD_AUDIO_CADENCE_RUN" --media-mode metadata-only --expect-audio 2>&1)" +AUX_BAD_AUDIO_CADENCE_STATUS=$? +set -e +[[ "$AUX_BAD_AUDIO_CADENCE_STATUS" -ne 0 ]] +grep -F "raw-device cadence is 24000.000 Hz" <<< "$AUX_BAD_AUDIO_CADENCE_OUTPUT" >/dev/null + +AUX_BAD_AUDIO_FORMAT_RUN="$TMPDIR_TEST/aux_bad_audio_format" +make_aux_verify_run "$AUX_BAD_AUDIO_FORMAT_RUN" bad_audio_format metadata-only +set +e +AUX_BAD_AUDIO_FORMAT_OUTPUT="$(PYTHONPATH="$AUX_MCAP_MODULES" PATH="$AUX_VERIFY_BIN:$PATH" \ + "$SCRIPT" verify "$AUX_BAD_AUDIO_FORMAT_RUN" --media-mode metadata-only --expect-audio 2>&1)" +AUX_BAD_AUDIO_FORMAT_STATUS=$? +set -e +[[ "$AUX_BAD_AUDIO_FORMAT_STATUS" -ne 0 ]] +grep -F "expected (48000, 1, 16, S16LE)" <<< "$AUX_BAD_AUDIO_FORMAT_OUTPUT" >/dev/null + +AUX_SPARSE_PCM_RUN="$TMPDIR_TEST/aux_sparse_pcm" +make_aux_verify_run "$AUX_SPARSE_PCM_RUN" sparse_pcm embedded +set +e +AUX_SPARSE_PCM_OUTPUT="$(PYTHONPATH="$AUX_MCAP_MODULES" PATH="$AUX_VERIFY_BIN:$PATH" \ + "$SCRIPT" verify "$AUX_SPARSE_PCM_RUN" --media-mode embedded --expect-audio 2>&1)" +AUX_SPARSE_PCM_STATUS=$? +set -e +[[ "$AUX_SPARSE_PCM_STATUS" -ne 0 ]] +grep -F "ego_media/Audio epoch 0 contains only" <<< "$AUX_SPARSE_PCM_OUTPUT" >/dev/null + +AUX_EMPTY_PCM_RUN="$TMPDIR_TEST/aux_empty_pcm" +make_aux_verify_run "$AUX_EMPTY_PCM_RUN" empty_pcm embedded +set +e +AUX_EMPTY_PCM_OUTPUT="$(PYTHONPATH="$AUX_MCAP_MODULES" PATH="$AUX_VERIFY_BIN:$PATH" \ + "$SCRIPT" verify "$AUX_EMPTY_PCM_RUN" --media-mode embedded --expect-audio 2>&1)" +AUX_EMPTY_PCM_STATUS=$? +set -e +[[ "$AUX_EMPTY_PCM_STATUS" -ne 0 ]] +grep -F "PCM byte count 0 does not match sample_count/profile" <<< "$AUX_EMPTY_PCM_OUTPUT" >/dev/null + +rm "$PACKAGE_DIR/ego_mcap_export_media" +expect_failure env PATH="$MOCK_BIN:$PATH" "$PACKAGE_DIR/ego.sh" doctor + +DIAGNOSTIC_RUN="$TMPDIR_TEST/diagnostic_off" +expect_failure "$SCRIPT" record --plugin "$MOCK_PLUGIN" --duration 1 --output "$DIAGNOSTIC_RUN" --record-device-diagnostics=off +grep -Fx -- "--record-device-diagnostics=off" "$MOCK_ARGS" >/dev/null +grep -F '"record_device_diagnostics": "off"' "$DIAGNOSTIC_RUN/requested_profile.json" >/dev/null +expect_failure "$SCRIPT" record --record-device-diagnostics=invalid diff --git a/tests/cpp/plugins/ego/test_time_alignment.cpp b/tests/cpp/plugins/ego/test_time_alignment.cpp new file mode 100644 index 0000000000..a6e6d88b87 --- /dev/null +++ b/tests/cpp/plugins/ego/test_time_alignment.cpp @@ -0,0 +1,346 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace +{ +const std::string kProfile = R"({ +"version":1,"scope":"capture_profile","status":"validated","model_id":"test-relative-only", +"max_capture_duration_ns":900000000000, +"reference_stream":"left_rgb","identity":{"serial":"S1","firmware":"1.1.1","sdk_version":"2.9.0", +"sdk_binary_sha256":"aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa"}, +"profile":{"video":{"format":"h264","width":1600,"height":1300,"fps":30},"imu_rate_hz":1000, +"audio":{"sample_rate_hz":48000,"channels":1,"bits_per_sample":16}}, +"streams":{"left_rgb":{"offset_ns":0,"scale":1},"right_rgb":{"offset_ns":-1000,"scale":1}, +"accel":{"offset_ns":-2000,"scale":1},"gyro":{"offset_ns":-3000,"scale":1}}, +"validation":{"method":"synthetic unit fixture, not physical evidence","held_out_duration_ns":1800000000000, +"max_abs_error_ns":1000,"uncertainty_ns":1000,"evidence":["synthetic"]}})"; + +plugins::ego::TimingIdentity identity() +{ + return { "S1", "1.1.1", "2.9.0", "h264", 1600, 1300, 30, 1000, 48000, 1, 16, true, std::string(64, 'a') }; +} + +struct ProfileFile +{ + std::filesystem::path path = + std::filesystem::temp_directory_path() / + ("ego_time_profile_" + std::to_string(std::chrono::steady_clock::now().time_since_epoch().count()) + ".json"); + explicit ProfileFile(const std::string& text = kProfile) + { + std::ofstream output(path); + output << text; + } + ~ProfileFile() + { + std::error_code ignored; + std::filesystem::remove(path, ignored); + } +}; + +std::string replace(std::string text, const std::string& before, const std::string& after) +{ + const auto position = text.find(before); + REQUIRE(position != std::string::npos); + text.replace(position, before.size(), after); + return text; +} +} // namespace + +TEST_CASE("EGO SDK timestamps reject conversion overflow without losing the raw value", "[ego][timing]") +{ + constexpr auto maximum_us = static_cast(std::numeric_limits::max() / 1000); + CHECK(plugins::ego::sdk_timestamp_ns(0) == 0); + CHECK(plugins::ego::sdk_timestamp_ns(123456789) == 123456789000); + CHECK(plugins::ego::sdk_timestamp_ns(maximum_us) == static_cast(maximum_us) * 1000); + for (const auto invalid_us : { maximum_us + 1, std::numeric_limits::max() }) + { + bool rejected = false; + try + { + plugins::ego::sdk_timestamp_ns(invalid_us); + } + catch (const std::overflow_error& error) + { + rejected = true; + CHECK(std::string(error.what()) == + "EGO SDK timestamp_us=" + std::to_string(invalid_us) + " exceeds int64 nanosecond range"); + } + CHECK(rejected); + } +} + +TEST_CASE("EGO missing timing model preserves callback times and unverified provenance", "[ego][timing]") +{ + plugins::ego::TimeAlignment alignment; + alignment.configure(identity()); + alignment.begin_epoch(0); + CHECK_FALSE(alignment.observe_left(100, 1000)); + CHECK(alignment.sample_time("gyro", 500, 9000) == 9000); + const auto json = alignment.snapshot(); + CHECK(json.find("\"status\":\"unverified\"") != std::string::npos); + CHECK(json.find("host_arrival_fallback") != std::string::npos); + CHECK(json.find("\"serial\":\"S1\"") != std::string::npos); + CHECK(json.find("\"sdk_binary_sha256\":\"" + std::string(64, 'a') + "\"") != std::string::npos); + CHECK_FALSE(alignment.has_update()); +} + +TEST_CASE("EGO relative profile uses one estimated host anchor and resets it on recovery", "[ego][timing]") +{ + ProfileFile file; + plugins::ego::TimeAlignment alignment(file.path.string()); + alignment.configure(identity()); + alignment.begin_epoch(0); + CHECK(alignment.sample_time("gyro", 1000, 10000) == 10000); + REQUIRE(alignment.observe_left(100000, 1000000)); + CHECK_FALSE(alignment.observe_left(200000, 9000000)); + CHECK(alignment.sample_time("left_rgb", 200000, 8000000) == 1100000); + CHECK(alignment.sample_time("right_rgb", 201000, 9000000) == 1100000); + CHECK(alignment.sample_time("accel", 202000, 1200000) == 1100000); + CHECK(alignment.sample_time("gyro", 203000, 1300000) == 1100000); + CHECK(alignment.sample_time("audio", 203000, 1400000) == 1400000); + const auto json = alignment.snapshot(); + CHECK(json.find("left_arrival_estimate") != std::string::npos); + CHECK(json.find("\"host_absolute_accuracy\":\"unverified\"") != std::string::npos); + CHECK(json.find("first_mapped_raw_ns") != std::string::npos); + alignment.begin_epoch(1); + CHECK(alignment.sample_time("gyro", 500, 2000000) == 2000000); + REQUIRE(alignment.observe_left(1000, 3000000)); + CHECK(alignment.sample_time("gyro", 104000, 3200000) == 3100000); + CHECK_THROWS(alignment.sample_time("gyro", 103000, 3300000)); +} + +TEST_CASE("EGO capture timing input rejects untrusted or recording-bound models", "[ego][timing]") +{ + std::string invalid; + SECTION("offline anchors") + { + invalid = replace(kProfile, "capture_profile", "recording"); + } + SECTION("unverified") + { + invalid = replace(kProfile, "\"status\":\"validated\"", "\"status\":\"unverified\""); + } + SECTION("boolean integer") + { + invalid = replace(kProfile, "\"offset_ns\":0", "\"offset_ns\":false"); + } + SECTION("missing independent evidence") + { + invalid = replace(kProfile, "[\"synthetic\"]", "[]"); + } + SECTION("zero validation duration") + { + invalid = replace(kProfile, "1800000000000", "0"); + } + SECTION("validation shorter than fifteen minutes") + { + invalid = replace(kProfile, "1800000000000", "899999999999"); + } + SECTION("negative scale") + { + invalid = replace(kProfile, "\"scale\":1", "\"scale\":-1"); + } + SECTION("nonunit scale") + { + invalid = replace(kProfile, "\"right_rgb\":{\"offset_ns\":-1000,\"scale\":1}", + "\"right_rgb\":{\"offset_ns\":-1000,\"scale\":1.00001}"); + } + SECTION("missing SDK binary identity") + { + invalid = replace(kProfile, ",\n\"sdk_binary_sha256\":\"" + std::string(64, 'a') + "\"", ""); + } + SECTION("malformed SDK binary digest") + { + invalid = replace(kProfile, std::string(64, 'a'), "not-a-digest"); + } + SECTION("zero allowed capture duration") + { + invalid = replace(kProfile, "\"max_capture_duration_ns\":900000000000", "\"max_capture_duration_ns\":0"); + } + SECTION("allowed capture duration exceeds evidence") + { + invalid = + replace(kProfile, "\"max_capture_duration_ns\":900000000000", "\"max_capture_duration_ns\":1800000000001"); + } + SECTION("duplicate keys") + { + invalid = replace(kProfile, "\"offset_ns\":0", "\"offset_ns\":10,\"offset_ns\":0"); + } + SECTION("validation error exceeds half a frame") + { + invalid = replace(kProfile, "\"max_abs_error_ns\":1000", "\"max_abs_error_ns\":20000000"); + } + SECTION("fractional profile integer") + { + invalid = replace(kProfile, "\"width\":1600", "\"width\":1600.5"); + } + SECTION("oversized") + { + invalid = std::string(8193, ' '); + } + ProfileFile file(invalid); + CHECK_THROWS(plugins::ego::TimeAlignment(file.path.string())); +} + +TEST_CASE("EGO reusable timing validation accepts exactly fifteen minutes", "[ego][timing]") +{ + ProfileFile accepted( + replace(kProfile, "\"held_out_duration_ns\":1800000000000", "\"held_out_duration_ns\":900000000000")); + CHECK_NOTHROW(plugins::ego::validate_time_alignment_profile(accepted.path.string())); + ProfileFile rejected( + replace(kProfile, "\"held_out_duration_ns\":1800000000000", "\"held_out_duration_ns\":899999999999")); + bool was_rejected = false; + try + { + plugins::ego::validate_time_alignment_profile(rejected.path.string()); + } + catch (const std::invalid_argument& error) + { + was_rejected = true; + CHECK(std::string(error.what()) == + "EGO capture profile requires at least 15 minutes of independent held-out validation"); + } + CHECK(was_rejected); +} + +TEST_CASE("EGO capture timing profile binding prevents device and mode substitution", "[ego][timing]") +{ + ProfileFile file; + plugins::ego::TimeAlignment alignment(file.path.string()); + auto selected = identity(); + SECTION("serial") + { + selected.serial = "other"; + } + SECTION("firmware") + { + selected.firmware = "1.1.2"; + } + SECTION("SDK") + { + selected.sdk_version = "2.10.0"; + } + SECTION("SDK binary with the same version") + { + selected.sdk_binary_sha256 = std::string(64, 'b'); + } + SECTION("video profile") + { + selected.width = 1280; + } + SECTION("IMU profile") + { + selected.imu_rate_hz = 400; + } + SECTION("audio profile") + { + selected.audio_channels = 2; + } + CHECK_THROWS(alignment.configure(selected)); +} + +TEST_CASE("EGO loaded SDK provenance resolves the linked version symbol", "[ego][timing]") +{ + const auto digest = plugins::ego::loaded_sdk_binary_sha256(); + CHECK(digest.size() == 64); + CHECK(digest.find_first_not_of("0123456789abcdef") == std::string::npos); + CHECK(plugins::ego::loaded_sdk_binary_sha256() == digest); +} + +TEST_CASE("EGO negative delay begins mapping despite a later arrival fallback", "[ego][timing]") +{ + ProfileFile file(replace(kProfile, "\"gyro\":{\"offset_ns\":-3000", "\"gyro\":{\"offset_ns\":-20000000")); + plugins::ego::TimeAlignment alignment(file.path.string()); + alignment.configure(identity()); + alignment.begin_epoch(0); + CHECK(alignment.sample_time("gyro", 499000000, 1040000000) == 1040000000); + REQUIRE(alignment.observe_left(500000000, 1000000000)); + alignment.snapshot(); + CHECK(alignment.sample_time("gyro", 519000000, 1050000000) == 1050000000); + CHECK_FALSE(alignment.has_update()); + CHECK(alignment.sample_time("gyro", 521000000, 1060000000) == 1001000000); + CHECK(alignment.has_update()); + CHECK(alignment.snapshot().find("\"first_mapped_raw_ns\":{\"gyro\":521000000}") != std::string::npos); + CHECK(alignment.sample_time("gyro", 522000000, 1070000000) == 1002000000); + CHECK_THROWS(alignment.sample_time("gyro", 520500000, 1080000000)); +} + +TEST_CASE("EGO capture profile expiry preserves arrival fallback and a bounded offline interval", "[ego][timing]") +{ + ProfileFile file( + replace(kProfile, "\"max_capture_duration_ns\":900000000000", "\"max_capture_duration_ns\":100000000")); + plugins::ego::TimeAlignment alignment(file.path.string()); + alignment.configure(identity()); + alignment.begin_epoch(0); + REQUIRE(alignment.observe_left(1000000000, 2000000000)); + CHECK(alignment.sample_time("left_rgb", 1050000000, 2100000000) == 2050000000); + CHECK(alignment.sample_time("gyro", 1090003000, 2110000000) == 2090000000); + alignment.snapshot(); + CHECK(alignment.sample_time("gyro", 1120003000, 2200000000) == 2200000000); + CHECK_FALSE(alignment.has_update()); + CHECK(alignment.sample_time("left_rgb", 1100000000, 2250000000) == 2250000000); + CHECK(alignment.has_update()); + const auto expired = alignment.snapshot(); + CHECK(expired.find("\"status\":\"expired\"") != std::string::npos); + CHECK(expired.find("\"valid_from_ns\":1000000000") != std::string::npos); + CHECK(expired.find("\"valid_to_ns\":1100000000") != std::string::npos); + CHECK(expired.find("\"expired_at_reference_ns\":1100000000") != std::string::npos); + CHECK(expired.find("expired_capture_profile_host_arrival_fallback") != std::string::npos); + CHECK(alignment.sample_time("gyro", 1110003000, 2300000000) == 2300000000); + alignment.begin_epoch(1); + REQUIRE(alignment.observe_left(10000000, 3000000000)); + CHECK(alignment.sample_time("gyro", 20003000, 3100000000) == 3010000000); + CHECK(alignment.snapshot().find("\"status\":\"active\"") != std::string::npos); +} + +TEST_CASE("EGO stereo timing profile does not require disabled IMU streams", "[ego][timing]") +{ + const auto stereo = + replace(replace(kProfile, "\"imu_rate_hz\":1000", "\"imu_rate_hz\":0"), + ",\n\"accel\":{\"offset_ns\":-2000,\"scale\":1},\"gyro\":{\"offset_ns\":-3000,\"scale\":1}", ""); + ProfileFile file(stereo); + plugins::ego::TimeAlignment alignment(file.path.string()); + auto selected = identity(); + selected.imu_rate_hz = 0; + REQUIRE_NOTHROW(alignment.configure(selected)); + CHECK(alignment.observe_left(100, 1000)); + CHECK(alignment.sample_time("right_rgb", 2100, 9000) == 2000); + selected.imu_rate_hz = 1000; + CHECK_THROWS(alignment.configure(selected)); +} + +TEST_CASE("EGO audio retained indices expose observed gaps without claiming physical continuity", "[ego][timing]") +{ + plugins::ego::TimeAlignment alignment; + alignment.configure(identity()); + alignment.begin_epoch(0); + const auto first = alignment.audio_timing(1000000000, 480, 48000); + CHECK(first.segment_id == 0); + CHECK(first.first_sample_index == 0); + CHECK_FALSE(first.continuity_known); + CHECK_FALSE(first.discontinuity); + const auto next = alignment.audio_timing(1010000000, 480, 48000); + CHECK(next.segment_id == 0); + CHECK(next.first_sample_index == 480); + const auto gap = alignment.audio_timing(1300000000, 480, 48000); + CHECK(gap.segment_id == 1); + CHECK(gap.first_sample_index == 0); + CHECK(gap.discontinuity); + alignment.begin_epoch(1); + const auto recovered = alignment.audio_timing(500, 480, 48000); + CHECK(recovered.segment_id == 2); + CHECK(recovered.first_sample_index == 0); + CHECK(recovered.discontinuity); + CHECK_FALSE(recovered.continuity_known); + CHECK_THROWS(alignment.audio_timing(600, 0, 48000)); +} diff --git a/tests/cpp/plugins/ego_mcap/CMakeLists.txt b/tests/cpp/plugins/ego_mcap/CMakeLists.txt new file mode 100644 index 0000000000..689d549e1b --- /dev/null +++ b/tests/cpp/plugins/ego_mcap/CMakeLists.txt @@ -0,0 +1,6 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 ORBBEC INC. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +add_executable(ego_mcap_tests test_merge.cpp) +target_link_libraries(ego_mcap_tests PRIVATE ego::mcap_merge Catch2::Catch2WithMain) +catch_discover_tests(ego_mcap_tests ADD_TAGS_AS_LABELS) diff --git a/tests/cpp/plugins/ego_mcap/test_merge.cpp b/tests/cpp/plugins/ego_mcap/test_merge.cpp new file mode 100644 index 0000000000..6657abbb69 --- /dev/null +++ b/tests/cpp/plugins/ego_mcap/test_merge.cpp @@ -0,0 +1,336 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 ORBBEC INC. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#ifdef _WIN32 +# include +#else +# include + +# include +#endif + +namespace +{ +namespace fs = std::filesystem; + +struct TempDirectory +{ + fs::path path; + TempDirectory() + { + static std::atomic counter{ 0 }; +#ifdef _WIN32 + const auto pid = _getpid(); +#else + const auto pid = getpid(); +#endif + path = fs::temp_directory_path() / ("ego_merge_" + std::to_string(pid) + "_" + std::to_string(counter++)); + REQUIRE(fs::create_directory(path)); + } + ~TempDirectory() + { + std::error_code error; + fs::remove_all(path, error); + } +}; + +void write_fixture(const fs::path& file, + const std::string& topic, + const std::string& schema_name, + const std::vector& times, + bool unknown = false, + size_t payload_size = 4, + const std::string& diagnostic_policy = {}) +{ + mcap::McapWriter writer; + mcap::McapWriterOptions options("teleop"); + options.compression = mcap::Compression::None; + options.chunkSize = 1; + REQUIRE(writer.open(file.string(), options).ok()); + mcap::Schema schema(schema_name, "flatbuffer", "opaque schema fixture"); + writer.addSchema(schema); + mcap::Channel channel(topic, "flatbuffer", schema.id, { { "source", file.filename().string() } }); + writer.addChannel(channel); + uint32_t sequence = 0; + for (const auto time : times) + { + std::vector payload(payload_size, std::byte(time)); + mcap::Message message; + message.channelId = channel.id; + message.sequence = ++sequence; + message.logTime = time; + message.publishTime = time + 5; + message.dataSize = payload.size(); + message.data = payload.data(); + REQUIRE(writer.write(message).ok()); + } + mcap::Metadata metadata; + metadata.name = "fixture"; + metadata.metadata = { { "file", file.filename().string() } }; + REQUIRE(writer.write(metadata).ok()); + if (!diagnostic_policy.empty()) + { + mcap::Metadata policy; + policy.name = "ego.recording"; + policy.metadata = { { "record_device_diagnostics", diagnostic_policy }, { "diagnostic_version", "2" } }; + REQUIRE(writer.write(policy).ok()); + } + const std::array attachment_data{ std::byte{ 1 }, std::byte{ 2 }, std::byte{ 3 } }; + mcap::Attachment attachment; + attachment.logTime = 7; + attachment.createTime = 6; + attachment.name = file.filename().string(); + attachment.mediaType = "application/octet-stream"; + attachment.dataSize = attachment_data.size(); + attachment.data = attachment_data.data(); + REQUIRE(writer.write(attachment).ok()); + if (unknown) + { + writer.closeLastChunk(); + const std::array record{ std::byte{ 0x80 } }; + writer.dataSink()->write(record.data(), record.size()); + } + writer.close(); +} + +void fixture_pair(const TempDirectory& temp, size_t payload_size = 4) +{ + write_fixture(temp.path / "session.mcap", "ego_metadata/ColorLeft", "core.EgoFrameMetadataRecord", { 30, 10 }, + false, payload_size); + write_fixture(temp.path / "capture.mcap", "ego_metadata/ColorLeft", "core.EgoFrameMetadataRecord", { 20, 5 }, false, + payload_size); +} +} // namespace + +TEST_CASE("ego_mcap_merge preserves opaque records and host observations", "[ego][unit]") +{ + TempDirectory temp; + fixture_pair(temp); + const auto output = temp.path / "final.mcap"; + plugins::ego::merge_recording(temp.path / "session.mcap", temp.path / "capture.mcap", output); + REQUIRE(fs::exists(temp.path / "session.mcap")); + REQUIRE(fs::exists(temp.path / "capture.mcap")); + REQUIRE_FALSE(fs::exists(output.string() + ".partial")); + mcap::McapReader reader; + REQUIRE(reader.open(output.string()).ok()); + REQUIRE(reader.readSummary(mcap::ReadSummaryMethod::NoFallbackScan).ok()); + REQUIRE(reader.header()->profile == "teleop"); + REQUIRE(reader.statistics()->messageCount == 4); + REQUIRE(reader.statistics()->metadataCount == 3); + REQUIRE(reader.statistics()->attachmentCount == 2); + std::vector times; + std::vector topics; + for (const auto& view : reader.readMessages([](const mcap::Status& status) { REQUIRE(status.ok()); })) + { + times.push_back(view.message.logTime); + topics.push_back(view.channel->topic); + REQUIRE(view.message.publishTime == view.message.logTime + 5); + REQUIRE(view.message.sequence == (view.message.logTime == 30 || view.message.logTime == 20 ? 1 : 2)); + REQUIRE(view.message.dataSize == 4); + for (uint64_t index = 0; index < view.message.dataSize; ++index) + REQUIRE(view.message.data[index] == std::byte(view.message.logTime)); + REQUIRE(view.channel->metadata.at("source") == + (view.channel->topic.starts_with("session_observed/") ? "session.mcap" : "capture.mcap")); + REQUIRE(view.schema->data.size() == std::string("opaque schema fixture").size()); + } + REQUIRE(times == std::vector{ 5, 10, 20, 30 }); + REQUIRE(topics == std::vector{ "ego_metadata/ColorLeft", "session_observed/ego_metadata/ColorLeft", + "ego_metadata/ColorLeft", "session_observed/ego_metadata/ColorLeft" }); + unsigned attachments = 0; + unsigned metadata = 0; + auto& data = *reader.dataSource(); + mcap::TypedRecordReader records(data, sizeof(mcap::Magic), data.size() - sizeof(mcap::Magic)); + records.onAttachment = [&](const mcap::Attachment& attachment, auto) + { + ++attachments; + REQUIRE(attachment.createTime == 6); + REQUIRE(attachment.logTime == 7); + REQUIRE(attachment.dataSize == 3); + REQUIRE(attachment.data[2] == std::byte{ 3 }); + }; + records.onMetadata = [&](const mcap::Metadata& record, auto) + { + ++metadata; + if (record.name == "ego.merge") + REQUIRE(record.metadata.at("recording.library").size() > 0); + else + REQUIRE(record.metadata.contains("file")); + }; + while (records.next()) + REQUIRE(records.status().ok()); + REQUIRE(attachments == 2); + REQUIRE(metadata == 3); +} + +TEST_CASE("ego_mcap_merge preserves ordinary session topics", "[ego][unit]") +{ + TempDirectory temp; + write_fixture(temp.path / "session.mcap", "head/Head", "core.HeadPoseRecord", { 10 }); + write_fixture(temp.path / "capture.mcap", "ego_media/ColorLeft", "core.EgoEncodedVideoFrameRecord", { 5 }); + plugins::ego::merge_recording(temp.path / "session.mcap", temp.path / "capture.mcap", temp.path / "final.mcap"); + mcap::McapReader reader; + REQUIRE(reader.open((temp.path / "final.mcap").string()).ok()); + REQUIRE(reader.readSummary(mcap::ReadSummaryMethod::NoFallbackScan).ok()); + bool head = false; + for (const auto& [id, channel] : reader.channels()) + head |= channel->topic == "head/Head"; + REQUIRE(head); +} + +TEST_CASE("ego_mcap_merge rejects ambiguous channels and unknown records", "[ego][unit]") +{ + TempDirectory temp; + write_fixture(temp.path / "session.mcap", "shared", "core.EgoFrameMetadataRecord", { 10 }); + SECTION("different schema") + { + write_fixture(temp.path / "capture.mcap", "shared", "core.EgoImuBatchRecord", { 5 }); + } + SECTION("reserved topic prefix") + { + write_fixture(temp.path / "capture.mcap", "session_observed/existing", "core.EgoImuBatchRecord", { 5 }); + } + SECTION("unknown semantic record") + { + write_fixture(temp.path / "capture.mcap", "camera", "core.EgoImuBatchRecord", { 5 }, true); + } + REQUIRE_THROWS(plugins::ego::merge_recording( + temp.path / "session.mcap", temp.path / "capture.mcap", temp.path / "final.mcap")); + REQUIRE_FALSE(fs::exists(temp.path / "final.mcap")); + REQUIRE(fs::exists(temp.path / "capture.mcap")); +} + +TEST_CASE("ego_mcap_merge rejects truncated input and existing output", "[ego][unit]") +{ + TempDirectory temp; + fixture_pair(temp); + SECTION("truncated footer") + { + const auto path = temp.path / "capture.mcap"; + fs::resize_file(path, fs::file_size(path) - 6); + } + SECTION("existing final") + { + std::ofstream(temp.path / "final.mcap") << "keep"; + } + SECTION("existing partial") + { + std::ofstream(temp.path / "final.mcap.partial") << "keep"; + } + REQUIRE_THROWS(plugins::ego::merge_recording( + temp.path / "session.mcap", temp.path / "capture.mcap", temp.path / "final.mcap")); + REQUIRE(fs::exists(temp.path / "session.mcap")); + REQUIRE(fs::exists(temp.path / "capture.mcap")); +} + +#ifndef _WIN32 +TEST_CASE("ego_mcap_merge retains partial output on write failure", "[ego][unit]") +{ + TempDirectory temp; + fixture_pair(temp, 16384); + struct FileLimit + { + rlimit previous{}; + using Handler = void (*)(int); + Handler handler; + FileLimit() + { + REQUIRE(getrlimit(RLIMIT_FSIZE, &previous) == 0); + handler = std::signal(SIGXFSZ, SIG_IGN); + const rlimit limit{ 1024, previous.rlim_max }; + REQUIRE(setrlimit(RLIMIT_FSIZE, &limit) == 0); + } + ~FileLimit() + { + setrlimit(RLIMIT_FSIZE, &previous); + std::signal(SIGXFSZ, handler); + } + }; + { + FileLimit limit; + REQUIRE_THROWS(plugins::ego::merge_recording( + temp.path / "session.mcap", temp.path / "capture.mcap", temp.path / "final.mcap")); + } + REQUIRE_FALSE(fs::exists(temp.path / "final.mcap")); + REQUIRE(fs::exists(temp.path / "final.mcap.partial")); + REQUIRE(fs::exists(temp.path / "session.mcap")); + REQUIRE(fs::exists(temp.path / "capture.mcap")); +} +#endif + +TEST_CASE("ego_mcap_merge enforces diagnostics off across both inputs by schema", "[ego][unit]") +{ + TempDirectory temp; + const auto parent = temp.path / "session.mcap"; + const auto fragment = temp.path / "capture.mcap"; + const auto output = temp.path / "final.mcap"; + SECTION("parent contains renamed diagnostics") + { + write_fixture(parent, "custom/diagnostic", "core.EgoDeviceStateRecord", { 1 }); + write_fixture(fragment, "video", "core.EgoFrameMetadataRecord", { 2 }, false, 4, "off"); + } + SECTION("fragment contains diagnostics despite policy") + { + write_fixture(parent, "head", "core.HeadPoseRecord", { 1 }); + write_fixture(fragment, "custom/diagnostic", "core.EgoDeviceStateRecord", { 2 }, false, 4, "off"); + } + SECTION("parent registers an empty diagnostic channel") + { + write_fixture(parent, "empty/diagnostic", "core.EgoDeviceStateRecord", {}); + write_fixture(fragment, "video", "core.EgoFrameMetadataRecord", { 2 }, false, 4, "off"); + } + SECTION("fragment registers an empty diagnostic channel") + { + write_fixture(parent, "head", "core.HeadPoseRecord", { 1 }); + write_fixture(fragment, "empty/diagnostic", "core.EgoDeviceStateRecord", {}, false, 4, "off"); + } + SECTION("parent declares a conflicting policy") + { + write_fixture(parent, "head", "core.HeadPoseRecord", { 1 }, false, 4, "on"); + write_fixture(fragment, "video", "core.EgoFrameMetadataRecord", { 2 }, false, 4, "off"); + } + REQUIRE_THROWS(plugins::ego::merge_recording(parent, fragment, output)); + CHECK(fs::exists(parent)); + CHECK(fs::exists(fragment)); + CHECK_FALSE(fs::exists(output)); +} + +TEST_CASE("ego_mcap_merge preserves off policy and every other record", "[ego][unit]") +{ + TempDirectory temp; + write_fixture(temp.path / "session.mcap", "head", "core.HeadPoseRecord", { 1 }); + write_fixture(temp.path / "capture.mcap", "video", "core.EgoFrameMetadataRecord", { 2 }, false, 4, "off"); + plugins::ego::merge_recording(temp.path / "session.mcap", temp.path / "capture.mcap", temp.path / "final.mcap"); + mcap::McapReader reader; + REQUIRE(reader.open((temp.path / "final.mcap").string()).ok()); + REQUIRE(reader.readSummary(mcap::ReadSummaryMethod::NoFallbackScan).ok()); + CHECK(reader.statistics()->messageCount == 2); + CHECK(reader.statistics()->metadataCount == 4); + CHECK(reader.statistics()->attachmentCount == 2); + auto& data = *reader.dataSource(); + mcap::TypedRecordReader records(data, sizeof(mcap::Magic), data.size() - sizeof(mcap::Magic)); + unsigned policy_count = 0; + records.onMetadata = [&](const mcap::Metadata& metadata, auto) + { + if (metadata.name == "ego.recording") + { + ++policy_count; + CHECK(metadata.metadata.at("record_device_diagnostics") == "off"); + } + }; + while (records.next()) + REQUIRE(records.status().ok()); + CHECK(policy_count == 1); +} diff --git a/tests/cpp/plugins/ego_media/CMakeLists.txt b/tests/cpp/plugins/ego_media/CMakeLists.txt new file mode 100644 index 0000000000..415d09ce11 --- /dev/null +++ b/tests/cpp/plugins/ego_media/CMakeLists.txt @@ -0,0 +1,9 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 ORBBEC INC. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +add_executable(ego_media_tests test_export.cpp) +target_link_libraries(ego_media_tests PRIVATE ego::mcap_support Catch2::Catch2WithMain) +target_compile_definitions(ego_media_tests PRIVATE + EGO_EXPORTER_PATH="$") +add_dependencies(ego_media_tests ego_mcap_export_media) +catch_discover_tests(ego_media_tests ADD_TAGS_AS_LABELS) diff --git a/tests/cpp/plugins/ego_media/test_export.cpp b/tests/cpp/plugins/ego_media/test_export.cpp new file mode 100644 index 0000000000..3893a49ed3 --- /dev/null +++ b/tests/cpp/plugins/ego_media/test_export.cpp @@ -0,0 +1,163 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 ORBBEC INC. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace +{ +namespace fs = std::filesystem; + +struct TemporaryDirectory +{ + fs::path path; + TemporaryDirectory() + { + static std::atomic counter{ 0 }; + path = + fs::temp_directory_path() / ("ego_media_test_" + std::to_string(getpid()) + "_" + std::to_string(counter++)); + fs::create_directory(path); + } + ~TemporaryDirectory() + { + std::error_code error; + fs::remove_all(path, error); + } +}; + +std::vector read_bytes(const fs::path& path) +{ + std::ifstream input(path, std::ios::binary); + return { std::istreambuf_iterator(input), std::istreambuf_iterator() }; +} + +int run_export(const fs::path& source, const fs::path& output) +{ + const pid_t process = fork(); + REQUIRE(process >= 0); + if (process == 0) + { + execl(EGO_EXPORTER_PATH, EGO_EXPORTER_PATH, source.c_str(), output.c_str(), nullptr); + _exit(127); + } + int status = 0; + REQUIRE(waitpid(process, &status, 0) == process); + REQUIRE(WIFEXITED(status)); + return WEXITSTATUS(status); +} + +void write_fixture(const fs::path& path, bool wrong_video_stream = false, bool wrong_pcm_size = false) +{ + mcap::McapWriter writer; + mcap::McapWriterOptions options("ego"); + options.compression = mcap::Compression::None; + REQUIRE(writer.open(path.string(), options).ok()); + core::McapTrackerChannels video(writer, "ego_media", { "ColorLeft", "ColorRight" }); + core::McapTrackerChannels audio(writer, "ego_media", { "Audio" }); + for (size_t index = 0; index < 2; ++index) + { + for (uint64_t sequence = 0; sequence < 2; ++sequence) + { + core::EgoEncodedVideoFrameT frame; + frame.stream = static_cast(wrong_video_stream ? 1 - index : index); + frame.sequence_number = sequence; + frame.width = 1600; + frame.height = 1300; + frame.fps = 30; + frame.encoded_data = { 0xff, 0xd8, static_cast(index), 0xff, 0xd9 }; + video.write(index, core::pack_record( + &frame, core::DeviceDataTimestamp(100 + sequence, 90 + sequence, 80 + sequence))); + } + } + core::EgoPcmAudioChunkT pcm; + pcm.sample_rate_hz = 48000; + pcm.channel_count = 1; + pcm.bits_per_sample = 16; + pcm.sample_count = wrong_pcm_size ? 3 : 2; + pcm.pcm_data = { 1, 2, 3, 4 }; + audio.write(0, core::pack_record(&pcm, core::DeviceDataTimestamp(101, 91, 81))); + writer.close(); +} +} // namespace + +TEST_CASE("ego_media_export preserves encoded bytes and PCM WAV", "[ego][unit]") +{ + TemporaryDirectory directory; + const auto source = directory.path / "source.mcap"; + const auto output = directory.path / "export"; + write_fixture(source); + REQUIRE(run_export(source, output) == 0); + CHECK(read_bytes(output / "ColorLeft.mjpg") == + std::vector{ 0xff, 0xd8, 0, 0xff, 0xd9, 0xff, 0xd8, 0, 0xff, 0xd9 }); + CHECK(read_bytes(output / "ColorRight.mjpg") == + std::vector{ 0xff, 0xd8, 1, 0xff, 0xd9, 0xff, 0xd8, 1, 0xff, 0xd9 }); + const auto wav = read_bytes(output / "Audio.wav"); + REQUIRE(wav.size() == 48); + CHECK(std::string(wav.begin(), wav.begin() + 4) == "RIFF"); + CHECK(wav[4] == 40); + CHECK(wav[22] == 1); + CHECK(wav[24] == 0x80); + CHECK(wav[25] == 0xbb); + CHECK(wav[34] == 16); + CHECK(wav[40] == 4); + CHECK(std::vector(wav.begin() + 44, wav.end()) == std::vector{ 1, 2, 3, 4 }); +} + +TEST_CASE("ego_media_export rejects mismatched media and incomplete input", "[ego][unit]") +{ + TemporaryDirectory directory; + const auto source = directory.path / "source.mcap"; + const auto output = directory.path / "export"; + SECTION("video topic does not match payload") + { + write_fixture(source, true); + } + SECTION("PCM size does not match sample count") + { + write_fixture(source, false, true); + } + SECTION("footer is truncated") + { + write_fixture(source); + fs::resize_file(source, fs::file_size(source) - 6); + } + SECTION("video schema has the wrong encoding") + { + mcap::McapWriter writer; + REQUIRE(writer.open(source.string(), mcap::McapWriterOptions("ego")).ok()); + mcap::Schema schema("core.EgoEncodedVideoFrameRecord", "jsonschema", "{}"); + writer.addSchema(schema); + mcap::Channel channel("ego_media/ColorLeft", "flatbuffer", schema.id); + writer.addChannel(channel); + writer.close(); + } + CHECK(run_export(source, output) != 0); + CHECK(fs::exists(source)); +} + +#if defined(__linux__) +TEST_CASE("ego_media_export reports final output flush failure", "[ego][unit]") +{ + TemporaryDirectory directory; + const auto source = directory.path / "source.mcap"; + const auto output = directory.path / "export"; + write_fixture(source); + fs::create_directory(output); + fs::create_symlink("/dev/full", output / "ColorLeft.mjpg"); + CHECK(run_export(source, output) != 0); + CHECK(fs::exists(source)); +} +#endif diff --git a/tests/python/CMakeLists.txt b/tests/python/CMakeLists.txt index ad4415cbeb..90147a4d69 100644 --- a/tests/python/CMakeLists.txt +++ b/tests/python/CMakeLists.txt @@ -18,6 +18,10 @@ if(BUILD_EXAMPLE_TELEOP_ROS2) add_subdirectory(examples/teleop_ros2) endif() +if(BUILD_EXAMPLES AND BUILD_PYTHON_BINDINGS) + add_subdirectory(examples/mcap_record_replay) +endif() + if(BUILD_VIZ AND BUILD_PYTHON_BINDINGS) add_subdirectory(viz) endif() diff --git a/tests/python/core/CMakeLists.txt b/tests/python/core/CMakeLists.txt index 1c96b71c32..ca20a2cab7 100644 --- a/tests/python/core/CMakeLists.txt +++ b/tests/python/core/CMakeLists.txt @@ -6,6 +6,7 @@ add_subdirectory(retargeting_engine) add_subdirectory(teleop_session_manager) if(BUILD_PYTHON_BINDINGS) + add_subdirectory(ego) add_subdirectory(cloudxr) # Needs `import isaaccapture`, and drives the C++ half through the emitter # built under tests/cpp/core/log_bridge. diff --git a/tests/python/core/ego/CMakeLists.txt b/tests/python/core/ego/CMakeLists.txt new file mode 100644 index 0000000000..7207ff3d5f --- /dev/null +++ b/tests/python/core/ego/CMakeLists.txt @@ -0,0 +1,18 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 ORBBEC INC. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# Use a writable UV_CACHE_DIR when validating from a filesystem sandbox. +# python_package is copied during build; rebuild it after changing EGO helpers. +add_test(NAME ego_embedded_recording_python + COMMAND uv run --python ${ISAAC_TELEOP_PYTHON_VERSION} --extra dev pytest -v --tb=short test_recording.py + WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}") +set_tests_properties(ego_embedded_recording_python PROPERTIES + ENVIRONMENT "PYTHONPATH=${CMAKE_BINARY_DIR}/python_package/$") + +foreach(ego_test sync calibration) + add_test(NAME ego_${ego_test}_python + COMMAND uv run --python ${ISAAC_TELEOP_PYTHON_VERSION} --extra dev pytest -v --tb=short test_${ego_test}.py + WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}") + set_tests_properties(ego_${ego_test}_python PROPERTIES + ENVIRONMENT "PYTHONPATH=${CMAKE_BINARY_DIR}/python_package/$;EGO_TEST_FLATC=$") +endforeach() diff --git a/tests/python/core/ego/pyproject.toml b/tests/python/core/ego/pyproject.toml new file mode 100644 index 0000000000..9f88a99c68 --- /dev/null +++ b/tests/python/core/ego/pyproject.toml @@ -0,0 +1,13 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 ORBBEC INC. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +[project] +name = "isaaccapture-ego-tests" +version = "0.0.0" +requires-python = ">=3.11,<3.14" + +[project.optional-dependencies] +dev = ["pytest", "numpy", "mcap>=1.2,<2", "flatbuffers"] + +[tool.pytest.ini_options] +python_files = ["test_*.py"] diff --git a/tests/python/core/ego/test_calibration.py b/tests/python/core/ego/test_calibration.py new file mode 100644 index 0000000000..2c230d696f --- /dev/null +++ b/tests/python/core/ego/test_calibration.py @@ -0,0 +1,327 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 ORBBEC INC. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +import json +from dataclasses import replace + +import pytest + +from isaaccapture.ego.calibration import ( + PairedTimeEvent, + VALIDATION_DURATION_NS, + evidence_sha256, + fit_time_alignment, + make_recording_alignment, +) + + +ORIGIN = 1_700_000_000_000_000_000 + + +def event(name, source, reference, uncertainty=1000): + return PairedTimeEvent(name, source, reference, uncertainty, f"raw-{name}.sha256") + + +def raw_events(offset=20_000_000, validation_error=0): + training = [ + event(f"cal-{i}", ORIGIN + i * 10**9, ORIGIN + i * 10**9 + offset) + for i in (0, 1, 2) + ] + held_out = [ + event( + f"val-{i}", + ORIGIN + i * 10**9, + ORIGIN + i * 10**9 + offset + validation_error, + ) + for i in (10, 450, 890) + ] + return training, held_out + + +def fit_raw(**kwargs): + training, held_out = raw_events(**kwargs) + return fit_time_alignment( + training, + held_out, + fps=30, + validation_start_ns=ORIGIN, + validation_end_ns=ORIGIN + VALIDATION_DURATION_NS, + ) + + +def test_integer_anchors_preserve_ns_at_epoch_scale(): + result = fit_raw(offset=20_000_001) + assert result.status == "validated" + assert result.reference_time(ORIGIN + 123) == ORIGIN + 20_000_124 + assert result.max_abs_error_ns == 0 + assert result.drift_ppm == 0 + assert result.validation_start_ns == ORIGIN + assert result.validation_end_ns == ORIGIN + VALIDATION_DURATION_NS + assert result.validation_report()["validation_end_ns"] == result.validation_end_ns + + +def test_training_fit_does_not_hide_independent_validation_error(): + result = fit_raw(validation_error=20_000_000) + assert result.status == "unverified" + assert result.max_abs_error_ns == 20_000_000 + assert "physical_error_and_uncertainty_exceed_half_frame" in result.reasons + + +def test_exposure_and_event_uncertainty_counts_against_half_frame(): + training, held_out = raw_events() + held_out[0] = event( + "val-10", held_out[0].source_value, held_out[0].reference_time_ns, 17_000_000 + ) + result = fit_time_alignment( + training, + held_out, + fps=30, + validation_start_ns=ORIGIN, + validation_end_ns=ORIGIN + VALIDATION_DURATION_NS, + ) + assert result.status == "unverified" + assert result.uncertainty_ns == 17_000_000 + + +def test_rejects_validation_data_reused_for_calibration(): + training, _ = raw_events() + with pytest.raises(ValueError, match="disjoint"): + fit_time_alignment( + training, + training, + fps=30, + validation_start_ns=ORIGIN, + validation_end_ns=ORIGIN + VALIDATION_DURATION_NS, + ) + + +def test_short_or_nonuniform_validation_is_not_certified(): + training, _ = raw_events() + short = [ + event(f"val-{i}", ORIGIN + i * 10**9, ORIGIN + i * 10**9 + 20_000_000) + for i in (10, 20, 30) + ] + result = fit_time_alignment( + training, + short, + fps=30, + validation_start_ns=ORIGIN, + validation_end_ns=ORIGIN + 60 * 10**9, + ) + assert result.status == "unverified" + assert "validation_shorter_than_15_minutes" in result.reasons + assert "missing_beginning_middle_or_end_validation" in result.reasons + late = [ + event(f"late-{i}", ORIGIN + i * 10**9, ORIGIN + i * 10**9 + 20_000_000) + for i in (100, 200, 300) + ] + result = fit_time_alignment( + training, + late, + fps=30, + validation_start_ns=ORIGIN, + validation_end_ns=ORIGIN + VALIDATION_DURATION_NS, + ) + assert "missing_beginning_middle_or_end_validation" in result.reasons + + +@pytest.mark.parametrize( + "duration, status", + [(900_000_000_000, "validated"), (899_999_999_999, "unverified")], +) +def test_fifteen_minute_validation_boundary(duration, status): + training, held_out = raw_events() + result = fit_time_alignment( + training, + held_out, + fps=30, + validation_start_ns=ORIGIN, + validation_end_ns=ORIGIN + duration, + ) + assert result.status == status + assert result.held_out_duration_ns == duration + assert result.reasons == ( + () if status == "validated" else ("validation_shorter_than_15_minutes",) + ) + + +def test_audio_period_fits_drift_instead_of_assuming_48000_hz(): + period = 20_834 + training = [ + event(f"cal-{i}", i * 48_000, ORIGIN + i * 48_000 * period) for i in (0, 1, 2) + ] + held_out = [ + event(f"val-{i}", i * 48_000, ORIGIN + i * 48_000 * period) + for i in (10, 450, 890) + ] + result = fit_time_alignment( + training, + held_out, + fps=30, + validation_start_ns=ORIGIN, + validation_end_ns=ORIGIN + VALIDATION_DURATION_NS, + source_unit="samples", + sample_rate_hz=48_000, + ) + assert result.status == "validated" + assert result.scale == period + assert result.drift_ppm == pytest.approx(32) + assert result.reference_time(48_000) == ORIGIN + 48_000 * period + + +@pytest.mark.parametrize("value", [True, "30", 0, -1, float("nan"), float("inf")]) +def test_invalid_fps_is_rejected(value): + training, held_out = raw_events() + with pytest.raises(ValueError, match="fps"): + fit_time_alignment( + training, + held_out, + fps=value, + validation_start_ns=ORIGIN, + validation_end_ns=ORIGIN + VALIDATION_DURATION_NS, + ) + + +def test_recording_model_is_bound_to_original_mcap_and_media(tmp_path): + recording = tmp_path / "capture.mcap" + recording.write_bytes(b"recording") + media = tmp_path / "raw" / "left.h264" + media.parent.mkdir() + media.write_bytes(b"original-video") + result = make_recording_alignment( + recording, + model_id="test-physical-events", + identity={"serial": "test", "firmware": "1", "sdk_version": "2.9"}, + profile={"video": {"format": "H264", "width": 1600, "height": 1300, "fps": 30}}, + epochs=[ + { + "epoch": 0, + "valid_from_ns": ORIGIN, + "valid_to_ns": ORIGIN + VALIDATION_DURATION_NS, + "streams": {"gyro": fit_raw()}, + } + ], + media_paths=[media], + ) + assert result["status"] == "validated" + assert result["recording_fingerprint"]["mcap_sha256"] == evidence_sha256(recording) + assert result["recording_fingerprint"]["media_sha256"] == { + "raw/left.h264": evidence_sha256(media) + } + assert ( + result["epochs"][0]["streams"]["gyro"]["reference_anchor_ns"] + == ORIGIN + 20_000_000 + ) + json.dumps(result, allow_nan=False) + + +def test_failed_physical_validation_cannot_emit_validated_document(tmp_path): + recording = tmp_path / "capture.mcap" + recording.write_bytes(b"recording") + result = make_recording_alignment( + recording, + model_id="failed-events", + identity={"serial": "test", "firmware": "1", "sdk_version": "2.9"}, + profile={"video": {"fps": 30}}, + epochs=[ + { + "epoch": 0, + "valid_from_ns": ORIGIN, + "valid_to_ns": ORIGIN + VALIDATION_DURATION_NS, + "streams": {"gyro": fit_raw(validation_error=20_000_000)}, + } + ], + ) + assert result["status"] == "unverified" + assert result["validation"]["max_abs_error_ns"] == 20_000_000 + + +def test_combined_error_budget_and_audio_continuity_are_not_certified(tmp_path): + recording = tmp_path / "capture.mcap" + recording.write_bytes(b"recording") + arguments = { + "model_id": "physical-events", + "identity": {"serial": "test", "firmware": "1", "sdk_version": "2.9"}, + "profile": {"video": {"fps": 30}}, + } + epoch = { + "epoch": 0, + "valid_from_ns": ORIGIN, + "valid_to_ns": ORIGIN + VALIDATION_DURATION_NS, + "streams": { + "gyro": replace(fit_raw(), max_abs_error_ns=12_000_000), + "accel": replace(fit_raw(), uncertainty_ns=8_000_000), + }, + } + result = make_recording_alignment(recording, epochs=[epoch], **arguments) + assert result["status"] == "unverified" + assert ( + "physical_error_and_uncertainty_exceed_half_frame" + in result["validation"]["reasons"] + ) + assert [report["stream"] for report in result["validation"]["stream_reports"]] == [ + "gyro", + "accel", + ] + epoch["streams"] = { + "audio": [ + { + "segment_id": 0, + "first_sample_index": 0, + "sample_count": 48000, + "fit": replace( + fit_raw(), source_unit="samples", scale=1_000_000_000 / 48000 + ), + } + ], + } + result = make_recording_alignment(recording, epochs=[epoch], **arguments) + assert result["status"] == "unverified" + assert result["validation"]["reasons"] == ["audio_continuity_unverified"] + epoch["streams"]["audio"][0]["continuity_verified"] = "true" + with pytest.raises(ValueError, match="boolean"): + make_recording_alignment(recording, epochs=[epoch], **arguments) + + +def test_overlapping_effective_ranges_are_rejected(tmp_path): + recording = tmp_path / "capture.mcap" + recording.write_bytes(b"recording") + epoch = { + "epoch": 0, + "valid_from_ns": ORIGIN, + "valid_to_ns": ORIGIN + VALIDATION_DURATION_NS, + "streams": {"gyro": fit_raw()}, + } + with pytest.raises(ValueError, match="effective interval"): + make_recording_alignment( + recording, + model_id="overlap", + identity={"serial": "test", "firmware": "1", "sdk_version": "2.9"}, + profile={"video": {"fps": 30}}, + epochs=[epoch, epoch], + ) + + +@pytest.mark.parametrize("extend_start", [False, True]) +def test_recording_model_cannot_extend_beyond_independent_evidence( + tmp_path, extend_start +): + recording = tmp_path / "capture.mcap" + recording.write_bytes(b"recording") + begin = ORIGIN - 1 if extend_start else ORIGIN + end = ORIGIN + VALIDATION_DURATION_NS + (0 if extend_start else 1) + with pytest.raises(ValueError, match="independent validation bounds"): + make_recording_alignment( + recording, + model_id="overextended", + identity={"serial": "test", "firmware": "1", "sdk_version": "2.9"}, + profile={"video": {"fps": 30}}, + epochs=[ + { + "epoch": 0, + "valid_from_ns": begin, + "valid_to_ns": end, + "streams": {"gyro": fit_raw()}, + } + ], + ) diff --git a/tests/python/core/ego/test_recording.py b/tests/python/core/ego/test_recording.py new file mode 100644 index 0000000000..5665140448 --- /dev/null +++ b/tests/python/core/ego/test_recording.py @@ -0,0 +1,378 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 ORBBEC INC. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +from pathlib import Path +from types import SimpleNamespace +import subprocess + +import pytest + +from isaaccapture import plugin_manager, teleop_session_manager +from isaaccapture.deviceio_session import McapRecordingConfig +from isaaccapture.ego import configure_recording, embedded_recording +from isaaccapture.ego import recording +from isaaccapture.teleop_session_manager import ( + PluginConfig, + SessionMode, + TeleopSessionConfig, +) + + +@pytest.fixture +def setup(tmp_path, monkeypatch): + fragment = tmp_path / "capture.mcap" + final = tmp_path / "final.mcap" + executable = tmp_path / "ego_mcap_merge" + executable.write_text("merge fixture") + executable.chmod(0o755) + plugin = PluginConfig( + "ego_camera", + "/camera/ego", + [tmp_path], + plugin_args=[ + "--collection-prefix=ego", + "--mcap-media=embedded", + f"--mcap-media-spool={fragment}", + ], + ) + config = TeleopSessionConfig( + "test", + SimpleNamespace(get_leaf_nodes=list), + plugins=[plugin], + mcap_config=McapRecordingConfig(str(final)), + ) + events = [] + state = SimpleNamespace( + config=config, + fragment=fragment, + final=final, + executable=executable, + events=events, + finish="formal", + cleanup_error=False, + merger_error=False, + session_config=None, + command=None, + ) + info = SimpleNamespace(working_dir=str(tmp_path), args=[]) + monkeypatch.setattr( + plugin_manager, + "PluginManager", + lambda paths: SimpleNamespace(get_plugin_info=lambda name: info), + ) + + class Session: + def __init__(self, session_config): + state.session_config = session_config + + def __enter__(self): + events.append("enter") + Path(state.session_config.mcap_config.filename).write_bytes( + b"session fixture" + ) + return self + + def __exit__(self, *exception): + events.append("exit") + if state.finish == "formal": + fragment.write_bytes(b"capture fixture") + elif state.finish == "partial": + Path(str(fragment) + ".partial").write_bytes(b"incomplete fixture") + if state.cleanup_error: + raise OSError("cleanup failed") + return False + + def merge(command, *, check): + assert events[-1] == "exit" + assert check + events.append("merge") + state.command = command + if state.merger_error: + Path(str(final) + ".partial").write_bytes(b"partial merge") + raise subprocess.CalledProcessError(1, command) + final.write_bytes(b"merged fixture") + + monkeypatch.setattr(teleop_session_manager, "TeleopSession", Session) + monkeypatch.setattr(recording.subprocess, "run", merge) + return state + + +def test_merge_follows_complete_session_cleanup_without_mutating_config(setup): + original = setup.config.mcap_config + with embedded_recording(setup.config, media_fragment=setup.fragment) as session: + assert session is not None + setup.events.append("body") + assert setup.events == ["enter", "body", "exit", "merge"] + assert setup.config.mcap_config is original + assert setup.config.mcap_config.filename == str(setup.final) + assert not setup.config.plugins[0].required + assert setup.session_config.plugins[0].required + assert setup.command == [ + str(setup.executable), + "--recording", + str(setup.final.with_name("final.session.mcap")), + "--media", + str(setup.fragment), + "--output", + str(setup.final), + ] + assert setup.fragment.exists() + assert setup.final.with_name("final.session.mcap").exists() + + +def test_body_error_propagates_and_keeps_inputs_without_merging(setup): + with pytest.raises(RuntimeError, match="body failed"): + with embedded_recording(setup.config, media_fragment=setup.fragment): + raise RuntimeError("body failed") + assert setup.events == ["enter", "exit"] + assert not setup.final.exists() + assert setup.fragment.exists() + + +def test_cleanup_error_is_not_reported_as_complete_recording(setup): + setup.cleanup_error = True + with pytest.raises(OSError, match="cleanup failed"): + with embedded_recording(setup.config, media_fragment=setup.fragment): + pass + assert setup.events == ["enter", "exit"] + assert not setup.final.exists() + + +@pytest.mark.parametrize("finish", ["partial", "missing"]) +def test_stop_without_committed_fragment_does_not_merge(setup, finish): + setup.finish = finish + with pytest.raises(RuntimeError, match="did not commit"): + with embedded_recording(setup.config, media_fragment=setup.fragment): + pass + assert setup.events == ["enter", "exit"] + assert not setup.final.exists() + + +def test_merge_failure_propagates_and_retains_evidence(setup): + setup.merger_error = True + with pytest.raises(subprocess.CalledProcessError): + with embedded_recording(setup.config, media_fragment=setup.fragment): + pass + assert setup.fragment.exists() + assert setup.final.with_name("final.session.mcap").exists() + assert Path(str(setup.final) + ".partial").exists() + assert not setup.final.exists() + + +@pytest.mark.parametrize( + "invalid", + [ + "replay", + "no_recording", + "duplicate", + "spool", + "mode", + "missing_merger", + "existing_output", + ], +) +def test_invalid_recording_fails_before_capture(setup, invalid): + if invalid == "replay": + setup.config.mode = SessionMode.REPLAY + elif invalid == "no_recording": + setup.config.mcap_config = None + elif invalid == "duplicate": + setup.config.plugins.append(setup.config.plugins[0]) + elif invalid == "spool": + setup.config.plugins[0].plugin_args[-1] = "--mcap-media-spool=other.mcap" + elif invalid == "mode": + setup.config.plugins[0].plugin_args[1] = "--mcap-media=metadata-only" + elif invalid == "missing_merger": + setup.executable.unlink() + elif invalid == "existing_output": + setup.final.write_bytes(b"keep") + with pytest.raises((ValueError, FileNotFoundError, FileExistsError)): + with embedded_recording(setup.config, media_fragment=setup.fragment): + pytest.fail("Capture must not start") + assert not setup.events + if invalid == "existing_output": + assert setup.final.read_bytes() == b"keep" + + +def test_caught_interrupt_is_a_normal_stop(setup): + with embedded_recording(setup.config, media_fragment=setup.fragment): + try: + raise KeyboardInterrupt + except KeyboardInterrupt: + pass + assert setup.events == ["enter", "exit", "merge"] + + +@pytest.mark.parametrize("value", [True, False]) +def test_configure_recording_normalizes_policy_without_changing_caller(setup, value): + original = list(setup.config.plugins[0].plugin_args) + setup.config.plugins[0].plugin_args += [ + "--record-device-diagnostics=off", + "--record-device-diagnostics=on", + ] + copied = configure_recording(setup.config, record_device_diagnostics=value) + assert copied is not setup.config + assert copied.plugins[0] is not setup.config.plugins[0] + assert copied.plugins[0].plugin_args == original + [ + f"--record-device-diagnostics={'on' if value else 'off'}" + ] + assert setup.config.plugins[0].plugin_args[-1] == "--record-device-diagnostics=on" + + +def test_configure_recording_honors_effective_plugin_arguments(setup): + setup.config.plugins[0].plugin_args += ["--record-device-diagnostics=off"] + assert configure_recording(setup.config).plugins[0].plugin_args[-1].endswith("=off") + setup.config.plugins[0].plugin_args[-1] = "--record-device-diagnostics=invalid" + with pytest.raises(ValueError, match="on or off"): + configure_recording(setup.config) + + +def test_off_rejects_explicit_diagnostic_recording_even_with_custom_topic(setup): + from isaaccapture.deviceio_trackers import EgoDeviceStateTracker + + tracker = EgoDeviceStateTracker("arbitrary/DeviceState") + setup.config.mcap_config = McapRecordingConfig( + str(setup.final), [(tracker, "renamed_diagnostics")] + ) + with pytest.raises(ValueError, match="renamed_diagnostics"): + with embedded_recording( + setup.config, media_fragment=setup.fragment, record_device_diagnostics=False + ): + pytest.fail("Must reject before capture") + assert not setup.events + + +@pytest.mark.parametrize("root", ["pipeline", "teleop_control_pipeline", "sinks"]) +def test_off_rejects_nv_automatic_recording_in_every_graph_root( + setup, monkeypatch, root +): + from isaaccapture.deviceio_trackers import EgoDeviceStateTracker + from isaaccapture.retargeting_engine import deviceio_source_nodes + + class Source: + name = "diagnostic_side_effect" + + def get_tracker(self): + return EgoDeviceStateTracker("ego/DeviceState") + + monkeypatch.setattr(deviceio_source_nodes, "IDeviceIOSource", Source) + graph = SimpleNamespace(get_leaf_nodes=lambda: [Source()]) + setattr(setup.config, root, [graph] if root == "sinks" else graph) + with pytest.raises(ValueError, match="automatically recorded"): + configure_recording(setup.config, record_device_diagnostics=False) + assert not setup.events + + +def test_off_keeps_manual_live_tracker_and_embedded_recording_lifecycle(setup): + from isaaccapture.deviceio_trackers import ( + EgoCalibrationTracker, + EgoDeviceStateTracker, + ) + + tracker = EgoDeviceStateTracker("ego/DeviceState") + setup.config.trackers = [tracker] + with embedded_recording( + setup.config, media_fragment=setup.fragment, record_device_diagnostics=False + ): + assert setup.session_config.trackers[0] is tracker + assert isinstance(setup.session_config.trackers[1], EgoCalibrationTracker) + assert setup.config.trackers == [tracker] + assert ( + setup.session_config.plugins[0].plugin_args[-1] + == "--record-device-diagnostics=off" + ) + assert setup.events == ["enter", "exit", "merge"] + + +def test_recording_retains_calibration_without_mutating_caller(setup): + from isaaccapture.deviceio_trackers import EgoCalibrationTracker + + copied = configure_recording(setup.config) + entries = copied.mcap_config.get_tracker_names() + assert len(entries) == 1 + calibration, name = entries[0] + assert isinstance(calibration, EgoCalibrationTracker) + assert name == "ego_calibration" + assert copied.trackers == [calibration] + assert copied.mcap_config.filename == setup.config.mcap_config.filename + assert setup.config.mcap_config.get_tracker_names() == [] + assert setup.config.trackers == [] + repeated = configure_recording(copied) + assert repeated.mcap_config.get_tracker_names() == entries + assert repeated.trackers == [calibration] + assert not setup.events + + +def test_recording_preserves_explicit_calibration_name_and_reference(setup): + from isaaccapture.deviceio_trackers import EgoCalibrationTracker + + calibration = EgoCalibrationTracker("ego/Calibration") + setup.config.mcap_config = McapRecordingConfig( + str(setup.final), [(calibration, "custom_calibration")] + ) + copied = configure_recording(setup.config) + assert copied.mcap_config.get_tracker_names() == [ + (calibration, "custom_calibration") + ] + assert copied.trackers == [calibration] + + +def test_recording_reuses_manual_calibration(setup): + from isaaccapture.deviceio_trackers import EgoCalibrationTracker + + calibration = EgoCalibrationTracker("ego/Calibration") + setup.config.trackers = [calibration] + copied = configure_recording(setup.config) + assert copied.mcap_config.get_tracker_names() == [(calibration, "ego_calibration")] + assert copied.trackers == [calibration] + + +def test_recording_does_not_duplicate_automatically_recorded_calibration( + setup, monkeypatch +): + from isaaccapture.deviceio_trackers import EgoCalibrationTracker + from isaaccapture.retargeting_engine import deviceio_source_nodes + + calibration = EgoCalibrationTracker("ego/Calibration") + + class Source: + name = "custom_calibration" + + def get_tracker(self): + return calibration + + monkeypatch.setattr(deviceio_source_nodes, "IDeviceIOSource", Source) + setup.config.pipeline = SimpleNamespace(get_leaf_nodes=lambda: [Source()]) + copied = configure_recording(setup.config) + assert copied.mcap_config.get_tracker_names() == [] + assert copied.trackers == [calibration] + + +def test_recording_rejects_calibration_name_collision_before_capture(setup): + from isaaccapture.deviceio_trackers import EgoDeviceStateTracker + + tracker = EgoDeviceStateTracker("ego/DeviceState") + setup.config.mcap_config = McapRecordingConfig( + str(setup.final), [(tracker, "ego_calibration")] + ) + with pytest.raises(ValueError, match="already used"): + configure_recording(setup.config) + assert not setup.events + + +def test_recording_requires_collection_prefix_for_automatic_calibration(setup): + setup.config.plugins[0].plugin_args = [ + item + for item in setup.config.plugins[0].plugin_args + if not item.startswith("--collection-prefix=") + ] + with pytest.raises(ValueError, match="collection-prefix"): + configure_recording(setup.config) + assert not setup.events + + +def test_configure_without_mcap_does_not_add_calibration(setup): + setup.config.mcap_config = None + copied = configure_recording(setup.config) + assert copied.mcap_config is None + assert copied.trackers == [] diff --git a/tests/python/core/ego/test_sync.py b/tests/python/core/ego/test_sync.py new file mode 100644 index 0000000000..0bb032d9f3 --- /dev/null +++ b/tests/python/core/ego/test_sync.py @@ -0,0 +1,1048 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 ORBBEC INC. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Exercise complete MCAP wrappers with independently generated FlatBuffers.""" + +from dataclasses import FrozenInstanceError +import json +import os +from pathlib import Path +import subprocess +import sys + +import pytest +import numpy as np +from mcap.writer import CompressionType, Writer + +from isaaccapture.ego.sync import ( + analyze_recording_sync, + iter_sync_windows, + recording_fingerprint, +) + +ORIGIN = 1_700_000_000_000_000_001 +PERIOD = 10_000_000 +IDENTITY = {"serial": "test-device", "firmware": "1.1.1", "sdk_version": "2.9.0"} +IDENTITY["sdk_binary_sha256"] = "12" * 32 +PROFILE = { + "video": {"format": "h264", "width": 16, "height": 12, "fps": 100}, + "imu_rate_hz": 1000, + "audio": {"sample_rate_hz": 1000, "channels": 1, "bits_per_sample": 16}, +} +ALL = ("left_rgb", "right_rgb", "accel", "gyro", "audio") + + +@pytest.fixture +def recording(tmp_path): + for parent in Path(__file__).resolve().parents: + if (parent / "repo_paths.py").is_file(): + sys.path.insert(0, str(parent)) + break + from repo_paths import repo_root + + compiler = os.environ.get("EGO_TEST_FLATC") + if not compiler: + pytest.fail("EGO_TEST_FLATC must name the build's flatc executable") + source = repo_root() / "src/core/schema/fbs" + schema_paths = {} + schema_data = {} + serial = 0 + + def compile_record(name, data): + nonlocal serial + family = { + "EgoFrameMetadataRecord": "ego_camera", + "EgoEncodedVideoFrameRecord": "ego_camera", + "EgoImuBatchRecord": "ego_imu", + "EgoAudioChunkRecord": "ego_audio", + "EgoPcmAudioChunkRecord": "ego_audio", + "EgoCalibrationRecord": "ego_calibration", + }[name] + if name not in schema_paths: + schema_paths[name] = source / f"{family}.fbs" + subprocess.run( + [ + compiler, + "--binary", + "--schema", + "--root-type", + f"core.{name}", + "-I", + str(source), + "-o", + str(tmp_path), + str(schema_paths[name]), + ], + check=True, + capture_output=True, + ) + schema_data[name] = (tmp_path / f"{family}.bfbs").read_bytes() + serial += 1 + document = tmp_path / f"record_{serial}.json" + document.write_text(json.dumps(data)) + subprocess.run( + [ + compiler, + "--binary", + "--strict-json", + "--root-type", + f"core.{name}", + "-I", + str(source), + "-o", + str(tmp_path), + str(schema_paths[name]), + str(document), + ], + check=True, + capture_output=True, + ) + return (tmp_path / f"record_{serial}.bin").read_bytes(), schema_data[name] + + def create( + *, + embedded=False, + legacy_audio=False, + epochs=(0,), + frame_ids=(0, 1, 2, 3), + skip_gyro_sample=None, + audio=True, + duplicate_observation=True, + right_offset_ns=300_000, + right_sequence_offset=0, + audio_chunks=((0, 17), (17, 23)), + raw_regression=False, + capture_profile=False, + capture_profile_change=None, + bridge_change=None, + bridge_snapshots=(), + recorded_identity=None, + local_transition=False, + pcm_timing_mismatch=False, + imu_samples=40, + ): + root = tmp_path / f"capture_{serial}" + root.mkdir() + path = root / "metadata.mcap" + schemas, channels = {}, {} + video_bytes = {"ColorLeft": bytearray(), "ColorRight": bytearray()} + wav = bytearray(44) + with path.open("wb") as output: + writer = Writer(output, compression=CompressionType.NONE) + writer.start() + + def add(topic, name, data, raw): + wrapped = { + "data": data, + "timestamp": { + "available_time_local_common_clock": raw + 90_000_000, + "sample_time_local_common_clock": raw + + ( + 90_000_000 + if local_transition and raw < ORIGIN + PERIOD + else 40_000_000 + ), + "sample_time_raw_device_clock": raw, + }, + } + payload, bfbs = compile_record(name, wrapped) + if name not in schemas: + schemas[name] = writer.register_schema( + f"core.{name}", "flatbuffer", bfbs + ) + if topic not in channels: + channels[topic] = writer.register_channel( + topic, "flatbuffer", schemas[name] + ) + # Arrival/log order is deliberately unrelated to the per-sample clock. + writer.add_message( + channels[topic], + log_time=serial, + publish_time=serial + 1, + data=payload, + ) + + for epoch in epochs: + baseline = { + "version": 1, + "scope": "recording", + "status": "unverified", + "identity": (recorded_identity or IDENTITY).copy(), + "profile": PROFILE, + "epochs": [], + } + if capture_profile: + baseline["capture_profile"] = { + "version": 1, + "scope": "capture_profile", + "status": "validated", + "reference_stream": "left_rgb", + "model_id": "imported-profile", + "max_capture_duration_ns": 1_800_000_000_000, + "identity": IDENTITY.copy(), + "profile": PROFILE, + "validation": { + "method": "held_out_physical_events", + "max_abs_error_ns": 1000, + "uncertainty_ns": 1000, + "held_out_duration_ns": 1_800_000_000_000, + "evidence": ["independent evidence"], + }, + "streams": { + stream: { + "offset_ns": -2_000_000 + if stream in ("accel", "gyro") + else 0, + "scale": 1.0, + } + for stream in ALL + if stream != "audio" + }, + } + if capture_profile_change: + capture_profile_change(baseline["capture_profile"]) + baseline["host_bridge"] = { + "epoch": epoch, + "reference_anchor_ns": ORIGIN, + "local_anchor_ns": ORIGIN + 123_000_000, + "first_mapped_raw_ns": { + "left_rgb": ORIGIN, + "right_rgb": ORIGIN + right_offset_ns, + "accel": ORIGIN + 2_000_000, + "gyro": ORIGIN + 2_000_000, + }, + "valid_from_ns": ORIGIN, + "valid_to_ns": ORIGIN + + baseline["capture_profile"]["max_capture_duration_ns"], + "status": "active", + "host_absolute_accuracy": "unverified", + } + if bridge_change: + bridge_change(baseline["host_bridge"]) + calibration_data = { + "device_uid": "usb-1", + "capture_epoch": epoch, + "color_left": { + "width": 16, + "height": 12, + "fx": 10.5, + "fy": 10.6, + "cx": 8.0, + "cy": 6.0, + "distortion_model": 5, + "distortion": [0.1, 0.2, 0.3, 0.4], + }, + "color_right": { + "width": 16, + "height": 12, + "fx": 10.4, + "fy": 10.7, + "cx": 8.0, + "cy": 6.0, + }, + "left_to_right": { + "rotation": [1, 0, 0, 0, 1, 0, 0, 0, 1], + "translation_mm": [-120, 0, 0], + }, + "accel_intrinsics": [1, 2, 3], + "gyro_intrinsics": [4, 5, 6], + "accel_to_left": { + "rotation": [1, 0, 0, 0, 1, 0, 0, 0, 1], + "translation_mm": [1, 2, 3], + }, + "gyro_to_left": { + "rotation": [1, 0, 0, 0, 1, 0, 0, 0, 1], + "translation_mm": [4, 5, 6], + }, + "raw_alignment_yaml": "factory: spatial", + "raw_imu_yaml": "factory: imu", + "time_alignment": {"json": json.dumps(baseline)}, + } + add( + "ego_calibration/Calibration", + "EgoCalibrationRecord", + calibration_data, + ORIGIN, + ) + for bridge in bridge_snapshots: + baseline["host_bridge"].update(bridge) + calibration_data["time_alignment"] = {"json": json.dumps(baseline)} + add( + "ego_calibration/Calibration", + "EgoCalibrationRecord", + calibration_data, + ORIGIN, + ) + for side, discriminator in ( + ("ColorLeft", "ColorLeft"), + ("ColorRight", "ColorRight"), + ): + for frame_id in frame_ids: + raw = ( + ORIGIN + + frame_id * PERIOD + + (right_offset_ns if side == "ColorRight" else 0) + ) + metadata = { + "stream": discriminator, + "sequence_number": frame_id + + (right_sequence_offset if side == "ColorRight" else 0), + "width": 16, + "height": 12, + "fps": 100, + "pixel_format": "H264", + "encoded_bytes": 4, + "capture_epoch": epoch, + "sdk_metadata": [{"key": 19, "value": 123456789}], + } + add( + f"ego_metadata/{side}", + "EgoFrameMetadataRecord", + metadata, + raw, + ) + if duplicate_observation: + add( + f"session_observed/ego_metadata/{side}", + "EgoFrameMetadataRecord", + metadata, + raw, + ) + if embedded: + media = { + key: value + for key, value in metadata.items() + if key not in ("encoded_bytes", "sdk_metadata") + } + media["encoded_data"] = [0, 0, 1, frame_id % 256] + add( + f"ego_media/{side}", + "EgoEncodedVideoFrameRecord", + media, + raw, + ) + else: + video_bytes[side].extend(bytes([0, 0, 1, frame_id % 256])) + for sensor in ("Accel", "Gyro"): + samples = [] + for index in range(imu_samples): + if sensor == "Gyro" and index == skip_gyro_sample: + continue + raw = ORIGIN + index * 1_000_000 + 2_000_000 + if raw_regression and sensor == "Gyro" and index == 15: + raw -= 2_000_000 + samples.append( + { + "x_si": index + 0.25, + "y_si": 2.5, + "z_si": -3.5, + "temperature_c": 28.5, + "sample_time_local_common_clock_ns": raw + 70_000_000, + "sample_time_raw_device_clock_ns": raw, + } + ) + add( + f"ego_imu/{sensor}", + "EgoImuBatchRecord", + { + "sensor": sensor, + "sequence_number": 0, + "sample_rate_hz": 1000, + "full_scale": 24, + "samples": samples, + "capture_epoch": epoch, + }, + ORIGIN + 80_000_000, + ) + if audio: + for sequence, (first, count) in enumerate(audio_chunks): + data = { + "sequence_number": sequence, + "sample_rate_hz": 1000, + "channel_count": 1, + "bits_per_sample": 16, + "sample_count": count, + "wav_data_offset": len(wav), + "byte_count": count * 2, + "capture_epoch": epoch, + } + if not legacy_audio: + data["timing"] = { + "segment_id": epoch, + "first_sample_index": first, + "continuity_known": False, + } + add("ego_audio/Audio", "EgoAudioChunkRecord", data, 0) + if embedded: + pcm = { + key: value + for key, value in data.items() + if key not in ("wav_data_offset", "byte_count") + } + pcm["pcm_data"] = [0, 1] * count + if pcm_timing_mismatch: + pcm["timing"] = { + **pcm["timing"], + "first_sample_index": first + 1, + } + add("ego_media/Audio", "EgoPcmAudioChunkRecord", pcm, 0) + wav.extend(b"\x00\x01" * count) + writer.finish() + if not embedded: + (root / "raw").mkdir() + for side, data in video_bytes.items(): + (root / "raw" / f"{side}.h264").write_bytes(data) + if audio: + (root / "Audio.wav").write_bytes(wav) + model = { + "version": 1, + "scope": "recording", + "status": "validated", + "reference_stream": "left_rgb", + "model_id": "independent-test-model", + "identity": IDENTITY.copy(), + "profile": PROFILE.copy(), + "recording_fingerprint": recording_fingerprint(path), + "validation": { + "method": "independent_physical_events", + "max_abs_error_ns": 1000, + "uncertainty_ns": 1000, + "held_out_duration_ns": 1_800_000_000_000, + "evidence": ["sha256:test-evidence"], + }, + "epochs": [], + } + for epoch in epochs: + maps = { + stream: { + "source_anchor_ns": ORIGIN + + (2_000_000 if stream in ("accel", "gyro") else 0), + "reference_anchor_ns": ORIGIN, + "scale": 1.0, + "validation_start_ns": ORIGIN, + "validation_end_ns": ORIGIN + 1_800_000_000_000, + } + for stream in ALL + if stream != "audio" + } + maps["audio"] = { + "segments": [ + { + "segment_id": epoch, + "first_sample_index": 0, + "sample_count": 40, + "reference_anchor_ns": ORIGIN, + "period_ns": 1_000_000, + "continuity_verified": True, + "validation_start_ns": ORIGIN, + "validation_end_ns": ORIGIN + 1_800_000_000_000, + } + ] + } + model["epochs"].append( + { + "epoch": epoch, + "valid_from_ns": ORIGIN, + "valid_to_ns": ORIGIN + (max(frame_ids) + 1) * PERIOD, + "streams": maps, + } + ) + return path, model + + return create + + +@pytest.mark.parametrize("embedded", [False, True]) +def test_full_record_samples_media_and_half_open_windows(recording, embedded): + path, model = recording(embedded=embedded) + windows = list(iter_sync_windows(path, required_streams=ALL, alignment=model)) + assert len(windows) == 3 + assert all(window.complete for window in windows) + assert windows[0].start_time_ns == ORIGIN + assert windows[0].accel[0].time_ns == ORIGIN + assert windows[0].accel[0].record.sample_time_raw_ns == ORIGIN + 80_000_000 + assert windows[0].accel[0].sample_time_raw_ns == ORIGIN + 2_000_000 + assert windows[0].accel[0].x_si == 0.25 + assert len(windows[0].gyro) == 10 + assert windows[0].gyro[-1].time_ns < windows[1].start_time_ns + assert windows[0].left_rgb.sdk_metadata == ((19, 123456789),) + assert [value.sample_count for value in windows[1].audio] == [7, 3] + assert windows[1].audio[0].sample_offset == 10 + assert windows[1].audio[1].sample_offset == 0 + assert windows[0].left_rgb.media.storage == ("mcap" if embedded else "sidecar") + assert windows[0].audio[0].media.byte_count == 20 + with pytest.raises(FrozenInstanceError): + windows[0].complete = False + report = analyze_recording_sync(path, required_streams=ALL, alignment=model) + assert report["physical_error_measurements"] is None + assert report["coverage"]["streams"]["gyro"]["unassigned_samples"] == 10 + assert len(report["coverage"]["unpaired_video"]["left_rgb"]) == 1 + assert report["ignored_noncanonical_records"] == 8 + assert report["five_stream_verified"] + assert "x_si" not in report["windows"][0]["gyro"][0] + assert report["windows"][0]["gyro"][0]["sample_count"] == 10 + assert ( + report["coverage"]["unassigned_sample_ranges"]["gyro"][0]["first_sample_index"] + == 30 + ) + json.dumps(report, allow_nan=False) + + +@pytest.mark.parametrize("missing_bounds", [False, True]) +def test_external_model_cannot_extrapolate_unmeasured_interval( + recording, missing_bounds +): + path, model = recording() + mapping = model["epochs"][0]["streams"]["gyro"] + if missing_bounds: + del mapping["validation_end_ns"] + else: + mapping["validation_end_ns"] = ORIGIN + 2 * PERIOD + with pytest.raises( + ValueError, match=r"validation_end_ns|independent validation bounds" + ): + list(iter_sync_windows(path, required_streams=ALL, alignment=model)) + + +def test_audio_outside_independent_validation_is_unassigned(recording): + path, model = recording() + audio = model["epochs"][0]["streams"]["audio"]["segments"][0] + audio["validation_start_ns"] = ORIGIN + PERIOD + audio["validation_end_ns"] = ORIGIN + 2 * PERIOD + windows = list(iter_sync_windows(path, required_streams=ALL, alignment=model)) + assert [window.complete for window in windows] == [False, True, False] + assert windows[0].reasons == ("missing_audio",) + assert windows[2].reasons == ("missing_audio",) + report = analyze_recording_sync(path, required_streams=ALL, alignment=model) + assert not report["five_stream_verified"] + assert report["coverage"]["streams"]["audio"]["assigned_samples"] == 10 + assert report["coverage"]["streams"]["audio"]["unassigned_samples"] == 30 + + +def test_audio_validation_boundary_is_not_hidden_by_sample_tolerance(recording): + path, model = recording() + audio = model["epochs"][0]["streams"]["audio"]["segments"][0] + audio["validation_end_ns"] = ORIGIN + 2 * PERIOD - 1 + windows = list(iter_sync_windows(path, required_streams=ALL, alignment=model)) + assert windows[0].complete + assert not windows[1].complete + assert "audio_validation_boundary" in windows[1].reasons + + +def test_stereo_sequence_origins_do_not_override_sample_time_pairing(recording): + path, model = recording(right_sequence_offset=1) + windows = list( + iter_sync_windows( + path, required_streams=("left_rgb", "right_rgb"), alignment=model + ) + ) + assert all(window.complete for window in windows) + assert len({window.right_rgb.record.record_index for window in windows}) == len( + windows + ) + for window in windows: + assert ( + window.right_rgb.record.sequence_number + == window.left_rgb.record.sequence_number + 1 + ) + assert window.right_rgb.time_ns - window.left_rgb.time_ns == 300_000 + + +def test_unrequested_imu_validation_bounds_do_not_block_stereo(recording): + path, model = recording() + del model["epochs"][0]["streams"]["gyro"]["validation_end_ns"] + windows = list( + iter_sync_windows( + path, required_streams=("left_rgb", "right_rgb"), alignment=model + ) + ) + assert all(window.complete for window in windows) + + +def test_legacy_audit_is_not_a_sync_certificate(recording): + path, _ = recording(legacy_audio=True) + report = analyze_recording_sync(path) + assert report["alignment"]["status"] == "unverified" + assert report["windows"] == [] + assert report["streams"]["gyro"]["samples"] == 40 + with pytest.raises(ValueError, match="validated"): + list(iter_sync_windows(path, required_streams=ALL)) + + +@pytest.mark.parametrize( + "change, match", + [ + (lambda model: model["identity"].update(serial="other"), "serial"), + ( + lambda model: model["recording_fingerprint"].update(mcap_sha256="wrong"), + "fingerprint", + ), + ( + lambda model: model["validation"].update(max_abs_error_ns=9_000_000), + "half-frame", + ), + ( + lambda model: model["validation"].update(held_out_duration_ns=1), + "held_out_duration", + ), + ( + lambda model: model["epochs"][0]["streams"]["gyro"].update( + scale=float("nan") + ), + "JSON compliant", + ), + ], +) +def test_reject_untrusted_or_mismatched_models(recording, change, match): + path, model = recording() + change(model) + with pytest.raises(ValueError, match=match): + list(iter_sync_windows(path, required_streams=ALL, alignment=model)) + + +def test_epoch_barrier_and_query(recording): + path, model = recording(epochs=(0, 1)) + windows = list(iter_sync_windows(path, required_streams=ALL, alignment=model)) + assert len(windows) == 6 + assert [window.epoch for window in windows] == [0, 0, 0, 1, 1, 1] + selected = list( + iter_sync_windows( + path, + required_streams=ALL, + alignment=model, + epoch=1, + start_time_ns=ORIGIN + PERIOD, + end_time_ns=ORIGIN + 3 * PERIOD, + ) + ) + assert len(selected) == 2 + assert all(window.epoch == 1 for window in selected) + + +def test_gaps_and_missing_stereo_are_incomplete(recording): + path, model = recording( + frame_ids=(0, 1, 3), skip_gyro_sample=15, right_offset_ns=3_000_000 + ) + windows = list(iter_sync_windows(path, required_streams=ALL, alignment=model)) + assert not any(window.complete for window in windows) + assert "missing_right_rgb" in windows[0].reasons + assert "reference_gap" in windows[1].reasons + assert "gyro_gap" in windows[1].reasons + assert windows[1].end_time_ns == ORIGIN + 2 * PERIOD + + +def test_audio_continuity_and_required_subsets(recording): + path, model = recording() + model["epochs"][0]["streams"]["audio"]["segments"][0]["continuity_verified"] = False + windows = list(iter_sync_windows(path, required_streams=ALL, alignment=model)) + assert all("audio_continuity_unverified" in window.reasons for window in windows) + stereo = list( + iter_sync_windows( + path, required_streams=("left_rgb", "right_rgb"), alignment=model + ) + ) + assert all(window.complete for window in stereo) + + +def test_absent_required_stream_fails_but_audit_succeeds(recording): + path, model = recording(audio=False) + with pytest.raises(ValueError, match="Required stream is absent: audio"): + list(iter_sync_windows(path, required_streams=ALL, alignment=model)) + report = analyze_recording_sync(path) + assert not report["streams"]["audio"]["available"] + assert report["alignment"]["status"] == "unverified" + + +def test_model_segment_barrier(recording): + path, model = recording() + first = model["epochs"][0] + second = json.loads(json.dumps(first)) + first["valid_to_ns"] = ORIGIN + 2 * PERIOD + second["valid_from_ns"] = ORIGIN + 2 * PERIOD + second["model_id"] = "next" + model["epochs"].append(second) + windows = list(iter_sync_windows(path, required_streams=ALL, alignment=model)) + assert len(windows) == 2 + assert [window.model_id for window in windows] == ["independent-test-model", "next"] + assert [window.model_segment for window in windows] == [0, 1] + + +@pytest.mark.parametrize("invalid_interval", ["empty", "overlap"]) +def test_model_validity_intervals_are_checked(recording, invalid_interval): + path, model = recording() + if invalid_interval == "empty": + model["epochs"][0]["valid_to_ns"] = model["epochs"][0]["valid_from_ns"] + else: + model["epochs"].append(json.loads(json.dumps(model["epochs"][0]))) + with pytest.raises(ValueError, match="nonempty and nonoverlapping"): + list(iter_sync_windows(path, required_streams=ALL, alignment=model)) + + +@pytest.mark.parametrize("invalid_audio_range", ["empty", "overlap"]) +def test_audio_sample_ranges_are_checked(recording, invalid_audio_range): + path, model = recording() + segments = model["epochs"][0]["streams"]["audio"]["segments"] + if invalid_audio_range == "empty": + segments[0]["sample_count"] = 0 + expected = "sample_count must be an integer >= 1" + else: + segments.append(json.loads(json.dumps(segments[0]))) + expected = "Overlapping audio sample ranges" + with pytest.raises(ValueError, match=expected): + list(iter_sync_windows(path, required_streams=ALL, alignment=model)) + + +def test_saved_capture_profile_maps_four_streams_without_host_accuracy_claim(recording): + path, _ = recording(capture_profile=True, epochs=(0, 1)) + windows = list(iter_sync_windows(path, required_streams=ALL[:-1])) + assert len(windows) == 6 + assert all(window.complete for window in windows) + assert windows[0].accel[0].time_ns == ORIGIN + assert windows[0].accel[0].sample_time_local_ns != ORIGIN + with pytest.raises(ValueError, match="missing required stream audio"): + list(iter_sync_windows(path, required_streams=ALL)) + + +@pytest.mark.parametrize("embedded_profile", [False, True]) +@pytest.mark.parametrize("duration", [900_000_000_000, 899_999_999_999]) +def test_offline_model_fifteen_minute_validation_boundary( + recording, embedded_profile, duration +): + def change(profile): + profile["max_capture_duration_ns"] = 900_000_000_000 + profile["validation"]["held_out_duration_ns"] = duration + + path, model = recording( + capture_profile=embedded_profile, + capture_profile_change=change, + ) + model["validation"]["held_out_duration_ns"] = duration + kwargs = {} if embedded_profile else {"alignment": model} + if duration < 900_000_000_000: + with pytest.raises(ValueError, match="held_out_duration_ns"): + list(iter_sync_windows(path, required_streams=ALL[:-1], **kwargs)) + else: + windows = list(iter_sync_windows(path, required_streams=ALL[:-1], **kwargs)) + assert len(windows) == 3 + assert all(window.complete for window in windows) + + +@pytest.mark.parametrize( + "change, expected", + [ + ( + lambda profile: profile["identity"].update(sdk_binary_sha256="34" * 32), + "identity mismatch", + ), + ( + lambda profile: profile["identity"].pop("sdk_binary_sha256"), + "requires sdk_binary_sha256", + ), + ( + lambda profile: profile["streams"]["gyro"].update(scale=1.00001), + "unit stream scale", + ), + ( + lambda profile: profile.update(max_capture_duration_ns=0), + "max_capture_duration_ns", + ), + ( + lambda profile: profile.update(max_capture_duration_ns=True), + "max_capture_duration_ns", + ), + ( + lambda profile: profile.update(max_capture_duration_ns=1_800_000_000_001), + "duration exceeds", + ), + ], +) +def test_reusable_profile_rejects_unverified_sdk_drift_and_duration( + recording, change, expected +): + path, _ = recording(capture_profile=True, capture_profile_change=change) + with pytest.raises(ValueError, match=expected): + list(iter_sync_windows(path, required_streams=ALL[:-1])) + + +def test_capture_bridge_cannot_extend_the_verified_profile_duration(recording): + path, _ = recording( + capture_profile=True, + bridge_change=lambda bridge: bridge.update( + valid_to_ns=bridge["valid_to_ns"] + PERIOD + ), + ) + with pytest.raises(ValueError, match="validity must match its verified duration"): + list(iter_sync_windows(path, required_streams=ALL[:-1])) + + +def test_expired_profile_keeps_only_its_trusted_interval(recording): + path, _ = recording( + capture_profile=True, + capture_profile_change=lambda profile: profile.update( + max_capture_duration_ns=2 * PERIOD + PERIOD // 2 + ), + bridge_change=lambda bridge: bridge.update( + status="expired", expired_at_reference_ns=ORIGIN + 3 * PERIOD + ), + ) + windows = list(iter_sync_windows(path, required_streams=ALL[:-1])) + assert len(windows) == 2 + assert all(window.complete for window in windows) + assert windows[-1].end_time_ns == ORIGIN + 2 * PERIOD + report = analyze_recording_sync(path, required_streams=ALL[:-1]) + assert report["coverage"]["streams"]["gyro"]["unassigned_samples"] == 20 + assert len(report["coverage"]["unpaired_video"]["left_rgb"]) == 2 + + +def test_first_mapped_boundaries_exclude_the_initial_arrival_fallback(recording): + path, _ = recording( + capture_profile=True, + bridge_change=lambda bridge: bridge["first_mapped_raw_ns"].update( + left_rgb=ORIGIN + PERIOD, right_rgb=ORIGIN + PERIOD + 300_000 + ), + local_transition=True, + ) + windows = list(iter_sync_windows(path, required_streams=ALL[:-1])) + assert len(windows) == 2 + assert windows[0].start_time_ns == ORIGIN + PERIOD + assert all(window.complete for window in windows) + + +def test_first_mapped_boundaries_can_grow_across_calibration_snapshots(recording): + path, _ = recording( + capture_profile=True, + bridge_change=lambda bridge: bridge.update(first_mapped_raw_ns={}), + bridge_snapshots=( + {"first_mapped_raw_ns": {"left_rgb": ORIGIN}}, + { + "first_mapped_raw_ns": { + "left_rgb": ORIGIN, + "right_rgb": ORIGIN + 300_000, + "accel": ORIGIN + 2_000_000, + "gyro": ORIGIN + 2_000_000, + } + }, + ), + ) + windows = list(iter_sync_windows(path, required_streams=ALL[:-1])) + assert len(windows) == 3 + assert all(window.complete for window in windows) + + +def test_first_mapped_boundary_cannot_change_within_an_epoch(recording): + path, _ = recording( + capture_profile=True, + bridge_snapshots=({"first_mapped_raw_ns": {"left_rgb": ORIGIN + PERIOD}},), + ) + with pytest.raises(ValueError, match="Conflicting first mapped"): + list(iter_sync_windows(path, required_streams=ALL[:-1])) + + +@pytest.mark.parametrize( + "change, expected", + [ + ( + lambda bridge: bridge.update( + status="expired", expired_at_reference_ns=ORIGIN + PERIOD + ), + "expiry contradicts", + ), + ( + lambda bridge: bridge.update(expired_at_reference_ns=bridge["valid_to_ns"]), + "expiry contradicts", + ), + ( + lambda bridge: bridge["first_mapped_raw_ns"].update( + gyro=bridge["valid_to_ns"] + 2_000_000 + ), + "First mapped sample is outside", + ), + ], +) +def test_capture_bridge_rejects_inconsistent_trust_boundaries( + recording, change, expected +): + path, _ = recording(capture_profile=True, bridge_change=change) + with pytest.raises(ValueError, match=expected): + list(iter_sync_windows(path, required_streams=ALL[:-1])) + + +def test_expired_bridge_cannot_reactivate_without_a_new_epoch(recording): + path, _ = recording( + capture_profile=True, + bridge_change=lambda bridge: bridge.update(status="expired"), + bridge_snapshots=({"status": "active"},), + ) + with pytest.raises(ValueError, match="cannot become active"): + list(iter_sync_windows(path, required_streams=ALL[:-1])) + + +def test_external_alignment_checks_sdk_digest_when_present(recording): + path, model = recording() + model["identity"]["sdk_binary_sha256"] = "34" * 32 + with pytest.raises(ValueError, match="SDK binary digest does not match"): + list(iter_sync_windows(path, required_streams=ALL, alignment=model)) + + +def test_recording_bound_alignment_can_read_legacy_identity_without_sdk_digest( + recording, +): + identity = { + key: value for key, value in IDENTITY.items() if key != "sdk_binary_sha256" + } + path, model = recording(recorded_identity=identity) + del model["identity"]["sdk_binary_sha256"] + windows = list(iter_sync_windows(path, required_streams=ALL, alignment=model)) + assert len(windows) == 3 + assert all(window.complete for window in windows) + + +def test_local_clock_transition_does_not_hide_a_valid_raw_time_relation(recording): + path, _ = recording(capture_profile=True, local_transition=True) + windows = list(iter_sync_windows(path, required_streams=ALL[:-1])) + assert len(windows) == 3 + assert all(window.complete for window in windows) + assert ( + windows[0].left_rgb.record.sample_time_local_ns + > windows[1].left_rgb.record.sample_time_local_ns + ) + assert windows[0].start_time_ns < windows[1].start_time_ns + + +def test_recording_bound_clock_drift_remains_supported(recording): + path, model = recording() + model["epochs"][0]["streams"]["gyro"]["scale"] = 1.00001 + windows = list(iter_sync_windows(path, required_streams=ALL, alignment=model)) + assert len(windows) == 3 + assert all(window.complete for window in windows) + assert windows[0].gyro[1].time_ns == ORIGIN + 1_000_010 + + +def test_populated_spatial_calibration_is_decoded_with_timing_provenance(recording): + from mcap.reader import make_reader + from isaaccapture.ego._records import RecordDecoder + + path, _ = recording() + with path.open("rb") as source: + schema, channel, message = next( + make_reader(source).iter_messages(log_time_order=False) + ) + assert channel.topic == "ego_calibration/Calibration" + data = RecordDecoder(schema.data, schema.name).decode(message.data)["data"] + assert data["color_left"]["width"] == 16 + assert data["color_left"]["distortion"] == pytest.approx([0.1, 0.2, 0.3, 0.4]) + assert data["left_to_right"]["rotation"] == [1, 0, 0, 0, 1, 0, 0, 0, 1] + assert data["gyro_to_left"]["translation_mm"] == [4, 5, 6] + assert json.loads(data["time_alignment"]["json"])["status"] == "unverified" + + +def test_right_window_reference_uses_left_model_clock(recording): + path, model = recording() + model["epochs"][0]["streams"]["right_rgb"]["reference_anchor_ns"] -= 300_000 + windows = list( + iter_sync_windows( + path, required_streams=ALL, alignment=model, reference_stream="right_rgb" + ) + ) + assert len(windows) == 3 + assert all(window.complete for window in windows) + assert windows[0].start_time_ns == ORIGIN + assert windows[0].right_rgb.record.sample_time_raw_ns == ORIGIN + 300_000 + + +def test_clock_regression_is_reported_instead_of_hidden_by_sort(recording): + path, model = recording(raw_regression=True) + with pytest.raises(ValueError, match="nonmonotonic raw"): + list(iter_sync_windows(path, required_streams=ALL, alignment=model)) + report = analyze_recording_sync(path) + assert "nonmonotonic raw" in report["issues"][0] + assert not report["five_stream_verified"] + + +def test_unrequested_imu_clock_failure_does_not_block_stereo(recording): + path, model = recording(raw_regression=True) + windows = list(iter_sync_windows(path, required_streams=ALL[:2], alignment=model)) + assert all(window.complete for window in windows) + assert not any(window.accel or window.gyro or window.audio for window in windows) + report = analyze_recording_sync(path, required_streams=ALL[:2], alignment=model) + assert report["alignment"]["status"] == "validated" + assert "nonmonotonic raw" in report["issues"][0] + + +def test_embedded_audio_timing_must_match_its_index(recording): + with pytest.raises( + ValueError, match="Embedded audio disagrees with its metadata: timing" + ): + recording(embedded=True, pcm_timing_mismatch=True) + + +def test_zero_length_audio_is_rejected(recording): + with pytest.raises( + ValueError, match="Audio sample count, rate, and channels must be positive" + ): + recording(audio_chunks=((0, 0),)) + + +def test_imu_vectors_are_immutable_views_without_per_sample_dicts(recording): + from mcap.reader import make_reader + from isaaccapture.ego._records import RecordDecoder + + path, _ = recording(imu_samples=2000) + with path.open("rb") as source: + for schema, channel, message in make_reader(source).iter_messages(): + if channel.topic == "ego_imu/Accel": + record = RecordDecoder(schema.data, schema.name).decode(message.data) + samples = record["data"]["samples"] + assert isinstance(samples, np.ndarray) + assert samples.shape == (2000,) + assert samples.nbytes == 2000 * 48 + assert not samples.flags.owndata + assert not samples.flags.writeable + assert ( + samples[1999]["sample_time_raw_device_clock_ns"] + == ORIGIN + 2_001_000_000 + ) + with pytest.raises(ValueError, match="read-only"): + samples[0]["x_si"] = 0 + break + else: + pytest.fail("Fixture did not contain an IMU record") + + +def test_continuity_flag_is_not_a_truthy_string(recording): + path, model = recording() + model["epochs"][0]["streams"]["audio"]["segments"][0]["continuity_verified"] = ( + "false" + ) + with pytest.raises(ValueError, match="continuity_verified must be a boolean"): + list(iter_sync_windows(path, required_streams=ALL, alignment=model)) + + +def test_sidecar_hash_and_external_path_are_checked(recording, tmp_path): + path, model = recording() + alignment_path = tmp_path / "alignment.json" + alignment_path.write_text(json.dumps(model)) + assert ( + len( + list( + iter_sync_windows(path, required_streams=ALL, alignment=alignment_path) + ) + ) + == 3 + ) + media = path.parent / "raw/ColorLeft.h264" + media.write_bytes(b"xxxx" * 4) + with pytest.raises(ValueError, match="fingerprint"): + list(iter_sync_windows(path, required_streams=ALL, alignment=alignment_path)) + + +def test_truncated_flatbuffer_is_rejected(recording): + from mcap.reader import make_reader + from isaaccapture.ego._records import RecordDecoder + + path, _ = recording() + with path.open("rb") as source: + schema, _, message = next(make_reader(source).iter_messages()) + decoder = RecordDecoder(schema.data, schema.name) + with pytest.raises(ValueError, match="Truncated or invalid FlatBuffer"): + decoder.decode(message.data[:15]) diff --git a/tests/python/core/schema/test_ego_camera.py b/tests/python/core/schema/test_ego_camera.py new file mode 100644 index 0000000000..1cc1cee306 --- /dev/null +++ b/tests/python/core/schema/test_ego_camera.py @@ -0,0 +1,241 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +import gc + +import pytest + +from isaaccapture.schema import ( + DeviceDataTimestamp, + EgoFrameMetadata, + EgoFrameMetadataRecord, + EgoAudioChunk, + EgoAudioChunkRecord, + EgoCalibration, + EgoCalibrationRecord, + EgoCameraIntrinsics, + EgoCameraStream, + EgoCaptureHealth, + EgoRecoveryStage, + EgoDevicePropertyValue, + EgoDiagnosticEvent, + EgoSdkReport, + EgoCaptureSummary, + EgoRecoveryEvent, + EgoDeviceSnapshot, + EgoDeviceState, + EgoDeviceStateRecord, + EgoEncodedVideoFrame, + EgoEncodedVideoFrameRecord, + EgoExtrinsics, + EgoFrameMetadataEntry, + EgoImuBatch, + EgoImuBatchRecord, + EgoImuSample, + EgoImuSensor, + EgoPcmAudioChunk, + EgoPcmAudioChunkRecord, + EgoPixelFormat, +) + + +def test_camera_payload_and_retained_record_views(): + metadata = EgoFrameMetadata( + stream=EgoCameraStream.ColorRight, + sequence_number=2**64 - 1, + width=1600, + height=1300, + fps=30, + pixel_format=EgoPixelFormat.H265, + encoded_bytes=123, + sdk_metadata=[EgoFrameMetadataEntry(1, 99)], + capture_epoch=3, + ) + assert metadata.stream == EgoCameraStream.ColorRight + assert metadata.sequence_number == 2**64 - 1 + assert metadata.sdk_metadata[0].value == 99 + assert "ColorRight" in repr(metadata) + with pytest.raises(AttributeError): + metadata.capture_epoch = 4 + + record = EgoFrameMetadataRecord(metadata, DeviceDataTimestamp(1, 2, 3)) + payload = record.data + timestamp = record.timestamp + del metadata, record + gc.collect() + assert payload.capture_epoch == 3 + assert timestamp.sample_time_raw_device_clock == 3 + assert EgoFrameMetadataRecord(None, DeviceDataTimestamp(1, 2, 3)).data is None + + +def test_auxiliary_payloads_and_nested_calibration(): + imu = EgoImuBatch( + sensor=EgoImuSensor.Gyro, + sample_rate_hz=1000, + samples=[EgoImuSample(1, 2, 3, 24, 100, 200)], + capture_epoch=2, + ) + assert imu.samples[0].z_si == 3 + assert imu.samples[0].sample_time_raw_device_clock_ns == 200 + audio = EgoAudioChunk(sample_rate_hz=48000, wav_data_offset=44, capture_epoch=2) + assert audio.wav_data_offset == 44 + intrinsics = EgoCameraIntrinsics(width=1600, fx=800, distortion=[1, 2]) + calibration = EgoCalibration( + device_uid="ego", + color_left=intrinsics, + left_to_right=EgoExtrinsics(rotation=[1, 0, 0], translation_mm=[60, 0, 0]), + capture_epoch=2, + ) + left = calibration.color_left + del calibration, intrinsics + gc.collect() + assert left.width == 1600 + assert left.distortion == [1, 2] + assert EgoCalibration().color_left is None + + state = EgoDeviceState( + "ego", + 2, + 100, + EgoCaptureSummary( + EgoCaptureHealth.Warning, "queue warning", 4096, 3482, 0, 0, False + ), + ) + assert state.event.capture_health == EgoCaptureHealth.Warning + assert not hasattr(state.event, "properties") + assert not hasattr(state.event, "stage") + + +def test_embedded_video_and_pcm_record_views(): + video = EgoEncodedVideoFrame( + stream=EgoCameraStream.ColorRight, + encoded_data=[0, 0, 0, 1, 0x65], + capture_epoch=2, + ) + audio = EgoPcmAudioChunk( + sample_rate_hz=48000, pcm_data=[1, 0, 2, 0], capture_epoch=2 + ) + timestamp = DeviceDataTimestamp(100, 90, 3) + video_record = EgoEncodedVideoFrameRecord(video, timestamp) + audio_record = EgoPcmAudioChunkRecord(audio, timestamp) + assert video_record.data.encoded_data == [0, 0, 0, 1, 0x65] + assert audio_record.data.pcm_data == [1, 0, 2, 0] + assert audio_record.timestamp.sample_time_local_common_clock == 90 + + +def test_no_tracked_wrapper_compatibility_layer(): + import isaaccapture.schema as schema + + assert not hasattr(schema, "EgoFrameMetadataTrackedT") + assert not hasattr(schema, "EgoAudioChunkTrackedT") + + +@pytest.mark.parametrize( + ("payload_type", "record_type"), + [ + (EgoFrameMetadata, EgoFrameMetadataRecord), + (EgoImuBatch, EgoImuBatchRecord), + (EgoAudioChunk, EgoAudioChunkRecord), + (EgoCalibration, EgoCalibrationRecord), + (EgoDeviceState, EgoDeviceStateRecord), + (EgoEncodedVideoFrame, EgoEncodedVideoFrameRecord), + (EgoPcmAudioChunk, EgoPcmAudioChunkRecord), + ], +) +def test_every_record_maps_absence_and_retains_immutable_payload( + payload_type, record_type +): + timestamp = DeviceDataTimestamp(100, 90, 80) + assert record_type(None, timestamp).data is None + payload = ( + payload_type("ego", 3, 90, EgoSdkReport(0, 0, 0, 0, 0)) + if payload_type is EgoDeviceState + else payload_type(capture_epoch=3) + ) + record = record_type(payload, timestamp) + payload = record.data + del record + gc.collect() + assert payload.capture_epoch == 3 + assert not hasattr(payload, "data") + with pytest.raises(AttributeError): + payload.capture_epoch = 4 + + +@pytest.mark.parametrize( + "payload,tag,field,value,absent", + [ + ( + EgoSdkReport(7, 2, 0, 0, 123), + EgoDiagnosticEvent.SdkReport, + "error_flags", + 0, + "imu_temperature_c", + ), + ( + EgoCaptureSummary(EgoCaptureHealth.Healthy, "", 4096, 1, 0, 0, True), + EgoDiagnosticEvent.CaptureSummary, + "dropped_events", + 0, + "work_mode", + ), + ( + EgoRecoveryEvent(EgoRecoveryStage.Recovered, "stream resumed", 4), + EgoDiagnosticEvent.RecoveryEvent, + "stage", + EgoRecoveryStage.Recovered, + "capture_health", + ), + ( + EgoDeviceSnapshot(0.0, []), + EgoDiagnosticEvent.DeviceSnapshot, + "imu_temperature_c", + 0.0, + "storage_free_bytes", + ), + ], +) +def test_typed_events_own_only_their_fields(payload, tag, field, value, absent): + state = EgoDeviceState("ego", 3, 100, payload) + assert state.diagnostic_version == 2 + assert state.event_type == tag + assert state.observed_at_ns == 100 + assert getattr(state.event, field) == value + assert not hasattr(state.event, absent) + assert not hasattr(state, "valid_fields") + with pytest.raises(AttributeError): + setattr(state.event, field, value) + + +def test_snapshot_optional_temperature_and_retained_union_view(): + snapshot = EgoDeviceSnapshot(None, [EgoDevicePropertyValue(279, 0)]) + state = EgoDeviceState("ego", 2, 100, snapshot) + record = EgoDeviceStateRecord(state, DeviceDataTimestamp(110, 100, 0)) + event = record.data.event + del record, state, snapshot + gc.collect() + assert event.imu_temperature_c is None + assert event.properties[0].value == 0 + assert EgoDeviceSnapshot(0.0, []).imu_temperature_c == 0.0 + assert EgoSdkReport(1, 0, 0, 17, 0).error_flags == 17 + assert EgoSdkReport(2, 0, 0, 0, 0).error_flags == 0 + + +@pytest.mark.parametrize("temperature", [float("nan"), float("inf"), -float("inf")]) +def test_snapshot_rejects_nonfinite_temperature(temperature): + with pytest.raises(ValueError): + EgoDeviceSnapshot(temperature, []) + + +def test_event_constructor_requires_its_observations(): + for cls in (EgoSdkReport, EgoCaptureSummary, EgoRecoveryEvent, EgoDeviceSnapshot): + with pytest.raises(TypeError): + cls() + with pytest.raises(TypeError): + EgoDeviceState("ego", 0, 100, None) + with pytest.raises(TypeError): + EgoDeviceState("ego", 0, 100, EgoCalibration()) + with pytest.raises(ValueError): + EgoDeviceState("ego", 0, 0, EgoSdkReport(0, 0, 0, 0, 0)) + with pytest.raises(ValueError): + EgoDeviceState("", 0, 100, EgoSdkReport(0, 0, 0, 0, 0)) diff --git a/tests/python/core/teleop_session_manager/test_teleop_session.py b/tests/python/core/teleop_session_manager/test_teleop_session.py index 7c37700560..f53de10c9d 100644 --- a/tests/python/core/teleop_session_manager/test_teleop_session.py +++ b/tests/python/core/teleop_session_manager/test_teleop_session.py @@ -707,12 +707,19 @@ def get_plugin_names(self): def get_plugin_info(self, plugin_name): return SimpleNamespace(name=plugin_name, devices=()) - def start(self, plugin_name, plugin_root_id, plugin_args=None): + def start( + self, + plugin_name, + plugin_root_id, + plugin_args=None, + shutdown_timeout_seconds=2.0, + ): self.start_calls.append( { "plugin_name": plugin_name, "plugin_root_id": plugin_root_id, "plugin_args": plugin_args, + "shutdown_timeout_seconds": shutdown_timeout_seconds, } ) return self._contexts[plugin_name] @@ -1515,8 +1522,9 @@ def test_no_plugins_configured(self): assert len(session.plugin_managers) == 0 assert len(session.plugin_contexts) == 0 - def test_plugin_args_passed_through(self, tmp_path): - """Plugin args from config are forwarded to manager.start().""" + @pytest.mark.parametrize("shutdown_timeout_seconds", [2.0, 15.0]) + def test_plugin_args_passed_through(self, tmp_path, shutdown_timeout_seconds): + """Plugin launch and teardown settings reach manager.start().""" pipeline = MockPipeline(leaf_nodes=[]) mock_pm = MockPluginManager(plugin_names=["test_plugin"]) @@ -1527,6 +1535,7 @@ def test_plugin_args_passed_through(self, tmp_path): search_paths=[tmp_path], enabled=True, plugin_args=["--flag", "value"], + shutdown_timeout_seconds=shutdown_timeout_seconds, ) config = make_config(pipeline, plugins=[plugin_config]) @@ -1538,6 +1547,7 @@ def test_plugin_args_passed_through(self, tmp_path): assert call["plugin_name"] == "test_plugin" assert call["plugin_root_id"] == "/root" assert call["plugin_args"] == ["--flag", "value"] + assert call["shutdown_timeout_seconds"] == shutdown_timeout_seconds def test_plugin_args_default_empty(self, tmp_path): """Plugin args default to an empty list when not specified.""" @@ -1627,8 +1637,15 @@ def test_device_plugin_is_monitored(self, tmp_path, plugin): tracker_cls.assert_called_once_with(f"{plugin}/device_status") assert session.get_device_status(f"{plugin}/{plugin}") is not None - def test_only_manus_is_monitored_and_plugin_launch_order_is_preserved( - self, tmp_path + @pytest.mark.parametrize( + "plugin_name,root,path", + [ + ("manus_hand_plugin", "/manus", "/hand/left"), + ("ego_camera", "ego_instance", "/camera/ego"), + ], + ) + def test_supported_inventory_preserves_plugin_launch_order( + self, tmp_path, plugin_name, root, path ): class StatusTracker: def __init__(self): @@ -1646,18 +1663,16 @@ def get_process_snapshot(self): self.process_poll_count += 1 return super().get_process_snapshot() - manager = MockPluginManager(plugin_names=["legacy_plugin", "manus_hand_plugin"]) + manager = MockPluginManager(plugin_names=["legacy_plugin", plugin_name]) legacy_context = CountingContext() manus_context = CountingContext() manager._contexts["legacy_plugin"] = legacy_context - manager._contexts["manus_hand_plugin"] = manus_context + manager._contexts[plugin_name] = manus_context manager.get_plugin_info = MagicMock( return_value=SimpleNamespace( name="Manus", devices=( - SimpleNamespace( - path="/hand/left", type="hand", description="Left hand" - ), + SimpleNamespace(path=path, type="hand", description="Left hand"), ), ) ) @@ -1672,8 +1687,8 @@ def get_process_snapshot(self): search_paths=[tmp_path], ), PluginConfig( - plugin_name="manus_hand_plugin", - plugin_root_id="/manus", + plugin_name=plugin_name, + plugin_root_id=root, search_paths=[tmp_path], ), ], @@ -1691,12 +1706,12 @@ def get_process_snapshot(self): ): session = TeleopSession(config) with session: - tracker_cls.assert_called_once_with("/manus/device_status") - manager.get_plugin_info.assert_called_once_with("manus_hand_plugin") + tracker_cls.assert_called_once_with(f"{root}/device_status") + manager.get_plugin_info.assert_called_once_with(plugin_name) assert collected_trackers == [tracker] assert [call["plugin_name"] for call in manager.start_calls] == [ "legacy_plugin", - "manus_hand_plugin", + plugin_name, ] assert len(session.plugin_managers) == 2 assert len(session.plugin_contexts) == 2 @@ -1704,11 +1719,11 @@ def get_process_snapshot(self): snapshot = session.get_status() assert {provider.id for provider in snapshot.providers} == { "openxr/runtime", - "plugin//manus", + f"plugin/{root}", } assert {device.id for device in snapshot.devices} == { "openxr/headset", - "/manus/hand/left", + f"{root}{path}", } assert session.get_provider_status("plugin//legacy") is None assert session.get_device_status("/legacy/device") is None @@ -2893,6 +2908,17 @@ def test_plugin_config_defaults(self, tmp_path): assert config.enabled is True assert config.required is False + assert config.shutdown_timeout_seconds == 2.0 + + @pytest.mark.parametrize("timeout", [0.0, -1.0, float("nan"), float("inf")]) + def test_plugin_config_rejects_invalid_shutdown_timeout(self, tmp_path, timeout): + with pytest.raises(ValueError, match="finite and positive"): + PluginConfig( + plugin_name="test", + plugin_root_id="/root", + search_paths=[tmp_path], + shutdown_timeout_seconds=timeout, + ) def test_plugin_config_disabled_and_required(self, tmp_path): """PluginConfig accepts explicit lifecycle policy flags.""" diff --git a/tests/python/examples/camera_viz/test_ego_and_stereo_sbs.py b/tests/python/examples/camera_viz/test_ego_and_stereo_sbs.py new file mode 100644 index 0000000000..eb8a2b3681 --- /dev/null +++ b/tests/python/examples/camera_viz/test_ego_and_stereo_sbs.py @@ -0,0 +1,338 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +"""EGO source configuration and window-only stereo adapter contracts.""" + +from contextlib import nullcontext +from dataclasses import fields +from types import SimpleNamespace +import sys +from unittest.mock import MagicMock + +import numpy as np +import pytest +import yaml + +import config +from pipeline import Frame, FrameSource, SourceSpec +from repo_paths import repo_root +from sources import build_local_camera +from sources.stereo_sbs import StereoSideBySideSource + + +class StaticSource(FrameSource): + def __init__(self, frame=None): + self.frame = frame + self.started = False + + @property + def spec(self): + return SourceSpec("ego", 2, 1) + + def start(self): + self.started = True + + def stop(self): + self.started = False + + def latest(self): + frame, self.frame = self.frame, None + return frame + + +def test_example_is_stereo_and_defaults_to_off_without_opening_hardware(capsys): + path = repo_root() / "examples/camera_viz/configs/ego.yaml" + cfg = yaml.safe_load(path.read_text()) + assert config.resolve_stereo_debug(cfg, None, "window") == "off" + (source,) = build_local_camera(cfg["cameras"][0]) + assert (source.spec.width, source.spec.height) == (1600, 1300) + assert source.latest() is None + config.build_local_entries(cfg, is_xr=False) + assert capsys.readouterr().err == "" + + +@pytest.mark.parametrize("field", ["width", "height", "fps"]) +def test_ego_rejects_bad_dimensions_before_sdk_start(field): + with pytest.raises(ValueError, match="positive"): + build_local_camera({"name": "ego", "type": "ego", "stereo": True, field: 0}) + + +def test_ego_requires_stereo(): + with pytest.raises(ValueError, match="stereo"): + build_local_camera({"name": "ego", "type": "ego"}) + + +def test_ego_stop_retains_a_handle_owned_by_a_live_sdk_thread(): + (source,) = build_local_camera({"name": "ego", "type": "ego", "stereo": True}) + joins = [] + thread = SimpleNamespace( + join=lambda timeout: joins.append(timeout), is_alive=lambda: True + ) + handle = object() + source._thread, source._capture = thread, handle + source.stop() + assert joins == [5.0] + assert source._thread is thread and source._capture is handle + assert source._stop.is_set() + + +@pytest.mark.parametrize("fails", [False, True]) +def test_ego_producer_closes_capture_after_normal_or_failed_exit(monkeypatch, fails): + (source,) = build_local_camera({"name": "ego", "type": "ego", "stereo": True}) + closed = [] + handle = SimpleNamespace(close=lambda: closed.append("close")) + + def capture_loop(): + source._capture = handle + if fails: + raise RuntimeError("decode failed") + + monkeypatch.setattr(source, "_capture_loop", capture_loop) + source._produce_loop() + assert closed == ["close"] + assert source._capture is None + + +@pytest.mark.parametrize( + "requested,renderer,current,expected", + [(0, 2, 0, 2), (0, 0, 2, 0), (2, 2, 0, 2), (0, None, 2, 2), (1, None, 2, 1)], +) +def test_ego_uses_renderer_gpu_or_standalone_selection( + monkeypatch, requested, renderer, current, expected +): + class Thread: + def __init__(self, **kwargs): + self.started = False + + def start(self): + self.started = True + + devices = [] + + def device(identifier): + devices.append(identifier) + return nullcontext() + + monkeypatch.setitem( + sys.modules, + "cupy", + SimpleNamespace( + empty=np.empty, + uint8=np.uint8, + cuda=SimpleNamespace( + Device=device, runtime=SimpleNamespace(getDevice=lambda: current) + ), + ), + ) + monkeypatch.setitem(sys.modules, "ego_preview", SimpleNamespace(Capture=object)) + monkeypatch.setattr("sources.ego.threading.Thread", Thread) + cfg = { + "cameras": [{"name": "ego", "type": "ego", "stereo": True, "gpu_id": requested}] + } + (entry,) = config.build_local_entries( + cfg, is_xr=False, renderer_cuda_device_id=renderer + ) + source = entry.source + source.start() + assert devices == [expected] and source._gpu_id == expected + assert source._thread.started + + +@pytest.mark.parametrize("requested,renderer", [(1, 2), (2, 0), (-1, 0), (0, -1)]) +def test_ego_rejects_gpu_conflicts_before_imports_or_allocation( + monkeypatch, requested, renderer +): + monkeypatch.setitem(sys.modules, "cupy", None) + monkeypatch.setitem(sys.modules, "ego_preview", None) + (source,) = build_local_camera( + {"name": "ego", "type": "ego", "stereo": True, "gpu_id": requested}, + renderer_cuda_device_id=renderer, + ) + with pytest.raises(ValueError, match="CUDA"): + source.start() + assert source._capture is None and source._thread is None + assert not source._buffers_left and not source._buffers_right + + +def test_renderer_gpu_context_does_not_change_other_sources(monkeypatch): + # SyntheticSource allocates in __init__; factory tests must stay GPU-free. + constructor = MagicMock() + monkeypatch.setattr("sources.SyntheticSource", constructor) + spec = {"name": "synthetic", "type": "synthetic", "width": 4, "height": 2} + (source,) = build_local_camera(spec, renderer_cuda_device_id=2) + assert source is constructor.return_value + constructor.assert_called_once_with( + name="synthetic", width=4, height=2, fps=60.0, hue_speed_hz=0.25 + ) + + +def test_ego_mjpeg_conversion_drop_keeps_stereo_publication_and_closes_sdk(monkeypatch): + class Array(np.ndarray): + def set(self, data): + np.copyto(self, data) + + class Stream: + def __enter__(self): + return self + + def __exit__(self, *exc): + return None + + def synchronize(self): + pass + + (source,) = build_local_camera( + { + "name": "ego", + "type": "ego", + "stereo": True, + "format": "mjpg", + "width": 2, + "height": 1, + } + ) + left, right = bytes([1, 2, 3, 4, 5, 6]), bytes([7, 8, 9, 10, 11, 12]) + closed = [] + + class Capture: + def __init__(self, *args): + self.calls = 0 + + def next_pair(self, timeout): + self.calls += 1 + if self.calls == 2: + return {"left": left, "right": right, "timestamp_ns": 123} + if self.calls == 3: + source._stop.set() + return None + + def close(self): + closed.append(True) + + monkeypatch.setitem(sys.modules, "ego_preview", SimpleNamespace(Capture=Capture)) + monkeypatch.setitem( + sys.modules, + "cupy", + SimpleNamespace( + empty=lambda shape, dtype: np.empty(shape, dtype=dtype).view(Array), + uint8=np.uint8, + cuda=SimpleNamespace( + Device=lambda device: nullcontext(), + Stream=lambda **kwargs: Stream(), + ), + ), + ) + monkeypatch.setattr("sources.ego.alloc_pinned_host", np.empty) + source._buffers_left = [np.full((1, 2, 4), 255, dtype=np.uint8)] + source._buffers_right = [np.full((1, 2, 4), 255, dtype=np.uint8)] + source._produce_loop() + frame = source.latest() + assert frame is not None and frame.timestamp_ns == 123 + np.testing.assert_array_equal(frame.image[..., :3], [[[1, 2, 3], [4, 5, 6]]]) + np.testing.assert_array_equal( + frame.image_right[..., :3], [[[7, 8, 9], [10, 11, 12]]] + ) + assert source._frames == 1 and source._incomplete_pairs == 2 + assert source.latest() is None + assert closed == [True] and source._capture is None + + +def test_sbs_cli_override_and_xr_rejection(): + cfg = {"display": {"window": {"stereo_debug": "sbs"}}} + assert config.resolve_stereo_debug(cfg, "off", "xr") == "off" + assert config.resolve_stereo_debug(cfg, None, "window") == "sbs" + with pytest.raises(ValueError, match="requires --mode window"): + config.resolve_stereo_debug(cfg, None, "xr") + with pytest.raises(ValueError, match="off\\|sbs"): + config.resolve_stereo_debug({}, "invalid", "window") + + +def test_sbs_preserves_nv_entry_fields_and_mono_identity(): + source = StaticSource() + entry = config.SourceEntry( + source, + object(), + stereo=True, + stereo_plane_distance_cm=3, + lock_mode="world", + placement_config=object(), + compositor="televiz", + cylinder_radius_m=4, + cylinder_angle_deg=120, + equirect_yaw_deg=30, + ) + mono = config.SourceEntry(StaticSource(), None) + result = config.apply_stereo_debug([entry, mono], "sbs") + assert not result[0].stereo + assert isinstance(result[0].source, StereoSideBySideSource) + assert result[1] is mono + for field in fields(entry): + if field.name not in ("source", "stereo"): + assert getattr(result[0], field.name) == getattr(entry, field.name) + + +def test_sbs_rejects_a_mono_frame_from_a_declared_stereo_source(): + wrapper = StereoSideBySideSource(StaticSource(Frame(object(), 123, "ego"))) + with pytest.raises(RuntimeError, match="requires a paired source"): + wrapper.latest() + + +def test_sbs_copies_left_then_right_on_the_frame_device_and_stream(monkeypatch): + class Array(np.ndarray): + @property + def device(self): + return SimpleNamespace(id=2) + + class Stream: + def __init__(self, pointer): + self.pointer = pointer + self.synced = False + + def __enter__(self): + return self + + def __exit__(self, *exc): + return None + + def synchronize(self): + self.synced = True + + streams, devices = [], [] + + def external_stream(pointer): + stream = Stream(pointer) + streams.append(stream) + return stream + + def device(identifier): + devices.append(identifier) + return nullcontext() + + monkeypatch.setitem( + sys.modules, + "cupy", + SimpleNamespace( + empty=lambda shape, dtype: np.empty(shape, dtype=dtype).view(Array), + uint8=np.uint8, + cuda=SimpleNamespace(Device=device, ExternalStream=external_stream), + ), + ) + left = np.full((1, 2, 4), 17, dtype=np.uint8).view(Array) + right = np.full((1, 2, 4), 29, dtype=np.uint8).view(Array) + source = StaticSource(Frame(left, 123, "ego", stream=456, image_right=right)) + wrapper = StereoSideBySideSource(source) + wrapper.start() + frame = wrapper.latest() + assert frame.image.shape == (1, 4, 4) + np.testing.assert_array_equal(frame.image[:, :2], left) + np.testing.assert_array_equal(frame.image[:, 2:], right) + assert (frame.timestamp_ns, frame.source_id, frame.stream, frame.image_right) == ( + 123, + "ego.sbs", + 456, + None, + ) + assert devices == [2] + assert streams[0].pointer == 456 and streams[0].synced + assert wrapper.latest() is None + wrapper.stop() + assert not source.started diff --git a/tests/python/examples/camera_viz/test_ego_failures.py b/tests/python/examples/camera_viz/test_ego_failures.py new file mode 100644 index 0000000000..2285999e47 --- /dev/null +++ b/tests/python/examples/camera_viz/test_ego_failures.py @@ -0,0 +1,361 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +"""EGO producer failures reach NV's runner without changing SDK retry policy.""" + +import sys +import threading +from types import SimpleNamespace +from unittest.mock import MagicMock + +import numpy as np +import pytest +import yaml + +from pipeline import VizRunner +from sources.ego import EgoSource +from sources.stereo_sbs import StereoSideBySideSource + + +@pytest.fixture +def preview(monkeypatch): + cp = MagicMock() + cp.uint8 = np.uint8 + cp.empty.side_effect = np.empty + cp.cuda.runtime.getDevice.return_value = 0 + capture = MagicMock() + monkeypatch.setitem(sys.modules, "cupy", cp) + monkeypatch.setitem(sys.modules, "ego_preview", SimpleNamespace(Capture=capture)) + source = EgoSource("ego-test", width=2, height=1) + yield source, cp, capture + source.stop() + + +def join_producer(source): + source._thread.join(timeout=2) + assert not source._thread.is_alive(), "EGO producer did not exit" + + +@pytest.mark.parametrize("failure_type", [RuntimeError, SystemExit]) +@pytest.mark.parametrize("sbs", [False, True]) +def test_producer_failure_reaches_consumer_before_sdk_close( + preview, monkeypatch, failure_type, sbs +): + source, _, capture = preview + failure = failure_type("injected CUDA failure") + closing = threading.Event() + release = threading.Event() + + def close(): + closing.set() + assert release.wait(timeout=3) + + capture.return_value.close.side_effect = close + + def produce(): + source._capture = capture.return_value + raise failure + + monkeypatch.setattr(source, "_capture_loop", produce) + consumer = StereoSideBySideSource(source) if sbs else source + try: + consumer.start() + assert closing.wait(timeout=2) + # Match source and cause text without fixing the wrapper punctuation. + with pytest.raises( + RuntimeError, match=r"ego-test.*injected CUDA failure" + ) as error: + consumer.latest() + assert error.value.__cause__ is failure + assert source._thread.is_alive() + finally: + release.set() + consumer.stop() + capture.return_value.close.assert_called_once() + + +@pytest.mark.parametrize("producer_fails", [False, True]) +def test_cleanup_failure_cannot_overwrite_producer_error( + preview, monkeypatch, producer_fails +): + source, _, capture = preview + failure = RuntimeError("decode failed") + cleanup = RuntimeError("SDK close failed") + capture.return_value.close.side_effect = cleanup + + def produce(): + source._capture = capture.return_value + if producer_fails: + raise failure + + monkeypatch.setattr(source, "_capture_loop", produce) + source.start() + join_producer(source) + with pytest.raises(RuntimeError) as error: + source.latest() + assert error.value.__cause__ is (failure if producer_fails else cleanup) + capture.return_value.close.assert_called_once() + + +def test_fatal_error_takes_precedence_over_unconsumed_frame(preview, monkeypatch): + source, _, _ = preview + + def produce(): + with source._lock: + source._publish_idx = 0 + source._timestamp_ns = 123 + raise RuntimeError("upload failed") + + monkeypatch.setattr(source, "_capture_loop", produce) + source.start() + join_producer(source) + with pytest.raises(RuntimeError, match="upload failed"): + source.latest() + + +def test_restart_clears_failure_and_mailbox_and_resolves_auto_gpu_again( + preview, monkeypatch +): + source, cp, _ = preview + + def fail(): + source._publish_idx = 0 + source._timestamp_ns = 123 + source._write_idx = 2 + source._frames = 8 + source._incomplete_pairs = 3 + raise RuntimeError("decode failed") + + monkeypatch.setattr(source, "_capture_loop", fail) + source.start() + join_producer(source) + source.stop() + cp.cuda.runtime.getDevice.return_value = 2 + monkeypatch.setattr(source, "_capture_loop", lambda: None) + source.start() + join_producer(source) + assert source.latest() is None + assert source._gpu_id == 2 + assert ( + source._timestamp_ns, + source._write_idx, + source._frames, + source._incomplete_pairs, + ) == (0, 0, 0, 0) + + +def test_failed_thread_start_allows_retry(preview, monkeypatch): + source, _, capture = preview + failure = RuntimeError("cannot start thread") + with monkeypatch.context() as patch: + patch.setattr(threading.Thread, "start", MagicMock(side_effect=failure)) + with pytest.raises(RuntimeError, match="cannot start thread"): + source.start() + assert source._thread is None + assert not source._buffers_left and not source._buffers_right + capture.assert_not_called() + monkeypatch.setattr(source, "_capture_loop", lambda: None) + source.start() + join_producer(source) + + +def test_interrupted_start_retains_thread_for_shutdown(preview, monkeypatch): + source, _, _ = preview + thread = MagicMock() + thread.start.side_effect = KeyboardInterrupt + thread.is_alive.return_value = False + monkeypatch.setattr("sources.ego.threading.Thread", lambda **kwargs: thread) + with pytest.raises(KeyboardInterrupt): + source.start() + assert source._thread is thread + assert source._buffers_left and source._buffers_right + source.stop() + thread.join.assert_called_once_with(timeout=5.0) + + +@pytest.mark.parametrize("codec", ["h264", "h265"]) +def test_reconnect_keeps_both_decoders_on_renderer_gpu(preview, monkeypatch, codec): + source, cp, capture = preview + source._format = codec + source._renderer_cuda_device_id = 2 + decoders = [MagicMock(), MagicMock()] + make_decoder = MagicMock(side_effect=decoders) + monkeypatch.setattr("sources.ego.NvVideoDecoder", make_decoder) + calls = 0 + + def next_pair(timeout): + nonlocal calls + calls += 1 + if calls == 1: + raise RuntimeError("transient SDK read failure") + source._stop.set() + return {"left": b"left", "right": b"right", "timestamp_ns": 123} + + capture.return_value.next_pair.side_effect = next_pair + source.start() + join_producer(source) + assert capture.call_count == 2 + assert capture.return_value.close.call_count == 2 + assert make_decoder.call_count == 2 + assert all( + c.kwargs == {"gpu_id": 2, "codec": codec} for c in make_decoder.call_args_list + ) + assert all(c.args == (2,) for c in cp.cuda.Device.call_args_list) + for decoder in decoders: + decoder.reset.assert_called_once() + decoder.decode.assert_called_once() + assert source.latest().timestamp_ns == 123 + + +def test_open_retry_is_cancellable_and_does_not_report_fatal_error(preview): + source, _, capture = preview + attempted = threading.Event() + + def open_device(*args): + attempted.set() + raise RuntimeError("EGO not found") + + capture.side_effect = open_device + source.start() + assert attempted.wait(timeout=2) + source.stop() + assert source.latest() is None + assert source._thread is None + + +@pytest.mark.parametrize("fails", [False, True]) +def test_decode_failure_is_fatal_but_waiting_for_output_is_not( + preview, monkeypatch, fails +): + source, _, capture = preview + decoder = MagicMock() + failure = RuntimeError("NVDEC failed") + if fails: + decoder.decode.side_effect = failure + else: + decoder.decode.return_value = False + monkeypatch.setattr("sources.ego.NvVideoDecoder", lambda *args, **kwargs: decoder) + + def next_pair(timeout): + source._stop.set() + return {"left": b"left", "right": b"right", "timestamp_ns": 123} + + capture.return_value.next_pair.side_effect = next_pair + source.start() + join_producer(source) + if fails: + with pytest.raises(RuntimeError, match="NVDEC failed") as error: + source.latest() + assert error.value.__cause__ is failure + else: + assert source.latest() is None + capture.return_value.close.assert_called_once() + + +@pytest.mark.parametrize("failure_kind", ["gpu-conflict", "producer"]) +def test_viewer_cleans_up_after_ego_startup_or_producer_failure( + preview, monkeypatch, tmp_path, failure_kind +): + import camera_viz + + _, _, capture = preview + events = [] + session = MagicMock() + session.is_xr_mode.return_value = False + session.should_close.return_value = False + session.cuda_device_id = 2 + session.destroy.side_effect = lambda: events.append("destroy") + capture.return_value.close.side_effect = lambda: events.append("close") + path = tmp_path / "ego.yaml" + path.write_text( + yaml.safe_dump( + { + "source": "local", + "display": {"mode": "window"}, + "cameras": [ + { + "name": "ego", + "type": "ego", + "stereo": True, + "width": 2, + "height": 1, + "gpu_id": 1 if failure_kind == "gpu-conflict" else 0, + } + ], + } + ) + ) + monkeypatch.setattr(camera_viz.signal, "signal", lambda *args: None) + monkeypatch.setattr( + camera_viz.display, "make_session", lambda *args, **kwargs: session + ) + monkeypatch.setattr( + camera_viz, + "_build_display", + lambda session, entries, shapes: ( + [entry.source for entry in entries], + [MagicMock()], + [None], + [{}], + ), + ) + dashboard = MagicMock() + dashboard.live = False + monkeypatch.setattr(camera_viz, "Dashboard", lambda: dashboard) + + def produce(source): + assert source._gpu_id == 2 + source._capture = capture() + raise RuntimeError("injected producer failure") + + monkeypatch.setattr(EgoSource, "_capture_loop", produce) + original_wait = VizRunner.wait + + def bounded_wait(runner, **kwargs): + runner._submit_thread.join(timeout=2) + runner._render_thread.join(timeout=2) + assert not runner._submit_thread.is_alive() + assert not runner._render_thread.is_alive() + return original_wait(runner, **kwargs) + + monkeypatch.setattr(VizRunner, "wait", bounded_wait) + expected = ( + "renderer uses CUDA GPU 2" + if failure_kind == "gpu-conflict" + else "injected producer failure" + ) + with pytest.raises((ValueError, RuntimeError), match=expected): + camera_viz.main([str(path)]) + assert events == ( + ["destroy"] if failure_kind == "gpu-conflict" else ["close", "destroy"] + ) + dashboard.close.assert_called_once() + if failure_kind == "gpu-conflict": + capture.assert_not_called() + + +def test_nv_runner_wait_reports_ego_producer_failure(preview, monkeypatch): + source, _, capture = preview + failure = RuntimeError("GPU upload failed") + + def produce(): + source._capture = capture.return_value + raise failure + + monkeypatch.setattr(source, "_capture_loop", produce) + session = MagicMock() + session.is_xr_mode.return_value = False + session.should_close.return_value = False + runner = VizRunner(session, [source], [MagicMock()]) + try: + runner.start() + runner._submit_thread.join(timeout=2) + runner._render_thread.join(timeout=2) + assert not runner._submit_thread.is_alive() + assert not runner._render_thread.is_alive() + with pytest.raises(RuntimeError, match="GPU upload failed") as error: + runner.wait() + assert error.value.__cause__ is failure + finally: + assert runner.stop() + assert source._thread is None + capture.return_value.close.assert_called_once() diff --git a/tests/python/examples/camera_viz/test_nv_decode_codec.py b/tests/python/examples/camera_viz/test_nv_decode_codec.py new file mode 100644 index 0000000000..e48ee84410 --- /dev/null +++ b/tests/python/examples/camera_viz/test_nv_decode_codec.py @@ -0,0 +1,53 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +"""Codec selection preserves the existing H.264 source API.""" + +import sys +from types import SimpleNamespace + +import pytest + +from sources._nv_decode import NvH264Decoder, NvVideoDecoder + + +@pytest.mark.parametrize( + "name, expected", [("h264", "h264"), ("h265", "hevc"), ("hevc", "hevc")] +) +def test_native_codec_selection_and_reset(monkeypatch, name, expected): + calls = [] + + class NativeDecoder: + def __init__(self, cfg): + calls.append(cfg) + + def decode(self, packet, output, width, height): + calls.append((packet, output, width, height)) + return True + + def reset(self): + calls.append("reset") + + monkeypatch.setitem( + sys.modules, + "codec", + SimpleNamespace( + DecoderConfig=SimpleNamespace, + DecoderCodec=SimpleNamespace(H264="h264", HEVC="hevc"), + VideoDecoder=NativeDecoder, + ), + ) + decoder = NvVideoDecoder(1600, 1300, gpu_id=2, codec=name) + assert calls == [] + output = object() + assert decoder.decode(b"packet", output) + assert calls[0].codec == expected + assert calls[0].gpu_id == 2 + assert calls[1] == (b"packet", output, 1600, 1300) + decoder.reset() + assert calls[-1] == "reset" + + +def test_legacy_h264_default_and_invalid_codec(): + assert NvH264Decoder(2, 1)._codec == "h264" + with pytest.raises(ValueError, match="unsupported decoder codec"): + NvVideoDecoder(2, 1, codec="vp9") diff --git a/tests/python/examples/mcap_record_replay/CMakeLists.txt b/tests/python/examples/mcap_record_replay/CMakeLists.txt new file mode 100644 index 0000000000..43804a15aa --- /dev/null +++ b/tests/python/examples/mcap_record_replay/CMakeLists.txt @@ -0,0 +1,26 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 ORBBEC INC. +# SPDX-License-Identifier: Apache-2.0 + +add_test( + NAME mcap_record_replay_ego + COMMAND uv run --python ${ISAAC_TELEOP_PYTHON_VERSION} --extra dev + pytest -v --tb=short test_ego_replay.py + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} +) +set_tests_properties(mcap_record_replay_ego PROPERTIES + ENVIRONMENT "PYTHONPATH=${CMAKE_BINARY_DIR}/python_package/$" + LABELS "mcap_record_replay" +) + +foreach(test_file test_ego_sync_cli test_ego_calibration_cli) + add_test( + NAME mcap_record_replay_${test_file} + COMMAND uv run --python ${ISAAC_TELEOP_PYTHON_VERSION} --extra dev + pytest -v --tb=short ${test_file}.py + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + ) + set_tests_properties(mcap_record_replay_${test_file} PROPERTIES + ENVIRONMENT "PYTHONPATH=${CMAKE_BINARY_DIR}/python_package/$" + LABELS "mcap_record_replay;ego" + ) +endforeach() diff --git a/tests/python/examples/mcap_record_replay/conftest.py b/tests/python/examples/mcap_record_replay/conftest.py new file mode 100644 index 0000000000..5098b95c1f --- /dev/null +++ b/tests/python/examples/mcap_record_replay/conftest.py @@ -0,0 +1,16 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 ORBBEC INC. +# SPDX-License-Identifier: Apache-2.0 + +import sys +from pathlib import Path + +_tests_python = next( + parent + for parent in Path(__file__).resolve().parents + if (parent / "repo_paths.py").is_file() +) +sys.path.insert(0, str(_tests_python)) + +from repo_paths import repo_root # noqa: E402 + +sys.path.insert(0, str(repo_root() / "examples" / "mcap_record_replay" / "python")) diff --git a/tests/python/examples/mcap_record_replay/pyproject.toml b/tests/python/examples/mcap_record_replay/pyproject.toml new file mode 100644 index 0000000000..d22c828dce --- /dev/null +++ b/tests/python/examples/mcap_record_replay/pyproject.toml @@ -0,0 +1,10 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 ORBBEC INC. +# SPDX-License-Identifier: Apache-2.0 + +[project] +name = "mcap-record-replay-tests" +version = "0.0.0" +requires-python = ">=3.11,<3.14" + +[project.optional-dependencies] +dev = ["pytest", "numpy", "mcap>=1.2.0"] diff --git a/tests/python/examples/mcap_record_replay/test_ego_calibration_cli.py b/tests/python/examples/mcap_record_replay/test_ego_calibration_cli.py new file mode 100644 index 0000000000..503c494590 --- /dev/null +++ b/tests/python/examples/mcap_record_replay/test_ego_calibration_cli.py @@ -0,0 +1,226 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 ORBBEC INC. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +import hashlib +import json + +import pytest + +from isaaccapture_examples.mcap_record_replay import calibrate_ego_sync + + +ORIGIN = 1_700_000_000_000_000_000 +DURATION = 1800 * 10**9 + + +def write_manifest(tmp_path, *, audio=False, error_ns=0): + capture = tmp_path / "capture" + capture.mkdir() + (capture / "metadata.mcap").write_bytes(b"original-mcap") + (capture / "raw").mkdir() + (capture / "raw" / "ColorLeft.h264").write_bytes(b"original-video") + (tmp_path / "events.csv").write_text("independent physical event annotations\n") + + def event(prefix, second): + return { + "event_id": f"{prefix}-{second}", + "source_value": second * 48_000 if audio else ORIGIN + second * 10**9, + "reference_time_ns": ORIGIN + + second * 10**9 + + (error_ns if prefix == "val" else 0), + "uncertainty_ns": 1000, + "evidence": "events.csv", + } + + clock = { + "calibration_events": [event("cal", second) for second in (0, 1, 2)], + "validation_events": [event("val", second) for second in (10, 900, 1790)], + "validation_start_ns": ORIGIN, + "validation_end_ns": ORIGIN + DURATION, + } + if audio: + clock.update(segment_id=7, first_sample_index=0, sample_count=1800 * 48_000) + streams = {"audio": {"segments": [clock]}} + else: + streams = {"gyro": clock} + manifest = { + "version": 1, + "model_id": "independent-events", + "recording": "capture/metadata.mcap", + "media_paths": ["capture/raw/ColorLeft.h264"], + "identity": { + "serial": "test-device", + "firmware": "1.1.1", + "sdk_version": "2.9.0", + }, + "profile": { + "video": {"format": "h264", "width": 1600, "height": 1300, "fps": 30}, + "imu_rate_hz": 1000, + "audio": {"sample_rate_hz": 48_000, "channels": 1, "bits_per_sample": 16}, + }, + "epochs": [ + { + "epoch": 0, + "valid_from_ns": ORIGIN, + "valid_to_ns": ORIGIN + DURATION, + "streams": streams, + } + ], + } + path = tmp_path / "events.json" + path.write_text(json.dumps(manifest)) + return path, manifest + + +def save_manifest(path, manifest): + path.write_text(json.dumps(manifest)) + + +def test_manifest_paths_resolve_independently_of_working_directory( + tmp_path, monkeypatch +): + path, _ = write_manifest(tmp_path) + other = tmp_path / "other" + other.mkdir() + monkeypatch.chdir(other) + result = calibrate_ego_sync.calibrate_manifest(path) + assert result["status"] == "validated" + assert result["epochs"][0]["streams"]["gyro"]["scale"] == 1 + assert result["recording_fingerprint"] == { + "mcap_sha256": hashlib.sha256(b"original-mcap").hexdigest(), + "media_sha256": { + "raw/ColorLeft.h264": hashlib.sha256(b"original-video").hexdigest() + }, + } + evidence_hash = hashlib.sha256((tmp_path / "events.csv").read_bytes()).hexdigest() + assert result["validation"]["evidence"] == [f"sha256:{evidence_hash}"] + assert result["validation"]["stream_reports"][0]["stream"] == "gyro" + + +def test_cli_writes_new_validated_document(tmp_path, capsys): + path, _ = write_manifest(tmp_path) + output = tmp_path / "alignment.json" + assert calibrate_ego_sync.main([str(path), "--output", str(output)]) == 0 + assert json.loads(output.read_text())["status"] == "validated" + assert "status=validated" in capsys.readouterr().out + + +def test_failed_independent_validation_is_saved_but_returns_failure(tmp_path): + path, _ = write_manifest(tmp_path, error_ns=20_000_000) + output = tmp_path / "alignment.json" + assert calibrate_ego_sync.main([str(path), "--output", str(output)]) == 2 + result = json.loads(output.read_text()) + assert result["status"] == "unverified" + assert result["validation"]["max_abs_error_ns"] == 20_000_000 + + +@pytest.mark.parametrize("continuity", [None, True]) +def test_audio_needs_independent_continuity_proof(tmp_path, continuity): + path, manifest = write_manifest(tmp_path, audio=True) + segment = manifest["epochs"][0]["streams"]["audio"]["segments"][0] + if continuity is not None: + segment["continuity_verified"] = continuity + save_manifest(path, manifest) + output = tmp_path / "alignment.json" + assert calibrate_ego_sync.main([str(path), "--output", str(output)]) == ( + 0 if continuity else 2 + ) + result = json.loads(output.read_text()) + assert result["epochs"][0]["streams"]["audio"]["segments"][0][ + "period_ns" + ] == pytest.approx(10**9 / 48_000) + assert ("audio_continuity_unverified" in result["validation"]["reasons"]) == ( + not continuity + ) + + +@pytest.mark.parametrize("symlink", [False, True]) +def test_cli_refuses_existing_output_without_changing_it(tmp_path, symlink): + path, _ = write_manifest(tmp_path) + output = tmp_path / "alignment.json" + if symlink: + output.symlink_to(tmp_path / "missing-target.json") + else: + output.write_text("keep this calibration") + with pytest.raises(SystemExit) as failure: + calibrate_ego_sync.main([str(path), "--output", str(output)]) + assert failure.value.code == 2 + if symlink: + assert output.is_symlink() + assert not output.exists() + else: + assert output.read_text() == "keep this calibration" + + +@pytest.mark.parametrize("bad_key", ["duplicate", "nonfinite"]) +def test_manifest_rejects_ambiguous_json(tmp_path, bad_key): + path = tmp_path / "events.json" + path.write_text( + '{"version":1,"version":2}' if bad_key == "duplicate" else '{"version":NaN}' + ) + with pytest.raises(ValueError, match=r"Duplicate|Nonfinite"): + calibrate_ego_sync.calibrate_manifest(path) + + +@pytest.mark.parametrize("coordinate", ["version", "fps", "source_value"]) +def test_manifest_rejects_boolean_integer_coordinates(tmp_path, coordinate): + path, manifest = write_manifest(tmp_path) + if coordinate == "version": + manifest["version"] = True + elif coordinate == "fps": + manifest["profile"]["video"]["fps"] = True + else: + manifest["epochs"][0]["streams"]["gyro"]["calibration_events"][0][ + "source_value" + ] = True + save_manifest(path, manifest) + with pytest.raises(ValueError, match="integer"): + calibrate_ego_sync.calibrate_manifest(path) + + +def test_missing_raw_evidence_does_not_create_a_model(tmp_path): + path, _ = write_manifest(tmp_path) + (tmp_path / "events.csv").unlink() + output = tmp_path / "alignment.json" + with pytest.raises(SystemExit) as failure: + calibrate_ego_sync.main([str(path), "--output", str(output)]) + assert failure.value.code == 2 + assert not output.exists() + + +def test_audio_annotations_must_belong_to_declared_sample_segment(tmp_path): + path, manifest = write_manifest(tmp_path, audio=True) + manifest["epochs"][0]["streams"]["audio"]["segments"][0]["sample_count"] = 48_000 + save_manifest(path, manifest) + with pytest.raises(ValueError, match="outside its declared sample segment"): + calibrate_ego_sync.calibrate_manifest(path) + + +def test_effective_epoch_cannot_exceed_validation_interval(tmp_path): + path, manifest = write_manifest(tmp_path) + manifest["epochs"][0]["valid_to_ns"] = ORIGIN + DURATION + 1 + save_manifest(path, manifest) + with pytest.raises(ValueError, match="independent validation bounds"): + calibrate_ego_sync.calibrate_manifest(path) + + +def test_effective_epoch_can_select_part_of_validated_interval(tmp_path): + path, manifest = write_manifest(tmp_path) + manifest["epochs"][0]["valid_from_ns"] = ORIGIN + 60 * 10**9 + manifest["epochs"][0]["valid_to_ns"] = ORIGIN + 900 * 10**9 + save_manifest(path, manifest) + result = calibrate_ego_sync.calibrate_manifest(path) + assert result["status"] == "validated" + mapping = result["epochs"][0]["streams"]["gyro"] + assert mapping["validation_start_ns"] == ORIGIN + assert mapping["validation_end_ns"] == ORIGIN + DURATION + + +def test_audio_continuity_flag_must_be_boolean(tmp_path): + path, manifest = write_manifest(tmp_path, audio=True) + manifest["epochs"][0]["streams"]["audio"]["segments"][0]["continuity_verified"] = ( + "true" + ) + save_manifest(path, manifest) + with pytest.raises(ValueError, match="continuity_verified must be a boolean"): + calibrate_ego_sync.calibrate_manifest(path) diff --git a/tests/python/examples/mcap_record_replay/test_ego_replay.py b/tests/python/examples/mcap_record_replay/test_ego_replay.py new file mode 100644 index 0000000000..cad1131692 --- /dev/null +++ b/tests/python/examples/mcap_record_replay/test_ego_replay.py @@ -0,0 +1,190 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 ORBBEC INC. +# SPDX-License-Identifier: Apache-2.0 + +from types import SimpleNamespace + +import pytest +from mcap.writer import CompressionType, IndexType, Writer + +from isaaccapture_examples.mcap_record_replay import replay_ego +from isaaccapture.schema import EgoRecoveryEvent, EgoRecoveryStage, EgoDiagnosticEvent + +TOPICS = { + "ego_metadata/ColorLeft", + "ego_metadata/ColorRight", + "ego_imu/Accel", + "ego_imu/Gyro", + "ego_audio/Audio", + "ego_calibration/Calibration", + "ego_device/DeviceState", +} + + +def write_recording(path, summary, topics=TOPICS): + with path.open("wb") as stream: + writer = Writer( + stream, + compression=CompressionType.NONE, + use_chunking=False, + index_types=IndexType.NONE, + repeat_schemas=summary, + repeat_channels=summary, + use_statistics=summary, + use_summary_offsets=summary, + ) + writer.start() + for topic in topics | {"ego_audio/Audio_tracked"}: + channel = writer.register_channel(topic, "test", 0) + for sequence in range(1, 4 if topic in topics else 10): + writer.add_message(channel, sequence, b"sample", sequence) + writer.finish() + + +class Replay: + def __init__(self): + self.updates = 0 + self.closed = False + + def __enter__(self): + return self + + def __exit__(self, *_): + self.closed = True + + def update(self): + self.updates += 1 + + +class Tracker: + def __init__(self, *_): + pass + + def get_data(self, session): + if session.updates == 2: + return None + return SimpleNamespace( + capture_epoch=1, + sequence_number=session.updates, + width=1600, + height=1300, + fps=30, + pixel_format="H264", + encoded_bytes=100, + sample_rate_hz=48000, + samples=[1], + sample_count=480, + byte_count=960, + device_uid="ego", + event_type=EgoDiagnosticEvent.RecoveryEvent, + diagnostic_version=2, + observed_at_ns=100, + event=EgoRecoveryEvent(EgoRecoveryStage.Recovered, "recovered", 1), + ) + + def get_stream_data(self, session, _): + return self.get_data(session) + + +def mock_trackers(monkeypatch): + for name in ( + "EgoFrameMetadataTracker", + "EgoImuTracker", + "EgoAudioTracker", + "EgoCalibrationTracker", + "EgoDeviceStateTracker", + ): + monkeypatch.setattr(replay_ego, name, Tracker) + session = Replay() + monkeypatch.setattr( + replay_ego, "ReplaySession", SimpleNamespace(run=lambda _: session) + ) + monkeypatch.setattr(replay_ego, "McapReplayConfig", lambda *_: None) + return session + + +@pytest.mark.parametrize("summary", [True, False]) +def test_gap_does_not_end_replay_and_tracked_topics_do_not_extend_it( + tmp_path, monkeypatch, capsys, summary +): + recording = tmp_path / "gap.mcap" + write_recording(recording, summary) + session = mock_trackers(monkeypatch) + counts = replay_ego.replay(recording, 0) + assert session.updates == 3 + assert session.closed + assert set(counts.values()) == {2} + assert "ColorLeft: epoch=1 seq=3" in capsys.readouterr().out + + +def test_explicit_update_limit_includes_gaps(tmp_path, monkeypatch): + recording = tmp_path / "gap.mcap" + write_recording(recording, True) + session = mock_trackers(monkeypatch) + counts = replay_ego.replay(recording, 2) + assert session.updates == 2 + assert set(counts.values()) == {1} + + +def test_recording_without_raw_topics_finishes_immediately(tmp_path, monkeypatch): + recording = tmp_path / "other.mcap" + write_recording(recording, True, {"head/head"}) + session = mock_trackers(monkeypatch) + counts = replay_ego.replay(recording, 0) + assert session.updates == 0 + assert set(counts.values()) == {0} + + +@pytest.mark.parametrize("enabled", [True, False]) +def test_recorded_policy_controls_diagnostic_tracker_registration( + tmp_path, monkeypatch, enabled +): + path = tmp_path / "policy.mcap" + with path.open("wb") as stream: + writer = Writer(stream, compression=CompressionType.NONE) + writer.start() + writer.add_metadata( + "ego.recording", + { + "record_device_diagnostics": "on" if enabled else "off", + "diagnostic_version": "2", + }, + ) + channel = writer.register_channel("ego_metadata/ColorLeft", "test", 0) + writer.add_message(channel, 1, b"test", 1) + writer.finish() + mock_trackers(monkeypatch) + counts = replay_ego.replay(path, 0) + assert ("DeviceState" in counts) == enabled + + +def test_off_rejects_diagnostic_channel_under_a_different_topic(tmp_path): + path = tmp_path / "conflict.mcap" + with path.open("wb") as stream: + writer = Writer(stream, compression=CompressionType.NONE) + writer.start() + writer.add_metadata( + "ego.recording", + {"record_device_diagnostics": "off", "diagnostic_version": "2"}, + ) + schema = writer.register_schema( + "core.EgoDeviceStateRecord", "flatbuffer", b"fixture" + ) + writer.register_channel("custom/topic", "flatbuffer", schema) + writer.finish() + with pytest.raises(ValueError, match="channel exists"): + replay_ego.replay(path, 0) + + +@pytest.mark.parametrize("version", ["0", "1", "99"]) +def test_unsupported_diagnostic_policy_is_rejected(tmp_path, version): + path = tmp_path / "unsupported.mcap" + with path.open("wb") as stream: + writer = Writer(stream, compression=CompressionType.NONE) + writer.start() + writer.add_metadata( + "ego.recording", + {"record_device_diagnostics": "on", "diagnostic_version": version}, + ) + writer.finish() + with pytest.raises(ValueError, match="Unsupported EGO"): + replay_ego.replay(path, 0) diff --git a/tests/python/examples/mcap_record_replay/test_ego_sync_cli.py b/tests/python/examples/mcap_record_replay/test_ego_sync_cli.py new file mode 100644 index 0000000000..93b7096642 --- /dev/null +++ b/tests/python/examples/mcap_record_replay/test_ego_sync_cli.py @@ -0,0 +1,140 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 ORBBEC INC. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +import json + +import pytest + +from isaaccapture_examples.mcap_record_replay import analyze_ego_sync + + +def _report(status="validated", incomplete=0): + return { + "alignment": {"status": status}, + "coverage": { + "window_count": 2, + "complete_windows": 2 - incomplete, + "incomplete_windows": incomplete, + }, + "windows": [{"start_time_ns": 100, "end_time_ns": 200}], + "physical_error_measurements": None, + } + + +def test_requested_scope_and_bounds_reach_audit(tmp_path, monkeypatch): + recording = tmp_path / "recording.mcap" + alignment = tmp_path / "alignment.json" + output = tmp_path / "audit.json" + calls = [] + + def audit(path, **kwargs): + calls.append((path, kwargs)) + return _report() + + monkeypatch.setattr(analyze_ego_sync, "analyze_recording_sync", audit) + assert ( + analyze_ego_sync.main( + [ + str(recording), + "--required-streams", + "right_rgb", + "gyro", + "--reference-stream", + "right_rgb", + "--alignment", + str(alignment), + "--epoch", + "3", + "--start-time-ns", + "100", + "--end-time-ns", + "200", + "--output", + str(output), + ] + ) + == 0 + ) + assert calls == [ + ( + recording, + { + "required_streams": ["right_rgb", "gyro"], + "alignment": alignment, + "reference_stream": "right_rgb", + "epoch": 3, + "start_time_ns": 100, + "end_time_ns": 200, + }, + ) + ] + assert json.loads(output.read_text()) == _report() + + +@pytest.mark.parametrize( + "status,incomplete", [("unverified", 0), ("invalid", 0), ("validated", 1)] +) +def test_failed_acceptance_still_preserves_report( + tmp_path, monkeypatch, status, incomplete +): + output = tmp_path / "audit.json" + monkeypatch.setattr( + analyze_ego_sync, + "analyze_recording_sync", + lambda *_args, **_kwargs: _report(status, incomplete), + ) + assert ( + analyze_ego_sync.main( + [ + "take.mcap", + "--required-streams", + "left_rgb", + "right_rgb", + "--output", + str(output), + ] + ) + == 2 + ) + assert json.loads(output.read_text()) == _report(status, incomplete) + + +def test_output_is_not_overwritten_or_audited(tmp_path, monkeypatch): + output = tmp_path / "audit.json" + output.write_text("retained evidence") + + def unexpected(*_args, **_kwargs): + pytest.fail("Existing output should be rejected before loading a recording") + + monkeypatch.setattr(analyze_ego_sync, "analyze_recording_sync", unexpected) + with pytest.raises(SystemExit, match="2"): + analyze_ego_sync.main( + ["take.mcap", "--required-streams", "left_rgb", "--output", str(output)] + ) + assert output.read_text() == "retained evidence" + + +def test_required_streams_cannot_be_implicit(tmp_path): + with pytest.raises(SystemExit, match="2"): + analyze_ego_sync.main(["take.mcap", "--output", str(tmp_path / "audit.json")]) + + +def test_missing_recording_error_creates_no_report(tmp_path, monkeypatch): + output = tmp_path / "audit.json" + + def audit(*_args, **_kwargs): + raise ValueError("Required stream is absent: gyro") + + monkeypatch.setattr(analyze_ego_sync, "analyze_recording_sync", audit) + with pytest.raises(SystemExit, match="2"): + analyze_ego_sync.main( + [ + "take.mcap", + "--required-streams", + "left_rgb", + "gyro", + "--output", + str(output), + ] + ) + assert not output.exists() From 8168f8f6a28e1c9e8405a2e2ad98251b6517550e Mon Sep 17 00:00:00 2001 From: obzhimin Date: Fri, 16 Oct 2026 22:22:17 +0800 Subject: [PATCH 2/2] docs(ego): describe Orbbec data collection suite Signed-off-by: obzhimin --- docs/source/device/ego.rst | 33 +++++++++++++++++++++------------ 1 file changed, 21 insertions(+), 12 deletions(-) diff --git a/docs/source/device/ego.rst b/docs/source/device/ego.rst index cabc06e24a..03d627738b 100644 --- a/docs/source/device/ego.rst +++ b/docs/source/device/ego.rst @@ -1,13 +1,21 @@ .. SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. .. SPDX-License-Identifier: Apache-2.0 -EGO Camera Plugin -================= +ORBBEC Data Collection Suite Plugin +=================================== -The ``ego_camera`` plugin connects EGO stereo cameras to Isaac Capture. -It records ColorLeft and ColorRight video, and captures accelerometer, -gyroscope, microphone, calibration, and device state when the device exposes -them. Detailed operator commands live in +The ORBBEC Data Collection Suite comprises a set of robot-free data capture +devices, including EGO (Egocentric Vision Data Collection Module), UMI (Handheld +Manipulation Data Collection Module), WristCam (Wrist-level Near-field +Interaction Capture Module), and Hub (Central Connection Device). + +At present, Orbbec EGO stereo cameras have been integrated with Isaac Capture. +Support for the remaining devices will be added in upcoming releases. The +system records ``ColorLeft`` and ``ColorRight`` video streams, and captures +accelerometer, gyroscope, microphone, calibration data, and device status, +where available from the hardware. + +Detailed operation instructions can be found in :code-file:`src/plugins/ego/README.md`. .. contents:: On this page @@ -17,12 +25,13 @@ them. Detailed operator commands live in Supported streams and profiles ------------------------------ -The plugin captures ``ColorLeft`` and ``ColorRight`` in MJPEG, H.264, or H.265. -The recording wrapper defaults to stereo H.264 ``1600x1300@30``. Use -``capabilities`` to inspect the connected device's available profiles; -unsupported simultaneous combinations fail without a fallback. The current -plugin rejects H.264/H.265 profiles above 30 FPS and does not capture depth or -infrared streams. +This plugin captures ``ColorLeft`` and ``ColorRight`` streams in MJPEG, H.264, +or H.265 encoding. The recording wrapper uses stereo H.264 at +``1600×1300@30`` as its default configuration. Use device ``capabilities`` to +inspect available profiles of the connected hardware. Unsupported simultaneous +profile combinations will fail with no fallback mechanism. The current plugin +rejects H.264/H.265 profiles exceeding 30 FPS, and does not support depth or +infrared stream capture. Installation ------------