From 8817011cfd9a089318e0d60015e7ddf03bcbe06a Mon Sep 17 00:00:00 2001 From: Teng Wei Date: Fri, 17 Jul 2026 01:30:43 +0800 Subject: [PATCH] Support argus camera for camera_viz Signed-off-by: Teng Wei --- docs/source/references/camera_streaming.rst | 64 +- examples/camera_viz/README.md | 2 +- examples/camera_viz/argus/CMakeLists.txt | 115 +++ examples/camera_viz/argus/__init__.py | 19 + examples/camera_viz/argus/argus_bindings.cpp | 58 ++ examples/camera_viz/argus/argus_camera.cpp | 822 ++++++++++++++++++ examples/camera_viz/argus/argus_camera.hpp | 143 +++ examples/camera_viz/argus/build.sh | 41 + examples/camera_viz/argus/yuv_to_rgba.cu | 214 +++++ examples/camera_viz/argus/yuv_to_rgba.cuh | 46 + examples/camera_viz/camera_viz.sh | 9 +- examples/camera_viz/configs/argus_shw5g.yaml | 53 ++ examples/camera_viz/scripts/_install_deps.sh | 17 +- examples/camera_viz/sources/__init__.py | 41 +- examples/camera_viz/sources/_helpers.py | 39 +- examples/camera_viz/sources/argus.py | 230 +++++ .../tests/test_paired_frame_source_race.py | 33 + 17 files changed, 1927 insertions(+), 19 deletions(-) create mode 100644 examples/camera_viz/argus/CMakeLists.txt create mode 100644 examples/camera_viz/argus/__init__.py create mode 100644 examples/camera_viz/argus/argus_bindings.cpp create mode 100644 examples/camera_viz/argus/argus_camera.cpp create mode 100644 examples/camera_viz/argus/argus_camera.hpp create mode 100755 examples/camera_viz/argus/build.sh create mode 100644 examples/camera_viz/argus/yuv_to_rgba.cu create mode 100644 examples/camera_viz/argus/yuv_to_rgba.cuh create mode 100644 examples/camera_viz/configs/argus_shw5g.yaml create mode 100644 examples/camera_viz/sources/argus.py diff --git a/docs/source/references/camera_streaming.rst b/docs/source/references/camera_streaming.rst index 9e245189e..7fccb1315 100644 --- a/docs/source/references/camera_streaming.rst +++ b/docs/source/references/camera_streaming.rst @@ -35,6 +35,9 @@ Requirements :doc:`quick start ` steps :ref:`run-cloudxr-server` and :ref:`connect-xr-headset`. No headset handy? ``--mode window`` renders to a desktop window instead and only needs a local display. +- For Jetson Argus cameras, a Jetson image with libargus, CUDA, EGL, and the Jetson Multimedia API + headers installed. The optional Argus native source is built only when setup is run with + ``--with-argus``. Setup ----- @@ -54,7 +57,9 @@ probes system packages (GStreamer plugins, cairo / girepository headers, JetPack ``ld.so`` wiring). When something is missing it prints the exact ``apt-get`` line and prompts ``[y/N]`` — answering ``n`` or running non-interactively aborts. -By default ``setup`` provisions everything except ZED support; flags trim or extend that: +By default ``setup`` provisions the portable sources and split-mode pieces. Jetson Argus and ZED +support are opt-in because they depend on vendor SDKs and headers that are not present on most +development hosts. Flags trim or extend setup: .. list-table:: :header-rows: 1 @@ -69,6 +74,9 @@ By default ``setup`` provisions everything except ZED support; flags trim or ext * - ``--no-rtp`` - Skip split-mode dependencies: the GStreamer system packages and the native NVENC/NVDEC codec build. Direct mode still works. + * - ``--with-argus`` + - Build the optional Jetson Argus native source (``type: argus``). Requires libargus, CUDA, + EGL, and Jetson Multimedia API headers on the machine. * - ``--with-zed`` - Also build + install the ZED SDK's Python API (``pyzed``). Requires the ZED SDK on the machine (default ``/usr/local/zed``; override with ``--zed-sdk PATH``). @@ -128,6 +136,9 @@ The source kind is selected by the ``type`` field of each entry in the YAML ``ca - OAK-D mono RGB / LEFT / RIGHT (stereo not yet wired). * - ``zed`` - ZED 2 / Mini / X One; mono or ``stereo: true`` (per-eye SDK retrieve, zero-copy on the GPU). + * - ``argus`` + - Jetson libargus cameras through CUDA EGLStream, including GMSL sensors exposed by + ``nvargus-daemon``. Build setup with ``--with-argus`` first. * - ``video`` - Video-file replay (anything OpenCV's FFmpeg backend reads). Loops by default; ``stereo: true`` splits side-by-side recordings into eyes (viewer only). @@ -142,11 +153,40 @@ run with the matching config: .. code-block:: bash - ./camera_viz.sh run configs/v4l2.yaml # or oakd.yaml / zed.yaml + ./camera_viz.sh run configs/v4l2.yaml # or oakd.yaml / zed.yaml / argus_shw5g.yaml **You should see** the same startup lines as above with the camera's tag (``[v4l2]``, -``[oakd]``, ``[zed]``) and the live feed. Multiple entries in the ``cameras`` list render as one -plane each. +``[oakd]``, ``[zed]``, ``[argus]``) and the live feed. Multiple entries in the ``cameras`` list +render as one plane each. + +Jetson Argus / GMSL cameras +^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Argus support is for Jetson-local capture with minimum camera-to-display latency. It bypasses +``nvarguscamerasrc`` and consumes Argus EGLStreams directly: + +.. code-block:: text + + nvargus-daemon -> libargus EGLStream -> CUDA YUV-to-RGBA -> camera_viz + +Install with the Argus backend enabled on the Jetson: + +.. code-block:: bash + + examples/camera_viz/camera_viz.sh setup --with-argus --jetson + +Use ``nvargus_nvraw`` to confirm the Argus sensor indices before editing the YAML: + +.. code-block:: bash + + /usr/sbin/nvargus_nvraw --lps + +The repository includes :code-file:`configs/argus_shw5g.yaml +` for a SENSING SHW5G stereo pair on Jetson AGX +Thor. Its tested settings use independent Argus sessions for sensors ``0`` and ``1``, +``repeat_capture: true`` for lower request latency, ``pair_emit_mode: both`` to avoid duplicate +stereo submits, and ``swap_uv: true`` for the observed SHW5G color layout. Some SENSING driver +stacks also require running ``nvargus-daemon`` with ``enableCamInfiniteTimeout=1``. Display modes ------------- @@ -234,13 +274,17 @@ its own plane (and, in split mode, its own RTP port). Abbreviated: cameras: - name: cam enabled: true - type: v4l2 # v4l2 | oakd | zed | video | synthetic + type: v4l2 # v4l2 | oakd | zed | argus | video | synthetic width: 2560 # video: optional — defaults to the file's size height: 720 # (required when source: rtp) fps: 30 stereo: false # zed / video / synthetic — per-eye capture + SBS in XR path: clip.mp4 # video only — file to replay, relative to this YAML loop: true # video only — rewind at end of file + sensor_id: 0 # argus mono only; use sensor_id_left/right for stereo + sensor_mode: 0 # argus only + repeat_capture: true # argus only — use repeat requests when supported + pair_emit_mode: both # argus independent stereo only: either | both rtp: port: 5000 # left eye when stereo port_right: 5001 # required when stereo + source: rtp @@ -273,6 +317,11 @@ Troubleshooting directory (``configs/``), not the directory you launched from. - **A source fails asking for CuPy / CUDA** — check ``nvidia-smi`` works and setup completed; all sources allocate their frame buffers on the GPU. +- **``type: argus`` is unavailable** — rerun setup on the Jetson with ``--with-argus`` and confirm + the Jetson Multimedia API Argus headers exist under ``/usr/src/jetson_multimedia_api/argus``. +- **Argus lists sensors but capture stalls** — confirm the selected ``sensor_id`` values with + ``/usr/sbin/nvargus_nvraw --lps``. Some GMSL driver stacks need ``nvargus-daemon`` configured + with ``enableCamInfiniteTimeout=1``. - **Split mode renders nothing** — check the sender is up (``./camera_viz.sh service-status``), ``$STREAMING_HOST`` was the workstation's IP at deploy time, and UDP ports (default 5000+) aren't firewalled. @@ -294,9 +343,10 @@ Televiz as the compositor at the end of the chain: ├── camera_streamer.py — robot-side RTP sender (per-camera supervisor) ├── pipeline/ — source ABC + threaded runner ├── placements/ — XR lock-mode strategies (world / head / lazy) - ├── sources/ — V4L2 / OAK-D / ZED / video replay / synthetic / rtp_h264 + ├── sources/ — V4L2 / OAK-D / ZED / Argus / video replay / synthetic / rtp_h264 ├── transports/ — RTP sender + receiver (native + GStreamer) ├── codec/ — native NVENC / NVDEC pybind module + ├── argus/ — optional Jetson libargus / CUDA EGLStream pybind module ├── configs/ — one YAML per source kind ├── test_data/ — sample replay clip (Git LFS) └── scripts/ — installer + systemd unit template @@ -305,6 +355,8 @@ Televiz as the compositor at the end of the chain: hand frames to a threaded runner in :code-dir:`pipeline/ `. Each source produces GPU frames where possible — e.g. the ZED source uses ``retrieve_image(MEM.GPU)`` so BGRA8 stays in VRAM and a CUDA kernel channel-swaps into contiguous RGBA with no host round-trip. + The optional Argus source consumes Jetson EGLStreams with CUDA and publishes RGBA buffers without a + CPU readback. - **The viewer** (:code-file:`camera_viz.py `) creates a ``VizSession`` and adds one ``QuadLayer`` per enabled camera, then submits each frame to its layer and calls ``render()`` once per frame. Stereo cameras submit both eyes. diff --git a/examples/camera_viz/README.md b/examples/camera_viz/README.md index c2fe4ba54..cc2f7993d 100644 --- a/examples/camera_viz/README.md +++ b/examples/camera_viz/README.md @@ -35,7 +35,7 @@ source examples/camera_viz/.venv/bin/activate `setup` installs `isaacteleop` (which bundles Televiz) and every other Python dep from PyPI into `.venv/` via `uv` (no `--system-site-packages`), builds the native NVENC/NVDEC codec, and probes system packages (GStreamer plugins, cairo / girepository headers, JetPack `cuda-nvrtc` + ld.so wiring). If anything's missing it prints the exact `apt-get` line and prompts `[y/N]` — `n` or non-interactive aborts. No need to build IsaacTeleop from source. -Flags: `--no-{v4l2,oakd,rtp}`, `--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,rtp}`, `--with-argus`, `--with-zed`, `--sender-only`, `--jetson`. Pass `--venv PATH` to install into an existing venv (symlinks `.venv` → PATH so `run` / `loopback` pick it up too). > **Developing against a local build?** Pass `--wheel ` (e.g. `camera_viz.sh setup --wheel build/wheels/isaacteleop-*.whl`) to install a locally built wheel instead of the PyPI release. See the [build-from-source guide](../../docs/source/getting_started/build_from_source/index.rst). diff --git a/examples/camera_viz/argus/CMakeLists.txt b/examples/camera_viz/argus/CMakeLists.txt new file mode 100644 index 000000000..eaa5f85f8 --- /dev/null +++ b/examples/camera_viz/argus/CMakeLists.txt @@ -0,0 +1,115 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# Native Argus camera source for camera_viz. The .so lands next to +# __init__.py so the package imports without an install step. + +cmake_minimum_required(VERSION 3.20) +project(camera_viz_argus LANGUAGES C CXX CUDA) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CUDA_STANDARD 17) +set(CMAKE_CUDA_STANDARD_REQUIRED ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +if(NOT DEFINED CMAKE_CUDA_ARCHITECTURES) + set(CMAKE_CUDA_ARCHITECTURES "80;86;87;89;90") +endif() + +find_package(CUDAToolkit REQUIRED) +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. `uv pip install pybind11` into the active venv.") +endif() +find_package(pybind11 REQUIRED CONFIG) + +list(APPEND CMAKE_MODULE_PATH "/usr/src/jetson_multimedia_api/argus/cmake") +find_package(Argus QUIET) +find_package(EGL QUIET) + +if(CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64|arm64") + set(_tegra_lib_dirs + "/usr/lib/aarch64-linux-gnu/nvidia" + "/usr/lib/aarch64-linux-gnu/tegra" + "/usr/lib/${CMAKE_LIBRARY_ARCHITECTURE}/nvidia" + "/usr/lib/${CMAKE_LIBRARY_ARCHITECTURE}/tegra" + ) +else() + set(_tegra_lib_dirs + "/usr/lib/x86_64-linux-gnu/nvidia" + "/usr/lib/${CMAKE_LIBRARY_ARCHITECTURE}/nvidia" + ) +endif() + +if(NOT ARGUS_INCLUDE_DIRS) + find_path(ARGUS_INCLUDE_DIRS Argus/Argus.h + PATHS "/usr/src/jetson_multimedia_api/argus/include" + "/usr/src/jetson_multimedia_api/include" + ) +endif() +if(NOT ARGUS_LIBRARIES) + find_library(ARGUS_SOCKETCLIENT_LIBRARY + NAMES nvargus_socketclient + PATHS ${_tegra_lib_dirs} + ) + find_library(ARGUS_LIBRARY + NAMES nvargus + PATHS ${_tegra_lib_dirs} + ) + if(ARGUS_SOCKETCLIENT_LIBRARY) + set(ARGUS_LIBRARIES ${ARGUS_SOCKETCLIENT_LIBRARY}) + elseif(ARGUS_LIBRARY) + set(ARGUS_LIBRARIES ${ARGUS_LIBRARY}) + endif() +endif() +if(NOT ARGUS_INCLUDE_DIRS OR NOT ARGUS_LIBRARIES) + message(FATAL_ERROR "Argus headers/libs not found. Install Jetson Multimedia API / libargus.") +endif() + +if(NOT EGL_INCLUDE_DIRS) + find_path(EGL_INCLUDE_DIRS EGL/egl.h) +endif() +if(NOT EGL_LIBRARIES) + find_library(EGL_LIBRARIES NAMES EGL) +endif() +if(NOT EGL_INCLUDE_DIRS OR NOT EGL_LIBRARIES) + message(FATAL_ERROR "EGL headers/libs not found.") +endif() + +message(STATUS "camera_viz argus: ARGUS=${ARGUS_LIBRARIES}") +message(STATUS "camera_viz argus: EGL=${EGL_LIBRARIES}") + +pybind11_add_module(_camera_viz_argus MODULE + argus_bindings.cpp + argus_camera.cpp + yuv_to_rgba.cu +) + +target_include_directories(_camera_viz_argus PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR} + ${ARGUS_INCLUDE_DIRS} + ${EGL_INCLUDE_DIRS} +) + +target_compile_options(_camera_viz_argus PRIVATE + $<$:-Wall -Wextra -Wno-unused-parameter> +) + +target_link_libraries(_camera_viz_argus PRIVATE + CUDA::cudart + CUDA::cuda_driver + ${ARGUS_LIBRARIES} + ${EGL_LIBRARIES} +) + +set_target_properties(_camera_viz_argus PROPERTIES + LIBRARY_OUTPUT_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} +) diff --git a/examples/camera_viz/argus/__init__.py b/examples/camera_viz/argus/__init__.py new file mode 100644 index 000000000..599347955 --- /dev/null +++ b/examples/camera_viz/argus/__init__.py @@ -0,0 +1,19 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +"""Native Argus camera source - pybind11 module re-exports. + +Build the .so with ``argus/build.sh``. This source is Jetson-only and +requires libargus, EGL, and CUDA from JetPack. +""" + +from __future__ import annotations + +try: + from ._camera_viz_argus import ArgusCamera, ArgusConfig, FrameView +except ImportError as e: + raise ImportError( + "camera_viz native Argus source not built. Run " + "`examples/camera_viz/argus/build.sh` on the Jetson." + ) from e + +__all__ = ["ArgusCamera", "ArgusConfig", "FrameView"] diff --git a/examples/camera_viz/argus/argus_bindings.cpp b/examples/camera_viz/argus/argus_bindings.cpp new file mode 100644 index 000000000..516f24b91 --- /dev/null +++ b/examples/camera_viz/argus/argus_bindings.cpp @@ -0,0 +1,58 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "argus_camera.hpp" + +#include +#include + +namespace py = pybind11; +using camera_viz::argus::ArgusCamera; +using camera_viz::argus::ArgusConfig; +using camera_viz::argus::FrameView; + +PYBIND11_MODULE(_camera_viz_argus, m) +{ + py::class_(m, "ArgusConfig") + .def(py::init<>()) + .def_readwrite("name", &ArgusConfig::name) + .def_readwrite("sensor_ids", &ArgusConfig::sensor_ids) + .def_readwrite("sensor_mode", &ArgusConfig::sensor_mode) + .def_readwrite("width", &ArgusConfig::width) + .def_readwrite("height", &ArgusConfig::height) + .def_readwrite("fps", &ArgusConfig::fps) + .def_readwrite("gpu_id", &ArgusConfig::gpu_id) + .def_readwrite("full_range", &ArgusConfig::full_range) + .def_readwrite("swap_uv", &ArgusConfig::swap_uv) + .def_readwrite("acquire_timeout_ms", &ArgusConfig::acquire_timeout_ms) + .def_readwrite("repeat_capture", &ArgusConfig::repeat_capture); + + py::class_(m, "FrameView") + .def_readonly("left_ptr", &FrameView::left_ptr) + .def_readonly("left_pitch", &FrameView::left_pitch) + .def_readonly("right_ptr", &FrameView::right_ptr) + .def_readonly("right_pitch", &FrameView::right_pitch) + .def_readonly("width", &FrameView::width) + .def_readonly("height", &FrameView::height) + .def_readonly("timestamp_ns", &FrameView::timestamp_ns) + .def_readonly("sequence", &FrameView::sequence) + .def_readonly("stereo", &FrameView::stereo); + + py::class_(m, "ArgusCamera") + .def(py::init()) + .def("start", &ArgusCamera::start, py::call_guard()) + .def("stop", &ArgusCamera::stop, py::call_guard()) + .def("latest", + [](ArgusCamera& camera) -> py::object + { + auto frame = camera.latest(); + if (!frame) + { + return py::none(); + } + return py::cast(*frame); + }) + .def_property_readonly("is_stereo", &ArgusCamera::is_stereo) + .def_property_readonly("width", &ArgusCamera::width) + .def_property_readonly("height", &ArgusCamera::height); +} diff --git a/examples/camera_viz/argus/argus_camera.cpp b/examples/camera_viz/argus/argus_camera.cpp new file mode 100644 index 000000000..01419bfaa --- /dev/null +++ b/examples/camera_viz/argus/argus_camera.cpp @@ -0,0 +1,822 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "argus_camera.hpp" + +#include "yuv_to_rgba.cuh" + +#include +#include +#include +#include +#include +#include +#include + +namespace camera_viz::argus +{ +namespace +{ + +void check_cuda(CUresult result, const char* what) +{ + if (result == CUDA_SUCCESS) + { + return; + } + const char* name = nullptr; + const char* text = nullptr; + cuGetErrorName(result, &name); + cuGetErrorString(result, &text); + std::ostringstream oss; + oss << what << " failed"; + if (name) + { + oss << ": " << name; + } + if (text) + { + oss << " (" << text << ")"; + } + throw std::runtime_error(oss.str()); +} + +void check_runtime(cudaError_t result, const char* what) +{ + if (result != cudaSuccess) + { + std::ostringstream oss; + oss << what << " failed: " << cudaGetErrorString(result); + throw std::runtime_error(oss.str()); + } +} + +void check_argus(Argus::Status status, const char* what) +{ + if (status != Argus::STATUS_OK) + { + std::ostringstream oss; + oss << what << " failed with Argus status " << static_cast(status); + throw std::runtime_error(oss.str()); + } +} + +constexpr uint32_t kCudaEglInfiniteTimeout = 0xffffffffu; + +uint32_t acquire_timeout_us(uint32_t timeout_ms) +{ + if (timeout_ms == kCudaEglInfiniteTimeout) + { + return kCudaEglInfiniteTimeout; + } + constexpr uint32_t kMaxFiniteTimeoutMs = (kCudaEglInfiniteTimeout - 1U) / 1000U; + if (timeout_ms > kMaxFiniteTimeoutMs) + { + return kCudaEglInfiniteTimeout - 1U; + } + return timeout_ms * 1000U; +} + +bool is_acquire_timeout(CUresult result) +{ + return result == CUDA_ERROR_TIMEOUT || result == CUDA_ERROR_LAUNCH_TIMEOUT; +} + +struct SharedCameraProvider +{ + std::mutex mutex; + Argus::UniqueObj provider; + Argus::ICameraProvider* iface = nullptr; + std::vector devices; + size_t refs = 0; +}; + +SharedCameraProvider g_provider; + +void retain_camera_provider(Argus::ICameraProvider*& iface, std::vector& devices) +{ + std::lock_guard guard(g_provider.mutex); + if (g_provider.refs == 0) + { + g_provider.provider.reset(Argus::CameraProvider::create()); + g_provider.iface = Argus::interface_cast(g_provider.provider); + if (!g_provider.iface) + { + g_provider.provider.reset(); + throw std::runtime_error("failed to create Argus CameraProvider"); + } + check_argus(g_provider.iface->getCameraDevices(&g_provider.devices), "getCameraDevices"); + if (g_provider.devices.empty()) + { + g_provider.iface = nullptr; + g_provider.provider.reset(); + throw std::runtime_error("Argus reported no camera devices"); + } + } + ++g_provider.refs; + iface = g_provider.iface; + devices = g_provider.devices; +} + +void release_camera_provider() +{ + std::lock_guard guard(g_provider.mutex); + if (g_provider.refs == 0) + { + return; + } + --g_provider.refs; + if (g_provider.refs == 0) + { + g_provider.devices.clear(); + g_provider.iface = nullptr; + g_provider.provider.reset(); + } +} + +uint64_t monotonic_ns() +{ + return static_cast( + std::chrono::duration_cast(std::chrono::steady_clock::now().time_since_epoch()).count()); +} + +bool is_yuv420_planar(CUeglColorFormat fmt) +{ + return fmt == CU_EGL_COLOR_FORMAT_YUV420_PLANAR || fmt == CU_EGL_COLOR_FORMAT_YUV420_PLANAR_ER || + fmt == CU_EGL_COLOR_FORMAT_YUV420_PLANAR_709 || fmt == CU_EGL_COLOR_FORMAT_YUV420_PLANAR_2020; +} + +bool is_yvu420_planar(CUeglColorFormat fmt) +{ + return fmt == CU_EGL_COLOR_FORMAT_YVU420_PLANAR || fmt == CU_EGL_COLOR_FORMAT_YVU420_PLANAR_ER || + fmt == CU_EGL_COLOR_FORMAT_YVU420_PLANAR_709 || fmt == CU_EGL_COLOR_FORMAT_YVU420_PLANAR_2020; +} + +bool is_yuv420_semiplanar(CUeglColorFormat fmt) +{ + return fmt == CU_EGL_COLOR_FORMAT_YUV420_SEMIPLANAR || fmt == CU_EGL_COLOR_FORMAT_YUV420_SEMIPLANAR_ER || + fmt == CU_EGL_COLOR_FORMAT_YUV420_SEMIPLANAR_709 || fmt == CU_EGL_COLOR_FORMAT_YUV420_SEMIPLANAR_2020; +} + +bool is_yvu420_semiplanar(CUeglColorFormat fmt) +{ + return fmt == CU_EGL_COLOR_FORMAT_YVU420_SEMIPLANAR || fmt == CU_EGL_COLOR_FORMAT_YVU420_SEMIPLANAR_ER || + fmt == CU_EGL_COLOR_FORMAT_YVU420_SEMIPLANAR_709 || fmt == CU_EGL_COLOR_FORMAT_YVU420_SEMIPLANAR_2020; +} + +bool is_extended_range(CUeglColorFormat fmt) +{ + return fmt == CU_EGL_COLOR_FORMAT_YUV420_PLANAR_ER || fmt == CU_EGL_COLOR_FORMAT_YVU420_PLANAR_ER || + fmt == CU_EGL_COLOR_FORMAT_YUV420_SEMIPLANAR_ER || fmt == CU_EGL_COLOR_FORMAT_YVU420_SEMIPLANAR_ER; +} + +YuvLayout layout_for(CUeglColorFormat fmt) +{ + if (is_yuv420_planar(fmt)) + { + return YuvLayout::YUV420Planar; + } + if (is_yvu420_planar(fmt)) + { + return YuvLayout::YVU420Planar; + } + if (is_yuv420_semiplanar(fmt)) + { + return YuvLayout::YUV420SemiPlanar; + } + if (is_yvu420_semiplanar(fmt)) + { + return YuvLayout::YVU420SemiPlanar; + } + std::ostringstream oss; + oss << "unsupported Argus CUDA EGL color format " << static_cast(fmt); + throw std::runtime_error(oss.str()); +} + +bool is_planar(YuvLayout layout) +{ + return layout == YuvLayout::YUV420Planar || layout == YuvLayout::YVU420Planar; +} + +YuvLayout swapped_uv_layout(YuvLayout layout) +{ + switch (layout) + { + case YuvLayout::YUV420Planar: + return YuvLayout::YVU420Planar; + case YuvLayout::YVU420Planar: + return YuvLayout::YUV420Planar; + case YuvLayout::YUV420SemiPlanar: + return YuvLayout::YVU420SemiPlanar; + case YuvLayout::YVU420SemiPlanar: + return YuvLayout::YUV420SemiPlanar; + } + return layout; +} + +cudaTextureObject_t texture_for_array(CUarray array) +{ + cudaResourceDesc resource_desc{}; + resource_desc.resType = cudaResourceTypeArray; + resource_desc.res.array.array = reinterpret_cast(array); + + cudaTextureDesc texture_desc{}; + texture_desc.addressMode[0] = cudaAddressModeClamp; + texture_desc.addressMode[1] = cudaAddressModeClamp; + texture_desc.filterMode = cudaFilterModePoint; + texture_desc.readMode = cudaReadModeElementType; + texture_desc.normalizedCoords = 0; + + cudaTextureObject_t texture = 0; + check_runtime(cudaCreateTextureObject(&texture, &resource_desc, &texture_desc, nullptr), "cudaCreateTextureObject"); + return texture; +} + +} // namespace + +ArgusCamera::ArgusCamera(const ArgusConfig& config) : config_(config) +{ + if (config_.sensor_ids.empty() || config_.sensor_ids.size() > 2) + { + throw std::invalid_argument("ArgusConfig.sensor_ids must contain one or two sensor ids"); + } + if (config_.width == 0 || config_.height == 0) + { + throw std::invalid_argument("ArgusConfig.width/height must be non-zero"); + } + stereo_ = config_.sensor_ids.size() == 2; + stream_count_ = config_.sensor_ids.size(); +} + +ArgusCamera::~ArgusCamera() +{ + stop(); +} + +void ArgusCamera::start() +{ + if (running_.load()) + { + return; + } + initialize(); + running_.store(true); + thread_ = std::thread(&ArgusCamera::producer_loop, this); +} + +void ArgusCamera::stop() +{ + if (!running_.exchange(false)) + { + cleanup(); + return; + } + + if (i_capture_session_) + { + i_capture_session_->stopRepeat(); + i_capture_session_->waitForIdle(); + } + for (size_t i = 0; i < stream_count_; ++i) + { + if (streams_[i].egl_stream) + { + streams_[i].egl_stream->disconnect(); + } + } + if (thread_.joinable()) + { + thread_.join(); + } + cleanup(); +} + +std::optional ArgusCamera::latest() +{ + throw_if_failed(); + std::lock_guard guard(publish_mutex_); + if (publish_idx_ < 0 || consumed_sequence_ == published_sequence_) + { + return std::nullopt; + } + consumed_sequence_ = published_sequence_; + + FrameView view; + view.left_ptr = reinterpret_cast(buffers_[0][publish_idx_].ptr); + view.left_pitch = buffers_[0][publish_idx_].pitch; + view.width = config_.width; + view.height = config_.height; + view.timestamp_ns = published_timestamp_ns_; + view.sequence = published_sequence_; + view.stereo = stereo_; + if (stereo_) + { + view.right_ptr = reinterpret_cast(buffers_[1][publish_idx_].ptr); + view.right_pitch = buffers_[1][publish_idx_].pitch; + } + return view; +} + +bool ArgusCamera::is_stereo() const +{ + return stereo_; +} + +uint32_t ArgusCamera::width() const +{ + return config_.width; +} + +uint32_t ArgusCamera::height() const +{ + return config_.height; +} + +void ArgusCamera::initialize() +{ + cleanup(); + failed_.store(false); + { + std::lock_guard guard(error_mutex_); + failure_message_.clear(); + } + + check_cuda(cuInit(0), "cuInit"); + check_cuda(cuDeviceGet(&cu_device_, config_.gpu_id), "cuDeviceGet"); + check_cuda(cuDevicePrimaryCtxRetain(&cu_context_, cu_device_), "cuDevicePrimaryCtxRetain"); + cu_context_retained_ = true; + check_cuda(cuCtxSetCurrent(cu_context_), "cuCtxSetCurrent"); + check_cuda(cuStreamCreate(&convert_stream_, CU_STREAM_NON_BLOCKING), "cuStreamCreate"); + + retain_camera_provider(i_camera_provider_, camera_devices_); + shared_provider_retained_ = true; + + std::vector selected; + selected.reserve(stream_count_); + for (uint32_t sensor_id : config_.sensor_ids) + { + selected.push_back(camera_device(sensor_id)); + } + + capture_session_.reset(stream_count_ == 1 ? i_camera_provider_->createCaptureSession(selected[0]) : + i_camera_provider_->createCaptureSession(selected)); + i_capture_session_ = Argus::interface_cast(capture_session_); + if (!i_capture_session_) + { + throw std::runtime_error("failed to create Argus CaptureSession"); + } + + for (size_t i = 0; i < stream_count_; ++i) + { + Argus::UniqueObj settings( + i_capture_session_->createOutputStreamSettings(Argus::STREAM_TYPE_EGL)); + auto* i_settings = Argus::interface_cast(settings); + auto* i_egl_settings = Argus::interface_cast(settings); + if (!i_settings || !i_egl_settings) + { + throw std::runtime_error("failed to create Argus EGL OutputStreamSettings"); + } + check_argus(i_settings->setCameraDevice(selected[i]), "setCameraDevice"); + check_argus(i_egl_settings->setPixelFormat(Argus::PIXEL_FMT_YCbCr_420_888), "setPixelFormat"); + check_argus( + i_egl_settings->setResolution(Argus::Size2D(config_.width, config_.height)), "setResolution"); + // Leave EGLDisplay unset for CUDA-only consumption. NVIDIA's cudaHistogram + // sample uses a display-agnostic EGLStream for CUDA consumers; binding a + // default EGL display can fail on headless/OpenXR-only Thor setups. + check_argus(i_egl_settings->setMode(Argus::EGL_STREAM_MODE_MAILBOX), "setMode MAILBOX"); + + streams_[i].output_stream.reset(i_capture_session_->createOutputStream(settings.get())); + streams_[i].egl_stream = Argus::interface_cast(streams_[i].output_stream); + if (!streams_[i].egl_stream) + { + throw std::runtime_error("failed to create Argus EGL OutputStream"); + } + } + + request_.reset(i_capture_session_->createRequest()); + auto* i_request = Argus::interface_cast(request_); + if (!i_request) + { + throw std::runtime_error("failed to create Argus Request"); + } + auto* i_source_settings = Argus::interface_cast(i_request->getSourceSettings()); + if (!i_source_settings) + { + throw std::runtime_error("failed to get Argus ISourceSettings"); + } + check_argus(i_source_settings->setSensorMode(sensor_mode_for(selected[0])), "setSensorMode"); + // Leave frame duration at the sensor-mode default. For SHW5G this mirrors + // NVIDIA's cudaHistogram sample, which succeeds on the same Argus device. + for (size_t i = 0; i < stream_count_; ++i) + { + check_argus(i_request->enableOutputStream(streams_[i].output_stream.get()), "enableOutputStream"); + } + + for (size_t eye = 0; eye < stream_count_; ++eye) + { + for (auto& buffer : buffers_[eye]) + { + CUdeviceptr ptr = 0; + check_cuda(cuMemAllocPitch(&ptr, &buffer.pitch, config_.width * 4, config_.height, 4), "cuMemAllocPitch"); + buffer.ptr = reinterpret_cast(static_cast(ptr)); + } + } +} + +void ArgusCamera::connect_cuda_consumers() +{ + for (size_t i = 0; i < stream_count_; ++i) + { + if (!streams_[i].egl_stream) + { + throw std::runtime_error("Argus EGL stream missing while connecting CUDA consumer"); + } + if (!streams_[i].connection) + { + std::ostringstream label; + label << "cuEGLStreamConsumerConnect stream " << i; + check_cuda(cuEGLStreamConsumerConnect(&streams_[i].connection, streams_[i].egl_stream->getEGLStream()), + label.str().c_str()); + } + } +} + +void ArgusCamera::wait_for_streams_connected() +{ + for (size_t i = 0; i < stream_count_; ++i) + { + if (!streams_[i].egl_stream) + { + throw std::runtime_error("Argus EGL stream missing while waiting for connection"); + } + std::ostringstream label; + label << "waitUntilConnected stream " << i; + check_argus(streams_[i].egl_stream->waitUntilConnected(), label.str().c_str()); + } +} + +void ArgusCamera::set_failure(const std::string& message) +{ + { + std::lock_guard guard(error_mutex_); + failure_message_ = message; + } + failed_.store(true); +} + +void ArgusCamera::throw_if_failed() const +{ + if (!failed_.load()) + { + return; + } + std::lock_guard guard(error_mutex_); + throw std::runtime_error(failure_message_.empty() ? "Argus producer failed" : failure_message_); +} + +void ArgusCamera::cleanup() +{ + if (thread_.joinable()) + { + thread_.join(); + } + + if (i_capture_session_) + { + i_capture_session_->stopRepeat(); + i_capture_session_->waitForIdle(); + } + + for (size_t i = 0; i < streams_.size(); ++i) + { + if (streams_[i].connection) + { + cuEGLStreamConsumerDisconnect(&streams_[i].connection); + streams_[i].connection = nullptr; + } + if (streams_[i].egl_stream) + { + streams_[i].egl_stream->disconnect(); + streams_[i].egl_stream = nullptr; + } + streams_[i].output_stream.reset(); + } + + request_.reset(); + capture_session_.reset(); + i_capture_session_ = nullptr; + camera_devices_.clear(); + i_camera_provider_ = nullptr; + if (shared_provider_retained_) + { + release_camera_provider(); + shared_provider_retained_ = false; + } + + for (auto& eye : buffers_) + { + for (auto& buffer : eye) + { + if (buffer.ptr) + { + cuMemFree(static_cast(reinterpret_cast(buffer.ptr))); + buffer.ptr = nullptr; + buffer.pitch = 0; + } + } + } + + if (convert_stream_) + { + cuStreamDestroy(convert_stream_); + convert_stream_ = nullptr; + } + + if (egl_initialized_) + { + eglTerminate(egl_display_); + egl_initialized_ = false; + } + egl_display_ = EGL_NO_DISPLAY; + + if (cu_context_retained_) + { + cuDevicePrimaryCtxRelease(cu_device_); + cu_context_retained_ = false; + cu_context_ = nullptr; + } + + { + std::lock_guard guard(publish_mutex_); + publish_idx_ = -1; + published_sequence_ = 0; + consumed_sequence_ = 0; + published_timestamp_ns_ = 0; + } +} + +void ArgusCamera::producer_loop() +{ + bool repeat_active = false; + try + { + check_cuda(cuCtxSetCurrent(cu_context_), "cuCtxSetCurrent producer"); + connect_cuda_consumers(); + if (config_.repeat_capture) + { + const Argus::Status repeat_status = i_capture_session_->repeat(request_.get()); + if (repeat_status == Argus::STATUS_OK) + { + repeat_active = true; + } + else + { + std::cerr << "[argus] repeat() failed with Argus status " << static_cast(repeat_status) + << "; falling back to per-frame capture()" << std::endl; + } + } + + while (running_.load()) + { + const uint32_t write_idx = pick_write_index(); + std::array acquired{}; + if (!repeat_active) + { + Argus::Status capture_status = Argus::STATUS_OK; + const uint64_t capture_timeout_ns = 1000000000ULL; + const uint32_t capture_id = + i_capture_session_->capture(request_.get(), capture_timeout_ns, &capture_status); + if (capture_id == 0 || capture_status != Argus::STATUS_OK) + { + check_argus(capture_status, "capture"); + throw std::runtime_error("Argus capture request timed out before submission"); + } + } + + bool ok = true; + for (size_t eye = 0; eye < stream_count_; ++eye) + { + if (!acquire(streams_[eye], acquired[eye])) + { + ok = false; + break; + } + } + if (ok) + { + std::vector pending_textures; + for (size_t eye = 0; eye < stream_count_; ++eye) + { + auto textures = convert_frame(acquired[eye].frame, buffers_[eye][write_idx]); + pending_textures.insert(pending_textures.end(), textures.begin(), textures.end()); + } + check_cuda(cuStreamSynchronize(convert_stream_), "cuStreamSynchronize"); + check_runtime(cudaGetLastError(), "Argus YUV to RGBA kernel"); + for (cudaTextureObject_t texture : pending_textures) + { + if (texture) + { + check_runtime(cudaDestroyTextureObject(texture), "cudaDestroyTextureObject"); + } + } + const uint64_t ts = monotonic_ns(); + for (size_t eye = 0; eye < stream_count_; ++eye) + { + release(streams_[eye], acquired[eye]); + } + publish(write_idx, ts); + } + else + { + for (size_t eye = 0; eye < stream_count_; ++eye) + { + release(streams_[eye], acquired[eye]); + } + } + } + } + catch (const std::exception& e) + { + std::ostringstream oss; + oss << "Argus producer error: " << e.what(); + std::cerr << "[argus] " << oss.str() << std::endl; + set_failure(oss.str()); + running_.store(false); + if (i_capture_session_) + { + i_capture_session_->stopRepeat(); + i_capture_session_->waitForIdle(); + } + } + catch (...) + { + const std::string msg = "Argus producer error: unknown exception"; + std::cerr << "[argus] " << msg << std::endl; + set_failure(msg); + running_.store(false); + if (i_capture_session_) + { + i_capture_session_->stopRepeat(); + i_capture_session_->waitForIdle(); + } + } +} + +bool ArgusCamera::acquire(StreamState& stream, AcquiredFrame& out) +{ + if (!stream.connection) + { + return false; + } + const uint32_t timeout_us = acquire_timeout_us(config_.acquire_timeout_ms); + CUresult result = cuEGLStreamConsumerAcquireFrame(&stream.connection, &out.resource, &out.stream, timeout_us); + if (is_acquire_timeout(result)) + { + return false; + } + if (result != CUDA_SUCCESS) + { + if (!running_.load()) + { + return false; + } + check_cuda(result, "cuEGLStreamConsumerAcquireFrame"); + } + result = cuGraphicsResourceGetMappedEglFrame(&out.frame, out.resource, 0, 0); + check_cuda(result, "cuGraphicsResourceGetMappedEglFrame"); + return true; +} + +void ArgusCamera::release(StreamState& stream, AcquiredFrame& acquired) +{ + if (acquired.resource && stream.connection) + { + cuEGLStreamConsumerReleaseFrame(&stream.connection, acquired.resource, &acquired.stream); + acquired.resource = nullptr; + acquired.stream = nullptr; + std::memset(&acquired.frame, 0, sizeof(acquired.frame)); + } +} + +std::vector ArgusCamera::convert_frame(const CUeglFrame& frame, DeviceBuffer& dest) +{ + std::vector pending_textures; + if (frame.width != config_.width || frame.height != config_.height) + { + std::ostringstream oss; + oss << "Argus frame size " << frame.width << "x" << frame.height << " does not match configured " + << config_.width << "x" << config_.height; + throw std::runtime_error(oss.str()); + } + if (frame.cuFormat != CU_AD_FORMAT_UNSIGNED_INT8) + { + throw std::runtime_error("Argus frame is not unsigned 8-bit YUV"); + } + + YuvLayout layout = layout_for(frame.eglColorFormat); + if (config_.swap_uv) + { + layout = swapped_uv_layout(layout); + } + const bool full_range = config_.full_range || is_extended_range(frame.eglColorFormat); + const bool planar = is_planar(layout); + const uint32_t required_planes = planar ? 3 : 2; + if (frame.planeCount < required_planes) + { + std::ostringstream oss; + oss << "Argus frame has " << frame.planeCount << " plane(s), expected at least " << required_planes; + throw std::runtime_error(oss.str()); + } + + if (frame.frameType == CU_EGL_FRAME_TYPE_PITCH) + { + if (planar) + { + throw std::runtime_error( + "planar Argus pitch frames are unsupported because CUeglFrame exposes only first-plane pitch"); + } + const auto* y_plane = static_cast(frame.frame.pPitch[0]); + const auto* uv_or_u_plane = static_cast(frame.frame.pPitch[1]); + const int y_pitch = static_cast(frame.pitch); + const int uv_pitch = static_cast(frame.pitch); + launch_yuv420_pitch_to_rgba(y_plane, uv_or_u_plane, nullptr, y_pitch, uv_pitch, 0, config_.width, + config_.height, dest.ptr, static_cast(dest.pitch), layout, full_range, + reinterpret_cast(convert_stream_)); + return pending_textures; + } + + if (frame.frameType == CU_EGL_FRAME_TYPE_ARRAY) + { + cudaTextureObject_t y_tex = texture_for_array(frame.frame.pArray[0]); + pending_textures.push_back(y_tex); + cudaTextureObject_t uv_or_u_tex = texture_for_array(frame.frame.pArray[1]); + pending_textures.push_back(uv_or_u_tex); + cudaTextureObject_t v_tex = 0; + if (planar) + { + v_tex = texture_for_array(frame.frame.pArray[2]); + pending_textures.push_back(v_tex); + } + launch_yuv420_array_to_rgba(y_tex, uv_or_u_tex, v_tex, config_.width, config_.height, dest.ptr, + static_cast(dest.pitch), layout, full_range, + reinterpret_cast(convert_stream_)); + return pending_textures; + } + + throw std::runtime_error("unsupported Argus CUDA EGL frame type"); +} + +void ArgusCamera::publish(uint32_t write_idx, uint64_t timestamp_ns) +{ + std::lock_guard guard(publish_mutex_); + publish_idx_ = static_cast(write_idx); + published_timestamp_ns_ = timestamp_ns; + ++published_sequence_; +} + +uint32_t ArgusCamera::pick_write_index() const +{ + std::lock_guard guard(publish_mutex_); + if (publish_idx_ < 0) + { + return 0; + } + return static_cast((publish_idx_ + 1) % 3); +} + +Argus::CameraDevice* ArgusCamera::camera_device(uint32_t sensor_id) const +{ + if (sensor_id >= camera_devices_.size()) + { + std::ostringstream oss; + oss << "Argus sensor_id " << sensor_id << " is out of range; Argus reports " << camera_devices_.size() + << " camera device(s)"; + throw std::out_of_range(oss.str()); + } + return camera_devices_[sensor_id]; +} + +Argus::SensorMode* ArgusCamera::sensor_mode_for(Argus::CameraDevice* device) const +{ + auto* props = Argus::interface_cast(device); + if (!props) + { + throw std::runtime_error("failed to get Argus ICameraProperties"); + } + std::vector modes; + check_argus(props->getAllSensorModes(&modes), "getAllSensorModes"); + if (modes.empty()) + { + throw std::runtime_error("Argus camera device has no sensor modes"); + } + if (config_.sensor_mode >= modes.size()) + { + std::ostringstream oss; + oss << "Argus sensor_mode " << config_.sensor_mode << " is out of range; camera has " << modes.size() + << " mode(s)"; + throw std::out_of_range(oss.str()); + } + return modes[config_.sensor_mode]; +} + +} // namespace camera_viz::argus diff --git a/examples/camera_viz/argus/argus_camera.hpp b/examples/camera_viz/argus/argus_camera.hpp new file mode 100644 index 000000000..8ff827c2e --- /dev/null +++ b/examples/camera_viz/argus/argus_camera.hpp @@ -0,0 +1,143 @@ +// 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 +#include + +namespace camera_viz::argus +{ + +struct ArgusConfig +{ + std::string name; + std::vector sensor_ids; + uint32_t sensor_mode = 0; + uint32_t width = 0; + uint32_t height = 0; + double fps = 30.0; + int gpu_id = 0; + bool full_range = false; + bool swap_uv = false; + uint32_t acquire_timeout_ms = 0xffffffffu; + bool repeat_capture = true; +}; + +struct FrameView +{ + uintptr_t left_ptr = 0; + size_t left_pitch = 0; + uintptr_t right_ptr = 0; + size_t right_pitch = 0; + uint32_t width = 0; + uint32_t height = 0; + uint64_t timestamp_ns = 0; + uint64_t sequence = 0; + bool stereo = false; +}; + +class ArgusCamera +{ +public: + explicit ArgusCamera(const ArgusConfig& config); + ~ArgusCamera(); + + ArgusCamera(const ArgusCamera&) = delete; + ArgusCamera& operator=(const ArgusCamera&) = delete; + + void start(); + void stop(); + std::optional latest(); + + bool is_stereo() const; + uint32_t width() const; + uint32_t height() const; + +private: + struct DeviceBuffer + { + uint8_t* ptr = nullptr; + size_t pitch = 0; + }; + + struct StreamState + { + Argus::UniqueObj output_stream; + Argus::IEGLOutputStream* egl_stream = nullptr; + CUeglStreamConnection connection = nullptr; + }; + + struct AcquiredFrame + { + CUgraphicsResource resource = nullptr; + CUstream stream = nullptr; + CUeglFrame frame{}; + }; + + void initialize(); + void cleanup(); + void producer_loop(); + void connect_cuda_consumers(); + void wait_for_streams_connected(); + void set_failure(const std::string& message); + void throw_if_failed() const; + bool acquire(StreamState& stream, AcquiredFrame& out); + void release(StreamState& stream, AcquiredFrame& acquired); + std::vector convert_frame(const CUeglFrame& frame, DeviceBuffer& dest); + void publish(uint32_t write_idx, uint64_t timestamp_ns); + uint32_t pick_write_index() const; + + Argus::CameraDevice* camera_device(uint32_t sensor_id) const; + Argus::SensorMode* sensor_mode_for(Argus::CameraDevice* device) const; + + ArgusConfig config_; + bool stereo_ = false; + + EGLDisplay egl_display_ = EGL_NO_DISPLAY; + bool egl_initialized_ = false; + + CUdevice cu_device_ = 0; + CUcontext cu_context_ = nullptr; + bool cu_context_retained_ = false; + CUstream convert_stream_ = nullptr; + + Argus::ICameraProvider* i_camera_provider_ = nullptr; + bool shared_provider_retained_ = false; + Argus::UniqueObj capture_session_; + Argus::ICaptureSession* i_capture_session_ = nullptr; + Argus::UniqueObj request_; + + std::vector camera_devices_; + std::array streams_; + size_t stream_count_ = 0; + + std::array, 2> buffers_{}; + + std::atomic running_{ false }; + std::atomic failed_{ false }; + std::thread thread_; + + mutable std::mutex error_mutex_; + std::string failure_message_; + + mutable std::mutex publish_mutex_; + int publish_idx_ = -1; + uint64_t published_sequence_ = 0; + uint64_t consumed_sequence_ = 0; + uint64_t published_timestamp_ns_ = 0; +}; + +} // namespace camera_viz::argus diff --git a/examples/camera_viz/argus/build.sh b/examples/camera_viz/argus/build.sh new file mode 100755 index 000000000..f3ab3bc69 --- /dev/null +++ b/examples/camera_viz/argus/build.sh @@ -0,0 +1,41 @@ +#!/usr/bin/env bash +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# Builds the native Argus camera source. The .so lands next to +# __init__.py so the package imports straight from the source tree. +set -euo pipefail + +here="$(cd "$(dirname "$0")" && pwd)" +build_dir="${here}/build" + +if [[ -z "${CUDACXX:-}" && -x /usr/local/cuda/bin/nvcc ]]; then + export CUDACXX=/usr/local/cuda/bin/nvcc + export CUDA_PATH=/usr/local/cuda + export PATH="/usr/local/cuda/bin:${PATH}" +fi + +if [[ -z "${VIRTUAL_ENV:-}" ]]; then + echo "build.sh: no active venv - activate examples/camera_viz/.venv first" >&2 + exit 1 +fi +if ! python -c "import pybind11" 2>/dev/null; then + echo "build.sh: installing pybind11 into venv..." >&2 + uv pip install pybind11 +fi + +generator=() +if command -v ninja >/dev/null 2>&1; then + generator=(-G Ninja) +fi + +cmake -S "${here}" -B "${build_dir}" "${generator[@]}" \ + "-UCUDA_*" \ + "-UCUDAToolkit_*" \ + -DCMAKE_BUILD_TYPE=Release \ + -DCUDAToolkit_ROOT="${CUDA_PATH:-/usr/local/cuda}" \ + -DPython3_EXECUTABLE="$(command -v python)" +cmake --build "${build_dir}" --parallel + +echo +echo "build.sh: argus source built -> ${here}/_camera_viz_argus*.so" diff --git a/examples/camera_viz/argus/yuv_to_rgba.cu b/examples/camera_viz/argus/yuv_to_rgba.cu new file mode 100644 index 000000000..7a1a975b1 --- /dev/null +++ b/examples/camera_viz/argus/yuv_to_rgba.cu @@ -0,0 +1,214 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 +// +// Argus YUV420 -> RGBA8 conversion for CUDA EGLStream consumers. + +#include "yuv_to_rgba.cuh" + +namespace +{ + +__device__ __forceinline__ unsigned char clamp_u8(float v) +{ + return static_cast(v < 0.f ? 0.f : (v > 255.f ? 255.f : v)); +} + +__device__ __forceinline__ void yuv_to_rgb( + int Y, int Cb, int Cr, bool full_range, unsigned char& r, unsigned char& g, unsigned char& b) +{ + float R, G, B; + if (full_range) + { + const float yf = static_cast(Y); + const float u = static_cast(Cb) - 128.f; + const float v = static_cast(Cr) - 128.f; + R = yf + 1.402f * v; + G = yf - 0.344136f * u - 0.714136f * v; + B = yf + 1.772f * u; + } + else + { + const float yf = (static_cast(Y) - 16.f) * 1.16438f; + const float u = static_cast(Cb) - 128.f; + const float v = static_cast(Cr) - 128.f; + R = yf + 1.79274f * v; + G = yf - 0.21325f * u - 0.53291f * v; + B = yf + 2.11240f * u; + } + r = clamp_u8(R); + g = clamp_u8(G); + b = clamp_u8(B); +} + +__global__ void yuv420_pitch_to_rgba_kernel(const uint8_t* __restrict__ y_plane, + const uint8_t* __restrict__ uv_or_u_plane, + const uint8_t* __restrict__ v_plane, + int y_pitch, + int uv_pitch, + int v_pitch, + int width, + int height, + uint8_t* __restrict__ rgba_out, + int rgba_row_bytes, + int layout_value, + int full_range_value) +{ + const int x = blockIdx.x * blockDim.x + threadIdx.x; + const int y = blockIdx.y * blockDim.y + threadIdx.y; + if (x >= width || y >= height) + { + return; + } + + const auto layout = static_cast(layout_value); + const int Y = y_plane[y * y_pitch + x]; + int Cb = 128; + int Cr = 128; + + if (layout == camera_viz::argus::YuvLayout::YUV420SemiPlanar || + layout == camera_viz::argus::YuvLayout::YVU420SemiPlanar) + { + const int uv_x = x & ~1; + const int uv_y = y >> 1; + const uint8_t a = uv_or_u_plane[uv_y * uv_pitch + uv_x + 0]; + const uint8_t b = uv_or_u_plane[uv_y * uv_pitch + uv_x + 1]; + if (layout == camera_viz::argus::YuvLayout::YUV420SemiPlanar) + { + Cb = a; + Cr = b; + } + else + { + Cr = a; + Cb = b; + } + } + else + { + const int uv_x = x >> 1; + const int uv_y = y >> 1; + const uint8_t a = uv_or_u_plane[uv_y * uv_pitch + uv_x]; + const uint8_t b = v_plane[uv_y * v_pitch + uv_x]; + if (layout == camera_viz::argus::YuvLayout::YUV420Planar) + { + Cb = a; + Cr = b; + } + else + { + Cr = a; + Cb = b; + } + } + + const int idx = y * rgba_row_bytes + x * 4; + yuv_to_rgb(Y, Cb, Cr, full_range_value != 0, rgba_out[idx + 0], rgba_out[idx + 1], rgba_out[idx + 2]); + rgba_out[idx + 3] = 255; +} + +__global__ void yuv420_array_to_rgba_kernel(cudaTextureObject_t y_tex, + cudaTextureObject_t uv_or_u_tex, + cudaTextureObject_t v_tex, + int width, + int height, + uint8_t* __restrict__ rgba_out, + int rgba_row_bytes, + int layout_value, + int full_range_value) +{ + const int x = blockIdx.x * blockDim.x + threadIdx.x; + const int y = blockIdx.y * blockDim.y + threadIdx.y; + if (x >= width || y >= height) + { + return; + } + + const auto layout = static_cast(layout_value); + const int Y = tex2D(y_tex, x, y); + int Cb = 128; + int Cr = 128; + + if (layout == camera_viz::argus::YuvLayout::YUV420SemiPlanar || + layout == camera_viz::argus::YuvLayout::YVU420SemiPlanar) + { + const int uv_x = x >> 1; + const int uv_y = y >> 1; + const uchar2 uv = tex2D(uv_or_u_tex, uv_x, uv_y); + if (layout == camera_viz::argus::YuvLayout::YUV420SemiPlanar) + { + Cb = uv.x; + Cr = uv.y; + } + else + { + Cr = uv.x; + Cb = uv.y; + } + } + else + { + const int uv_x = x >> 1; + const int uv_y = y >> 1; + const uint8_t a = tex2D(uv_or_u_tex, uv_x, uv_y); + const uint8_t b = tex2D(v_tex, uv_x, uv_y); + if (layout == camera_viz::argus::YuvLayout::YUV420Planar) + { + Cb = a; + Cr = b; + } + else + { + Cr = a; + Cb = b; + } + } + + const int idx = y * rgba_row_bytes + x * 4; + yuv_to_rgb(Y, Cb, Cr, full_range_value != 0, rgba_out[idx + 0], rgba_out[idx + 1], rgba_out[idx + 2]); + rgba_out[idx + 3] = 255; +} + +} // namespace + +namespace camera_viz::argus +{ + +void launch_yuv420_pitch_to_rgba(const uint8_t* y_plane, + const uint8_t* uv_or_u_plane, + const uint8_t* v_plane, + int y_pitch, + int uv_pitch, + int v_pitch, + int width, + int height, + uint8_t* rgba_out, + int rgba_row_bytes, + YuvLayout layout, + bool full_range, + cudaStream_t stream) +{ + const dim3 block(16, 16, 1); + const dim3 grid((width + 15) / 16, (height + 15) / 16, 1); + yuv420_pitch_to_rgba_kernel<<>>(y_plane, uv_or_u_plane, v_plane, y_pitch, uv_pitch, v_pitch, + width, height, rgba_out, rgba_row_bytes, + static_cast(layout), full_range ? 1 : 0); +} + +void launch_yuv420_array_to_rgba(cudaTextureObject_t y_tex, + cudaTextureObject_t uv_or_u_tex, + cudaTextureObject_t v_tex, + int width, + int height, + uint8_t* rgba_out, + int rgba_row_bytes, + YuvLayout layout, + bool full_range, + cudaStream_t stream) +{ + const dim3 block(16, 16, 1); + const dim3 grid((width + 15) / 16, (height + 15) / 16, 1); + yuv420_array_to_rgba_kernel<<>>( + y_tex, uv_or_u_tex, v_tex, width, height, rgba_out, rgba_row_bytes, static_cast(layout), full_range ? 1 : 0); +} + +} // namespace camera_viz::argus diff --git a/examples/camera_viz/argus/yuv_to_rgba.cuh b/examples/camera_viz/argus/yuv_to_rgba.cuh new file mode 100644 index 000000000..752ee59f8 --- /dev/null +++ b/examples/camera_viz/argus/yuv_to_rgba.cuh @@ -0,0 +1,46 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include + +namespace camera_viz::argus +{ + +enum class YuvLayout +{ + YUV420Planar, + YVU420Planar, + YUV420SemiPlanar, + YVU420SemiPlanar, +}; + +void launch_yuv420_pitch_to_rgba(const uint8_t* y_plane, + const uint8_t* uv_or_u_plane, + const uint8_t* v_plane, + int y_pitch, + int uv_pitch, + int v_pitch, + int width, + int height, + uint8_t* rgba_out, + int rgba_row_bytes, + YuvLayout layout, + bool full_range, + cudaStream_t stream); + +void launch_yuv420_array_to_rgba(cudaTextureObject_t y_tex, + cudaTextureObject_t uv_or_u_tex, + cudaTextureObject_t v_tex, + int width, + int height, + uint8_t* rgba_out, + int rgba_row_bytes, + YuvLayout layout, + bool full_range, + cudaStream_t stream); + +} // namespace camera_viz::argus diff --git a/examples/camera_viz/camera_viz.sh b/examples/camera_viz/camera_viz.sh index a3dc5a424..5c06c79da 100755 --- a/examples/camera_viz/camera_viz.sh +++ b/examples/camera_viz/camera_viz.sh @@ -6,7 +6,7 @@ # deployment of the camera_viz example. # # Local: -# ./camera_viz.sh setup [--sender-only] install deps + build codec +# ./camera_viz.sh setup [--sender-only] install deps + build native modules # ./camera_viz.sh loopback CONFIG run streamer + viz on 127.0.0.1 # ./camera_viz.sh run CONFIG [--mode M] run the viewer (honors source:) # @@ -110,6 +110,7 @@ rsync_to_remote() { rsync -az --delete \ --exclude='.venv/' \ --exclude='codec/build/' \ + --exclude='argus/build/' \ --exclude='__pycache__/' \ --exclude='*.pyc' \ --exclude='.pytest_cache/' \ @@ -379,9 +380,9 @@ camera_viz.sh — local development + Jetson deployment for camera_viz LOCAL setup [--venv PATH] [--sender-only] [--jetson] - [--no-v4l2] [--no-oakd] [--no-rtp] [--with-zed] + [--no-v4l2] [--no-oakd] [--no-rtp] [--with-argus] [--with-zed] Create .venv, install Python deps via uv into - the venv, build native codec. Python deps stay + the venv, build native codec / Argus modules. Python deps stay inside .venv (no --system-site-packages). System-side prerequisites (GStreamer plugins, cairo/girepository dev headers, cuda-nvrtc on @@ -398,6 +399,8 @@ LOCAL --jetson adds JetPack-only checks: unversioned CUDA lib symlinks + ld.so wiring that JetPack skips. Off on desktop. + --with-argus builds the optional Jetson Argus + native source when Argus headers are present. loopback CONFIG Run camera_streamer + camera_viz on 127.0.0.1. diff --git a/examples/camera_viz/configs/argus_shw5g.yaml b/examples/camera_viz/configs/argus_shw5g.yaml new file mode 100644 index 000000000..dc6ea662b --- /dev/null +++ b/examples/camera_viz/configs/argus_shw5g.yaml @@ -0,0 +1,53 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# SENSING SHW5G on Jetson AGX Thor through Argus. +# Assumes sensors 0/1 are the tested SHW5G entries from nvargus_nvraw --lps. +# Run nvargus-daemon with enableCamInfiniteTimeout=1 if the driver requires it. +# sync_session: false opens sensors 0/1 as independent Argus sessions; this is +# more tolerant of GMSL drivers that do not support a dual-sensor Argus session. +# The CUDA EGL acquire timeout is infinite because this JetPack/driver stack +# returns CUDA_ERROR_UNKNOWN for finite acquire timeouts before first frame. + +source: local +streaming: + host: 127.0.0.1 +encoder: auto + +cameras: + - name: shw5g + enabled: true + type: argus + stereo: true + sync_session: false + pair_emit_mode: both + sensor_id_left: 0 + sensor_id_right: 1 + sensor_mode: 0 + width: 2560 + height: 1984 + fps: 60 + gpu_id: 0 + acquire_timeout_ms: 4294967295 # CUDA_EGL_INFINITE_TIMEOUT + repeat_capture: true + full_range: false + swap_uv: true + rtp: + port: 5000 + port_right: 5001 + bitrate_mbps: 30 + +display: + mode: xr # window | xr + window: + width: 2560 + height: 1984 + xr: + near_z: 0.05 + far_z: 100.0 + clear_color: [0.0, 0.0, 0.0, 0.0] + placements: + shw5g: # size defaults to 1.0 m wide × aspect-derived height + lock_mode: lazy + distance: 0.5 + stereo_baseline_mm: 50.0 diff --git a/examples/camera_viz/scripts/_install_deps.sh b/examples/camera_viz/scripts/_install_deps.sh index eec3176c2..e6c356ef8 100755 --- a/examples/camera_viz/scripts/_install_deps.sh +++ b/examples/camera_viz/scripts/_install_deps.sh @@ -11,7 +11,7 @@ # --sender-only sender path only. No isaacteleop, no vulkan deps. # # Flags: --venv, --wheel, --python, --no-v4l2, --no-oakd, --no-rtp, -# --with-zed, --zed-sdk. +# --with-argus, --with-zed, --zed-sdk. set -euo pipefail @@ -33,6 +33,7 @@ WHEEL= WITH_V4L2=true WITH_OAKD=true WITH_RTP=true +WITH_ARGUS=false WITH_ZED=false ZED_SDK_DIR=/usr/local/zed # Jetson-specific provisioning: apt-install cuda-nvrtc and create the @@ -51,6 +52,7 @@ while (( $# )); do --no-v4l2) WITH_V4L2=false; shift;; --no-oakd) WITH_OAKD=false; shift;; --no-rtp) WITH_RTP=false; shift;; + --with-argus) WITH_ARGUS=true; shift;; --with-zed) WITH_ZED=true; shift;; --zed-sdk) ZED_SDK_DIR=$2; shift 2;; *) echo "_install_deps.sh: unknown arg: $1" >&2; exit 1;; @@ -430,6 +432,19 @@ if $WITH_RTP; then fi fi +# Native Argus camera source. Jetson-only and opt-in; failures are non-fatal +# because some Jetson images may still lack matching libargus/CUDA headers. +ARGUS_DIR="$CAMERA_VIZ_DIR/argus" +if $WITH_ARGUS && [[ -d "$ARGUS_DIR" && -d /usr/src/jetson_multimedia_api/argus ]]; then + echo "==> building native Argus source" + # shellcheck disable=SC1091 + source "$VENV_DIR/bin/activate" + if ! "$ARGUS_DIR/build.sh"; then + echo "_install_deps.sh: Argus source build failed - type: argus will be unavailable" >&2 + fi + deactivate +fi + # Smoke imports. ``gi`` is in the list under RTP to confirm PyGObject # built and installed cleanly into the venv. echo "==> import smoke" diff --git a/examples/camera_viz/sources/__init__.py b/examples/camera_viz/sources/__init__.py index defdadbb3..edfeef8ae 100644 --- a/examples/camera_viz/sources/__init__.py +++ b/examples/camera_viz/sources/__init__.py @@ -14,6 +14,7 @@ from pipeline import FrameSource from ._helpers import PairedFrameSource, set_verbose +from .argus import ArgusSource from .oakd import OakdSource from .rtp_h264 import RtpH264Source from .synthetic import SyntheticSource, SyntheticStereoSource @@ -22,6 +23,7 @@ from .zed import ZedSource __all__ = [ + "ArgusSource", "OakdSource", "PairedFrameSource", "RtpH264Source", @@ -94,6 +96,43 @@ def build_local_camera(spec: dict) -> List[FrameSource]: fourcc=spec.get("fourcc"), ) ] + if kind == "argus": + + def make_argus(source_name: str, sensor_ids: list[int]) -> ArgusSource: + return ArgusSource( + name=source_name, + sensor_ids=sensor_ids, + width=int(spec["width"]), + height=int(spec["height"]), + sensor_mode=int(spec.get("sensor_mode", 0)), + fps=float(spec.get("fps", 30.0)), + gpu_id=int(spec.get("gpu_id", 0)), + full_range=bool(spec.get("full_range", False)), + swap_uv=bool(spec.get("swap_uv", False)), + acquire_timeout_ms=int(spec.get("acquire_timeout_ms", 0xFFFFFFFF)), + repeat_capture=bool(spec.get("repeat_capture", True)), + ) + + if stereo: + left_id = int(spec.get("sensor_id_left", spec.get("left_sensor_id", 0))) + right_id = int(spec.get("sensor_id_right", spec.get("right_sensor_id", 1))) + sync_session = bool(spec.get("sync_session", False)) + if sync_session: + source = make_argus(name, [left_id, right_id]) + left = source.eye_source("left", f"{name}_left") + right = source.eye_source("right", f"{name}_right") + else: + left = make_argus(f"{name}_left", [left_id]) + right = make_argus(f"{name}_right", [right_id]) + emit_mode = spec.get("pair_emit_mode", "both") + return [ + PairedFrameSource( + name=name, left=left, right=right, emit_mode=emit_mode + ) + ] + + sensor_id = int(spec.get("sensor_id", spec.get("sensor_id_left", 0))) + return [make_argus(name, [sensor_id])] if kind == "oakd": # ``stereo: true`` shorthand for ``mode: stereo``; explicit mode wins. mode = spec.get("mode", "stereo" if stereo else "mono") @@ -157,5 +196,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, argus, oakd, zed, video)" ) diff --git a/examples/camera_viz/sources/_helpers.py b/examples/camera_viz/sources/_helpers.py index 48f777b1d..2afce93c4 100644 --- a/examples/camera_viz/sources/_helpers.py +++ b/examples/camera_viz/sources/_helpers.py @@ -345,14 +345,21 @@ def _mark_disconnected(self) -> None: class PairedFrameSource(FrameSource): """Pair two per-eye FrameSources into one stereo source. - Caches each child's latest Frame and emits a pair whenever either - side updates. The "wait for both, drop on miss" alternative loses - frames at the producer-publishes-L-then-R / consumer-polls-between - race, halving effective fps. Inter-eye drift is bounded to one - producer frame — well under perceptual threshold. + Caches each child's latest Frame. By default it emits a pair whenever + either side updates, preserving the historical behavior for SDKs that + publish eyes in lockstep. ``emit_mode="both"`` waits until both eyes + have refreshed since the last emit, which reduces redundant stereo + submits for independent Argus sessions. Inter-eye drift is bounded to + one producer frame. """ - def __init__(self, name: str, left: FrameSource, right: FrameSource) -> None: + def __init__( + self, + name: str, + left: FrameSource, + right: FrameSource, + emit_mode: str = "either", + ) -> None: if left.spec.width != right.spec.width or left.spec.height != right.spec.height: raise ValueError( f"PairedFrameSource: left/right resolution mismatch " @@ -370,10 +377,15 @@ def __init__(self, name: str, left: FrameSource, right: FrameSource) -> None: height=left.spec.height, pixel_format=left.spec.pixel_format, ) + if emit_mode not in ("either", "both"): + raise ValueError("PairedFrameSource emit_mode must be 'either' or 'both'") self._left = left self._right = right + self._emit_mode = emit_mode self._cached_left: Optional[Frame] = None self._cached_right: Optional[Frame] = None + self._left_dirty = False + self._right_dirty = False @property def spec(self) -> SourceSpec: @@ -390,7 +402,13 @@ def right(self) -> FrameSource: def start(self) -> None: self._left.start() - self._right.start() + try: + self._right.start() + except Exception: + try: + self._left.stop() + finally: + raise def stop(self) -> None: self._left.stop() @@ -401,16 +419,23 @@ def latest(self) -> Optional[Frame]: fl = self._left.latest() if fl is not None: self._cached_left = fl + self._left_dirty = True updated = True fr = self._right.latest() if fr is not None: self._cached_right = fr + self._right_dirty = True updated = True if not updated: return None # Both eyes must have produced at least once before we emit. if self._cached_left is None or self._cached_right is None: return None + if self._emit_mode == "both": + if not (self._left_dirty and self._right_dirty): + return None + self._left_dirty = False + self._right_dirty = False return Frame( image=self._cached_left.image, image_right=self._cached_right.image, diff --git a/examples/camera_viz/sources/argus.py b/examples/camera_viz/sources/argus.py new file mode 100644 index 000000000..7d6d957e5 --- /dev/null +++ b/examples/camera_viz/sources/argus.py @@ -0,0 +1,230 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +"""Jetson Argus camera source for low-latency local capture.""" + +from __future__ import annotations + +import threading +from typing import Optional, Sequence + +from pipeline import Frame, FrameSource, SourceSpec + +from ._helpers import notify + + +class _CudaDevice: + def __init__(self, gpu_id: int) -> None: + self.id = int(gpu_id) + + +class _CudaFrameView: + """Small CUDA-array-interface wrapper around native-owned device memory.""" + + def __init__( + self, data: int, width: int, height: int, pitch: int, gpu_id: int + ) -> None: + self._iface = { + "shape": (int(height), int(width), 4), + "strides": (int(pitch), 4, 1), + "typestr": "|u1", + "data": (int(data), False), + "version": 3, + } + self.device = _CudaDevice(gpu_id) + + @property + def __cuda_array_interface__(self): + return self._iface + + +class ArgusSource(FrameSource): + """Argus STREAM_TYPE_EGL -> CUDA -> RGBA8 source. + + The native module owns Argus/CUDA resources and publishes a latest-frame + mailbox backed by CUDA device memory. Python exposes those pointers via + ``__cuda_array_interface__`` so QuadLayer.submit() and the native encoder + can consume them without a copy or an isaacteleop.viz dependency. + """ + + _kind = "argus" + + def __init__( + self, + name: str, + sensor_ids: Sequence[int], + width: int, + height: int, + sensor_mode: int = 0, + fps: float = 30.0, + gpu_id: int = 0, + full_range: bool = False, + swap_uv: bool = False, + acquire_timeout_ms: int = 0xFFFFFFFF, + repeat_capture: bool = True, + ) -> None: + try: + from argus import ArgusCamera, ArgusConfig + except ImportError as e: + raise RuntimeError( + "Argus source requires the native module. Build it with " + "`examples/camera_viz/argus/build.sh` on the Jetson." + ) from e + + sensor_ids = [int(s) for s in sensor_ids] + if len(sensor_ids) not in (1, 2): + raise ValueError("ArgusSource requires one or two sensor ids") + + cfg = ArgusConfig() + cfg.name = name + cfg.sensor_ids = sensor_ids + cfg.sensor_mode = int(sensor_mode) + cfg.width = int(width) + cfg.height = int(height) + cfg.fps = float(fps) + cfg.gpu_id = int(gpu_id) + cfg.full_range = bool(full_range) + cfg.swap_uv = bool(swap_uv) + cfg.acquire_timeout_ms = int(acquire_timeout_ms) + cfg.repeat_capture = bool(repeat_capture) + + self._spec = SourceSpec( + name=name, width=int(width), height=int(height), pixel_format="rgba8" + ) + self._camera = ArgusCamera(cfg) + self._gpu_id = int(gpu_id) + self._stereo = len(sensor_ids) == 2 + + self._lock = threading.Lock() + self._start_refs = 0 + self._cached_view = None + self._paired_consumed_sequence = 0 + self._eye_consumed_sequence = {"left": 0, "right": 0} + + @property + def spec(self) -> SourceSpec: + return self._spec + + def start(self) -> None: + self._acquire_start_ref() + + def stop(self) -> None: + self._release_start_ref() + + def latest(self) -> Optional[Frame]: + with self._lock: + view = self._fresh_view_locked("paired") + if view is None: + return None + self._paired_consumed_sequence = int(view.sequence) + left = self._make_cuda_view(int(view.left_ptr), int(view.left_pitch)) + right = None + if bool(view.stereo): + right = self._make_cuda_view(int(view.right_ptr), int(view.right_pitch)) + return Frame( + image=left, + image_right=right, + timestamp_ns=int(view.timestamp_ns), + source_id=self._spec.name, + stream=0, + ) + + def eye_source(self, eye: str, name: Optional[str] = None) -> FrameSource: + if not self._stereo: + raise ValueError("ArgusSource.eye_source() requires a stereo ArgusSource") + if eye not in ("left", "right"): + raise ValueError("eye must be 'left' or 'right'") + return _ArgusEyeSource( + parent=self, eye=eye, name=name or f"{self._spec.name}_{eye}" + ) + + def _acquire_start_ref(self) -> None: + with self._lock: + if self._start_refs == 0: + notify(self._kind, f"opening {self._spec.name}...") + try: + self._camera.start() + except Exception: + self._start_refs = 0 + raise + notify(self._kind, f"{self._spec.name} streaming") + self._start_refs += 1 + + def _release_start_ref(self) -> None: + with self._lock: + if self._start_refs <= 0: + return + self._start_refs -= 1 + if self._start_refs == 0: + self._camera.stop() + self._cached_view = None + self._paired_consumed_sequence = 0 + self._eye_consumed_sequence = {"left": 0, "right": 0} + + def _latest_eye(self, eye: str, source_id: str) -> Optional[Frame]: + with self._lock: + view = self._fresh_view_locked(eye) + if view is None: + return None + self._eye_consumed_sequence[eye] = int(view.sequence) + if eye == "left": + image = self._make_cuda_view(int(view.left_ptr), int(view.left_pitch)) + else: + image = self._make_cuda_view(int(view.right_ptr), int(view.right_pitch)) + return Frame( + image=image, + timestamp_ns=int(view.timestamp_ns), + source_id=source_id, + stream=0, + ) + + def _fresh_view_locked(self, consumer: str): + cached = self._cached_view + if cached is not None: + seq = int(cached.sequence) + if consumer == "paired" and self._paired_consumed_sequence != seq: + return cached + if ( + consumer in ("left", "right") + and self._eye_consumed_sequence[consumer] != seq + ): + return cached + + view = self._camera.latest() + if view is None: + return None + self._cached_view = view + return view + + def _make_cuda_view(self, data: int, pitch: int) -> _CudaFrameView: + return _CudaFrameView( + data=data, + width=self._spec.width, + height=self._spec.height, + pitch=pitch, + gpu_id=self._gpu_id, + ) + + +class _ArgusEyeSource(FrameSource): + def __init__(self, parent: ArgusSource, eye: str, name: str) -> None: + self._parent = parent + self._eye = eye + self._spec = SourceSpec( + name=name, + width=parent.spec.width, + height=parent.spec.height, + pixel_format=parent.spec.pixel_format, + ) + + @property + def spec(self) -> SourceSpec: + return self._spec + + def start(self) -> None: + self._parent._acquire_start_ref() + + def latest(self) -> Optional[Frame]: + return self._parent._latest_eye(self._eye, self._spec.name) + + def stop(self) -> None: + self._parent._release_start_ref() diff --git a/examples/camera_viz/tests/test_paired_frame_source_race.py b/examples/camera_viz/tests/test_paired_frame_source_race.py index 4a353be8c..363cbe92a 100644 --- a/examples/camera_viz/tests/test_paired_frame_source_race.py +++ b/examples/camera_viz/tests/test_paired_frame_source_race.py @@ -99,6 +99,39 @@ def test_paired_no_drop_at_full_rate(): assert not missing, f"lost pairs: {missing}" +def test_paired_emit_mode_both_waits_for_both_eyes_each_cycle(): + """Argus independent stereo can suppress duplicate paired submits. + + In this mode a new pair is emitted only after both cached eyes have + refreshed since the previous pair. + """ + left = FakeSource("L") + right = FakeSource("R") + paired = PairedFrameSource("pair", left, right, emit_mode="both") + + left.publish("L0") + right.publish("R0") + f = paired.latest() + assert f is not None + assert f.image == "L0" and f.image_right == "R0" + + left.publish("L1") + assert paired.latest() is None + + right.publish("R1") + f = paired.latest() + assert f is not None + assert f.image == "L1" and f.image_right == "R1" + + right.publish("R2") + assert paired.latest() is None + + left.publish("L2") + f = paired.latest() + assert f is not None + assert f.image == "L2" and f.image_right == "R2" + + def test_paired_returns_none_when_nothing_new(): """latest() returns None when neither child has produced since the last call. Otherwise the consumer would keep submitting stale data