diff --git a/CMakeLists.txt b/CMakeLists.txt index a2f74a8f4..0e2ff8f70 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -62,6 +62,7 @@ option(BUILD_PLUGIN_OAK_CAMERA "Build OAK camera plugin (requires vcpkg for Dept 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) +option(BUILD_PLUGIN_SENSING "Build SENSING GMSL camera plugin (Jetson only; requires libargus + NVENC)" OFF) option(BUILD_EXAMPLES "Build examples" ON) option(BUILD_EXAMPLE_TELEOP_ROS2 "Build only the teleop_ros2 ROS 2 reference integration (e.g. for Docker)" OFF) option(BUILD_TESTING "Build unit tests" ON) @@ -203,6 +204,10 @@ if(BUILD_PLUGINS) if(BUILD_PLUGIN_WUJI_GLOVE) add_subdirectory(src/plugins/wuji_glove) endif() + # Gated: Jetson only — libargus and the Tegra NVENC entry point are L4T-only. + if(BUILD_PLUGIN_SENSING) + add_subdirectory(src/plugins/sensing) + endif() endif() # Formatting enforcement (runs on Linux by default) diff --git a/docs/source/device/sensing.rst b/docs/source/device/sensing.rst new file mode 100644 index 000000000..4f0eade81 --- /dev/null +++ b/docs/source/device/sensing.rst @@ -0,0 +1,357 @@ +.. SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +.. SPDX-License-Identifier: Apache-2.0 + +SENSING GMSL Camera Plugin +========================== + +C++ plugin that captures from SENSING GMSL2 cameras through **libargus** on a +Jetson AGX Orin, and either encodes H.264 on the Jetson's V4L2 engine or hands +frames to another process as CUDA device memory. Source and plugin README: +:code-file:`src/plugins/sensing/README.md`. + +.. contents:: On this page + :local: + :depth: 2 + +Supported hardware +------------------ + +This plugin is developed and verified against exactly one configuration: + +.. list-table:: + :widths: 30 70 + :header-rows: 0 + + * - Host + - NVIDIA Jetson AGX Orin + * - Software + - JetPack 6.2 / L4T R36.4.3, kernel ``5.15.148-tegra`` + * - Carrier + - SENSING **SG10A-AGON-G2M-A1** (GMSL2) + * - Camera + - Orbbec Astra **S56C**, 1920×1080 @ 30 fps, ``sensor_mode=0`` (its only mode) + * - Device tree + - ``Jetson Sensing SG10A_AGON_G2M_A1 S56Cx1 SHF3Lx6`` + +The carrier also takes up to six SHF3L/SHF3H modules. Those are **out of scope** +for this plugin — they are already ISP-processed on the module and stream YUV +over plain V4L2, so ``examples/camera_viz/configs/v4l2.yaml`` reads them +directly. The S56C emits 10-bit Bayer and *must* go through the ISP, which is +why it needs Argus. + +Drivers come from the vendor package, +`nvidia-jetson-camera-drivers `_ +(kernel ``Image``, sensor ``.ko`` files, ``.dtbo`` overlays, ISP tuning). The +scripts below wrap it; they do not replace it. + +Setup +----- + +Bring-up is two-sided and neither half can do the other's job: kernel modules, +the device-tree overlay and the POC/PWM register writes only exist on the host, +while the Argus client socket and the build headers only matter inside the +devcontainer. + +.. code-block:: bash + + src/plugins/sensing/setup.sh # on the Jetson host -> setup_host.sh + src/plugins/sensing/setup.sh # in the devcontainer -> setup_container.sh + src/plugins/sensing/verify.sh # anywhere, read-only + +``setup.sh`` auto-detects the context; ``--host`` / ``--container`` override it. +Each script prints every privileged action it will take before the first +``sudo`` prompt and asks again per optional action. ``--yes`` accepts them all; +non-interactive stdin declines them all. + +Host — first install +~~~~~~~~~~~~~~~~~~~~ + +.. code-block:: bash + + src/plugins/sensing/setup_host.sh --install-drivers + +This runs the vendor ``install.sh`` and stops. Select the overlay and reboot +before continuing: + +.. code-block:: bash + + sudo /opt/nvidia/jetson-io/jetson-io.py # Configure Jetson AGX CSI Connector + # -> Jetson Sensing SG10A_AGON_G2M_A1 S56Cx1 SHF3Lx6 + +The package is autodetected under ``~/Sensing/``, ``~``, ``/home/*/Sensing/`` +and ``/opt/sensing/``; ``--pkg`` or ``$SENSING_PKG_DIR`` overrides. + +Host — every boot +~~~~~~~~~~~~~~~~~ + +.. code-block:: bash + + src/plugins/sensing/setup_host.sh [--fps 30] [--free-run|--trigger-sync] [--service] + +The vendor ``install.sh`` never copies the sensor ``.ko`` files into +``/lib/modules``, so nothing auto-loads them and ``/dev/video*`` is empty after +every reboot. That is the most common cause of "the cameras stopped working". +``setup_host.sh`` loads them, then offers to install ``sensing-camera.service`` +so it happens at boot. + +.. note:: + + The unit is ordered ``Before=nvargus-daemon.service``, **not** + ``After=multi-user.target``. ``nvargus-daemon`` is itself part of + ``multi-user.target``; a daemon that starts before the sensors exist + enumerates an empty camera list and never retries. + +The vendor default slaves every sensor to the carrier's PWM trigger, which only +fires when **J19 pins 2 and 4 are strapped together** — without the strap the +camera opens fine and then delivers no frames, which looks like a software +hang. ``--free-run`` (the default) clears the trigger mode. Use +``--trigger-sync`` only when the strap is fitted and you need cross-camera sync. + +Container +~~~~~~~~~ + +.. code-block:: bash + + src/plugins/sensing/setup_container.sh [--argus-include DIR] [--yes] + +One thing the container cannot fix for itself: ``libnvargus_socketclient`` +reaches ``nvargus-daemon`` through ``/tmp/argus_socket``, and a container gets +its own ``/tmp``. Add the bind mount to ``runArgs`` and rebuild: + +.. code-block:: json + + "-v", "/tmp/argus_socket:/tmp/argus_socket" + +The script also checks for the Argus headers (``v4l-utils``, EGL headers, +``nvcc``), and offers to symlink an Argus tree it finds to +``/usr/src/jetson_multimedia_api/argus``. + +Build +----- + +.. code-block:: bash + + cmake -B build -DBUILD_PLUGIN_SENSING=ON + cmake --build build --target camera_plugin_sensing --parallel + +Both dependencies live outside the repo. On a Jetson host they come from +``sudo apt install nvidia-l4t-jetson-multimedia-api``. In a container, copy the +tree in and point CMake at it: + +.. code-block:: bash + + cmake -B build -DBUILD_PLUGIN_SENSING=ON \ + -DARGUS_INCLUDE_DIRS=$HOME/Sensing/argus/include \ + -DJETSON_MMAPI_DIR=$HOME/Sensing/jetson_multimedia_api + +.. warning:: + + Argus needs NVIDIA's EGL. GLVND picks the EGL vendor from ``DISPLAY``, and + Tegra's EGL cannot drive an Xvfb or forwarded X server, so it loses the probe + to Mesa and every Argus-to-CUDA path fails. The plugin clears ``DISPLAY`` + itself before the first EGL call and refuses to start on a non-NVIDIA vendor, + but anything else in the process that renders will be affected by this. + +Sensor ids are not ``/dev/video`` numbers +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +``sensor=`` in ``--add-stream`` is the **Argus sensor id**, which follows +device-tree module order. On this overlay ids 0–3 are the S56C group and 4–9 the +SHF3L ports. Re-derive after any overlay change: + +.. code-block:: bash + + for i in $(seq 0 9); do + printf '%s ' "$i" + cat /proc/device-tree/tegra-camera-platform/modules/module$i/badge + echo + done + +Recording H.264 +--------------- + +``output=`` on a stream encodes that sensor on the Jetson V4L2 M2M engine +and writes raw Annex-B H.264 — no container, no timestamps. ``--add-stream`` is +repeatable, one per sensor: + +.. code-block:: bash + + ./build/src/plugins/sensing/camera_plugin_sensing \ + --add-stream=sensor=2,output=./left.h264 \ + --add-stream=sensor=3,output=./right.h264 \ + --mcap-filename=./meta.mcap + +Press ``Ctrl+C`` to stop. Run with ``--help`` for the full list; the knobs that +matter most: + +.. list-table:: + :widths: 30 15 55 + :header-rows: 1 + + * - Option + - Default + - Description + * - ``--add-stream=sensor=[,output=][,ipc=]`` + - (at least one required) + - Add a capture stream. At least one of ``output`` / ``ipc`` is required; + both may be given. Repeatable. + * - ``--sensor-mode=N`` + - 0 + - Argus sensor mode. The S56C has only mode 0. + * - ``--width=N`` / ``--height=N`` + - 1920 / 1080 + - Capture resolution + * - ``--fps=N`` + - 30 + - Frame rate for all streams + * - ``--bitrate=N`` + - 20000000 + - H.264 bitrate (bps) + * - ``--gop=N`` + - ``fps*5`` + - IDR period in frames + * - ``--mcap-filename=PATH`` + - none + - Record per-frame metadata to an MCAP file + * - ``--collection-prefix=PREFIX`` + - none + - Push the same metadata over OpenXR instead. Mutually exclusive with + ``--mcap-filename``. + +Metadata +~~~~~~~~ + +Each frame emits a ``core.FrameMetadataSensingRecord`` (sequence number, +timestamps). ``--mcap-filename`` writes it to channel +``sensing_metadata/sensor``; the binary schema is embedded, so ``mcap cat +--json meta.mcap`` decodes it without any Isaac headers. + +``--collection-prefix`` pushes it via OpenXR ``SchemaPusher`` instead, for +recording into the same MCAP as the rest of a teleop session. The wiring is the +same as the OAK plugin's — see :doc:`oak` and :doc:`trackers`. + +Playback +~~~~~~~~ + +Since the file carries no timestamps, a player has to be told the frame rate: + +.. code-block:: bash + + ffplay -f h264 left.h264 + ffmpeg -f h264 -framerate 30 -i left.h264 -c copy left.mp4 # -framerate must match --fps + + # on the Jetson, decoded on the hardware engine that wrote it + gst-launch-1.0 filesrc location=left.h264 ! h264parse ! nvv4l2decoder ! nv3dsink + +To sanity-check a file with no tools installed, the 5th byte after each ``00 00 +01`` start code carries the NAL type in its low 5 bits — a healthy recording +opens ``67`` (SPS), ``68`` (PPS), ``65`` (IDR): ``xxd -l 16 left.h264``. + +Live streaming with camera_viz +------------------------------ + +``ipc=`` serves a sensor's frames to another process as **CUDA device +memory** — RGBA8, no encode, no host round-trip — and +:code-file:`camera_viz ` consumes it with +``type: cuda_ipc``. It is independent of ``output=``; an ``ipc``-only stream +never starts an encoder. Measured on an AGX Orin at 1920×1080: **~1 ms** from +producer timestamp to consumer receipt, sustained at 60 fps. + +.. code-block:: bash + + # terminal 1 — producer + ./build/src/plugins/sensing/camera_plugin_sensing \ + --add-stream=sensor=2,ipc=/tmp/sensing2.sock --width=1920 --height=1080 + + # terminal 2 — viewer + cd examples/camera_viz + ./camera_viz.sh setup # one-time + ./camera_viz.sh run configs/cuda_ipc.yaml --mode window # desktop window + ./camera_viz.sh run configs/cuda_ipc.yaml # XR headset + +:code-file:`configs/cuda_ipc.yaml ` +needs only the socket path and the frame size: + +.. code-block:: yaml + + cameras: + - name: cam + type: cuda_ipc + socket: /tmp/sensing2.sock # must match the producer's ipc= path + width: 1920 # must match what the producer serves + height: 1080 + +Order does not matter — the source retries until the socket appears and survives +the producer restarting under it. ``width``/``height`` are checked during the +handshake and a mismatch is refused rather than rendered at the wrong stride. +**One consumer at a time**: the producer serves whoever connected most recently, +so a second viewer silently takes the feed from the first. + +Testing without a camera +~~~~~~~~~~~~~~~~~~~~~~~~ + +``sensing_ipc_testsrc`` publishes an animated pattern over the same protocol. It +needs CUDA only — no Argus, no encoder — so the viewer can be developed with +nothing attached: + +.. code-block:: bash + + cmake --build build --target sensing_ipc_testsrc + ./build/src/plugins/sensing/sensing_ipc_testsrc --socket=/tmp/sensing2.sock \ + --width=1920 --height=1080 --fps=60 + +Each frame carries its 16-bit frame number as a binary bar across the top, so a +stale or torn frame is visible rather than merely suspected. + +.. note:: + + Legacy CUDA IPC does not work on Tegra and fails misleadingly: + ``cudaIpcGetMemHandle`` returns ``cudaSuccess`` in the producer, then the + consumer's ``cudaIpcOpenMemHandle`` fails with ``cudaErrorInvalidValue``. The + transport therefore uses the virtual-memory-management API — ``cuMemCreate`` + plus ``cuMemExportToShareableHandle`` to a POSIX fd, passed over the Unix + socket with ``SCM_RIGHTS``. Do not rewrite it back to + ``cudaIpcMemHandle_t``. The wire format lives in + :code-file:`src/plugins/sensing/core/cuda_ipc_protocol.hpp` and is + re-declared by the consumer in + :code-file:`examples/camera_viz/sources/cuda_ipc.py`, so the two change + together. + +Troubleshooting +--------------- + +Run ``verify.sh`` first — it names the fixing script for each failure. + +.. list-table:: + :widths: 45 55 + :header-rows: 1 + + * - Symptom + - Cause and fix + * - No ``/dev/video*`` after reboot + - Drivers not loaded. Run ``setup_host.sh``, then install the service. + * - Camera opens, no frames + - Sensor slaved to an absent trigger. ``setup_host.sh --free-run``. + * - Argus finds no cameras + - ``nvargus-daemon`` started before the drivers. + ``sudo systemctl restart nvargus-daemon``. + * - ``EGL_NO_STREAM_KHR``, or ``NvBufSurfaceMapEglImage`` "Failed to create + EGLImage" + - EGL resolved to Mesa, not NVIDIA. Unset ``DISPLAY``, or point it at an X + server Tegra EGL can drive. + * - ``Connection refused`` on every Argus call right after restarting + ``nvargus-daemon`` + - The container bind-mounts ``/tmp/argus_socket`` as a *file*; the daemon + unlinks and recreates it, so the mount pins a deleted inode + (``grep argus /proc/self/mountinfo`` shows ``//deleted``). Mount the + host's ``/tmp`` and symlink instead, or restart the container. + * - ``/tmp/argus_socket`` is a *directory* + - The container started before ``nvargus-daemon``, so Docker created the + missing bind source. + ``sudo rmdir /tmp/argus_socket && sudo systemctl restart nvargus-daemon``. + * - ``insmod: invalid module format`` + - Kernel is not ``5.15.148-tegra``; the prebuilt ``.ko`` files will not + load. + * - ``argus_camera: command not found`` in the container + - It is installed at ``/usr/local/bin`` on the *host*. + ``setup_container.sh`` offers to link a built copy. diff --git a/docs/source/index.rst b/docs/source/index.rst index cee6b9985..eb2da0df6 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -67,6 +67,7 @@ Table of Contents device/manus device/oak device/oglo + device/sensing device/wuji_glove device/haptikos diff --git a/examples/camera_viz/README.md b/examples/camera_viz/README.md index e90b314de..8e8ea43b4 100644 --- a/examples/camera_viz/README.md +++ b/examples/camera_viz/README.md @@ -21,6 +21,7 @@ SPDX-License-Identifier: Apache-2.0 | `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) | | `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) | +| `cuda_ipc` | RGBA8 frames mapped straight out of another process's CUDA memory — no encode, no host copy. Pairs with the [sensing plugin](../../src/plugins/sensing/README.md#cuda-ipc); see [below](#cuda_ipc-frames-from-another-process) | 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; `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`. @@ -141,6 +142,47 @@ display: # camera_viz only Multiple cameras → multiple `cameras:` entries; each gets its own `rtp.port` (plus `port_right` if stereo) and renders as its own plane. +## `cuda_ipc` — frames from another process + +A producer on the same machine publishes captured frames as CUDA device +memory; camera_viz maps that memory once and renders from it. Nothing is +encoded and nothing crosses host RAM, so the cost per frame is one 24-byte +socket message. Measured on an AGX Orin at 1920×1080: **~1 ms** from producer +timestamp to consumer receipt, sustained at 60 fps. + +```yaml +cameras: + - name: cam + type: cuda_ipc + socket: /tmp/sensing2.sock # must match the producer's ipc= path + width: 1920 # must match what the producer serves + height: 1080 +``` + +Start either side first — the source retries until the socket appears, and +survives the producer restarting under it. `width`/`height` are checked during +the handshake and a mismatch is refused rather than rendered at the wrong +stride. + +Producer is the sensing plugin: + +```bash +camera_plugin_sensing --add-stream=sensor=2,ipc=/tmp/sensing2.sock \ + --width=1920 --height=1080 +``` + +To develop against this without a camera, the plugin ships an animated test +pattern that speaks the same protocol: + +```bash +cmake --build build --target sensing_ipc_testsrc +./build/src/plugins/sensing/sensing_ipc_testsrc --socket=/tmp/sensing2.sock \ + --width=1920 --height=1080 +``` + +**One consumer at a time.** The producer serves whoever connected most +recently, so a second camera_viz silently takes the feed from the first. + ## Known issues ### No video on Orin diff --git a/examples/camera_viz/configs/cuda_ipc.yaml b/examples/camera_viz/configs/cuda_ipc.yaml new file mode 100644 index 000000000..ba1dbf79e --- /dev/null +++ b/examples/camera_viz/configs/cuda_ipc.yaml @@ -0,0 +1,44 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# CUDA IPC source — frames arrive as GPU memory mapped straight out of the +# producer process. No encode, no decode, no host copy. +# +# Producer is the sensing plugin: +# camera_plugin_sensing --add-stream=sensor=2,ipc=/tmp/sensing2.sock \ +# --width=1920 --height=1080 +# +# Or, with no camera attached: +# sensing_ipc_testsrc --socket=/tmp/sensing2.sock --width=1920 --height=1080 +# +# Then, in a second terminal: +# ./camera_viz.sh run configs/cuda_ipc.yaml --mode window # desktop window +# ./camera_viz.sh run configs/cuda_ipc.yaml # XR headset +# +# ``width``/``height`` must match what the producer serves; the handshake +# rejects a mismatch rather than rendering garbage. Order does not matter — +# the source retries until the socket appears, and survives it restarting. + +source: local + +cameras: + - name: cam + enabled: true + type: cuda_ipc + socket: /tmp/sensing2.sock + width: 1920 + height: 1080 + +display: + mode: xr # xr | window (xr is the default) + window: + width: 1920 + height: 1080 + xr: + near_z: 0.05 + far_z: 100.0 + clear_color: [0.0, 0.0, 0.0, 0.0] + placements: + cam: + lock_mode: lazy + distance: 1.0 diff --git a/examples/camera_viz/sources/__init__.py b/examples/camera_viz/sources/__init__.py index defdadbb3..900d422ce 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 .cuda_ipc import CudaIpcSource from .oakd import OakdSource from .rtp_h264 import RtpH264Source from .synthetic import SyntheticSource, SyntheticStereoSource @@ -22,6 +23,7 @@ from .zed import ZedSource __all__ = [ + "CudaIpcSource", "OakdSource", "PairedFrameSource", "RtpH264Source", @@ -94,6 +96,22 @@ def build_local_camera(spec: dict) -> List[FrameSource]: fourcc=spec.get("fourcc"), ) ] + if kind == "cuda_ipc": + if stereo: + # Each publisher serves one sensor, so a stereo rig is two + # cameras: entries here, paired by the caller. + raise ValueError( + f"build_local_camera: cuda_ipc camera {name!r} cannot be stereo — " + "declare one entry per socket." + ) + return [ + CudaIpcSource( + name=name, + socket_path=spec["socket"], + width=int(spec["width"]), + height=int(spec["height"]), + ) + ] if kind == "oakd": # ``stereo: true`` shorthand for ``mode: stereo``; explicit mode wins. mode = spec.get("mode", "stereo" if stereo else "mono") @@ -157,5 +175,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, cuda_ipc, oakd, zed, video)" ) diff --git a/examples/camera_viz/sources/_cuda_vmm.py b/examples/camera_viz/sources/_cuda_vmm.py new file mode 100644 index 000000000..f8485c632 --- /dev/null +++ b/examples/camera_viz/sources/_cuda_vmm.py @@ -0,0 +1,195 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +"""Minimal ctypes binding for the CUDA virtual-memory-management API. + +Only what importing a shared device allocation needs. ``libcuda.so.1`` ships +with the driver, so this keeps the ``cuda_ipc`` source free of any new Python +dependency — CuPy alone is not enough, as it exposes no ``cuMemImport*``. + +Legacy CUDA IPC (``cudaIpcOpenMemHandle``) is not an option here: on Tegra it +fails with ``cudaErrorInvalidValue`` even though the producer's +``cudaIpcGetMemHandle`` succeeded. The VMM path is the one that works. +""" + +from __future__ import annotations + +import ctypes + +# CUmemAllocationHandleType +CU_MEM_HANDLE_TYPE_POSIX_FILE_DESCRIPTOR = 1 +# CUmemLocationType +CU_MEM_LOCATION_TYPE_DEVICE = 1 +# CUmemAccess_flags +CU_MEM_ACCESS_FLAGS_PROT_READWRITE = 3 + + +class _CUmemLocation(ctypes.Structure): + _fields_ = [("type", ctypes.c_int), ("id", ctypes.c_int)] + + +class CUmemAccessDesc(ctypes.Structure): + _fields_ = [("location", _CUmemLocation), ("flags", ctypes.c_int)] + + +class CudaDriverError(RuntimeError): + pass + + +class CudaDriver: + """Lazily-loaded handle onto the few driver entry points we need.""" + + def __init__(self) -> None: + try: + self._lib = ctypes.CDLL("libcuda.so.1") + except OSError as e: + raise CudaDriverError( + "libcuda.so.1 not found — the NVIDIA driver is not installed " + "or not visible in this container." + ) from e + + # cuMemImportFromShareableHandle takes the fd by value in a void*, not + # a pointer to it; passing &fd yields CUDA_ERROR_INVALID_VALUE. + self._lib.cuMemImportFromShareableHandle.argtypes = [ + ctypes.POINTER(ctypes.c_ulonglong), + ctypes.c_void_p, + ctypes.c_int, + ] + self._lib.cuMemAddressReserve.argtypes = [ + ctypes.POINTER(ctypes.c_ulonglong), + ctypes.c_size_t, + ctypes.c_size_t, + ctypes.c_ulonglong, + ctypes.c_ulonglong, + ] + self._lib.cuMemMap.argtypes = [ + ctypes.c_ulonglong, + ctypes.c_size_t, + ctypes.c_size_t, + ctypes.c_ulonglong, + ctypes.c_ulonglong, + ] + self._lib.cuMemSetAccess.argtypes = [ + ctypes.c_ulonglong, + ctypes.c_size_t, + ctypes.POINTER(CUmemAccessDesc), + ctypes.c_size_t, + ] + self._lib.cuMemUnmap.argtypes = [ctypes.c_ulonglong, ctypes.c_size_t] + self._lib.cuMemAddressFree.argtypes = [ctypes.c_ulonglong, ctypes.c_size_t] + self._lib.cuMemRelease.argtypes = [ctypes.c_ulonglong] + self._lib.cuDevicePrimaryCtxRetain.argtypes = [ + ctypes.POINTER(ctypes.c_void_p), + ctypes.c_int, + ] + self._lib.cuDevicePrimaryCtxRelease.argtypes = [ctypes.c_int] + self._lib.cuCtxSetCurrent.argtypes = [ctypes.c_void_p] + self._lib.cuInit.argtypes = [ctypes.c_uint] + self._lib.cuGetErrorName.argtypes = [ + ctypes.c_int, + ctypes.POINTER(ctypes.c_char_p), + ] + + self._check(self._lib.cuInit(0), "cuInit") + + def _check(self, result: int, what: str) -> None: + if result == 0: + return + name = ctypes.c_char_p() + self._lib.cuGetErrorName(result, ctypes.byref(name)) + detail = name.value.decode() if name.value else f"code {result}" + raise CudaDriverError(f"{what} failed: {detail}") + + # ── context ─────────────────────────────────────────────────────── + + def primary_ctx_retain(self, device_id: int) -> int: + ctx = ctypes.c_void_p() + self._check( + self._lib.cuDevicePrimaryCtxRetain(ctypes.byref(ctx), device_id), + "cuDevicePrimaryCtxRetain", + ) + return ctx.value or 0 + + def primary_ctx_release(self, device_id: int) -> None: + self._lib.cuDevicePrimaryCtxRelease(device_id) + + def ctx_set_current(self, ctx: int) -> None: + self._check(self._lib.cuCtxSetCurrent(ctypes.c_void_p(ctx)), "cuCtxSetCurrent") + + # ── shared allocation ───────────────────────────────────────────── + + def import_fd(self, fd: int) -> int: + """Import a POSIX fd exported by ``cuMemExportToShareableHandle``.""" + handle = ctypes.c_ulonglong() + self._check( + self._lib.cuMemImportFromShareableHandle( + ctypes.byref(handle), + ctypes.c_void_p(fd), + CU_MEM_HANDLE_TYPE_POSIX_FILE_DESCRIPTOR, + ), + "cuMemImportFromShareableHandle", + ) + return handle.value + + def map_readwrite(self, handle: int, size: int, device_id: int) -> int: + """Reserve VA, map the handle into it, and grant this device access. + + Returns the device pointer. On failure everything already acquired is + rolled back, so the caller never has to unwind a partial mapping. + """ + ptr = ctypes.c_ulonglong() + self._check( + self._lib.cuMemAddressReserve(ctypes.byref(ptr), size, 0, 0, 0), + "cuMemAddressReserve", + ) + try: + self._check(self._lib.cuMemMap(ptr, size, 0, handle, 0), "cuMemMap") + except CudaDriverError: + self._lib.cuMemAddressFree(ptr, size) + raise + + desc = CUmemAccessDesc() + desc.location.type = CU_MEM_LOCATION_TYPE_DEVICE + desc.location.id = device_id + desc.flags = CU_MEM_ACCESS_FLAGS_PROT_READWRITE + try: + self._check( + self._lib.cuMemSetAccess(ptr, size, ctypes.byref(desc), 1), + "cuMemSetAccess", + ) + except CudaDriverError: + self._lib.cuMemUnmap(ptr, size) + self._lib.cuMemAddressFree(ptr, size) + raise + + return ptr.value + + def unmap(self, ptr: int, size: int, handle: int) -> list: + """Tear down a ``map_readwrite`` mapping. Idempotent. + + Returns the name of each step that failed rather than raising, so a + caller unwinding after an error still completes the rest. Silence here + would turn a failed release into a slow leak, so callers should log + whatever comes back. + """ + failures = [] + if ptr: + for name, fn in ( + ("cuMemUnmap", self._lib.cuMemUnmap), + ("cuMemAddressFree", self._lib.cuMemAddressFree), + ): + if fn(ptr, size) != 0: + failures.append(name) + if handle and self._lib.cuMemRelease(handle) != 0: + failures.append("cuMemRelease") + return failures + + +_driver: CudaDriver | None = None + + +def driver() -> CudaDriver: + """Process-wide driver handle; ``cuInit`` runs once.""" + global _driver + if _driver is None: + _driver = CudaDriver() + return _driver diff --git a/examples/camera_viz/sources/cuda_ipc.py b/examples/camera_viz/sources/cuda_ipc.py new file mode 100644 index 000000000..3200c5203 --- /dev/null +++ b/examples/camera_viz/sources/cuda_ipc.py @@ -0,0 +1,415 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +"""Zero-copy CUDA frame source over a Unix socket. + +Consumes frames published by the sensing plugin's ``CudaIpcPublisher`` +(``src/plugins/sensing/core/cuda_ipc_publisher.cpp``). The producer exports one +CUDA allocation holding a ring of RGBA8 slots; this source maps it once and +hands the renderer a CuPy view straight onto producer memory. No encode, no +decode, no host round-trip — the only per-frame traffic on the socket is a +24-byte ready message. + +Unlike every other source here, the pixels are *not* ours: a slot stays valid +only until we release it. ``latest()`` therefore releases the previously +returned slot, which is safe because the viz layer copies during ``submit()`` +before the next poll comes round. + +Wire format mirrors ``core/cuda_ipc_protocol.hpp``; the struct format strings +below are that file's layout and must change with it. +""" + +from __future__ import annotations + +import os +import socket +import struct +import threading +import time +from typing import Optional + +from pipeline import Frame, FrameSource, SourceSpec + +from ._cuda_vmm import CudaDriverError, driver +from ._helpers import notify, notify_verbose + +# core/cuda_ipc_protocol.hpp. '<' pins little-endian and kills native padding; +# the C++ side is checked against these sizes by static_assert. +_HELLO_FMT = "<8I3Q" +_HELLO_SIZE = struct.calcsize(_HELLO_FMT) +_FRAME_FMT = "<2I2Q" +_FRAME_SIZE = struct.calcsize(_FRAME_FMT) +_RELEASE_FMT = "<2I" + +_HELLO_MAGIC = 0x44554349 +_FRAME_MAGIC = 0x4D524649 +_RELEASE_MAGIC = 0x4C455249 +_PROTOCOL_VERSION = 1 +_FORMAT_RGBA8 = 0 + +# A producer that answers but disagrees (geometry, protocol version, pixel +# format) is a config error, not a race, so back off hard. Retrying at the +# normal cadence would evict a correctly-configured consumer once a second, +# since the producer serves whoever connected last. +_REJECT_DELAY_S = 5.0 + +# A retired mapping is freed only after the renderer has provably stopped +# touching it. The mailbox is cleared first, so one poll interval is enough; +# a quarter second is many intervals and still bounds the memory held. +_RETIRE_GRACE_S = 0.25 + + +class CudaIpcSource(FrameSource): + """RGBA8 frames mapped directly from a producer process's CUDA memory.""" + + _kind = "cuda_ipc" + + def __init__( + self, + name: str, + socket_path: str, + width: int, + height: int, + reconnect_delay_s: float = 1.0, + ) -> None: + try: + import cupy as cp + except ImportError as e: + raise RuntimeError( + "cuda_ipc source requires CuPy (cupy-cuda12x). " + "Install via `uv pip install cupy-cuda12x`." + ) from e + + self._cp = cp + self._spec = SourceSpec( + name=name, width=width, height=height, pixel_format="rgba8" + ) + self._socket_path = socket_path + self._reconnect_delay_s = float(reconnect_delay_s) + + self._sock: Optional[socket.socket] = None + self._send_lock = threading.Lock() + + # Mapping state, replaced wholesale on every (re)connect. + self._slots: list = [] + self._ptr = 0 + self._size = 0 + self._handle = 0 + self._device_id = 0 + self._ctx = 0 + + # Mailbox. ``_pending`` is published-but-unconsumed, ``_inflight`` is + # the slot the renderer received last and may still be reading. + self._lock = threading.Lock() + self._pending: Optional[tuple] = None + self._inflight: Optional[int] = None + + self._stop = threading.Event() + self._thread: Optional[threading.Thread] = None + self._frame_count = 0 + self._last_report_s = 0.0 + + # ── FrameSource interface ───────────────────────────────────────── + + @property + def spec(self) -> SourceSpec: + return self._spec + + def start(self) -> None: + if self._thread is not None: + return + self._stop.clear() + self._thread = threading.Thread( + target=self._reader_loop, + name=f"cuda_ipc_{self._spec.name}", + daemon=False, + ) + self._thread.start() + + def stop(self) -> None: + self._stop.set() + if self._thread is not None: + self._thread.join(timeout=5.0) + if self._thread.is_alive(): + notify(self._kind, "reader thread did not exit; leaking mapping") + return + self._thread = None + self._teardown() + + def latest(self) -> Optional[Frame]: + with self._lock: + if self._pending is None: + return None + slot, sequence, timestamp_ns = self._pending + self._pending = None + previous, self._inflight = self._inflight, slot + slots = self._slots + if slot >= len(slots): + return None # torn down between publish and poll + image = slots[slot] + + # The renderer asking for a new frame means it is done with the last + # one, so its slot can go back to the producer. + if previous is not None and previous != slot: + self._release(previous) + + self._frame_count += 1 + now = time.monotonic() + if now - self._last_report_s >= 5.0: + notify_verbose(self._kind, f"{self._frame_count} frames, seq={sequence}") + self._last_report_s = now + + # stream=0: the producer synchronized its copy stream before telling us + # the slot was ready, so the pixels are complete on any stream we read from. + return Frame( + image=image, + timestamp_ns=timestamp_ns, + source_id=self._spec.name, + stream=0, + ) + + # ── reader thread ───────────────────────────────────────────────── + + def _reader_loop(self) -> None: + opening_notified = False + while not self._stop.is_set(): + try: + self._connect() + except (ValueError, CudaDriverError) as e: + # The producer is there and we could not agree with it. Say so + # every time: unlike a missing socket, this will not fix itself. + self._teardown() + notify(self._kind, f"rejected producer on {self._socket_path}: {e}") + self._stop.wait(timeout=_REJECT_DELAY_S) + continue + except OSError as e: + self._teardown() + if not opening_notified: + notify( + self._kind, f"waiting for producer on {self._socket_path} ({e})" + ) + opening_notified = True + self._stop.wait(timeout=self._reconnect_delay_s) + continue + + notify( + self._kind, + f"attached to {self._socket_path} " + f"({self._spec.width}x{self._spec.height}, {len(self._slots)} slots)", + ) + opening_notified = False + self._pump() + self._teardown() + + def _connect(self) -> None: + sock = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) + sock.settimeout(2.0) + sock.connect(self._socket_path) + # Short timeout so stop() is responsive while blocked in recv. + sock.settimeout(0.5) + + fd = -1 + try: + msg, fds, _flags, _addr = socket.recv_fds(sock, _HELLO_SIZE, 1) + if not fds: + raise ValueError("producer sent no memory handle") + fd = fds[0] + if len(msg) < _HELLO_SIZE: + # Ancillary data rides the first byte, so a short read still + # carries the fd; finish the header before using it. + msg += _recv_exact(sock, _HELLO_SIZE - len(msg)) + self._handshake(sock, msg, fd) + except BaseException: + sock.close() + raise + finally: + # cuMemImportFromShareableHandle dups what it needs; holding the fd + # open past the import would pin the producer's allocation after it + # exits. + if fd >= 0: + os.close(fd) + + def _handshake(self, sock: socket.socket, msg: bytes, fd: int) -> None: + ( + magic, + version, + width, + height, + pixel_format, + slot_count, + device_id, + _sensor_id, + pitch, + slot_stride, + total_bytes, + ) = struct.unpack(_HELLO_FMT, msg) + + if magic != _HELLO_MAGIC: + raise ValueError(f"bad handshake magic 0x{magic:08x}") + if version != _PROTOCOL_VERSION: + raise ValueError( + f"producer speaks protocol v{version}, this source speaks " + f"v{_PROTOCOL_VERSION} — rebuild both sides" + ) + if pixel_format != _FORMAT_RGBA8: + raise ValueError( + f"unsupported pixel format {pixel_format} (expected RGBA8)" + ) + if (width, height) != (self._spec.width, self._spec.height): + raise ValueError( + f"producer serves {width}x{height} but the config declares " + f"{self._spec.width}x{self._spec.height}" + ) + if pitch != width * 4: + raise ValueError(f"expected tightly packed rows, got pitch {pitch}") + # These size the mapping and the per-slot views, so a producer that + # disagrees with itself must be rejected here rather than turned into + # out-of-bounds device pointers. + if not 2 <= slot_count <= 64: + raise ValueError(f"implausible slot count {slot_count}") + if slot_stride < height * pitch: + raise ValueError( + f"slot stride {slot_stride} is smaller than a {width}x{height} frame" + ) + if slot_count * slot_stride > total_bytes: + raise ValueError( + f"{slot_count} slots of {slot_stride} B overrun the " + f"{total_bytes} B allocation" + ) + + cp = self._cp + # Import into the primary context, which is the one CuPy allocates and + # launches on; a pointer from any other context would fault on use. + cp.cuda.Device(device_id).use() + drv = driver() + self._ctx = drv.primary_ctx_retain(device_id) + drv.ctx_set_current(self._ctx) + self._device_id = device_id + + self._handle = drv.import_fd(fd) + self._ptr = drv.map_readwrite(self._handle, total_bytes, device_id) + self._size = total_bytes + + # One CuPy view per slot, built once. ``owner=self`` keeps this source + # alive for as long as any handed-out frame references its memory. + self._slots = [] + for i in range(slot_count): + mem = cp.cuda.UnownedMemory( + self._ptr + i * slot_stride, + height * pitch, + self, + device_id, + ) + self._slots.append( + cp.ndarray( + (height, width, 4), + dtype=cp.uint8, + memptr=cp.cuda.MemoryPointer(mem, 0), + ) + ) + + self._sock = sock + + def _pump(self) -> None: + """Read ready messages until the producer goes away or we're stopped.""" + sock = self._sock + assert sock is not None + while not self._stop.is_set(): + try: + header = _recv_exact(sock, _FRAME_SIZE) + except socket.timeout: + continue + except OSError as e: + notify(self._kind, f"producer connection lost ({e})") + return + except EOFError: + notify(self._kind, "producer disconnected") + return + + magic, slot, sequence, timestamp_ns = struct.unpack(_FRAME_FMT, header) + if magic != _FRAME_MAGIC or slot >= len(self._slots): + notify(self._kind, "protocol desync; reconnecting") + return + + with self._lock: + superseded = self._pending + self._pending = (slot, sequence, timestamp_ns) + + # A frame the renderer never picked up is dropped here rather than + # queued: this is a mailbox, and a stale frame is worth less than + # the slot it occupies. + if superseded is not None and superseded[0] != slot: + self._release(superseded[0]) + + # ── plumbing ────────────────────────────────────────────────────── + + def _release(self, slot: int) -> None: + sock = self._sock + if sock is None: + return + payload = struct.pack(_RELEASE_FMT, _RELEASE_MAGIC, slot) + try: + with self._send_lock: + sock.sendall(payload) + except OSError: + # Producer is gone; _pump will notice and reconnect. + pass + + def _teardown(self) -> None: + with self._lock: + self._pending = None + self._inflight = None + if self._sock is not None: + try: + self._sock.close() + except OSError: + pass + self._sock = None + + # The context is retained before the mapping exists, so release it even + # when a connect failed partway — otherwise every retry adds a + # reference the primary context is never destroyed under. + if not self._ctx: + return + + if self._ptr: + # Drop the views before unmapping, then wait out any submit() + # already in flight on the render thread — freeing under it would + # fault. + self._slots = [] + time.sleep(_RETIRE_GRACE_S) + + drv = driver() + drv.ctx_set_current(self._ctx) + failures = drv.unmap(self._ptr, self._size, self._handle) + if failures: + notify(self._kind, f"leaked shared mapping: {', '.join(failures)} failed") + self._slots = [] + self._ptr = 0 + self._size = 0 + self._handle = 0 + drv.primary_ctx_release(self._device_id) + self._ctx = 0 + + +def _recv_exact(sock: socket.socket, count: int) -> bytes: + """Read exactly ``count`` bytes. SOCK_STREAM may split any message. + + The socket carries a short timeout so the reader stays responsive to + stop(). That timeout may only surface *between* messages: abandoning a + half-read one would leave the stream misaligned, so once any byte has + arrived this keeps waiting for the rest. + """ + chunks = [] + remaining = count + while remaining: + try: + chunk = sock.recv(remaining) + except socket.timeout: + if remaining == count: + raise + continue + if not chunk: + raise EOFError("peer closed the connection") + chunks.append(chunk) + remaining -= len(chunk) + return b"".join(chunks) diff --git a/examples/camera_viz/tests/test_cuda_ipc_source.py b/examples/camera_viz/tests/test_cuda_ipc_source.py new file mode 100644 index 000000000..e9c07fe19 --- /dev/null +++ b/examples/camera_viz/tests/test_cuda_ipc_source.py @@ -0,0 +1,253 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +"""Tests for the CUDA IPC source (``type: cuda_ipc``). + +Wire-format and config checks run everywhere. The end-to-end tests drive the +real producer, ``sensing_ipc_testsrc``, and skip unless it has been built and a +CUDA device is present: + + cmake --build build --target sensing_ipc_testsrc + +``sensing_ipc_testsrc`` paints a 16-bit frame counter into the top-left of +every frame, so a test can tell a fresh frame from a stale or torn one by +reading pixels rather than trusting the sequence number the producer sends. +""" + +from __future__ import annotations + +import os +import signal +import struct +import subprocess +import time +from pathlib import Path + +import pytest + +from sources import build_local_camera # noqa: E402 +from sources.cuda_ipc import ( # noqa: E402 + _FRAME_FMT, + _HELLO_FMT, + _RELEASE_FMT, + CudaIpcSource, +) + +_REPO_ROOT = Path(__file__).resolve().parents[3] +_TESTSRC_REL = "src/plugins/sensing/sensing_ipc_testsrc" + + +def _find_testsrc() -> Path | None: + override = os.environ.get("SENSING_IPC_TESTSRC") + if override: + p = Path(override) + return p if p.is_file() else None + for build_dir in (_REPO_ROOT / "build", _REPO_ROOT / "cmake-build-debug"): + candidate = build_dir / _TESTSRC_REL + if candidate.is_file(): + return candidate + return None + + +def _cuda_available() -> bool: + try: + import cupy as cp + except ImportError: + return False + try: + return cp.cuda.runtime.getDeviceCount() > 0 + except Exception: + return False + + +_testsrc = _find_testsrc() + +requires_producer = pytest.mark.skipif( + _testsrc is None or not _cuda_available(), + reason="needs cupy + a CUDA device + a built sensing_ipc_testsrc", +) + + +# ── wire format ─────────────────────────────────────────────────────── + + +def test_struct_sizes_match_the_cpp_static_asserts(): + """core/cuda_ipc_protocol.hpp static_asserts these exact sizes.""" + assert struct.calcsize(_HELLO_FMT) == 56 + assert struct.calcsize(_FRAME_FMT) == 24 + assert struct.calcsize(_RELEASE_FMT) == 8 + + +def test_magics_are_the_ascii_tags_the_producer_sends(): + from sources import cuda_ipc + + assert struct.pack(" "_Producer": + self._proc = subprocess.Popen( + self._argv, stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL + ) + time.sleep(1.5) # let it allocate + bind before anyone connects + assert self._proc.poll() is None, "producer exited during startup" + return self + + def stop(self) -> None: + if self._proc is not None and self._proc.poll() is None: + self._proc.send_signal(signal.SIGINT) + try: + self._proc.wait(timeout=5) + except subprocess.TimeoutExpired: + self._proc.kill() + self._proc = None + + def __exit__(self, *exc) -> None: + self.stop() + + +def _frame_counter(image) -> int: + """Read the producer's 16-bit counter bar: 16 cells, 24 px each.""" + import cupy as cp + + centres = cp.asnumpy(image[12, 12 : 16 * 24 : 24, 0]) + return int("".join("1" if v > 127 else "0" for v in centres), 2) + + +def _collect(source: CudaIpcSource, want: int, timeout_s: float = 15.0) -> list: + counters = [] + deadline = time.monotonic() + timeout_s + while len(counters) < want and time.monotonic() < deadline: + frame = source.latest() + if frame is None: + time.sleep(0.002) + continue + counters.append(_frame_counter(frame.image)) + return counters + + +@requires_producer +def test_frames_arrive_and_always_advance(tmp_path): + """The headline property: every frame handed out is fresh, never torn. + + A slot the producer reused while we were reading it would show a counter + that stalls or goes backwards. + """ + sock = str(tmp_path / "ipc.sock") + with _Producer(sock, 640, 480) as producer: + source = CudaIpcSource(name="cam", socket_path=sock, width=640, height=480) + source.start() + try: + counters = _collect(source, 60) + finally: + source.stop() + producer.stop() + + assert len(counters) >= 60, f"only {len(counters)} frames arrived" + deltas = [b - a for a, b in zip(counters, counters[1:])] + assert all(d > 0 for d in deltas), f"stale or torn frame: deltas={deltas}" + + +@requires_producer +def test_image_is_a_gpu_rgba_view_of_the_declared_size(tmp_path): + import cupy as cp + + sock = str(tmp_path / "ipc.sock") + with _Producer(sock, 320, 240) as producer: + source = CudaIpcSource(name="cam", socket_path=sock, width=320, height=240) + source.start() + try: + deadline = time.monotonic() + 10.0 + frame = None + while frame is None and time.monotonic() < deadline: + frame = source.latest() + if frame is None: + time.sleep(0.002) + assert frame is not None, "no frame within 10s" + assert frame.image.shape == (240, 320, 4) + assert frame.image.dtype == cp.uint8 + assert hasattr(frame.image, "__cuda_array_interface__") + assert frame.source_id == "cam" + assert frame.timestamp_ns > 0 + finally: + source.stop() + producer.stop() + + +@requires_producer +def test_waits_for_a_late_producer_then_recovers_from_its_restart(tmp_path): + sock = str(tmp_path / "ipc.sock") + source = CudaIpcSource(name="cam", socket_path=sock, width=320, height=240) + source.start() + try: + # Started before any producer exists: must idle, not raise. + time.sleep(1.0) + assert source.latest() is None + + with _Producer(sock, 320, 240): + assert len(_collect(source, 10)) >= 10 + + # Producer gone; the source should be idle again rather than serving + # pixels out of an allocation that no longer has an owner. + time.sleep(1.0) + + with _Producer(sock, 320, 240): + assert len(_collect(source, 10)) >= 10, "did not recover" + finally: + source.stop() + + +@requires_producer +def test_geometry_mismatch_is_refused(tmp_path): + """A config that disagrees with the producer must yield no frames rather + than reinterpreting its bytes at the wrong stride.""" + sock = str(tmp_path / "ipc.sock") + with _Producer(sock, 320, 240): + source = CudaIpcSource(name="cam", socket_path=sock, width=640, height=480) + source.start() + try: + time.sleep(3.0) + assert source.latest() is None + finally: + source.stop() diff --git a/src/core/schema/fbs/sensing.fbs b/src/core/schema/fbs/sensing.fbs new file mode 100644 index 000000000..709a7f1dc --- /dev/null +++ b/src/core/schema/fbs/sensing.fbs @@ -0,0 +1,31 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +include "timestamp.fbs"; + +namespace core; + +// Per-frame metadata pushed by the SENSING camera plugin (one per stream). +// Streams are keyed by Argus sensor id rather than an enum: the SG10A carrier +// exposes ten sensors and the populated set varies per rig. +// All fields are always present when the parent Tracked/Record wrapper's data is non-null. +table FrameMetadataSensing { + // Argus sensor id, which follows device-tree module order (not /dev/videoN). + sensor_id: uint32 (id: 0); + + // Per-stream frame sequence number, counted by the plugin from Argus publishes. + sequence_number: uint64 (id: 1); +} + +// Tracked wrapper for the in-memory tracker API (data is null when no metadata available). +table FrameMetadataSensingTracked { + data: FrameMetadataSensing (id: 0); +} + +// MCAP recording wrapper for FrameMetadataSensing. +table FrameMetadataSensingRecord { + data: FrameMetadataSensing (id: 0); + timestamp: DeviceDataTimestamp (id: 1); +} + +root_type FrameMetadataSensingRecord; diff --git a/src/plugins/sensing/CMakeLists.txt b/src/plugins/sensing/CMakeLists.txt new file mode 100644 index 000000000..9d12f3bf3 --- /dev/null +++ b/src/plugins/sensing/CMakeLists.txt @@ -0,0 +1,227 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# ============================================================================== +# SENSING Camera Plugin (Jetson libargus capture + V4L2 H.264 encode) +# ============================================================================== +# Jetson-only: libargus and the V4L2 M2M encoder exist only on L4T. +# +# Prerequisites: +# sudo apt install nvidia-l4t-jetson-multimedia-api # Argus headers +# src/plugins/sensing/setup.sh # drivers + argus socket +# +# Configure with: +# cmake -B build -DBUILD_PLUGIN_SENSING=ON +# ============================================================================== + +enable_language(CUDA) + +if(NOT DEFINED CMAKE_CUDA_ARCHITECTURES) + # Orin is 87, Thor is 101; the rest cover desktop dev boxes. + set(CMAKE_CUDA_ARCHITECTURES "80;86;87;89;90") +endif() + +find_package(CUDAToolkit REQUIRED) + +# ============================================================================== +# Argus +# ============================================================================== +list(APPEND CMAKE_MODULE_PATH "/usr/src/jetson_multimedia_api/argus/cmake") +find_package(Argus QUIET) + +if(CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64|arm64") + set(_tegra_lib_dirs "/usr/lib/aarch64-linux-gnu/nvidia" "/usr/lib/aarch64-linux-gnu/tegra") +else() + set(_tegra_lib_dirs "/usr/lib/x86_64-linux-gnu/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) + # The socket client is the multi-process entry point that talks to + # nvargus-daemon, which owns the sensors. Do not switch to the in-process + # libnvargus to chase a CUDA EGLStream consumer: the daemon already holds + # the hardware, so sensor open fails with NvPclStateControllerOpen / + # ImagerGUID errors and EGLStream creation reports NotSupported. + 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 + "================================================================================\n" + "SENSING camera plugin requires the Jetson Multimedia API (libargus).\n" + "\n" + "Install on the Jetson:\n" + " sudo apt install nvidia-l4t-jetson-multimedia-api\n" + "\n" + "In a container the L4T apt repo is usually absent; point CMake at a copy:\n" + " cmake -B build -DARGUS_INCLUDE_DIRS=/path/to/argus/include\n" + "\n" + "Or disable this plugin:\n" + " cmake -B build -DBUILD_PLUGIN_SENSING=OFF\n" + "================================================================================\n" + ) +endif() +message(STATUS "SENSING plugin: ARGUS=${ARGUS_LIBRARIES}") + +find_path(EGL_INCLUDE_DIRS EGL/egl.h) +find_library(EGL_LIBRARIES NAMES EGL) +if(NOT EGL_INCLUDE_DIRS OR NOT EGL_LIBRARIES) + message(FATAL_ERROR "SENSING camera plugin requires EGL. Install libegl1-mesa-dev.") +endif() + +# ============================================================================== +# Jetson V4L2 encoder +# ============================================================================== +# Jetson has no libnvidia-encode (the Video Codec SDK is dGPU-only), so H.264 +# comes from the V4L2 M2M engine via NvVideoEncoder in the Multimedia API. +if(NOT JETSON_MMAPI_DIR) + find_path(JETSON_MMAPI_DIR include/NvVideoEncoder.h + PATHS "/usr/src/jetson_multimedia_api" + "$ENV{HOME}/Sensing/jetson_multimedia_api" + ) +endif() +if(NOT JETSON_MMAPI_DIR) + message(FATAL_ERROR + "================================================================================\n" + "SENSING camera plugin requires the Jetson Multimedia API encoder sources.\n" + "\n" + "Install on the Jetson:\n" + " sudo apt install nvidia-l4t-jetson-multimedia-api\n" + "\n" + "In a container, copy the tree in and point CMake at it:\n" + " cmake -B build -DJETSON_MMAPI_DIR=/path/to/jetson_multimedia_api\n" + "================================================================================\n" + ) +endif() +message(STATUS "SENSING plugin: Jetson Multimedia API=${JETSON_MMAPI_DIR}") + +set(_mmapi_classes "${JETSON_MMAPI_DIR}/samples/common/classes") +set(JETSON_MMAPI_SOURCES + "${_mmapi_classes}/NvVideoEncoder.cpp" + "${_mmapi_classes}/NvV4l2Element.cpp" + "${_mmapi_classes}/NvV4l2ElementPlane.cpp" + "${_mmapi_classes}/NvElement.cpp" + "${_mmapi_classes}/NvElementProfiler.cpp" + "${_mmapi_classes}/NvBuffer.cpp" + "${_mmapi_classes}/NvLogging.cpp" +) + +find_library(NVBUFSURFACE_LIBRARY NAMES nvbufsurface PATHS ${_tegra_lib_dirs}) +if(NOT NVBUFSURFACE_LIBRARY) + message(FATAL_ERROR "libnvbufsurface not found. Install nvidia-l4t-multimedia.") +endif() + +# NVIDIA's patched libv4l2 carries the Tegra M2M encoder ioctls, so search the +# tegra directories before the generic one. L4T ships no .so dev symlink, hence +# the explicit soname in NAMES. +find_library(V4L2_LIBRARY + NAMES v4l2 libv4l2.so.0 + PATHS ${_tegra_lib_dirs} + NO_DEFAULT_PATH +) +if(NOT V4L2_LIBRARY) + find_library(V4L2_LIBRARY NAMES v4l2 libv4l2.so.0) +endif() +if(NOT V4L2_LIBRARY) + message(FATAL_ERROR "libv4l2 not found. Install nvidia-l4t-multimedia (or libv4l-dev).") +endif() +message(STATUS "SENSING plugin: V4L2=${V4L2_LIBRARY}") + +# libnvbufsurface pulls in NVIDIA's patched libjpeg (jpeg_set_hardware_- +# acceleration_parameters_enc is NV-only), which is not auto-resolved. +find_library(NVJPEG_LIBRARY NAMES nvjpeg PATHS ${_tegra_lib_dirs} NO_DEFAULT_PATH) +if(NOT NVJPEG_LIBRARY) + message(FATAL_ERROR "libnvjpeg not found. Install nvidia-l4t-multimedia.") +endif() + +# ============================================================================== +# Build +# ============================================================================== +message(STATUS "Building SENSING camera plugin") + +add_executable(camera_plugin_sensing + main.cpp + core/sensing_camera.cpp + core/cuda_ipc_publisher.cpp + core/frame_sink.cpp + core/rawdata_writer.cpp + core/argus_camera.cpp + core/yuv_to_rgba.cu + core/jetson_encoder.cpp + core/rgba_to_nv12.cu + ${JETSON_MMAPI_SOURCES} +) + +set_target_properties(camera_plugin_sensing PROPERTIES + CUDA_SEPARABLE_COMPILATION ON + INSTALL_RPATH "$ORIGIN" +) + +# core/ holds this leaf target's private headers, reached by relative "..." +# paths; only the vendored SDK and system trees need include dirs. +target_include_directories(camera_plugin_sensing PRIVATE + ${ARGUS_INCLUDE_DIRS} + ${EGL_INCLUDE_DIRS} + ${JETSON_MMAPI_DIR}/include +) + +# The Multimedia API reference sources carry legacy warnings. +target_compile_options(camera_plugin_sensing PRIVATE + $<$:-Wno-reorder -Wno-uninitialized -Wno-unused-variable + -Wno-unused-parameter -Wno-missing-field-initializers -Wno-sign-compare> +) + +target_link_libraries(camera_plugin_sensing + PRIVATE + CUDA::cudart + CUDA::cuda_driver + ${ARGUS_LIBRARIES} + ${EGL_LIBRARIES} + ${NVBUFSURFACE_LIBRARY} + ${NVJPEG_LIBRARY} + ${V4L2_LIBRARY} + isaacteleop_schema + mcap::mcap + oxr::oxr_core + pusherio::pusherio +) + +# Animated test pattern on the IPC socket, for exercising a consumer without a +# camera. Needed because Argus capture does not deliver frames on this L4T +# release; it depends only on CUDA, not on Argus or the encoder. +add_executable(sensing_ipc_testsrc + tools/ipc_testsrc.cu + core/cuda_ipc_publisher.cpp +) + +set_target_properties(sensing_ipc_testsrc PROPERTIES + CUDA_SEPARABLE_COMPILATION ON + INSTALL_RPATH "$ORIGIN" +) + +target_link_libraries(sensing_ipc_testsrc + PRIVATE + CUDA::cudart + CUDA::cuda_driver +) + +install(TARGETS camera_plugin_sensing sensing_ipc_testsrc + RUNTIME DESTINATION plugins/sensing_camera +) + +install(FILES + "${CMAKE_CURRENT_SOURCE_DIR}/plugin.yaml" + "${CMAKE_CURRENT_SOURCE_DIR}/README.md" + DESTINATION plugins/sensing_camera +) diff --git a/src/plugins/sensing/README.md b/src/plugins/sensing/README.md new file mode 100644 index 000000000..f130770b6 --- /dev/null +++ b/src/plugins/sensing/README.md @@ -0,0 +1,318 @@ + + +# SENSING GMSL camera setup + +Driver provisioning for the SENSING **SG10A-AGON-G2M-A1** carrier on a Jetson +AGX Orin — one Astra **S56C** plus up to six **SHF3L/SHF3H** cameras over GMSL2, +on JetPack 6.2 / L4T R36.4.3. + +Two halves: the setup scripts that bring the drivers up, and +`camera_plugin_sensing`, which captures through libargus and either encodes +H.264 on the Jetson V4L2 engine or hands frames to another process as CUDA +memory ([CUDA IPC](#cuda-ipc)). + +> **Argus needs NVIDIA's EGL, and `DISPLAY` can silently deny it.** If `DISPLAY` +> names an X server Tegra EGL cannot drive — Xvfb, X11 forwarding — GLVND hands +> the process Mesa's EGL and every Argus-to-CUDA path fails. See +> [EGL vendor](#egl-vendor-selection). + +Vendor package (the `.ko`, `Image`, `.dtbo` and ISP files this wraps): + + +## Why two scripts + +Setup is genuinely two-sided, and neither half can do the other's job: + +| | Host | Container | +|---|---|---| +| `insmod` sensor drivers | ✅ host kernel | ❌ | +| device-tree overlay, `jetson-io` | ✅ | ❌ | +| POC / PWM `devmem` writes | ✅ | ❌ | +| `nvargus-daemon` | ✅ runs here | ❌ | +| `/tmp/argus_socket` bind mount | — | ✅ needs it | +| Argus headers to build against | apt | ✅ needs them | + +```bash +# on the Jetson host +src/plugins/sensing/setup.sh # -> setup_host.sh + +# inside the devcontainer +src/plugins/sensing/setup.sh # -> setup_container.sh + +# anywhere, read-only +src/plugins/sensing/verify.sh +``` + +`setup.sh` auto-detects the context; `--host` / `--container` override it. +Every script announces **exactly which privileged actions it will take** in a +coloured banner before the first `sudo` prompt, and asks before each optional +one. `--yes` accepts them all; a non-interactive stdin declines them all. + +## Host setup + +```bash +src/plugins/sensing/setup_host.sh [--pkg DIR] [--fps 30] [--free-run|--trigger-sync] + [--install-drivers] [--service|--no-service] [--yes] +``` + +The package is autodetected under `~/Sensing/`, `~`, `/home/*/Sensing/` and +`/opt/sensing/`; `--pkg` or `$SENSING_PKG_DIR` overrides. + +**First install only** — `--install-drivers` runs the vendor `install.sh` +(kernel `Image`, `.dtbo`, ISP tuning), then stops. Select the overlay and +reboot before continuing: + +```bash +sudo /opt/nvidia/jetson-io/jetson-io.py # Configure Jetson AGX CSI Connector + # -> Jetson Sensing SG10A_AGON_G2M_A1 S56Cx1 SHF3Lx6 +``` + +**Every boot** — the vendor `install.sh` never copies the sensor `.ko` files +into `/lib/modules`, so nothing auto-loads them and `/dev/video*` is empty after +each reboot until `load_modules.sh` runs. That is the single most common cause +of "the cameras stopped working". `setup_host.sh` runs it, then offers to +install `sensing-camera.service` so it happens at boot. + +The unit is ordered `After=basic.target` / `Before=nvargus-daemon.service` — +**not** `After=multi-user.target`, which races: `nvargus-daemon` is itself part +of `multi-user.target`, and a daemon that starts before the sensors exist +enumerates an empty camera list and never retries. + +### Trigger mode + +`load_modules.sh` leaves sensors slaved to the carrier's PWM trigger +(`trig_mode=1` on S56C, `2` on SHF3L). That only fires when **J19 pins 2 and 4 +are strapped together**. Without the strap the camera opens fine and then +delivers no frames — a failure that looks like a software hang. + +`--free-run` (the default) sets `trig_mode=0` on every node. Use +`--trigger-sync` to keep vendor behaviour when the strap is fitted and you need +cross-camera sync. + +## Container setup + +```bash +src/plugins/sensing/setup_container.sh [--build-argus] [--argus-include DIR] [--yes] +``` + +Checks the container can *consume* the host's drivers, and fixes what it can: + +- **`/tmp/argus_socket`** — the one hard blocker. `libnvargus_socketclient` + reaches `nvargus-daemon` through it, and a container gets its own `/tmp` even + when `/tmp/.X11-unix` is bind-mounted. Add to `runArgs`: + ```jsonc + "-v", "/tmp/argus_socket:/tmp/argus_socket" + ``` + and rebuild the container. Nothing in the container can work around this. +- **Argus headers** — `nvidia-l4t-jetson-multimedia-api` is usually not + installable in a container (no L4T apt repo). If the tree is found elsewhere, + the script offers to symlink it to `/usr/src/jetson_multimedia_api/argus`, + which is the path both `camera_viz/argus/build.sh` and + `camera_viz/scripts/_install_deps.sh` hardcode. +- `v4l-utils`, EGL headers, `nvcc`, and `argus_camera` on `PATH`. + +## The plugin + +```bash +cmake -B build -DBUILD_PLUGIN_SENSING=ON +cmake --build build --target camera_plugin_sensing --parallel +``` + +Both dependencies live outside the repo. On a Jetson they come from +`sudo apt install nvidia-l4t-jetson-multimedia-api`; in a container, copy the +tree in and point CMake at it: + +```bash +cmake -B build -DBUILD_PLUGIN_SENSING=ON \ + -DARGUS_INCLUDE_DIRS=$HOME/Sensing/argus/include \ + -DJETSON_MMAPI_DIR=$HOME/Sensing/jetson_multimedia_api +``` + +`sensor=` is the **Argus sensor id**, not the `/dev/videoN` number — see +[the mapping below](#argus-sensor-ids-are-not-devvideo-numbers). + +```bash +./build/src/plugins/sensing/camera_plugin_sensing \ + --add-stream=sensor=2,output=./left.h264 \ + --add-stream=sensor=3,output=./right.h264 \ + --mcap-filename=./meta.mcap +``` + +`--collection-prefix` pushes the same metadata over OpenXR instead; the two are +mutually exclusive. `--help` lists the capture and encoder knobs. + +## CUDA IPC + +`ipc=` on a stream serves that sensor's frames to another process as +**CUDA device memory** — RGBA8, no encode, no host round-trip. It is +independent of `output=`; give one, the other, or both. An `ipc`-only stream +never starts an encoder: + +```bash +./build/src/plugins/sensing/camera_plugin_sensing \ + --add-stream=sensor=2,ipc=/tmp/sensing2.sock + +# consume it +cd examples/camera_viz && ./camera_viz.sh run configs/cuda_ipc.yaml +``` + +The producer allocates a ring of frame slots in one shared allocation and hands +the consumer a file descriptor for it; per frame it copies into a free slot and +sends a 24-byte ready message. The consumer maps the allocation once and reads +slots in place, releasing each one when it moves on, so the producer never +overwrites a slot that is still being read. A consumer that falls behind gets +frames dropped rather than stalling capture. One consumer at a time — the +latest connection wins, so the viewer can restart without restarting the +plugin. Measured at 1920×1080 on an AGX Orin: ~1 ms producer-to-consumer, +60 fps sustained. + +**Legacy CUDA IPC does not work on Tegra, and fails misleadingly.** +`cudaIpcGetMemHandle` returns `cudaSuccess` in this process, then the consumer's +`cudaIpcOpenMemHandle` fails with `cudaErrorInvalidValue`. The working route is +the virtual-memory-management API — `cuMemCreate` + +`cuMemExportToShareableHandle` to a POSIX fd, passed over the Unix socket with +`SCM_RIGHTS`. Do not rewrite this back to `cudaIpcMemHandle_t`. + +Wire format is in [`core/cuda_ipc_protocol.hpp`](core/cuda_ipc_protocol.hpp); +the consumer re-declares it in +[`sources/cuda_ipc.py`](../../../examples/camera_viz/sources/cuda_ipc.py), so +the two change together. + +### Testing without a camera + +`sensing_ipc_testsrc` publishes an animated pattern over the same protocol, so +the consumer can be developed with no camera attached. It needs CUDA only — no +Argus, no encoder: + +```bash +cmake --build build --target sensing_ipc_testsrc +./build/src/plugins/sensing/sensing_ipc_testsrc --socket=/tmp/sensing2.sock \ + --width=1920 --height=1080 --fps=60 +``` + +Each frame carries its 16-bit frame number as a binary bar across the top, so a +stale or torn frame is visible rather than merely suspected — the +[camera_viz tests](../../../examples/camera_viz/tests/test_cuda_ipc_source.py) +assert on it. + +### Playback + +The output is raw Annex-B H.264 with no container and no timestamps, so a +player has to be told the frame rate: + +```bash +ffplay -f h264 left.h264 +ffmpeg -f h264 -framerate 30 -i left.h264 -c copy left.mp4 # -framerate must match --fps +``` + +On the Jetson, GStreamer decodes it on the hardware engine that wrote it: + +```bash +gst-launch-1.0 filesrc location=left.h264 ! h264parse ! nvv4l2decoder ! nv3dsink +``` + +To check a file without any tools installed, the 5th byte after each +`00 00 01` start code carries the NAL type in its low 5 bits — a healthy +recording opens `67` (SPS), `68` (PPS), `65` (IDR): + +```bash +xxd -l 16 left.h264 +``` + +The MCAP sidecar holds one `core.FrameMetadataSensingRecord` per frame on +channel `sensing_metadata/sensor`. Its binary schema is embedded, so +`mcap cat --json meta.mcap` decodes it without any Isaac headers. + +## Status + +| Stage | State | +|---|---| +| V4L2 M2M encoder (`NvVideoEncoder`) | works — opens, negotiates NV12M→H264, CBR/GOP applied | +| MCAP + OpenXR metadata | works | +| CUDA IPC publish + camera_viz consume | works — verified end to end against `sensing_ipc_testsrc` | +| Argus capture (S56C, `STREAM_TYPE_EGL`) | works — 1920x1080 into CUDA, with NVIDIA EGL selected | + +### EGL vendor selection + +`ArgusCamera` consumes `STREAM_TYPE_EGL` via `cuEGLStreamConsumerConnect`, and +that works under multi-process Argus — but only when the process resolves EGL to +NVIDIA's driver. GLVND picks the vendor from `DISPLAY`, and Tegra's EGL cannot +drive an Xvfb or forwarded X server, so it loses the probe to Mesa. Same binary, +same daemon, same sensor: + +| `EGL_VENDOR` | `IEGLOutputStream::getEGLStream()` | +|---|---| +| `NVIDIA` | valid handle | +| `Mesa Project` | `EGL_NO_STREAM_KHR` | + +Multi-process Argus is not the problem, and `STREAM_TYPE_BUFFER` is not required +to fix it. `NvBufSurfaceMapEglImage` fails the same way for the same reason +("Failed to create EGLImage"), so a buffer-stream rewrite would have hit the +identical wall. + +`main()` therefore calls `unsetenv("DISPLAY")` before the first EGL call — this +process captures and never renders, and `libnvbufsurface` resolves its own +display via `eglGetDisplay(EGL_DEFAULT_DISPLAY)`, so the choice cannot be made +per-call. `ArgusCamera` additionally refuses to start on a non-NVIDIA vendor +rather than failing later with a misleading Argus error. + +## Streaming + +Two sensor families, two paths — the difference is not cosmetic: + +| Camera | Nodes | Resolution | Output | camera_viz `type:` | +|---|---|---|---|---| +| Astra S56C | `video0`–`video3` | 1920×1080 (`sensor_mode=0`) | **RAW/Bayer** | `argus` | +| SHF3L/SHF3H | `video4`–`video9` | 1920×1536 (`sensor_mode=2`) | **YUV** | `v4l2` | + +The S56C needs the ISP, so it must go through Argus. The SHF3L is already +ISP-processed on the module and works with the stock V4L2 source today. + +For SHF3L, the stock [`examples/camera_viz/configs/v4l2.yaml`](../../../examples/camera_viz/configs/v4l2.yaml) +works as-is — point `device:` at the node from the table above and set +`1920x1536`. + +For S56C, [`configs/argus_s56c.yaml`](configs/argus_s56c.yaml) carries the +Argus-specific knobs. It needs the native Argus source +(`examples/camera_viz/argus/`), which arrives with the Argus camera support PR; +`setup_container.sh` reports whether it is present. + +### Argus sensor ids are not `/dev/video` numbers + +Argus enumerates in device-tree module order. On this overlay: + +| `sensor_id` | badge | `sensor_id` | badge | +|---|---|---|---| +| 0 | `cam7_frontright` | 5 | `cam1_bottomright` | +| 1 | `cam6_frontleft` | 6 | `cam2_centerleft` | +| 2 | `cam9_topcenter` | 7 | `cam3_centerright` | +| 3 | `cam8_bottomcenter` | 8 | `cam4_topleft` | +| 4 | `cam0_bottomleft` | 9 | `cam5_topright` | + +Ids 0–3 are the S56C group, 4–9 the SHF3L ports. Re-derive after any overlay +change: + +```bash +for i in $(seq 0 9); do + printf '%s ' "$i"; cat /proc/device-tree/tegra-camera-platform/modules/module$i/badge; echo +done +``` + +## Troubleshooting + +Run `verify.sh` first — it names the fixing script for each failure. + +| Symptom | Cause | +|---|---| +| No `/dev/video*` after reboot | Drivers not loaded. Run `setup_host.sh`, then install the service. | +| Camera opens, no frames | Sensor slaved to an absent trigger. `setup_host.sh --free-run`. | +| Argus finds no cameras | `nvargus-daemon` started before the drivers. `sudo systemctl restart nvargus-daemon`. | +| `argus_camera: command not found` in container | It is installed at `/usr/local/bin` on the *host*, which is container-local. `setup_container.sh` offers to link a built copy. | +| `insmod: invalid module format` | Kernel is not `5.15.148-tegra`; the prebuilt `.ko` files will not load. | +| `EGL_NO_STREAM_KHR`, or `NvBufSurfaceMapEglImage` "Failed to create EGLImage" | EGL resolved to Mesa, not NVIDIA. Unset `DISPLAY`, or point it at an X server Tegra EGL can drive. Check with `eglQueryString(dpy, EGL_VENDOR)`. | +| `Connection refused` on every Argus call, right after restarting `nvargus-daemon` | The container bind-mounts `/tmp/argus_socket` as a *file*; the daemon unlinks and recreates it, so the mount pins a deleted inode (`grep argus /proc/self/mountinfo` shows `//deleted`). Mount the host's `/tmp` and symlink instead, or restart the container. | +| `/tmp/argus_socket` is a *directory* | The container started before `nvargus-daemon`, so Docker created the missing bind source. `sudo rmdir /tmp/argus_socket && sudo systemctl restart nvargus-daemon`. | +| `bmi088 ... softreset failed` at boot | The IMU probed before camera power came up. Harmless; `load_modules.sh` reloads it. | diff --git a/src/plugins/sensing/_common.sh b/src/plugins/sensing/_common.sh new file mode 100644 index 000000000..d9a629d7a --- /dev/null +++ b/src/plugins/sensing/_common.sh @@ -0,0 +1,142 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# Shared helpers for the SENSING setup scripts. Sourced, never executed. + +# Colour is opt-out (NO_COLOR) and auto-disabled when stdout is not a terminal, +# so piped/CI output stays greppable. +if [[ -t 1 && -z "${NO_COLOR:-}" ]]; then + C_RESET=$'\033[0m'; C_BOLD=$'\033[1m'; C_DIM=$'\033[2m' + C_RED=$'\033[31m'; C_GREEN=$'\033[32m'; C_YELLOW=$'\033[33m' + C_BLUE=$'\033[34m'; C_CYAN=$'\033[36m' +else + C_RESET=''; C_BOLD=''; C_DIM='' + C_RED=''; C_GREEN=''; C_YELLOW=''; C_BLUE=''; C_CYAN='' +fi + +step() { printf '\n%s==>%s %s%s%s\n' "$C_BLUE$C_BOLD" "$C_RESET" "$C_BOLD" "$1" "$C_RESET"; } +ok() { printf ' %s✓%s %s\n' "$C_GREEN" "$C_RESET" "$1"; } +info() { printf ' %s·%s %s\n' "$C_DIM" "$C_RESET" "$1"; } +warn() { printf ' %s!%s %s\n' "$C_YELLOW$C_BOLD" "$C_RESET" "$1" >&2; } +bad() { printf ' %s✗%s %s\n' "$C_RED$C_BOLD" "$C_RESET" "$1" >&2; } +hint() { printf ' %s%s%s\n' "$C_CYAN" "$1" "$C_RESET"; } + +die() { + printf '\n%sError:%s %s\n' "$C_RED$C_BOLD" "$C_RESET" "$1" >&2 + [[ -z "${2:-}" ]] || printf '%sAction:%s %s\n' "$C_CYAN$C_BOLD" "$C_RESET" "$2" >&2 + exit 1 +} + +have() { command -v "$1" >/dev/null 2>&1; } + +in_container() { + [[ -f /.dockerenv ]] || grep -qE '(docker|containerd|kubepods)' /proc/1/cgroup 2>/dev/null +} + +# Loud, coloured notice naming every privileged action before the first prompt, +# then pre-authenticate so the password prompt lands here rather than midway +# through a driver load. `sudo -n true` first: on NOPASSWD hosts `sudo -v` still +# demands a password and would break unattended runs. +require_sudo() { + local width=72 line reason + + # Full banner once per run; later privileged actions get a one-liner so the + # notice stays visible without turning into wallpaper. + if [[ "${_SUDO_BANNER_SHOWN:-0}" == "1" ]]; then + for reason in "$@"; do + printf ' %ssudo:%s %s\n' "$C_YELLOW$C_BOLD" "$C_RESET" "$reason" + done + sudo -n true 2>/dev/null && return 0 + sudo -v || die "sudo authentication failed." "Re-run as a user with sudo privileges." + return 0 + fi + _SUDO_BANNER_SHOWN=1 + + printf -v line '%*s' "$width" ''; line=${line// /═} + + printf '\n%s%s╔%s╗%s\n' "$C_YELLOW" "$C_BOLD" "$line" "$C_RESET" + printf '%s%s║%s SUDO REQUIRED %*s║%s\n' \ + "$C_YELLOW" "$C_BOLD" "$C_RED" "$((width - 15))" '' "$C_RESET" + printf '%s%s╚%s╝%s\n' "$C_YELLOW" "$C_BOLD" "$line" "$C_RESET" + printf '%sThis script needs root privileges for:%s\n' "$C_BOLD" "$C_RESET" + local reason + for reason in "$@"; do + printf ' %s•%s %s\n' "$C_YELLOW$C_BOLD" "$C_RESET" "$reason" + done + printf '%sYou will be asked again before each optional action. Ctrl-C now to abort.%s\n\n' \ + "$C_DIM" "$C_RESET" + + if sudo -n true 2>/dev/null; then + ok "sudo already authenticated" + return 0 + fi + printf '%sEnter your password to continue.%s\n' "$C_CYAN" "$C_RESET" + sudo -v || die "sudo authentication failed." "Re-run as a user with sudo privileges." +} + +# Yes/no prompt. ASSUME_YES=1 auto-accepts; a non-interactive stdin declines, +# so unattended runs never silently take a privileged optional action. +confirm() { + local prompt="$1" reply + if [[ "${ASSUME_YES:-0}" == "1" ]]; then + info "$prompt ${C_DIM}[auto-yes]${C_RESET}" + return 0 + fi + if [[ ! -t 0 ]]; then + warn "$prompt — declined (non-interactive; pass --yes to accept)" + return 1 + fi + printf '%s%s%s [y/N] ' "$C_YELLOW$C_BOLD" "$prompt" "$C_RESET" + read -r reply + [[ "$reply" =~ ^[Yy]$ ]] +} + +# --------------------------------------------------------------------------- +# Rig facts. Ports, resolutions and node ranges come from the vendor Readme.md; +# ARGUS_ID_* come from the /proc/device-tree/tegra-camera-platform/modules +# ordering, which is what Argus enumerates as sensor-id. +# --------------------------------------------------------------------------- +SENSING_PKG_GLOB='SG10A_AGON_G2M_A1_AGX_ORIN_S56Cx1_SHF3Lx6_JP6.2_L4TR36.4.3' +S56C_NODES=(0 1 2 3) # J27 -> video0/1, J29 -> video2/3; RAW, 1920x1080 +SHF3L_NODES=(4 5 6 7 8 9) # J25 J26 J23 J24 J21 J22; YUV, 1920x1536 + +# Locate the vendor driver package. $SENSING_PKG_DIR wins; otherwise search the +# usual drop points. Prints the path, empty if not found. +find_sensing_pkg() { + if [[ -n "${SENSING_PKG_DIR:-}" ]]; then + printf '%s\n' "$SENSING_PKG_DIR" + return 0 + fi + local d + for d in "$HOME/Sensing/$SENSING_PKG_GLOB" \ + "$HOME/$SENSING_PKG_GLOB" \ + /home/*/Sensing/"$SENSING_PKG_GLOB" \ + /home/*/"$SENSING_PKG_GLOB" \ + /opt/sensing/"$SENSING_PKG_GLOB"; do + [[ -f "$d/load_modules.sh" ]] && { printf '%s\n' "$d"; return 0; } + done + printf '\n' +} + +# Locate Argus/Argus.h. The apt package puts it under /usr/src; containers +# rarely have the L4T apt repo, so a copy of the jetson_multimedia_api argus +# tree next to the driver package is accepted too. Prints the include dir. +find_argus_include() { + local d + for d in "${ARGUS_INCLUDE_DIR:-}" \ + /usr/src/jetson_multimedia_api/argus/include \ + /usr/src/jetson_multimedia_api/include \ + "$HOME/Sensing/argus/include" \ + /usr/local/src/argus/include; do + [[ -n "$d" && -f "$d/Argus/Argus.h" ]] && { printf '%s\n' "$d"; return 0; } + done + return 1 +} + +# Video nodes that currently exist, as bare indices. +sensing_video_nodes() { + local dev + for dev in /dev/video*; do + [[ -e "$dev" ]] && printf '%s\n' "${dev#/dev/video}" + done +} diff --git a/src/plugins/sensing/configs/argus_s56c.yaml b/src/plugins/sensing/configs/argus_s56c.yaml new file mode 100644 index 000000000..7cf263c56 --- /dev/null +++ b/src/plugins/sensing/configs/argus_s56c.yaml @@ -0,0 +1,71 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# SENSING Astra S56C stereo pair on the SG10A carrier (AGX Orin, JetPack 6.2). +# +# The S56C is a RAW/Bayer sensor, so it has to go through Argus/ISP — the plain +# ``type: v4l2`` source cannot debayer it. Requires the native Argus module +# (examples/camera_viz/argus, built by ``camera_viz.sh setup --with-argus``). +# +# sensor_id is the Argus index, which follows the device-tree module order — not +# the /dev/video number. On this overlay: +# 0 cam7_frontright 1 cam6_frontleft 2 cam9_topcenter 3 cam8_bottomcenter +# Modules 0-3 are the S56C group (J27 -> video0/1, J29 -> video2/3); 4-9 are the +# six SHF3L ports. Re-derive after any overlay change with: +# for i in $(seq 0 9); do +# cat /proc/device-tree/tegra-camera-platform/modules/module$i/badge; echo +# done +# +# Ids 2/3 are the J29 pair. Only one S56C is supported at a time, so confirm +# which port yours is on before trusting these — a sensor that has streamed +# logs its AE loop: +# sudo dmesg | grep -oE 's56-shw3g 12-00[0-9a-f]+: sensor_set_gain' | sort -u +# and 12-001b/1c/1d/1e map to cam_6/7/8/9 = ids 1/0/3/2 respectively. +# The left/right assignment below is not verified: swap if stereo looks inverted. +# +# Copy into examples/camera_viz/configs/ (or pass the path directly) and run: +# ./camera_viz.sh run + +source: local +streaming: + host: 127.0.0.1 +encoder: auto + +cameras: + - name: s56c + enabled: true + type: argus + stereo: true + # Independent Argus sessions per sensor: the GMSL driver does not support a + # dual-sensor session, and a shared one fails to open. + sync_session: false + pair_emit_mode: both + sensor_id_left: 2 # cam9_topcenter (J29, /dev/video2) + sensor_id_right: 3 # cam8_bottomcenter (J29, /dev/video3) + sensor_mode: 0 # 1920x1080, the only S56C mode + width: 1920 + height: 1080 + fps: 30 # must match setup_host.sh --fps when trigger-synced + gpu_id: 0 + repeat_capture: true + full_range: false + swap_uv: true + rtp: + port: 5000 + port_right: 5001 + bitrate_mbps: 20 + +display: + mode: xr + window: + width: 1920 + height: 1080 + xr: + near_z: 0.05 + far_z: 100.0 + clear_color: [0.0, 0.0, 0.0, 0.0] + placements: + s56c: + lock_mode: lazy + distance: 0.5 + stereo_baseline_mm: 50.0 diff --git a/src/plugins/sensing/core/argus_camera.cpp b/src/plugins/sensing/core/argus_camera.cpp new file mode 100644 index 000000000..e28a8efcc --- /dev/null +++ b/src/plugins/sensing/core/argus_camera.cpp @@ -0,0 +1,869 @@ +// 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"); + } + + // Orin (JetPack 6.2) rejects a display-agnostic EGLStream: the handle that + // reaches cuEGLStreamConsumerConnect is unusable and CUDA reports + // CUDA_ERROR_INVALID_HANDLE. Bind a real display when one is available and + // fall back to display-agnostic for the headless Thor path below. + egl_display_ = eglGetDisplay(EGL_DEFAULT_DISPLAY); + if (egl_display_ != EGL_NO_DISPLAY && eglInitialize(egl_display_, nullptr, nullptr)) + { + egl_initialized_ = true; + } + else + { + egl_display_ = EGL_NO_DISPLAY; + } + + // With DISPLAY set, GLVND hands out Mesa's EGL: Tegra's EGL cannot drive an + // X11 display, so the Mesa ICD wins the vendor probe. Nothing NVIDIA- + // specific works on that display -- Argus hands back EGL_NO_STREAM_KHR and + // NvBufSurfaceMapEglImage fails with "Failed to create EGLImage" -- and both + // read as Argus faults rather than as the wrong EGL library. Checking the + // vendor turns that into one legible error. main() clears DISPLAY for this + // reason; libnvbufsurface calls eglGetDisplay(EGL_DEFAULT_DISPLAY) itself, + // so it cannot be steered from here with a display we picked. + if (egl_initialized_) + { + const char* vendor = eglQueryString(egl_display_, EGL_VENDOR); + if (!vendor || std::string(vendor).find("NVIDIA") == std::string::npos) + { + std::ostringstream oss; + oss << "EGL vendor is '" << (vendor ? vendor : "unknown") + << "', not NVIDIA. Argus and NvBufSurface need the Tegra EGL driver. " + "Unset DISPLAY before starting this plugin."; + throw std::runtime_error(oss.str()); + } + } + + 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"); + if (egl_initialized_) + { + check_argus(i_egl_settings->setEGLDisplay(egl_display_), "setEGLDisplay"); + } + 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; + // Multi-process Argus (libnvargus_socketclient) returns + // EGL_NO_STREAM_KHR here on L4T R36.4.3: the stream lives in + // nvargus-daemon, so a local CUDA consumer cannot attach and this + // call fails with CUDA_ERROR_INVALID_HANDLE. + EGLStreamKHR handle = streams_[i].egl_stream->getEGLStream(); + if (handle == EGL_NO_STREAM_KHR) + { + throw std::runtime_error( + "Argus returned EGL_NO_STREAM_KHR: multi-process Argus does not expose its EGLStream to a " + "local CUDA consumer on this L4T release. STREAM_TYPE_BUFFER + NvBufSurface is the " + "supported Argus-to-CUDA path here."); + } + check_cuda(cuEGLStreamConsumerConnect(&streams_[i].connection, handle), 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/src/plugins/sensing/core/argus_camera.hpp b/src/plugins/sensing/core/argus_camera.hpp new file mode 100644 index 000000000..8ff827c2e --- /dev/null +++ b/src/plugins/sensing/core/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/src/plugins/sensing/core/cuda_ipc_protocol.hpp b/src/plugins/sensing/core/cuda_ipc_protocol.hpp new file mode 100644 index 000000000..4dbc1a3ae --- /dev/null +++ b/src/plugins/sensing/core/cuda_ipc_protocol.hpp @@ -0,0 +1,79 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 +// +// Wire format for the CUDA frame IPC socket. +// +// The Python consumer decodes these with struct.unpack in +// examples/camera_viz/sources/cuda_ipc.py. Both sides hardcode the layout, so +// any change here is a wire break: bump kProtocolVersion and update the +// consumer's format strings in the same commit. +// +// Layout is fixed little-endian with explicit padding rather than whatever the +// compiler picks, so the Python side can spell it as a plain struct format. + +#pragma once + +#include + +namespace plugins +{ +namespace sensing +{ +namespace ipc +{ + +constexpr uint32_t kHelloMagic = 0x44554349; // 'ICUD' +constexpr uint32_t kFrameMagic = 0x4d524649; // 'IFRM' +constexpr uint32_t kReleaseMagic = 0x4c455249; // 'IREL' +constexpr uint32_t kProtocolVersion = 1; + +/// Pixel format tag carried in Hello::format. +enum class PixelFormat : uint32_t +{ + Rgba8 = 0, +}; + +/** + * @brief Sent once on connect, alongside the export fd via SCM_RIGHTS. + * + * The single fd maps one allocation holding `slot_count` frames; slot i starts + * at `i * slot_stride` and each row is `pitch` bytes. + */ +struct Hello +{ + uint32_t magic; + uint32_t version; + uint32_t width; + uint32_t height; + uint32_t format; + uint32_t slot_count; + uint32_t device_id; + uint32_t sensor_id; + uint64_t pitch; + uint64_t slot_stride; + /// Total mapped size, already rounded up to the CUDA allocation granularity. + uint64_t total_bytes; +}; +static_assert(sizeof(Hello) == 56, "Hello layout is wire format; see cuda_ipc.py"); + +/// Sent per published frame. The named slot is the consumer's until it releases it. +struct FrameReady +{ + uint32_t magic; + uint32_t slot; + uint64_t sequence; + uint64_t timestamp_ns; +}; +static_assert(sizeof(FrameReady) == 24, "FrameReady layout is wire format; see cuda_ipc.py"); + +/// Consumer -> producer: the slot is done being read and may be overwritten. +struct SlotRelease +{ + uint32_t magic; + uint32_t slot; +}; +static_assert(sizeof(SlotRelease) == 8, "SlotRelease layout is wire format; see cuda_ipc.py"); + +} // namespace ipc +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/cuda_ipc_publisher.cpp b/src/plugins/sensing/core/cuda_ipc_publisher.cpp new file mode 100644 index 000000000..258996ca4 --- /dev/null +++ b/src/plugins/sensing/core/cuda_ipc_publisher.cpp @@ -0,0 +1,366 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "cuda_ipc_publisher.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace sensing +{ +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 << "CudaIpcPublisher: " << 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 << "CudaIpcPublisher: " << what << " failed: " << cudaGetErrorString(result); + throw std::runtime_error(oss.str()); + } +} + +size_t round_up(size_t value, size_t multiple) +{ + return ((value + multiple - 1) / multiple) * multiple; +} + +/// Send a fixed-size record, optionally with one fd attached. Non-blocking: +/// returns false on EAGAIN so a wedged consumer never stalls capture. +bool send_record(int fd, const void* data, size_t size, int passed_fd) +{ + iovec io{ const_cast(data), size }; + msghdr msg{}; + msg.msg_iov = &io; + msg.msg_iovlen = 1; + + // CMSG_SPACE is not constexpr-friendly on all libcs; size for exactly one fd. + alignas(cmsghdr) char control[CMSG_SPACE(sizeof(int))]; + if (passed_fd >= 0) + { + std::memset(control, 0, sizeof(control)); + msg.msg_control = control; + msg.msg_controllen = sizeof(control); + cmsghdr* cmsg = CMSG_FIRSTHDR(&msg); + cmsg->cmsg_level = SOL_SOCKET; + cmsg->cmsg_type = SCM_RIGHTS; + cmsg->cmsg_len = CMSG_LEN(sizeof(int)); + std::memcpy(CMSG_DATA(cmsg), &passed_fd, sizeof(int)); + } + + // MSG_NOSIGNAL: a consumer that exits mid-send must not SIGPIPE the plugin. + ssize_t sent = ::sendmsg(fd, &msg, MSG_NOSIGNAL); + return sent == static_cast(size); +} + +} // namespace + +CudaIpcPublisher::CudaIpcPublisher(const CudaIpcConfig& config) : m_config(config) +{ + if (m_config.socket_path.empty()) + throw std::runtime_error("CudaIpcPublisher: socket path is empty"); + if (m_config.slot_count < 2 || m_config.slot_count > 64) + throw std::runtime_error("CudaIpcPublisher: slot_count must be in [2, 64]"); + + check_cuda(cuInit(0), "cuInit"); + check_cuda(cuDeviceGet(&m_device, m_config.gpu_id), "cuDeviceGet"); + check_cuda(cuDevicePrimaryCtxRetain(&m_context, m_device), "cuDevicePrimaryCtxRetain"); + m_context_retained = true; + check_cuda(cuCtxSetCurrent(m_context), "cuCtxSetCurrent"); + check_cuda(cuStreamCreate(&m_stream, CU_STREAM_NON_BLOCKING), "cuStreamCreate"); + + allocate_slots(); + open_socket(); + + std::cout << "CUDA IPC: sensor " << m_config.sensor_id << " serving " << m_config.width << "x" << m_config.height + << " RGBA8 on " << m_config.socket_path << " (" << m_config.slot_count << " slots, " + << (m_reserved_bytes >> 20) << " MiB)" << std::endl; +} + +CudaIpcPublisher::~CudaIpcPublisher() +{ + if (m_client_fd >= 0) + ::close(m_client_fd); + if (m_listen_fd >= 0) + ::close(m_listen_fd); + if (m_socket_bound) + ::unlink(m_config.socket_path.c_str()); + if (m_export_fd >= 0) + ::close(m_export_fd); + + if (m_base_ptr) + { + cuMemUnmap(m_base_ptr, m_reserved_bytes); + cuMemAddressFree(m_base_ptr, m_reserved_bytes); + } + if (m_alloc_handle) + cuMemRelease(m_alloc_handle); + if (m_stream) + cuStreamDestroy(m_stream); + if (m_context_retained) + cuDevicePrimaryCtxRelease(m_device); +} + +void CudaIpcPublisher::allocate_slots() +{ + CUmemAllocationProp prop{}; + prop.type = CU_MEM_ALLOCATION_TYPE_PINNED; + prop.location.type = CU_MEM_LOCATION_TYPE_DEVICE; + prop.location.id = m_config.gpu_id; + prop.requestedHandleTypes = CU_MEM_HANDLE_TYPE_POSIX_FILE_DESCRIPTOR; + + size_t granularity = 0; + check_cuda(cuMemGetAllocationGranularity(&granularity, &prop, CU_MEM_ALLOC_GRANULARITY_RECOMMENDED), + "cuMemGetAllocationGranularity"); + + // Rows are tightly packed so the consumer can wrap a slot as a plain + // contiguous HxWx4 array. Slots are 256-byte aligned to keep every slot + // base at CUDA's texture alignment. + m_pitch = static_cast(m_config.width) * 4; + m_slot_stride = round_up(m_pitch * m_config.height, 256); + m_reserved_bytes = round_up(m_slot_stride * m_config.slot_count, granularity); + + check_cuda(cuMemCreate(&m_alloc_handle, m_reserved_bytes, &prop, 0), "cuMemCreate"); + check_cuda(cuMemExportToShareableHandle(&m_export_fd, m_alloc_handle, CU_MEM_HANDLE_TYPE_POSIX_FILE_DESCRIPTOR, 0), + "cuMemExportToShareableHandle"); + check_cuda(cuMemAddressReserve(&m_base_ptr, m_reserved_bytes, 0, 0, 0), "cuMemAddressReserve"); + check_cuda(cuMemMap(m_base_ptr, m_reserved_bytes, 0, m_alloc_handle, 0), "cuMemMap"); + + CUmemAccessDesc access{}; + access.location = prop.location; + access.flags = CU_MEM_ACCESS_FLAGS_PROT_READWRITE; + check_cuda(cuMemSetAccess(m_base_ptr, m_reserved_bytes, &access, 1), "cuMemSetAccess"); + + // Opaque memory starts undefined; a consumer that attaches before the + // first frame would otherwise render whatever the allocator handed back. + check_runtime(cudaMemset(reinterpret_cast(m_base_ptr), 0, m_reserved_bytes), "cudaMemset"); + + m_slot_sequence.assign(m_config.slot_count, 0); +} + +void CudaIpcPublisher::open_socket() +{ + sockaddr_un addr{}; + addr.sun_family = AF_UNIX; + if (m_config.socket_path.size() >= sizeof(addr.sun_path)) + { + throw std::runtime_error("CudaIpcPublisher: socket path exceeds " + std::to_string(sizeof(addr.sun_path) - 1) + + " bytes: " + m_config.socket_path); + } + std::memcpy(addr.sun_path, m_config.socket_path.c_str(), m_config.socket_path.size()); + + m_listen_fd = ::socket(AF_UNIX, SOCK_STREAM | SOCK_NONBLOCK | SOCK_CLOEXEC, 0); + if (m_listen_fd < 0) + throw std::runtime_error(std::string("CudaIpcPublisher: socket() failed: ") + std::strerror(errno)); + + // A previous run that died on SIGKILL leaves the node behind and bind() + // would fail with EADDRINUSE. + ::unlink(m_config.socket_path.c_str()); + + if (::bind(m_listen_fd, reinterpret_cast(&addr), sizeof(addr)) < 0) + { + std::string err = std::strerror(errno); + ::close(m_listen_fd); + m_listen_fd = -1; + throw std::runtime_error("CudaIpcPublisher: bind(" + m_config.socket_path + ") failed: " + err); + } + m_socket_bound = true; + + if (::listen(m_listen_fd, 1) < 0) + throw std::runtime_error(std::string("CudaIpcPublisher: listen() failed: ") + std::strerror(errno)); +} + +void CudaIpcPublisher::accept_client() +{ + int fd = ::accept4(m_listen_fd, nullptr, nullptr, SOCK_NONBLOCK | SOCK_CLOEXEC); + if (fd < 0) + return; // EAGAIN: nobody waiting. + + if (m_client_fd >= 0) + { + // Last connect wins, so restarting the viewer does not need a plugin + // restart. The old consumer sees EOF. + std::cout << "CUDA IPC: sensor " << m_config.sensor_id << " replacing consumer" << std::endl; + ::close(m_client_fd); + m_client_fd = -1; + m_unreleased = 0; + } + + ipc::Hello hello{}; + hello.magic = ipc::kHelloMagic; + hello.version = ipc::kProtocolVersion; + hello.width = m_config.width; + hello.height = m_config.height; + hello.format = static_cast(ipc::PixelFormat::Rgba8); + hello.slot_count = m_config.slot_count; + hello.device_id = static_cast(m_config.gpu_id); + hello.sensor_id = m_config.sensor_id; + hello.pitch = m_pitch; + hello.slot_stride = m_slot_stride; + hello.total_bytes = m_reserved_bytes; + + if (!send_record(fd, &hello, sizeof(hello), m_export_fd)) + { + std::cerr << "CUDA IPC: sensor " << m_config.sensor_id << " handshake send failed: " << std::strerror(errno) + << std::endl; + ::close(fd); + return; + } + + m_client_fd = fd; + std::cout << "CUDA IPC: sensor " << m_config.sensor_id << " consumer attached" << std::endl; +} + +void CudaIpcPublisher::drain_releases() +{ + if (m_client_fd < 0) + return; + + ipc::SlotRelease release{}; + while (true) + { + ssize_t got = ::recv(m_client_fd, &release, sizeof(release), MSG_DONTWAIT); + if (got == 0) + { + drop_client("consumer disconnected"); + return; + } + if (got < 0) + { + if (errno == EAGAIN || errno == EWOULDBLOCK) + return; + drop_client(std::strerror(errno)); + return; + } + if (got != sizeof(release) || release.magic != ipc::kReleaseMagic) + { + drop_client("protocol desync on release message"); + return; + } + if (release.slot >= m_config.slot_count) + { + drop_client("release named an out-of-range slot"); + return; + } + m_unreleased &= ~(uint64_t{ 1 } << release.slot); + } +} + +void CudaIpcPublisher::drop_client(const char* reason) +{ + if (m_client_fd < 0) + return; + std::cout << "CUDA IPC: sensor " << m_config.sensor_id << " consumer detached (" << reason << ")" << std::endl; + ::close(m_client_fd); + m_client_fd = -1; + m_unreleased = 0; +} + +void CudaIpcPublisher::poll() +{ + drain_releases(); + accept_client(); +} + +int CudaIpcPublisher::pick_slot() const +{ + int best = -1; + uint64_t best_sequence = UINT64_MAX; + for (uint32_t i = 0; i < m_config.slot_count; ++i) + { + if (m_unreleased & (uint64_t{ 1 } << i)) + continue; + if (m_slot_sequence[i] < best_sequence) + { + best_sequence = m_slot_sequence[i]; + best = static_cast(i); + } + } + return best; +} + +bool CudaIpcPublisher::publish(uintptr_t src_ptr, size_t src_pitch, uint64_t timestamp_ns) +{ + if (m_client_fd < 0 || src_ptr == 0) + return false; + + const int slot = pick_slot(); + if (slot < 0) + { + ++m_dropped; + return false; + } + + check_cuda(cuCtxSetCurrent(m_context), "cuCtxSetCurrent publish"); + + void* dst = reinterpret_cast(m_base_ptr + static_cast(slot) * m_slot_stride); + check_runtime(cudaMemcpy2DAsync(dst, m_pitch, reinterpret_cast(src_ptr), src_pitch, m_pitch, + m_config.height, cudaMemcpyDeviceToDevice, m_stream), + "cudaMemcpy2DAsync"); + + // The consumer reads on its own context and stream and has no way to wait + // on ours, so the copy must be complete before it is told the slot is + // ready. Without this it renders a torn frame. + check_runtime(cudaStreamSynchronize(m_stream), "cudaStreamSynchronize"); + + ++m_sequence; + m_slot_sequence[slot] = m_sequence; + m_unreleased |= uint64_t{ 1 } << slot; + + ipc::FrameReady ready{}; + ready.magic = ipc::kFrameMagic; + ready.slot = static_cast(slot); + ready.sequence = m_sequence; + ready.timestamp_ns = timestamp_ns; + + if (!send_record(m_client_fd, &ready, sizeof(ready), -1)) + { + if (errno == EAGAIN || errno == EWOULDBLOCK) + { + // Consumer is behind and its socket buffer is full. Dropping the + // notification is the mailbox-correct outcome: it will pick up the + // next frame instead of falling further behind. + m_unreleased &= ~(uint64_t{ 1 } << slot); + ++m_dropped; + return false; + } + drop_client(std::strerror(errno)); + return false; + } + + return true; +} + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/cuda_ipc_publisher.hpp b/src/plugins/sensing/core/cuda_ipc_publisher.hpp new file mode 100644 index 000000000..c9b16db68 --- /dev/null +++ b/src/plugins/sensing/core/cuda_ipc_publisher.hpp @@ -0,0 +1,125 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 +// +// Publishes captured frames to another process as CUDA device memory, with no +// encode and no host round-trip. +// +// Legacy CUDA IPC (cudaIpcGetMemHandle) does not work here. On Orin the +// producer-side call returns cudaSuccess and the consumer's +// cudaIpcOpenMemHandle then fails with cudaErrorInvalidValue, so the failure +// only shows up in the far process. The working route on Tegra is the virtual +// memory management API: cuMemCreate exports a POSIX file descriptor that the +// consumer maps with cuMemImportFromShareableHandle. Do not "simplify" this +// back to cudaIpcMemHandle_t. +// +// Wire protocol lives in cuda_ipc_protocol.hpp, shared with the Python +// consumer in examples/camera_viz/sources/cuda_ipc.py. + +#pragma once + +#include "cuda_ipc_protocol.hpp" + +#include +#include +#include + +#include + +namespace plugins +{ +namespace sensing +{ + +struct CudaIpcConfig +{ + /// Unix domain socket path the consumer connects to. + std::string socket_path; + uint32_t width = 1920; + uint32_t height = 1080; + uint32_t sensor_id = 0; + int gpu_id = 0; + /// Ring depth. One slot is held by the consumer while it renders, so + /// three is the minimum that lets the producer keep writing; four leaves + /// headroom for a consumer that briefly stalls. + uint32_t slot_count = 4; +}; + +/** + * @brief Serves RGBA8 frames to one consumer process over CUDA VMM + a Unix socket. + * + * Single-threaded and non-blocking throughout: poll() accepts connections and + * reaps slot releases, publish() copies into a free slot and notifies. Neither + * ever blocks the capture loop, so a stalled or absent consumer costs the + * pipeline nothing. + * + * One consumer at a time. A second connection replaces the first, so + * restarting the viewer does not require restarting the plugin. + */ +class CudaIpcPublisher +{ +public: + explicit CudaIpcPublisher(const CudaIpcConfig& config); + ~CudaIpcPublisher(); + + CudaIpcPublisher(const CudaIpcPublisher&) = delete; + CudaIpcPublisher& operator=(const CudaIpcPublisher&) = delete; + + /** @brief Accept a pending consumer and reap released slots. Never blocks. */ + void poll(); + + /** + * @brief Copy one RGBA8 frame into a free slot and publish it. + * + * @param src_ptr Device pointer to RGBA8 source (e.g. ArgusCamera's + * converted buffer). + * @param src_pitch Source row stride in bytes. + * @return false if no consumer is attached, or the frame was dropped. + */ + bool publish(uintptr_t src_ptr, size_t src_pitch, uint64_t timestamp_ns); + + bool has_consumer() const { return m_client_fd >= 0; } + uint64_t published_count() const { return m_sequence; } + uint64_t dropped_count() const { return m_dropped; } + +private: + void allocate_slots(); + void open_socket(); + void accept_client(); + void drain_releases(); + void drop_client(const char* reason); + /// Least-recently-published slot the consumer has released, or -1. + int pick_slot() const; + + CudaIpcConfig m_config; + + CUdevice m_device = 0; + CUcontext m_context = nullptr; + bool m_context_retained = false; + CUstream m_stream = nullptr; + + /// One allocation carrying every slot; one fd exports the lot. + CUmemGenericAllocationHandle m_alloc_handle = 0; + CUdeviceptr m_base_ptr = 0; + size_t m_reserved_bytes = 0; + int m_export_fd = -1; + + size_t m_pitch = 0; + size_t m_slot_stride = 0; + + /// Publish sequence each slot last carried; 0 means never written. + std::vector m_slot_sequence; + /// Bit i set while slot i is published but not yet released by the + /// consumer. Overwriting one of these would tear the frame it is reading, + /// so publish() drops instead. Caps slot_count at 64. + uint64_t m_unreleased = 0; + + int m_listen_fd = -1; + int m_client_fd = -1; + bool m_socket_bound = false; + + uint64_t m_sequence = 0; + uint64_t m_dropped = 0; +}; + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/frame_sink.cpp b/src/plugins/sensing/core/frame_sink.cpp new file mode 100644 index 000000000..029428f32 --- /dev/null +++ b/src/plugins/sensing/core/frame_sink.cpp @@ -0,0 +1,212 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#define MCAP_IMPLEMENTATION +#include "frame_sink.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace plugins +{ +namespace sensing +{ + +// ============================================================================= +// FrameSink +// ============================================================================= + +FrameSink::FrameSink(const std::vector& streams, std::unique_ptr metadata_pusher) + : m_metadata_pusher(std::move(metadata_pusher)) +{ + for (const auto& config : streams) + { + // An ipc-only stream has nothing to write; on_frame still forwards its + // metadata. + if (config.output_path.empty()) + continue; + + std::filesystem::path p(config.output_path); + auto parent = p.parent_path(); + if (!parent.empty()) + std::filesystem::create_directories(parent); + + m_writers[config.sensor_id] = std::make_unique(config.output_path); + std::cout << "Add stream: sensor " << config.sensor_id << " -> " << config.output_path << std::endl; + } +} + +void FrameSink::on_frame(const SensingFrame& frame) +{ + auto it = m_writers.find(frame.sensor_id); + if (it != m_writers.end()) + it->second->write(frame.h264_data); + + if (m_metadata_pusher) + m_metadata_pusher->on_frame_metadata( + frame.metadata, frame.sample_time_local_common_clock_ns, frame.sample_time_raw_device_clock_ns); +} + +// ============================================================================= +// SchemaMetadataPusher — pushes frame metadata over OpenXR +// ============================================================================= + +class SchemaMetadataPusher : public IMetadataPusher +{ +public: + SchemaMetadataPusher(const std::vector& streams, const std::string& collection_prefix) + : m_oxr_session(std::make_shared( + "SensingCameraPlugin", core::SchemaPusher::get_required_extensions())) + { + for (const auto& config : streams) + { + auto collection_id = collection_prefix + "/sensor" + std::to_string(config.sensor_id); + m_pushers[config.sensor_id] = std::make_unique( + m_oxr_session->get_handles(), core::SchemaPusherConfig{ .collection_id = collection_id, + .max_flatbuffer_size = MAX_FLATBUFFER_SIZE, + .tensor_identifier = "frame_metadata", + .localized_name = "Frame Metadata Pusher", + .app_name = "SensingCameraPlugin" }); + std::cout << " Metadata: " << collection_id << std::endl; + } + } + + void on_frame_metadata(const core::FrameMetadataSensingT& metadata, + int64_t sample_time_local_common_clock_ns, + int64_t sample_time_raw_device_clock_ns) override + { + auto it = m_pushers.find(metadata.sensor_id); + if (it == m_pushers.end()) + { + std::cout << "Sensor " << metadata.sensor_id << " not found in SchemaMetadataPusher" << std::endl; + return; + } + + flatbuffers::FlatBufferBuilder builder(MAX_FLATBUFFER_SIZE); + auto offset = core::FrameMetadataSensing::Pack(builder, &metadata); + builder.Finish(offset); + it->second->push_buffer(builder.GetBufferPointer(), builder.GetSize(), sample_time_local_common_clock_ns, + sample_time_raw_device_clock_ns); + } + +private: + static constexpr size_t MAX_FLATBUFFER_SIZE = 128; + std::shared_ptr m_oxr_session; + std::map> m_pushers; +}; + +// ============================================================================= +// McapMetadataPusher — writes frame metadata to an MCAP file +// ============================================================================= + +class McapMetadataPusher : public IMetadataPusher +{ +public: + McapMetadataPusher(const std::vector& streams, const std::string& mcap_filename) + { + mcap::McapWriterOptions options("sensing_camera"); + options.compression = mcap::Compression::None; + + auto status = m_writer.open(mcap_filename, options); + if (!status.ok()) + throw std::runtime_error("McapMetadataPusher: Failed to open " + mcap_filename + ": " + status.message); + + mcap::Schema schema( + "core.FrameMetadataSensingRecord", "flatbuffer", + std::string(reinterpret_cast(core::FrameMetadataSensingRecordBinarySchema::data()), + core::FrameMetadataSensingRecordBinarySchema::size())); + m_writer.addSchema(schema); + + for (const auto& config : streams) + { + std::string channel_name = "sensing_metadata/sensor" + std::to_string(config.sensor_id); + mcap::Channel channel(channel_name, "flatbuffer", schema.id); + m_writer.addChannel(channel); + m_channel_ids[config.sensor_id] = channel.id; + std::cout << " MCAP channel: " << channel_name << std::endl; + } + + std::cout << "MCAP recording to: " << mcap_filename << std::endl; + } + + ~McapMetadataPusher() override + { + m_writer.close(); + std::cout << "MCAP closed with " << m_message_count << " messages" << std::endl; + } + + void on_frame_metadata(const core::FrameMetadataSensingT& metadata, + int64_t sample_time_local_common_clock_ns, + int64_t sample_time_raw_device_clock_ns) override + { + auto it = m_channel_ids.find(metadata.sensor_id); + if (it == m_channel_ids.end()) + { + std::cerr << "McapMetadataPusher: Sensor " << metadata.sensor_id << " not found in MCAP" << std::endl; + return; + } + + const int64_t now_ns = core::os_monotonic_now_ns(); + + flatbuffers::FlatBufferBuilder builder(MAX_FLATBUFFER_SIZE); + auto data_offset = core::FrameMetadataSensing::Pack(builder, &metadata); + core::DeviceDataTimestamp timestamp(now_ns, sample_time_local_common_clock_ns, sample_time_raw_device_clock_ns); + core::FrameMetadataSensingRecordBuilder record_builder(builder); + record_builder.add_data(data_offset); + record_builder.add_timestamp(×tamp); + builder.Finish(record_builder.Finish()); + + mcap::Message msg; + msg.channelId = it->second; + msg.logTime = static_cast(now_ns); + msg.publishTime = static_cast(now_ns); + msg.sequence = static_cast(m_message_count); + msg.data = reinterpret_cast(builder.GetBufferPointer()); + msg.dataSize = builder.GetSize(); + + auto status = m_writer.write(msg); + if (!status.ok()) + std::cerr << "McapMetadataPusher: write failed: " << status.message << std::endl; + + ++m_message_count; + } + +private: + static constexpr size_t MAX_FLATBUFFER_SIZE = 128; + mcap::McapWriter m_writer; + std::map m_channel_ids; + uint64_t m_message_count = 0; +}; + +// ============================================================================= +// Factory +// ============================================================================= + +std::unique_ptr create_frame_sink(const std::vector& streams, + const std::string& collection_prefix, + const std::string& mcap_filename) +{ + if (!collection_prefix.empty() && !mcap_filename.empty()) + throw std::runtime_error("Cannot specify both --collection-prefix and --mcap-filename"); + + std::unique_ptr pusher; + + if (!collection_prefix.empty()) + pusher = std::make_unique(streams, collection_prefix); + else if (!mcap_filename.empty()) + pusher = std::make_unique(streams, mcap_filename); + + return std::make_unique(streams, std::move(pusher)); +} + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/frame_sink.hpp b/src/plugins/sensing/core/frame_sink.hpp new file mode 100644 index 000000000..bca2fc09b --- /dev/null +++ b/src/plugins/sensing/core/frame_sink.hpp @@ -0,0 +1,68 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "rawdata_writer.hpp" +#include "sensing_types.hpp" + +#include +#include +#include +#include + +namespace plugins +{ +namespace sensing +{ + +/** + * @brief Interface to push per-sensor frame metadata. + * + * Concrete implementations push metadata over OpenXR (SchemaMetadataPusher) + * or write it to an MCAP file (McapMetadataPusher). + */ +class IMetadataPusher +{ +public: + virtual ~IMetadataPusher() = default; + virtual void on_frame_metadata(const core::FrameMetadataSensingT& metadata, + int64_t sample_time_local_common_clock_ns, + int64_t sample_time_raw_device_clock_ns) = 0; +}; + +/** + * @brief Multi-sensor output sink for SENSING frames. + * + * Always writes raw H.264 data per sensor. Optionally delegates to an + * IMetadataPusher for additional output (OXR schema push, MCAP recording). + */ +class FrameSink +{ +public: + explicit FrameSink(const std::vector& streams, + std::unique_ptr metadata_pusher = nullptr); + + FrameSink(const FrameSink&) = delete; + FrameSink& operator=(const FrameSink&) = delete; + + void on_frame(const SensingFrame& frame); + +private: + std::map> m_writers; + std::unique_ptr m_metadata_pusher; +}; + +/** + * @brief Factory that creates a FrameSink with the appropriate metadata pusher. + * + * - If collection_prefix is non-empty, attaches a SchemaMetadataPusher. + * - If mcap_filename is non-empty, attaches a McapMetadataPusher. + * - Otherwise creates a plain FrameSink (raw-data only). + */ +std::unique_ptr create_frame_sink(const std::vector& streams, + const std::string& collection_prefix, + const std::string& mcap_filename); + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/jetson_encoder.cpp b/src/plugins/sensing/core/jetson_encoder.cpp new file mode 100644 index 000000000..1979f1f6c --- /dev/null +++ b/src/plugins/sensing/core/jetson_encoder.cpp @@ -0,0 +1,260 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "jetson_encoder.hpp" + +#include "rgba_to_nv12.cuh" + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace sensing +{ + +namespace +{ + +constexpr uint32_t kOutputBuffers = 6; +constexpr uint32_t kCaptureBuffers = 6; + +void check(int ret, const char* what) +{ + if (ret < 0) + throw std::runtime_error(std::string("JetsonEncoder: ") + what + " failed"); +} + +void check_cuda(cudaError_t err, const char* what) +{ + if (err != cudaSuccess) + throw std::runtime_error(std::string("JetsonEncoder: ") + what + ": " + cudaGetErrorString(err)); +} + +} // namespace + +struct JetsonEncoder::Impl +{ + EncoderConfig config; + NvVideoEncoder* encoder = nullptr; + + // NV12 staging in device memory; the V4L2 output plane is host-mapped, so + // each frame costs one device-to-host copy per plane. + uint8_t* d_y = nullptr; + uint8_t* d_uv = nullptr; + size_t d_y_pitch = 0; + size_t d_uv_pitch = 0; + + std::mutex mutex; + std::deque> ready; + bool eos_reached = false; + + ~Impl() + { + if (d_y) + cudaFree(d_y); + if (d_uv) + cudaFree(d_uv); + delete encoder; + } + + // Capture-plane dequeue callback; runs on the encoder's own thread. + static bool capture_dq(struct v4l2_buffer* v4l2_buf, NvBuffer* buffer, NvBuffer* /*shared*/, void* arg) + { + auto* self = static_cast(arg); + + if (!v4l2_buf) + return false; + + if (buffer && buffer->planes[0].bytesused > 0) + { + const auto* data = static_cast(buffer->planes[0].data); + std::lock_guard lock(self->mutex); + self->ready.emplace_back(data, data + buffer->planes[0].bytesused); + } + + // A zero-length unit marks end of stream; stop the thread rather than + // re-queueing, otherwise the dq thread spins on a finished encoder. + if (buffer && buffer->planes[0].bytesused == 0) + { + std::lock_guard lock(self->mutex); + self->eos_reached = true; + return false; + } + + if (self->encoder->capture_plane.qBuffer(*v4l2_buf, nullptr) < 0) + return false; + + return true; + } +}; + +JetsonEncoder::JetsonEncoder(const EncoderConfig& config) : m_impl(std::make_unique()) +{ + if (config.width == 0 || config.height == 0) + throw std::runtime_error("JetsonEncoder: width/height must be non-zero"); + if ((config.width % 2) != 0 || (config.height % 2) != 0) + throw std::runtime_error("JetsonEncoder: width/height must be even for NV12"); + + m_impl->config = config; + + m_impl->encoder = NvVideoEncoder::createVideoEncoder("enc0"); + if (!m_impl->encoder) + throw std::runtime_error("JetsonEncoder: createVideoEncoder failed (is /dev/v4l2-nvenc present?)"); + + auto* enc = m_impl->encoder; + + // Capture format must be set before the output format. + const uint32_t bitstream_size = config.width * config.height * 3 / 2; + check(enc->setCapturePlaneFormat(V4L2_PIX_FMT_H264, config.width, config.height, bitstream_size), + "setCapturePlaneFormat"); + check(enc->setOutputPlaneFormat(V4L2_PIX_FMT_NV12M, config.width, config.height), "setOutputPlaneFormat"); + + check(enc->setBitrate(config.bitrate_bps), "setBitrate"); + check(enc->setProfile(V4L2_MPEG_VIDEO_H264_PROFILE_HIGH), "setProfile"); + check(enc->setRateControlMode(V4L2_MPEG_VIDEO_BITRATE_MODE_CBR), "setRateControlMode"); + check(enc->setFrameRate(config.fps ? config.fps : 30, 1), "setFrameRate"); + + const uint32_t gop = config.gop ? config.gop : (config.fps ? config.fps * 5 : 150); + check(enc->setIDRInterval(gop), "setIDRInterval"); + check(enc->setIFrameInterval(gop), "setIFrameInterval"); + + // Low-latency shape: no B-frames, SPS/PPS on every IDR so a receiver can + // join mid-stream, and the max-performance clock preset. + check(enc->setNumBFrames(0), "setNumBFrames"); + check(enc->setInsertSpsPpsAtIdrEnabled(true), "setInsertSpsPpsAtIdrEnabled"); + check(enc->setMaxPerfMode(1), "setMaxPerfMode"); + + check(enc->output_plane.setupPlane(V4L2_MEMORY_MMAP, kOutputBuffers, true, false), "output setupPlane"); + check(enc->capture_plane.setupPlane(V4L2_MEMORY_MMAP, kCaptureBuffers, true, false), "capture setupPlane"); + + check(enc->output_plane.setStreamStatus(true), "output setStreamStatus"); + check(enc->capture_plane.setStreamStatus(true), "capture setStreamStatus"); + + enc->capture_plane.setDQThreadCallback(&Impl::capture_dq); + enc->capture_plane.startDQThread(m_impl.get()); + + // Prime the capture plane so the encoder always has somewhere to write. + for (uint32_t i = 0; i < enc->capture_plane.getNumBuffers(); ++i) + { + struct v4l2_buffer v4l2_buf; + struct v4l2_plane planes[MAX_PLANES]; + std::memset(&v4l2_buf, 0, sizeof(v4l2_buf)); + std::memset(planes, 0, sizeof(planes)); + v4l2_buf.index = i; + v4l2_buf.m.planes = planes; + check(enc->capture_plane.qBuffer(v4l2_buf, nullptr), "capture qBuffer"); + } + + check_cuda(cudaMallocPitch(reinterpret_cast(&m_impl->d_y), &m_impl->d_y_pitch, config.width, config.height), + "cudaMallocPitch(Y)"); + check_cuda( + cudaMallocPitch(reinterpret_cast(&m_impl->d_uv), &m_impl->d_uv_pitch, config.width, config.height / 2), + "cudaMallocPitch(UV)"); +} + +JetsonEncoder::~JetsonEncoder() +{ + if (m_impl && m_impl->encoder) + { + m_impl->encoder->capture_plane.stopDQThread(); + m_impl->encoder->capture_plane.waitForDQThread(1000); + } +} + +bool JetsonEncoder::submit(uintptr_t rgba_device_ptr, std::size_t row_pitch_bytes) +{ + auto* enc = m_impl->encoder; + const auto& config = m_impl->config; + + launch_rgba_to_nv12(reinterpret_cast(rgba_device_ptr), static_cast(row_pitch_bytes), m_impl->d_y, + static_cast(m_impl->d_y_pitch), m_impl->d_uv, static_cast(m_impl->d_uv_pitch), + static_cast(config.width), static_cast(config.height), config.full_range, nullptr); + check_cuda(cudaGetLastError(), "rgba_to_nv12 launch"); + check_cuda(cudaStreamSynchronize(nullptr), "rgba_to_nv12 sync"); + + struct v4l2_buffer v4l2_buf; + struct v4l2_plane planes[MAX_PLANES]; + std::memset(&v4l2_buf, 0, sizeof(v4l2_buf)); + std::memset(planes, 0, sizeof(planes)); + v4l2_buf.m.planes = planes; + + NvBuffer* buffer = nullptr; + // Until every output buffer has been queued once, index i is free by + // construction; after that a dequeue is what frees one. + if (m_queued < enc->output_plane.getNumBuffers()) + { + buffer = enc->output_plane.getNthBuffer(m_queued); + v4l2_buf.index = m_queued; + ++m_queued; + } + else if (enc->output_plane.dqBuffer(v4l2_buf, &buffer, nullptr, 0) < 0) + { + return false; // encoder still busy; drop this frame rather than block + } + + check_cuda(cudaMemcpy2D(buffer->planes[0].data, buffer->planes[0].fmt.stride, m_impl->d_y, m_impl->d_y_pitch, + config.width, config.height, cudaMemcpyDeviceToHost), + "cudaMemcpy2D(Y)"); + check_cuda(cudaMemcpy2D(buffer->planes[1].data, buffer->planes[1].fmt.stride, m_impl->d_uv, m_impl->d_uv_pitch, + config.width, config.height / 2, cudaMemcpyDeviceToHost), + "cudaMemcpy2D(UV)"); + + buffer->planes[0].bytesused = buffer->planes[0].fmt.stride * config.height; + buffer->planes[1].bytesused = buffer->planes[1].fmt.stride * (config.height / 2); + v4l2_buf.m.planes[0].bytesused = buffer->planes[0].bytesused; + v4l2_buf.m.planes[1].bytesused = buffer->planes[1].bytesused; + + check(enc->output_plane.qBuffer(v4l2_buf, nullptr), "output qBuffer"); + return true; +} + +std::vector JetsonEncoder::poll() +{ + std::lock_guard lock(m_impl->mutex); + if (m_impl->ready.empty()) + return {}; + + auto out = std::move(m_impl->ready.front()); + m_impl->ready.pop_front(); + return out; +} + +std::vector JetsonEncoder::end_of_stream() +{ + auto* enc = m_impl->encoder; + + // A zero-length output buffer is the EOS marker for the V4L2 encoder. + struct v4l2_buffer v4l2_buf; + struct v4l2_plane planes[MAX_PLANES]; + std::memset(&v4l2_buf, 0, sizeof(v4l2_buf)); + std::memset(planes, 0, sizeof(planes)); + v4l2_buf.m.planes = planes; + + NvBuffer* buffer = nullptr; + if (enc->output_plane.dqBuffer(v4l2_buf, &buffer, nullptr, 10) >= 0) + { + v4l2_buf.m.planes[0].bytesused = 0; + v4l2_buf.m.planes[1].bytesused = 0; + enc->output_plane.qBuffer(v4l2_buf, nullptr); + } + + enc->capture_plane.waitForDQThread(2000); + + std::vector out; + std::lock_guard lock(m_impl->mutex); + for (auto& chunk : m_impl->ready) + out.insert(out.end(), chunk.begin(), chunk.end()); + m_impl->ready.clear(); + return out; +} + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/jetson_encoder.hpp b/src/plugins/sensing/core/jetson_encoder.hpp new file mode 100644 index 000000000..1f4c367ee --- /dev/null +++ b/src/plugins/sensing/core/jetson_encoder.hpp @@ -0,0 +1,71 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include + +namespace plugins +{ +namespace sensing +{ + +struct EncoderConfig +{ + uint32_t width = 0; + uint32_t height = 0; + uint32_t bitrate_bps = 20'000'000; + uint32_t fps = 30; + /// IDR period in frames; 0 defers to fps*5. + uint32_t gop = 0; + bool full_range = false; +}; + +/** + * @brief H.264 encoder on the Jetson V4L2 M2M engine (/dev/v4l2-nvenc). + * + * Jetson has no libnvidia-encode — the NVIDIA Video Codec SDK is dGPU-only — + * so this wraps NvVideoEncoder from the Jetson Multimedia API instead. + * + * Submission is asynchronous: the encoder needs several input frames before it + * emits the first bitstream unit, so submit() never blocks waiting for output + * and poll() drains whatever a capture-plane thread has completed. + */ +class JetsonEncoder +{ +public: + explicit JetsonEncoder(const EncoderConfig& config); + ~JetsonEncoder(); + + JetsonEncoder(const JetsonEncoder&) = delete; + JetsonEncoder& operator=(const JetsonEncoder&) = delete; + JetsonEncoder(JetsonEncoder&&) = delete; + JetsonEncoder& operator=(JetsonEncoder&&) = delete; + + /** + * @brief Convert one GPU-resident RGBA8 frame to NV12 and queue it. + * @param rgba_device_ptr Device pointer to a HxWx4 RGBA8 buffer. + * @param row_pitch_bytes Byte stride between rows. + * @return false when no input buffer is free (frame dropped). + */ + bool submit(uintptr_t rgba_device_ptr, std::size_t row_pitch_bytes); + + /** @brief Take any completed Annex-B data; empty during encoder warmup. */ + std::vector poll(); + + /** @brief Signal EOS and drain the remaining bitstream. */ + std::vector end_of_stream(); + +private: + struct Impl; + std::unique_ptr m_impl; + + /// Output buffers queued so far; below the buffer count they are free by + /// construction and need no dequeue first. + uint32_t m_queued = 0; +}; + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/rawdata_writer.cpp b/src/plugins/sensing/core/rawdata_writer.cpp new file mode 100644 index 000000000..e011df712 --- /dev/null +++ b/src/plugins/sensing/core/rawdata_writer.cpp @@ -0,0 +1,51 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "rawdata_writer.hpp" + +#include +#include + +namespace plugins +{ +namespace sensing +{ + +RawDataWriter::RawDataWriter(const std::string& path) +{ + if (path.empty()) + { + throw std::runtime_error("No output path specified"); + } + + m_file.open(path, std::ios::binary); + if (!m_file.is_open()) + { + throw std::runtime_error("Failed to open file: " + path); + } +} + +RawDataWriter::~RawDataWriter() +{ + if (m_file.is_open()) + { + m_file.close(); + } +} + +void RawDataWriter::write(const std::vector& data) +{ + if (data.empty()) + { + return; + } + + m_file.write(reinterpret_cast(data.data()), data.size()); + if (!m_file.good()) + { + throw std::runtime_error("Write error"); + } +} + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/rawdata_writer.hpp b/src/plugins/sensing/core/rawdata_writer.hpp new file mode 100644 index 000000000..f8743a649 --- /dev/null +++ b/src/plugins/sensing/core/rawdata_writer.hpp @@ -0,0 +1,44 @@ +// 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 +{ +namespace sensing +{ + +/** + * @brief Raw H.264 file writer + * + * Writes H.264 NAL units directly to a file without container. + * File opens in constructor and closes in destructor (RAII). + */ +class RawDataWriter +{ +public: + /** + * @brief Construct the writer and open the file. + * @param path Output file path. Must not be empty. + * @throws std::runtime_error if the file cannot be opened. + */ + explicit RawDataWriter(const std::string& path); + ~RawDataWriter(); + + // Non-copyable, non-movable + RawDataWriter(const RawDataWriter&) = delete; + RawDataWriter& operator=(const RawDataWriter&) = delete; + + void write(const std::vector& data); + +private: + std::ofstream m_file; +}; + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/rgba_to_nv12.cu b/src/plugins/sensing/core/rgba_to_nv12.cu new file mode 100644 index 000000000..2c0ca5ec6 --- /dev/null +++ b/src/plugins/sensing/core/rgba_to_nv12.cu @@ -0,0 +1,103 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "rgba_to_nv12.cuh" + +namespace +{ + +__device__ __forceinline__ uint8_t clamp_u8(float v) +{ + return static_cast(fminf(fmaxf(v, 0.0f), 255.0f) + 0.5f); +} + +// BT.601 RGB->YCbCr, matching the coefficients yuv_to_rgba.cu inverts on the +// capture side so a round trip through RGBA is close to identity. +__device__ __forceinline__ float rgb_to_y(float r, float g, float b, bool full_range) +{ + return full_range ? (0.299f * r + 0.587f * g + 0.114f * b) : (16.0f + 0.257f * r + 0.504f * g + 0.098f * b); +} + +__device__ __forceinline__ float rgb_to_u(float r, float g, float b, bool full_range) +{ + return full_range ? (128.0f - 0.168736f * r - 0.331264f * g + 0.5f * b) : + (128.0f - 0.148f * r - 0.291f * g + 0.439f * b); +} + +__device__ __forceinline__ float rgb_to_v(float r, float g, float b, bool full_range) +{ + return full_range ? (128.0f + 0.5f * r - 0.418688f * g - 0.081312f * b) : + (128.0f + 0.439f * r - 0.368f * g - 0.071f * b); +} + +__global__ void rgba_to_nv12_kernel(const uint8_t* __restrict__ rgba, + int rgba_pitch, + uint8_t* __restrict__ y_plane, + int y_pitch, + uint8_t* __restrict__ uv_plane, + int uv_pitch, + int width, + int height, + bool full_range) +{ + // One thread per 2x2 block: writes four luma samples and one chroma pair. + const int bx = blockIdx.x * blockDim.x + threadIdx.x; + const int by = blockIdx.y * blockDim.y + threadIdx.y; + const int x = bx * 2; + const int y = by * 2; + if (x >= width || y >= height) + return; + + for (int dy = 0; dy < 2; ++dy) + { + for (int dx = 0; dx < 2; ++dx) + { + const int px = x + dx; + const int py = y + dy; + if (px >= width || py >= height) + continue; + + const uint8_t* pixel = rgba + static_cast(py) * rgba_pitch + static_cast(px) * 4; + const float r = pixel[0]; + const float g = pixel[1]; + const float b = pixel[2]; + y_plane[static_cast(py) * y_pitch + px] = clamp_u8(rgb_to_y(r, g, b, full_range)); + } + } + + const uint8_t* top_left = rgba + static_cast(y) * rgba_pitch + static_cast(x) * 4; + const float r = top_left[0]; + const float g = top_left[1]; + const float b = top_left[2]; + + uint8_t* uv = uv_plane + static_cast(by) * uv_pitch + static_cast(bx) * 2; + uv[0] = clamp_u8(rgb_to_u(r, g, b, full_range)); + uv[1] = clamp_u8(rgb_to_v(r, g, b, full_range)); +} + +} // namespace + +namespace plugins +{ +namespace sensing +{ + +void launch_rgba_to_nv12(const uint8_t* rgba, + int rgba_pitch, + uint8_t* y_plane, + int y_pitch, + uint8_t* uv_plane, + int uv_pitch, + int width, + int height, + bool full_range, + cudaStream_t stream) +{ + const dim3 block(16, 16); + const dim3 grid((width / 2 + block.x - 1) / block.x, (height / 2 + block.y - 1) / block.y); + rgba_to_nv12_kernel<<>>( + rgba, rgba_pitch, y_plane, y_pitch, uv_plane, uv_pitch, width, height, full_range); +} + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/rgba_to_nv12.cuh b/src/plugins/sensing/core/rgba_to_nv12.cuh new file mode 100644 index 000000000..53d4bc2ef --- /dev/null +++ b/src/plugins/sensing/core/rgba_to_nv12.cuh @@ -0,0 +1,36 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include + +namespace plugins +{ +namespace sensing +{ + +/** + * @brief Convert packed RGBA8 to NV12 (Y plane + interleaved UV plane). + * + * Chroma is point-sampled from the top-left pixel of each 2x2 block rather + * than averaged: the Argus ISP already delivered 4:2:0, so the RGBA the + * capture path produced was upsampled from it and averaging would only blur + * chroma that was never independent. Width and height must be even. + * + * @param full_range true for [0,255] luma, false for broadcast [16,235]. + */ +void launch_rgba_to_nv12(const uint8_t* rgba, + int rgba_pitch, + uint8_t* y_plane, + int y_pitch, + uint8_t* uv_plane, + int uv_pitch, + int width, + int height, + bool full_range, + cudaStream_t stream); + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/sensing_camera.cpp b/src/plugins/sensing/core/sensing_camera.cpp new file mode 100644 index 000000000..543851f47 --- /dev/null +++ b/src/plugins/sensing/core/sensing_camera.cpp @@ -0,0 +1,182 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "sensing_camera.hpp" + +#include "frame_sink.hpp" + +#include +#include + +namespace plugins +{ +namespace sensing +{ + +namespace +{ + +camera_viz::argus::ArgusConfig make_argus_config(const SensingConfig& config, uint32_t sensor_id) +{ + camera_viz::argus::ArgusConfig argus{}; + argus.name = "sensor" + std::to_string(sensor_id); + argus.sensor_ids = { sensor_id }; + argus.sensor_mode = config.sensor_mode; + argus.width = config.width; + argus.height = config.height; + argus.fps = config.fps; + argus.gpu_id = config.gpu_id; + argus.full_range = config.full_range; + argus.swap_uv = config.swap_uv; + // Finite acquire timeouts return CUDA_ERROR_UNKNOWN on this JetPack/driver + // stack before the first frame arrives, so block indefinitely instead. + argus.acquire_timeout_ms = 0xffffffffu; + argus.repeat_capture = true; + return argus; +} + +EncoderConfig make_encoder_config(const SensingConfig& config) +{ + EncoderConfig encoder{}; + encoder.width = config.width; + encoder.height = config.height; + encoder.bitrate_bps = config.bitrate_bps; + encoder.fps = static_cast(config.fps); + encoder.gop = config.gop; + encoder.full_range = config.full_range; + return encoder; +} + +} // namespace + +SensingCamera::SensingCamera(const SensingConfig& config, + const std::vector& streams, + std::unique_ptr sink) + : m_config(config), m_sink(std::move(sink)) +{ + if (streams.empty()) + throw std::runtime_error("SensingCamera: no streams requested"); + + m_streams.reserve(streams.size()); + for (const auto& stream_config : streams) + { + Stream stream; + stream.sensor_id = stream_config.sensor_id; + stream.camera = + std::make_unique(make_argus_config(config, stream_config.sensor_id)); + + if (!stream_config.output_path.empty()) + stream.encoder = std::make_unique(make_encoder_config(config)); + + if (!stream_config.ipc_socket_path.empty()) + { + CudaIpcConfig ipc{}; + ipc.socket_path = stream_config.ipc_socket_path; + ipc.width = config.width; + ipc.height = config.height; + ipc.sensor_id = stream_config.sensor_id; + ipc.gpu_id = config.gpu_id; + stream.publisher = std::make_unique(ipc); + } + + stream.camera->start(); + m_streams.push_back(std::move(stream)); + + std::cout << "Sensor " << stream_config.sensor_id << ": " << config.width << "x" << config.height << " @ " + << config.fps << " fps" << std::endl; + } +} + +SensingCamera::~SensingCamera() +{ + for (auto& stream : m_streams) + { + if (stream.camera) + stream.camera->stop(); + } +} + +void SensingCamera::update() +{ + for (auto& stream : m_streams) + { + // Accept consumers and reap released slots even on a frameless tick, + // so a viewer can attach before the camera produces anything. + if (stream.publisher) + stream.publisher->poll(); + + auto view = stream.camera->latest(); + if (view.has_value() && view->sequence != stream.last_sequence) + { + stream.last_sequence = view->sequence; + stream.pending_timestamp_ns = static_cast(view->timestamp_ns); + + // Publish before encoding: the IPC consumer is the latency- + // sensitive path, and the encoder submit below is pipelined anyway. + if (stream.publisher) + stream.publisher->publish(view->left_ptr, view->left_pitch, view->timestamp_ns); + + if (stream.encoder) + stream.encoder->submit(view->left_ptr, view->left_pitch); + } + + // Submission and output are decoupled: the V4L2 encoder needs several + // input frames before it emits the first unit, so drain independently. + // The stamp is the most recent submission, not this unit's own frame. + if (stream.encoder) + { + for (auto h264 = stream.encoder->poll(); !h264.empty(); h264 = stream.encoder->poll()) + dispatch(stream, std::move(h264), stream.pending_timestamp_ns); + } + } +} + +void SensingCamera::flush() +{ + for (auto& stream : m_streams) + { + if (!stream.encoder) + continue; + auto h264 = stream.encoder->end_of_stream(); + if (!h264.empty()) + dispatch(stream, std::move(h264), 0); + } +} + +void SensingCamera::dispatch(Stream& stream, std::vector h264, int64_t timestamp_ns) +{ + SensingFrame frame; + frame.sensor_id = stream.sensor_id; + frame.h264_data = std::move(h264); + frame.metadata.sensor_id = stream.sensor_id; + frame.metadata.sequence_number = stream.frame_count; + + // ArgusCamera stamps frames with CLOCK_MONOTONIC at YUV->RGBA conversion, + // not with Argus getSensorTimestamp(), so there is no separate device + // clock to report: both fields carry the same host-side capture stamp and + // include acquire + convert latency. Do not treat the difference between + // them as sensor-to-host latency. + frame.sample_time_local_common_clock_ns = timestamp_ns; + frame.sample_time_raw_device_clock_ns = timestamp_ns; + + ++stream.frame_count; + m_sink->on_frame(frame); +} + +void SensingCamera::print_stats() const +{ + for (const auto& stream : m_streams) + { + std::cout << " sensor " << stream.sensor_id << ": " << stream.frame_count << " frames"; + if (stream.publisher) + { + std::cout << " | ipc " << stream.publisher->published_count() << " published, " + << stream.publisher->dropped_count() << " dropped" + << (stream.publisher->has_consumer() ? "" : ", no consumer"); + } + std::cout << std::endl; + } +} + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/sensing_camera.hpp b/src/plugins/sensing/core/sensing_camera.hpp new file mode 100644 index 000000000..c27fb08f8 --- /dev/null +++ b/src/plugins/sensing/core/sensing_camera.hpp @@ -0,0 +1,78 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "argus_camera.hpp" +#include "cuda_ipc_publisher.hpp" +#include "jetson_encoder.hpp" +#include "sensing_types.hpp" + +#include +#include +#include + +namespace plugins +{ +namespace sensing +{ + +class FrameSink; + +/** + * @brief Multi-sensor SENSING camera manager. + * + * One independent Argus session per sensor: the GMSL drivers on this carrier + * reject a multi-sensor Argus session, so sensors are never grouped even when + * they form a stereo pair. Each update() polls every sensor's latest-frame + * mailbox and fans the new frame out to whichever destinations that stream + * configured — an H.264 encoder, a CUDA IPC socket, or both. A stream may have + * an encoder or not; the CUDA path deliberately needs neither. + */ +class SensingCamera +{ +public: + SensingCamera(const SensingConfig& config, const std::vector& streams, std::unique_ptr sink); + ~SensingCamera(); + + SensingCamera(const SensingCamera&) = delete; + SensingCamera& operator=(const SensingCamera&) = delete; + SensingCamera(SensingCamera&&) = delete; + SensingCamera& operator=(SensingCamera&&) = delete; + + /** @brief Poll every sensor and dispatch newly encoded frames. */ + void update(); + + /** @brief Flush each encoder's queued packets into the sink. */ + void flush(); + + /** @brief Print per-sensor frame counts to stdout. */ + void print_stats() const; + +private: + struct Stream + { + uint32_t sensor_id = 0; + std::unique_ptr camera; + /// Null when the stream requested no H.264 output. + std::unique_ptr encoder; + /// Null when the stream requested no CUDA IPC socket. + std::unique_ptr publisher; + /// Argus publish counter of the frame already encoded; skips re-reads. + uint64_t last_sequence = 0; + /// Capture stamp of the most recent submission. The encoder reorders + /// nothing (no B-frames) but is pipelined, so an emitted unit is + /// attributed to the latest frame submitted, not necessarily its own. + int64_t pending_timestamp_ns = 0; + uint64_t frame_count = 0; + }; + + void dispatch(Stream& stream, std::vector h264, int64_t timestamp_ns); + + SensingConfig m_config; + std::vector m_streams; + std::unique_ptr m_sink; +}; + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/sensing_types.hpp b/src/plugins/sensing/core/sensing_types.hpp new file mode 100644 index 000000000..c96f2ac15 --- /dev/null +++ b/src/plugins/sensing/core/sensing_types.hpp @@ -0,0 +1,63 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 +// +// Plain configuration and frame types, deliberately free of the Argus, EGL and +// V4L2 headers. Those define X11-style macros (Success, Status, None) that +// collide with mcap's enum class StatusCode, so the sink layer must be able to +// describe a frame without pulling them in. + +#pragma once + +#include + +#include +#include +#include + +namespace plugins +{ +namespace sensing +{ + +/// One captured sensor and where its frames go. Either destination may be +/// empty: an ipc-only stream skips the encoder entirely, which is the point of +/// the CUDA path. +struct StreamConfig +{ + uint32_t sensor_id = 0; + /// H.264 elementary stream path; empty disables encoding for this sensor. + std::string output_path; + /// Unix socket serving RGBA8 frames as CUDA memory; empty disables it. + std::string ipc_socket_path; +}; + +struct SensingConfig +{ + /// Argus sensor mode. 0 is the only S56C mode (1920x1080); SHF3L uses 2. + uint32_t sensor_mode = 0; + uint32_t width = 1920; + uint32_t height = 1080; + double fps = 30.0; + int gpu_id = 0; + uint32_t bitrate_bps = 20'000'000; + /// IDR period; 0 defers to the encoder default of fps*5. + uint32_t gop = 0; + bool full_range = false; + bool swap_uv = true; +}; + +struct SensingFrame +{ + uint32_t sensor_id = 0; + + /// H.264 Annex-B data for one frame. + std::vector h264_data; + + core::FrameMetadataSensingT metadata; + + int64_t sample_time_local_common_clock_ns = 0; + int64_t sample_time_raw_device_clock_ns = 0; +}; + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/yuv_to_rgba.cu b/src/plugins/sensing/core/yuv_to_rgba.cu new file mode 100644 index 000000000..7a1a975b1 --- /dev/null +++ b/src/plugins/sensing/core/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/src/plugins/sensing/core/yuv_to_rgba.cuh b/src/plugins/sensing/core/yuv_to_rgba.cuh new file mode 100644 index 000000000..752ee59f8 --- /dev/null +++ b/src/plugins/sensing/core/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/src/plugins/sensing/main.cpp b/src/plugins/sensing/main.cpp new file mode 100644 index 000000000..7cd9bda78 --- /dev/null +++ b/src/plugins/sensing/main.cpp @@ -0,0 +1,274 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "core/frame_sink.hpp" +#include "core/sensing_camera.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace plugins::sensing; + +// ============================================================================= +// Signal handling +// ============================================================================= + +static std::atomic g_stop_requested{ false }; + +void signal_handler(int signal) +{ + if (signal == SIGINT || signal == SIGTERM) + { + g_stop_requested.store(true, std::memory_order_relaxed); + } +} + +// ============================================================================= +// --add-stream parser +// ============================================================================= + +static StreamConfig parse_stream_arg(const std::string& arg) +{ + StreamConfig cfg{}; + bool has_sensor = false; + + std::istringstream ss(arg); + std::string token; + while (std::getline(ss, token, ',')) + { + auto eq = token.find('='); + if (eq == std::string::npos) + throw std::runtime_error("Invalid key=value in --add-stream: '" + token + "'"); + + auto key = token.substr(0, eq); + auto val = token.substr(eq + 1); + + if (key == "sensor") + { + cfg.sensor_id = static_cast(std::stoul(val)); + has_sensor = true; + } + else if (key == "output") + { + cfg.output_path = val; + } + else if (key == "ipc") + { + cfg.ipc_socket_path = val; + } + else + { + throw std::runtime_error("Unknown key in --add-stream: '" + key + "'"); + } + } + + if (!has_sensor) + throw std::runtime_error("--add-stream requires sensor="); + if (cfg.output_path.empty() && cfg.ipc_socket_path.empty()) + throw std::runtime_error("--add-stream requires output= or ipc=, or both"); + + return cfg; +} + +// ============================================================================= +// Usage +// ============================================================================= + +void print_usage(const char* program_name) +{ + std::cout << "Usage: " << program_name << " [options] --add-stream ...\n" + << "\nStream Configuration (repeatable):\n" + << " --add-stream sensor=[,output=][,ipc=]\n" + << " sensor: Argus sensor id (device-tree module order, NOT /dev/videoN)\n" + << " output: file path for this stream's H.264 data\n" + << " ipc: Unix socket serving raw RGBA8 frames as CUDA memory to\n" + << " another process (camera_viz `type: cuda_ipc`). No encode.\n" + << " At least one of output/ipc is required; both may be given.\n" + << "\nGlobal Camera Settings:\n" + << " --sensor-mode=N Argus sensor mode (default: 0; S56C has only 0, SHF3L uses 2)\n" + << " --width=N Capture width (default: 1920)\n" + << " --height=N Capture height (default: 1080)\n" + << " --fps=N Frame rate for all streams (default: 30)\n" + << " --bitrate=N H.264 bitrate in bps (default: 20000000)\n" + << " --gop=N IDR period in frames (default: fps*5)\n" + << " --gpu-id=N CUDA device index (default: 0)\n" + << " --full-range Treat luma as full range instead of broadcast range\n" + << " --no-swap-uv Do not swap the chroma planes\n" + << "\nMetadata (mutually exclusive):\n" + << " --collection-prefix=PREFIX Push metadata via OpenXR tensor extensions\n" + << " --mcap-filename=PATH Record metadata to an MCAP file\n" + << "\nGeneral:\n" + << " --help Show this help message\n" + << "\nExamples:\n" + << " " << program_name << " --add-stream=sensor=2,output=./left.h264\n" + << " " << program_name << " --add-stream=sensor=2,ipc=/tmp/sensing2.sock\n" + << " " << program_name + << " --add-stream=sensor=2,output=./left.h264 --add-stream=sensor=3,output=./right.h264 " + "--mcap-filename=./meta.mcap\n"; +} + +// ============================================================================= +// Main +// ============================================================================= + +int main(int argc, char** argv) +try +{ + SensingConfig camera_config; + std::map stream_map; + std::string collection_prefix; + std::string mcap_filename; + + for (int i = 1; i < argc; ++i) + { + std::string arg = argv[i]; + + if (arg == "--help" || arg == "-h") + { + print_usage(argv[0]); + return 0; + } + else if (arg.find("--add-stream=") == 0) + { + auto cfg = parse_stream_arg(arg.substr(13)); + stream_map[cfg.sensor_id] = cfg; + } + else if (arg.find("--sensor-mode=") == 0) + { + camera_config.sensor_mode = static_cast(std::stoul(arg.substr(14))); + } + else if (arg.find("--width=") == 0) + { + camera_config.width = static_cast(std::stoul(arg.substr(8))); + } + else if (arg.find("--height=") == 0) + { + camera_config.height = static_cast(std::stoul(arg.substr(9))); + } + else if (arg.find("--fps=") == 0) + { + camera_config.fps = std::stod(arg.substr(6)); + } + else if (arg.find("--bitrate=") == 0) + { + camera_config.bitrate_bps = static_cast(std::stoul(arg.substr(10))); + } + else if (arg.find("--gop=") == 0) + { + camera_config.gop = static_cast(std::stoul(arg.substr(6))); + } + else if (arg.find("--gpu-id=") == 0) + { + camera_config.gpu_id = std::stoi(arg.substr(9)); + } + else if (arg == "--full-range") + { + camera_config.full_range = true; + } + else if (arg == "--no-swap-uv") + { + camera_config.swap_uv = false; + } + else if (arg.find("--collection-prefix=") == 0) + { + collection_prefix = arg.substr(20); + } + else if (arg.find("--mcap-filename=") == 0) + { + mcap_filename = arg.substr(16); + } + else if (arg.find("--plugin-root-id=") == 0) + { + // plugin-root-id is a default argument, so we don't need to store it + } + else + { + std::cerr << "Unknown option: " << arg << std::endl; + print_usage(argv[0]); + return 1; + } + } + + if (stream_map.empty()) + { + std::cerr << "Error: at least one --add-stream is required." << std::endl; + print_usage(argv[0]); + return 1; + } + + std::vector stream_configs; + stream_configs.reserve(stream_map.size()); + for (auto& [_, cfg] : stream_map) + { + stream_configs.push_back(std::move(cfg)); + } + + // This process captures and never renders, but EGL is on its critical path: + // Argus and NvBufSurface both need the Tegra EGL driver, and GLVND hands out + // Mesa's instead whenever DISPLAY names an X server Tegra EGL cannot drive + // (Xvfb, or X11 forwarding). libnvbufsurface resolves its own display via + // eglGetDisplay(EGL_DEFAULT_DISPLAY), so the choice cannot be made per-call + // -- it has to be gone from the environment before the first EGL call. + ::unsetenv("DISPLAY"); + + std::signal(SIGINT, signal_handler); + std::signal(SIGTERM, signal_handler); + + std::cout << "============================================================" << std::endl; + std::cout << "SENSING Camera Plugin Starting" << std::endl; + std::cout << "============================================================" << std::endl; + + SensingCamera camera( + camera_config, stream_configs, create_frame_sink(stream_configs, collection_prefix, mcap_filename)); + + std::cout << "------------------------------------------------------------" << std::endl; + std::cout << "Running capture loop. Press Ctrl+C to stop." << std::endl; + + constexpr auto stats_interval = std::chrono::seconds(5); + auto last_stats_time = std::chrono::steady_clock::now(); + + // ArgusCamera::latest() is a non-blocking mailbox read, so poll at roughly + // twice the frame rate rather than spinning a core flat out. + const auto poll_interval = + std::chrono::microseconds(static_cast(500'000.0 / (camera_config.fps > 0.0 ? camera_config.fps : 30.0))); + + while (!g_stop_requested.load(std::memory_order_relaxed)) + { + camera.update(); + + auto now = std::chrono::steady_clock::now(); + if (now - last_stats_time >= stats_interval) + { + camera.print_stats(); + last_stats_time = now; + } + + std::this_thread::sleep_for(poll_interval); + } + + std::cout << "------------------------------------------------------------" << std::endl; + std::cout << "Shutting down SENSING Camera Plugin..." << std::endl; + camera.flush(); + camera.print_stats(); + std::cout << "Plugin stopped" << std::endl; + std::cout << "============================================================" << std::endl; + + return 0; +} +catch (const std::exception& e) +{ + std::cerr << argv[0] << ": " << e.what() << std::endl; + return 1; +} +catch (...) +{ + std::cerr << argv[0] << ": Unknown error occurred" << std::endl; + return 1; +} diff --git a/src/plugins/sensing/plugin.yaml b/src/plugins/sensing/plugin.yaml new file mode 100644 index 000000000..1a8931f94 --- /dev/null +++ b/src/plugins/sensing/plugin.yaml @@ -0,0 +1,11 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +name: sensing_camera +description: "SENSING GMSL camera plugin (Argus capture, NVENC H.264, MCAP metadata)" +command: "./camera_plugin_sensing" +version: "1.0.0" +devices: + - path: "/camera" + type: "camera" + description: "Camera video stream" diff --git a/src/plugins/sensing/scripts/sensing-camera.service.in b/src/plugins/sensing/scripts/sensing-camera.service.in new file mode 100644 index 000000000..b2ec624a1 --- /dev/null +++ b/src/plugins/sensing/scripts/sensing-camera.service.in @@ -0,0 +1,24 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# sensing-camera.service — installed by `setup_host.sh`. +# Placeholders are substituted at install time; do not hand-edit on the robot. + +[Unit] +Description=SENSING GMSL camera drivers (SG10A) +# basic.target, not multi-user.target: the loader has to finish before +# nvargus-daemon starts, and nvargus-daemon is itself part of multi-user.target. +After=basic.target +Before=nvargus-daemon.service +Wants=nvargus-daemon.service + +[Service] +Type=oneshot +RemainAfterExit=yes +WorkingDirectory={{PKG_DIR}} +ExecStart={{LOADER}} --pkg {{PKG_DIR}} --fps {{FPS}}{{FREE_RUN}} +StandardOutput=journal +StandardError=journal + +[Install] +WantedBy=multi-user.target diff --git a/src/plugins/sensing/scripts/sensing-load.sh b/src/plugins/sensing/scripts/sensing-load.sh new file mode 100755 index 000000000..3f27350b4 --- /dev/null +++ b/src/plugins/sensing/scripts/sensing-load.sh @@ -0,0 +1,70 @@ +#!/usr/bin/env bash +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# Root-only, idempotent driver load for the SENSING rig. Shared by setup_host.sh +# (interactively, via sudo) and by sensing-camera.service (at boot, as root), so +# both paths bring the rig up identically. +# +# Do not add prompts or colour here — it runs under systemd with no tty. +# +# Usage: +# sensing-load.sh --pkg DIR [--fps N] [--free-run] [--restart-argus] +# +# --restart-argus restart nvargus-daemon afterwards. Correct for an +# interactive run; wrong under the boot unit, which is +# already ordered Before=nvargus-daemon.service. + +set -euo pipefail + +PKG_DIR="" +FPS=30 +FREE_RUN=0 +RESTART_ARGUS=0 +SHF3L_NODES=(4 5 6 7 8 9) + +while (( $# )); do + case "$1" in + --pkg) PKG_DIR="$2"; shift 2 ;; + --fps) FPS="$2"; shift 2 ;; + --free-run) FREE_RUN=1; shift ;; + --restart-argus) RESTART_ARGUS=1; shift ;; + *) echo "sensing-load.sh: unknown argument: $1" >&2; exit 1 ;; + esac +done + +[[ "$(id -u)" -eq 0 ]] || { echo "sensing-load.sh: must run as root" >&2; exit 1; } +[[ -n "$PKG_DIR" && -f "$PKG_DIR/load_modules.sh" ]] \ + || { echo "sensing-load.sh: --pkg must point at the vendor package (got '$PKG_DIR')" >&2; exit 1; } + +# load_modules.sh resolves ./ko/*.ko and ./gpio-pwm.sh relative to $PWD, and +# rmmods before insmod, so re-running it is the supported way to reload. +echo "sensing-load: loading drivers from $PKG_DIR at ${FPS} Hz" +cd "$PKG_DIR" +./load_modules.sh "$FPS" + +if [[ "$FREE_RUN" -eq 1 ]]; then + if ! command -v v4l2-ctl >/dev/null 2>&1; then + echo "sensing-load: v4l2-ctl missing, cannot set free-run mode" >&2 + exit 1 + fi + for dev in /dev/video*; do + [[ -e "$dev" ]] || continue + i="${dev#/dev/video}" + ctrls="trig_mode=0" + for s in "${SHF3L_NODES[@]}"; do [[ "$i" == "$s" ]] && ctrls="sensor_mode=2,trig_mode=0"; done + # An unpopulated port rejects the write; that is expected, not fatal. + if v4l2-ctl -d "$dev" -c "$ctrls" 2>/dev/null; then + echo "sensing-load: $dev -> $ctrls" + else + echo "sensing-load: $dev skipped (no module on that port)" + fi + done +fi + +if [[ "$RESTART_ARGUS" -eq 1 ]]; then + # nvargus-daemon enumerates sensors once at startup; a daemon that started + # before the drivers loaded reports an empty camera list until restarted. + echo "sensing-load: restarting nvargus-daemon" + systemctl restart nvargus-daemon +fi diff --git a/src/plugins/sensing/setup.sh b/src/plugins/sensing/setup.sh new file mode 100755 index 000000000..e9a98264a --- /dev/null +++ b/src/plugins/sensing/setup.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 +# +# Entry point for SENSING SG10A camera setup. Detects whether it is running on +# the Jetson host or inside a container and delegates to the right half; every +# argument is forwarded unchanged. +# +# Setup is genuinely two-sided: kernel modules, device-tree overlays and the +# POC/PWM register writes only exist on the host, while the Argus client socket +# and build headers only matter inside the container. Run it in both places. +# +# Usage: +# ./setup.sh [--host|--container] [options...] +# +# --host force the host path (see setup_host.sh --help) +# --container force the container path (see setup_container.sh --help) +# --verify run the read-only health report and exit +# +# With no flag the context is auto-detected. + +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck source=./_common.sh +source "$SCRIPT_DIR/_common.sh" + +usage() { sed -n '4,21p' "${BASH_SOURCE[0]}" | sed 's/^# \?//'; } + +TARGET=auto +case "${1:-}" in + --host) TARGET=host; shift ;; + --container) TARGET=container; shift ;; + --verify) shift; exec "$SCRIPT_DIR/verify.sh" "$@" ;; + -h|--help) usage; exit 0 ;; +esac + +if [[ "$TARGET" == auto ]]; then + if in_container; then TARGET=container; else TARGET=host; fi + info "detected context: $TARGET ${C_DIM}(override with --host / --container)${C_RESET}" +fi + +exec "$SCRIPT_DIR/setup_${TARGET}.sh" "$@" diff --git a/src/plugins/sensing/setup_container.sh b/src/plugins/sensing/setup_container.sh new file mode 100755 index 000000000..5b5de939e --- /dev/null +++ b/src/plugins/sensing/setup_container.sh @@ -0,0 +1,197 @@ +#!/usr/bin/env bash +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# CONTAINER-side setup for the SENSING SG10A rig. +# +# The drivers themselves live in the host kernel — this half only wires up what +# a container needs to *consume* them: the Argus client socket, v4l-utils, and +# the headers the camera_viz Argus source builds against. +# +# Usage: +# ./setup_container.sh [options] +# +# Options: +# --build-argus build the camera_viz native Argus module (needs an active venv) +# --argus-include D Argus header dir, if it is not in a standard location +# -y, --yes assume yes for every prompt +# -h, --help this text + +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +REPO_ROOT="$(cd "$SCRIPT_DIR/../../.." && pwd)" +# shellcheck source=./_common.sh +source "$SCRIPT_DIR/_common.sh" + +BUILD_ARGUS=0 +ASSUME_YES=0 + +usage() { sed -n '4,18p' "${BASH_SOURCE[0]}" | sed 's/^# \?//'; } + +while (( $# )); do + case "$1" in + --build-argus) BUILD_ARGUS=1; shift ;; + --argus-include) ARGUS_INCLUDE_DIR="$2"; export ARGUS_INCLUDE_DIR; shift 2 ;; + -y|--yes) ASSUME_YES=1; shift ;; + -h|--help) usage; exit 0 ;; + *) die "unknown argument: $1" "Run $0 --help" ;; + esac +done +export ASSUME_YES + +in_container || die "this script is for the CONTAINER side." \ + "You appear to be on the host — run setup_host.sh instead." + +printf '%sSENSING container setup%s\n' "$C_BOLD" "$C_RESET" + +BLOCKED=0 + +# --- 1. Video nodes --------------------------------------------------------- +# Driver loading is a host-kernel operation; nothing here can fix its absence. +step "Video nodes from the host" +mapfile -t NODES < <(sensing_video_nodes) +if [[ "${#NODES[@]}" -gt 0 ]]; then + ok "${#NODES[@]} node(s) visible: $(printf 'video%s ' "${NODES[@]}")" +else + bad "no /dev/video* visible in this container" + hint "On the HOST: $REPO_ROOT/src/plugins/sensing/setup_host.sh" + hint "If the host has them but this container does not, the container needs --device /dev or -v /dev:/dev" + BLOCKED=1 +fi + +# --- 2. Argus socket -------------------------------------------------------- +# libnvargus_socketclient talks to nvargus-daemon over this UNIX socket. A +# container that bind-mounts /tmp/.X11-unix still gets its own /tmp, so the +# socket has to be mounted explicitly. +step "Argus daemon socket" +if [[ -S /tmp/argus_socket ]]; then + ok "/tmp/argus_socket present" +else + bad "/tmp/argus_socket is not visible in this container" + hint "Argus capture (and argus_camera) cannot connect without it." + printf '\n %sdocker run:%s add\n' "$C_BOLD" "$C_RESET" + hint ' -v /tmp/argus_socket:/tmp/argus_socket' + printf ' %sdevcontainer.json:%s add to "runArgs", next to the X11 mount\n' "$C_BOLD" "$C_RESET" + hint ' "-v", "/tmp/argus_socket:/tmp/argus_socket"' + printf ' then rebuild the container. On the host, confirm the socket exists:\n' + hint ' ls -l /tmp/argus_socket || sudo systemctl restart nvargus-daemon' + BLOCKED=1 +fi + +# --- 3. v4l-utils ----------------------------------------------------------- +step "v4l-utils" +if have v4l2-ctl; then + ok "v4l2-ctl present" +else + info "v4l2-ctl is needed to read/set trig_mode and sensor_mode." + require_sudo "apt-get install v4l-utils inside this container" + if confirm "Install v4l-utils now?"; then + sudo apt-get update -qq && sudo apt-get install -y v4l-utils + ok "v4l-utils installed" + else + warn "skipped — trig_mode checks will be unavailable" + fi +fi + +# --- 4. Argus build prerequisites ------------------------------------------ +# Both camera_viz/argus/build.sh and camera_viz/scripts/_install_deps.sh hardcode +# /usr/src/jetson_multimedia_api/argus — build.sh does not forward an override to +# CMake, and _install_deps.sh skips the build outright when that path is absent. +# So a header tree found anywhere else gets symlinked into place rather than +# passed as a flag. +JMA_ARGUS=/usr/src/jetson_multimedia_api/argus +step "Argus build prerequisites" +if ARGUS_INC="$(find_argus_include)"; then + ok "headers: $ARGUS_INC" + if [[ ! -d "$JMA_ARGUS" ]]; then + argus_tree="$(dirname "$ARGUS_INC")" + warn "camera_viz expects them at $JMA_ARGUS" + require_sudo "symlink $JMA_ARGUS -> $argus_tree" + if confirm "Create that symlink?"; then + sudo mkdir -p "$(dirname "$JMA_ARGUS")" + sudo ln -sfn "$argus_tree" "$JMA_ARGUS" + ok "linked $JMA_ARGUS -> $argus_tree" + else + warn "declined — 'camera_viz.sh setup --with-argus' will skip the Argus build" + fi + fi +else + ARGUS_INC="" + bad "Argus/Argus.h not found" + hint "Containers rarely carry the L4T apt repo, so nvidia-l4t-jetson-multimedia-api" + hint "is usually not installable here. Either mount the host copy:" + hint " -v /usr/src/jetson_multimedia_api:/usr/src/jetson_multimedia_api:ro" + hint "or copy that tree in and pass --argus-include /argus/include" +fi + +if find /usr/lib -maxdepth 3 -name 'libnvargus_socketclient.so*' 2>/dev/null | grep -q .; then + ok "libnvargus_socketclient.so present" +else + bad "libnvargus_socketclient.so not found — the NVIDIA container runtime should provide it" + BLOCKED=1 +fi +[[ -f /usr/include/EGL/egl.h ]] && ok "EGL headers present" \ + || { bad "EGL headers missing"; hint "sudo apt-get install -y libegl1-mesa-dev"; } +if have nvcc || [[ -x /usr/local/cuda/bin/nvcc ]]; then + ok "nvcc: $("${CUDA_PATH:-/usr/local/cuda}/bin/nvcc" --version 2>/dev/null | sed -n 's/.*release \([0-9.]*\).*/CUDA \1/p' | tail -1)" +else + bad "nvcc not found"; hint "Expected at /usr/local/cuda/bin/nvcc" +fi + +# --- 5. Native Argus module ------------------------------------------------- +# examples/camera_viz/argus/ arrives with the Argus camera source (PR #833). +# Absent it, camera_viz can still drive the YUV SHF3L nodes via type: v4l2. +ARGUS_SRC="$REPO_ROOT/examples/camera_viz/argus" +step "camera_viz native Argus module" +if [[ ! -d "$ARGUS_SRC" ]]; then + info "examples/camera_viz/argus not in this checkout — 'type: argus' unavailable." + hint "It lands with the Argus camera support PR; until then use 'type: v4l2' for the SHF3L nodes." +elif [[ "$BUILD_ARGUS" -eq 0 ]]; then + info "present but not built (pass --build-argus)" +elif [[ -z "$ARGUS_INC" ]]; then + bad "cannot build without Argus headers — see above" + BLOCKED=1 +elif [[ -z "${VIRTUAL_ENV:-}" ]]; then + bad "no active venv; build.sh requires one" + hint "source $REPO_ROOT/examples/camera_viz/.venv/bin/activate" + BLOCKED=1 +else + info "building against $ARGUS_INC" + "$ARGUS_SRC/build.sh" \ + && ok "native Argus module built" \ + || { bad "build failed"; BLOCKED=1; } +fi + +# --- 6. argus_camera on PATH ------------------------------------------------ +# The vendor's smoke test. /usr/local/bin is container-local, so a copy the host +# installed there is invisible here; a build in the mounted home is not. +step "argus_camera" +if have argus_camera; then + ok "argus_camera on PATH" +else + # The Argus sample tree nests it at argus/build/apps/camera/ui/camera/, so + # search deep — but only under likely roots, not all of $HOME. + found="$(find "$HOME/Sensing" "$REPO_ROOT" /usr/local/bin \ + -maxdepth 9 -type f -name argus_camera -perm -u+x 2>/dev/null | head -1)" + if [[ -n "$found" ]]; then + info "not on PATH, but built at: $found" + if confirm "Symlink it into ~/.local/bin?"; then + mkdir -p "$HOME/.local/bin" && ln -sf "$found" "$HOME/.local/bin/argus_camera" + ok "linked ~/.local/bin/argus_camera" + fi + else + info "not built in this container — optional, only a smoke-test tool" + hint "Build it from the jetson_multimedia_api argus tree, or run it on the host." + fi +fi + +step "Verifying" +"$SCRIPT_DIR/verify.sh" || true + +printf '\n' +if [[ "$BLOCKED" -eq 0 ]]; then + printf '%s%sContainer setup complete.%s\n' "$C_GREEN" "$C_BOLD" "$C_RESET" +else + printf '%s%sContainer setup incomplete — see the actions above.%s\n' "$C_YELLOW" "$C_BOLD" "$C_RESET" +fi diff --git a/src/plugins/sensing/setup_host.sh b/src/plugins/sensing/setup_host.sh new file mode 100755 index 000000000..1ad57d8cc --- /dev/null +++ b/src/plugins/sensing/setup_host.sh @@ -0,0 +1,175 @@ +#!/usr/bin/env bash +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# HOST-side setup for the SENSING SG10A GMSL rig (Astra S56C + SHF3L) on an +# AGX Orin running JetPack 6.2 / L4T R36.4.3. +# +# Kernel modules, device-tree overlays and PWM/POC register writes only exist on +# the host, so this must not run in a container — see setup_container.sh for the +# other half. +# +# Usage: +# ./setup_host.sh [options] +# +# Options: +# --pkg DIR vendor driver package dir (default: autodetect, or $SENSING_PKG_DIR) +# --fps N trigger PWM frame rate: 10|15|20|30|60 (default 30) +# --free-run force trig_mode=0 on every sensor (default) +# --trigger-sync keep the vendor trigger mode; needs the J19 pin 2<->4 strap +# --install-drivers also run the vendor install.sh (Image + DTBO + ISP); needs a reboot +# --service install and enable the boot-time loader unit without asking +# --no-service never touch systemd +# -y, --yes assume yes for every prompt +# -h, --help this text + +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck source=./_common.sh +source "$SCRIPT_DIR/_common.sh" + +PKG_DIR="" +FPS=30 +FREE_RUN=1 +INSTALL_DRIVERS=0 +SERVICE_MODE=ask +ASSUME_YES=0 +SERVICE_NAME=sensing-camera +SERVICE_TEMPLATE="$SCRIPT_DIR/scripts/${SERVICE_NAME}.service.in" + +usage() { sed -n '4,25p' "${BASH_SOURCE[0]}" | sed 's/^# \?//'; } + +while (( $# )); do + case "$1" in + --pkg) PKG_DIR="$2"; shift 2 ;; + --fps) FPS="$2"; shift 2 ;; + --free-run) FREE_RUN=1; shift ;; + --trigger-sync) FREE_RUN=0; shift ;; + --install-drivers) INSTALL_DRIVERS=1; shift ;; + --service) SERVICE_MODE=yes; shift ;; + --no-service) SERVICE_MODE=no; shift ;; + -y|--yes) ASSUME_YES=1; shift ;; + -h|--help) usage; exit 0 ;; + *) die "unknown argument: $1" "Run $0 --help" ;; + esac +done +export ASSUME_YES + +case "$FPS" in 10|15|20|30|60) ;; *) die "--fps must be one of 10 15 20 30 60 (got '$FPS')" ;; esac + +# --- Preconditions ---------------------------------------------------------- +if in_container; then + die "this script must run on the HOST, not inside a container." \ + "Open a host terminal and run: $SCRIPT_DIR/setup_host.sh + Inside the container, run setup_container.sh instead." +fi +[[ "$(uname -m)" == "aarch64" ]] || die "this rig is Jetson-only (found $(uname -m) )." + +[[ -n "$PKG_DIR" ]] || PKG_DIR="$(find_sensing_pkg)" +[[ -n "$PKG_DIR" && -f "$PKG_DIR/load_modules.sh" ]] || die \ + "SENSING driver package not found." \ + "Download $SENSING_PKG_GLOB from + https://github.com/SENSING-Technology/nvidia-jetson-camera-drivers + then re-run with --pkg /path/to/package (or set SENSING_PKG_DIR)." +PKG_DIR="$(cd "$PKG_DIR" && pwd)" + +printf '%sSENSING host setup%s\n' "$C_BOLD" "$C_RESET" +info "package: $PKG_DIR" +info "trigger: $([[ "$FREE_RUN" -eq 1 ]] && echo "free run (trig_mode=0)" || echo "vendor sync @ ${FPS} Hz")" + +SUDO_REASONS=( + "insmod the SENSING sensor drivers (s56c-shw3gc.ko, sgx-yuv-gmsl2.ko, pwm-gpio.ko)" + "devmem writes that enable camera power-over-coax and the PWM trigger pin" + "restarting nvargus-daemon so Argus re-enumerates the sensors" +) +[[ "$INSTALL_DRIVERS" -eq 0 ]] || SUDO_REASONS+=( + "installing the vendor kernel Image, device-tree overlay and ISP tuning file") +[[ "$SERVICE_MODE" == no ]] || SUDO_REASONS+=( + "writing /etc/systemd/system/${SERVICE_NAME}.service (you will be asked first)") +[[ "$(command -v v4l2-ctl)" ]] || SUDO_REASONS+=("apt-get install v4l-utils") +require_sudo "${SUDO_REASONS[@]}" + +# --- 1. Vendor install.sh (optional; needs a reboot) ------------------------ +if [[ "$INSTALL_DRIVERS" -eq 1 ]]; then + step "Installing vendor kernel Image, DTBO and ISP tuning" + warn "This overwrites /boot/Image and wipes /var/nvidia/nvcam/settings/." + if confirm "Proceed with the vendor install.sh?"; then + ( cd "$PKG_DIR" && sudo ./install.sh ) + ok "vendor artifacts installed" + printf '\n%s%sReboot required.%s Select the overlay first:\n' "$C_YELLOW" "$C_BOLD" "$C_RESET" + hint "sudo /opt/nvidia/jetson-io/jetson-io.py" + hint " Configure Jetson AGX CSI Connector" + hint " -> Jetson Sensing SG10A_AGON_G2M_A1 S56Cx1 SHF3Lx6 -> Save and reboot" + printf 'Re-run this script (without --install-drivers) after the reboot.\n' + exit 0 + fi + warn "skipped vendor install.sh" +fi + +# --- 2. Overlay must already be live --------------------------------------- +step "Checking the live device tree" +DT_MODULES=/proc/device-tree/tegra-camera-platform/modules +[[ -d "$DT_MODULES" ]] || die \ + "the SENSING device-tree overlay is not applied." \ + "Run: $0 --install-drivers then select the overlay in jetson-io and reboot." +ok "overlay applied ($(find "$DT_MODULES" -mindepth 1 -maxdepth 1 -name 'module*' | wc -l) camera modules)" + +# --- 3. Load the sensor drivers -------------------------------------------- +# install.sh never copies the sensor .ko files into /lib/modules, so nothing +# auto-loads them. This is the step that has to happen on every boot. +step "Loading sensor drivers (${FPS} Hz trigger)" +have v4l2-ctl || sudo apt-get install -y v4l-utils +LOADER="$SCRIPT_DIR/scripts/sensing-load.sh" +[[ -x "$LOADER" ]] || die "loader not found or not executable: $LOADER" + +LOADER_ARGS=(--pkg "$PKG_DIR" --fps "$FPS" --restart-argus) +[[ "$FREE_RUN" -eq 0 ]] || LOADER_ARGS+=(--free-run) +sudo "$LOADER" "${LOADER_ARGS[@]}" + +mapfile -t NODES < <(sensing_video_nodes) +[[ "${#NODES[@]}" -gt 0 ]] || die \ + "drivers loaded but no /dev/video* nodes appeared." \ + "Check camera power and cabling, then: sudo dmesg | grep -iE 's56|sgx|max96'" +ok "${#NODES[@]} video node(s): $(printf 'video%s ' "${NODES[@]}")" + +if [[ "$FREE_RUN" -eq 0 ]]; then + warn "Sensors are slaved to the PWM trigger. Without the J19 pin 2<->4 strap they will never deliver a frame." +fi + +for _ in $(seq 1 20); do [[ -S /tmp/argus_socket ]] && break; sleep 0.25; done +[[ -S /tmp/argus_socket ]] && ok "/tmp/argus_socket ready" \ + || warn "/tmp/argus_socket did not appear — check: systemctl status nvargus-daemon" + +# --- 6. Persist across reboots --------------------------------------------- +install_service=0 +case "$SERVICE_MODE" in + yes) install_service=1 ;; + no) info "systemd unit skipped (--no-service)" ;; + ask) + step "Persisting the driver load across reboots" + info "Without this, /dev/video* disappears on every reboot until load_modules.sh is re-run." + confirm "Install and enable ${SERVICE_NAME}.service?" && install_service=1 + ;; +esac + +if [[ "$install_service" -eq 1 ]]; then + [[ -f "$SERVICE_TEMPLATE" ]] || die "service template missing: $SERVICE_TEMPLATE" + unit="/etc/systemd/system/${SERVICE_NAME}.service" + free_run_flag="" + [[ "$FREE_RUN" -eq 0 ]] || free_run_flag=" --free-run" + sed -e "s|{{LOADER}}|$LOADER|g" \ + -e "s|{{PKG_DIR}}|$PKG_DIR|g" \ + -e "s|{{FPS}}|$FPS|g" \ + -e "s|{{FREE_RUN}}|$free_run_flag|g" \ + "$SERVICE_TEMPLATE" | sudo tee "$unit" >/dev/null + sudo systemctl daemon-reload + sudo systemctl enable "${SERVICE_NAME}.service" + ok "installed and enabled $unit" +fi + +step "Verifying" +"$SCRIPT_DIR/verify.sh" || true + +printf '\n%s%sHost setup complete.%s\n' "$C_GREEN" "$C_BOLD" "$C_RESET" +printf 'Next: run %ssetup_container.sh%s inside the devcontainer.\n' "$C_BOLD" "$C_RESET" diff --git a/src/plugins/sensing/tools/ipc_testsrc.cu b/src/plugins/sensing/tools/ipc_testsrc.cu new file mode 100644 index 000000000..81df7b56d --- /dev/null +++ b/src/plugins/sensing/tools/ipc_testsrc.cu @@ -0,0 +1,196 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 +// +// Animated RGBA test pattern on the CUDA IPC socket, with no camera attached. +// +// Lets the consumer side be developed and tested with no camera attached. Also +// the quickest way to tell a broken consumer from a broken camera: if the +// pattern animates here and the camera does not, the fault is upstream of the +// IPC. +// +// ./sensing_ipc_testsrc --socket=/tmp/sensing0.sock --width=1920 --height=1080 +// ./camera_viz.sh run configs/cuda_ipc.yaml + +#include "../core/cuda_ipc_publisher.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace +{ + +std::sig_atomic_t volatile g_stop = 0; + +void on_signal(int) +{ + g_stop = 1; +} + +/// Scrolling colour bars plus a box tracking the frame counter, so a stale or +/// torn frame is obvious on screen rather than merely plausible. +__global__ void test_pattern(uint8_t* out, int width, int height, size_t pitch, float t, unsigned frame) +{ + 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 float u = static_cast(x) / width; + const float v = static_cast(y) / height; + + uint8_t r = static_cast(255.0f * fminf(1.0f, fmaxf(0.0f, 0.5f + 0.5f * __sinf(6.2831f * (u + t))))); + uint8_t g = static_cast(255.0f * v); + uint8_t b = static_cast(255.0f * fminf(1.0f, fmaxf(0.0f, 0.5f + 0.5f * __cosf(6.2831f * (v - t))))); + + // A white square orbiting the centre: any duplicated frame freezes it. + const float cx = 0.5f + 0.3f * __cosf(6.2831f * t); + const float cy = 0.5f + 0.3f * __sinf(6.2831f * t); + if (fabsf(u - cx) < 0.04f && fabsf(v - cy) < 0.04f * width / height) + { + r = g = b = 255; + } + + // Top-left binary readout of the frame counter, 16 bits, 24px cells. + if (y < 24 && x < 16 * 24) + { + const unsigned bit = static_cast(x / 24); + const bool on = (frame >> (15u - bit)) & 1u; + r = g = b = on ? 255 : 0; + } + + uint8_t* px = out + static_cast(y) * pitch + static_cast(x) * 4; + px[0] = r; + px[1] = g; + px[2] = b; + px[3] = 255; +} + +uint64_t monotonic_ns() +{ + timespec ts{}; + clock_gettime(CLOCK_MONOTONIC, &ts); + return static_cast(ts.tv_sec) * 1'000'000'000ull + static_cast(ts.tv_nsec); +} + +bool match(const std::string& arg, const char* key, std::string& value) +{ + const std::string prefix = std::string("--") + key + "="; + if (arg.rfind(prefix, 0) != 0) + return false; + value = arg.substr(prefix.size()); + return true; +} + +} // namespace + +int main(int argc, char** argv) +try +{ + plugins::sensing::CudaIpcConfig config; + config.socket_path = "/tmp/sensing_cuda0.sock"; + double fps = 30.0; + + for (int i = 1; i < argc; ++i) + { + std::string arg = argv[i]; + std::string value; + if (arg == "--help" || arg == "-h") + { + std::cout << "Usage: " << argv[0] << " [--socket=PATH] [--width=N] [--height=N]\n" + << " [--fps=N] [--sensor=N] [--gpu-id=N] [--slots=N]\n"; + return 0; + } + else if (match(arg, "socket", value)) + config.socket_path = value; + else if (match(arg, "width", value)) + config.width = static_cast(std::stoul(value)); + else if (match(arg, "height", value)) + config.height = static_cast(std::stoul(value)); + else if (match(arg, "sensor", value)) + config.sensor_id = static_cast(std::stoul(value)); + else if (match(arg, "gpu-id", value)) + config.gpu_id = std::stoi(value); + else if (match(arg, "slots", value)) + config.slot_count = static_cast(std::stoul(value)); + else if (match(arg, "fps", value)) + fps = std::stod(value); + else + { + std::cerr << "Unknown option: " << arg << std::endl; + return 1; + } + } + if (fps <= 0.0) + fps = 30.0; + + std::signal(SIGINT, on_signal); + std::signal(SIGTERM, on_signal); + + plugins::sensing::CudaIpcPublisher publisher(config); + + // Scratch frame standing in for the camera's converted RGBA output, so the + // publisher's copy path is exercised exactly as it is in the plugin. + uint8_t* scratch = nullptr; + size_t scratch_pitch = 0; + if (cudaMallocPitch(&scratch, &scratch_pitch, static_cast(config.width) * 4, config.height) != cudaSuccess) + throw std::runtime_error("cudaMallocPitch failed"); + + const dim3 block(16, 16); + const dim3 grid((config.width + block.x - 1) / block.x, (config.height + block.y - 1) / block.y); + const auto period = std::chrono::nanoseconds(static_cast(1e9 / fps)); + + std::cout << "Test source running at " << fps << " fps. Ctrl+C to stop." << std::endl; + + auto next = std::chrono::steady_clock::now(); + auto last_report = std::chrono::steady_clock::now(); + unsigned frame = 0; + + while (!g_stop) + { + publisher.poll(); + + if (publisher.has_consumer()) + { + const float t = static_cast(frame % 120) / 120.0f; + test_pattern<<>>(scratch, config.width, config.height, scratch_pitch, t, frame); + if (cudaGetLastError() != cudaSuccess) + throw std::runtime_error("test_pattern launch failed"); + cudaStreamSynchronize(0); + publisher.publish(reinterpret_cast(scratch), scratch_pitch, monotonic_ns()); + ++frame; + } + + auto now = std::chrono::steady_clock::now(); + if (now - last_report >= std::chrono::seconds(5)) + { + std::cout << " published=" << publisher.published_count() << " dropped=" << publisher.dropped_count() + << (publisher.has_consumer() ? " (consumer attached)" : " (waiting for consumer)") << std::endl; + last_report = now; + } + + next += period; + if (next > now) + std::this_thread::sleep_until(next); + else + next = now; // Fell behind; do not spiral trying to catch up. + } + + std::cout << "\nStopped. published=" << publisher.published_count() << " dropped=" << publisher.dropped_count() + << std::endl; + cudaFree(scratch); + return 0; +} +catch (const std::exception& e) +{ + std::cerr << argv[0] << ": " << e.what() << std::endl; + return 1; +} diff --git a/src/plugins/sensing/verify.sh b/src/plugins/sensing/verify.sh new file mode 100755 index 000000000..72f6a5d25 --- /dev/null +++ b/src/plugins/sensing/verify.sh @@ -0,0 +1,148 @@ +#!/usr/bin/env bash +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# Health report for the SENSING GMSL rig. Read-only, no sudo, safe to run from +# the host or from inside a container — it reports what the current context can +# actually see and names the script that fixes each gap. +# +# Exit: 0 all good, 1 something is broken. + +set -uo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck source=./_common.sh +source "$SCRIPT_DIR/_common.sh" + +FAILED=0 +fail() { bad "$1"; [[ -z "${2:-}" ]] || hint "$2"; FAILED=1; } + +usage() { sed -n '4,9p' "${BASH_SOURCE[0]}" | sed 's/^# \?//'; } +[[ "${1:-}" != "--help" && "${1:-}" != "-h" ]] || { usage; exit 0; } + +if in_container; then CONTEXT=container; else CONTEXT=host; fi +printf '%sSENSING rig report%s %s(context: %s)%s\n' \ + "$C_BOLD" "$C_RESET" "$C_DIM" "$CONTEXT" "$C_RESET" + +# --- Platform --------------------------------------------------------------- +step "Platform" +KERNEL="$(uname -r)" +if [[ -r /etc/nv_tegra_release ]]; then + ok "L4T: $(sed -n '1s/^# //p' /etc/nv_tegra_release)" +else + warn "/etc/nv_tegra_release not readable — cannot confirm the L4T release" +fi +# The vendor package ships prebuilt .ko files for exactly this kernel; a +# mismatch means insmod will fail with "invalid module format". +if [[ "$KERNEL" == "5.15.148-tegra" ]]; then + ok "kernel: $KERNEL" +else + fail "kernel is $KERNEL, package .ko files are built for 5.15.148-tegra" \ + "Reflash to JetPack 6.2 / L4T R36.4.3, or get a package matching this kernel." +fi + +# --- Device tree ------------------------------------------------------------ +step "Device tree overlay" +DT_MODULES=/proc/device-tree/tegra-camera-platform/modules +if [[ -d "$DT_MODULES" ]]; then + n_modules="$(find "$DT_MODULES" -mindepth 1 -maxdepth 1 -name 'module*' | wc -l)" + if [[ "$n_modules" -ge 10 ]]; then + ok "overlay applied — $n_modules camera modules in the live tree" + else + fail "only $n_modules camera modules in the live tree (expected 10)" \ + "The wrong DTBO is selected. Run: sudo /opt/nvidia/jetson-io/jetson-io.py" + fi +else + fail "no tegra-camera-platform modules in the live device tree" \ + "Run setup_host.sh --install-drivers, then select the overlay in jetson-io and reboot." +fi + +# --- Sensor drivers --------------------------------------------------------- +# The vendor install.sh never copies these into /lib/modules, so nothing +# auto-loads them; load_modules.sh insmods them from the package directory. +step "Sensor drivers" +for mod in s56c_shw3gc sgx_yuv_gmsl2; do + if grep -q "^${mod} " /proc/modules 2>/dev/null; then + ok "$mod loaded" + else + fail "$mod not loaded" "Run setup_host.sh on the HOST (not in a container)." + fi +done +for drv in s56-shw3g sgx-yuv-gmsl2; do + if [[ -d "/sys/bus/i2c/drivers/$drv" ]]; then + n_bound="$(find "/sys/bus/i2c/drivers/$drv" -maxdepth 1 -name '*-00*' | wc -l)" + [[ "$n_bound" -gt 0 ]] \ + && ok "$drv bound to $n_bound i2c device(s)" \ + || warn "$drv registered but bound to nothing — check camera power and cabling" + fi +done + +# --- Video nodes ------------------------------------------------------------ +step "Video nodes" +mapfile -t NODES < <(sensing_video_nodes) +if [[ "${#NODES[@]}" -eq 0 ]]; then + fail "no /dev/video* nodes" "Run setup_host.sh on the HOST to load the drivers." +else + ok "${#NODES[@]} node(s): $(printf 'video%s ' "${NODES[@]}")" + for want in "${S56C_NODES[@]}"; do + [[ -e "/dev/video$want" ]] || info "video$want (S56C) absent — normal if that port is empty" + done + for want in "${SHF3L_NODES[@]}"; do + [[ -e "/dev/video$want" ]] || info "video$want (SHF3L) absent — normal if that port is empty" + done +fi + +# --- Trigger mode ----------------------------------------------------------- +# load_modules.sh leaves the sensors slaved to the PWM trigger, which only +# fires when J19 pins 2 and 4 are strapped. Unstrapped, a sensor in that mode +# opens fine and then never delivers a frame — so flag it rather than let it +# look healthy. +step "Trigger mode" +if ! have v4l2-ctl; then + warn "v4l2-ctl not installed — cannot read trig_mode" + hint "sudo apt-get install -y v4l-utils (or run setup_container.sh)" +elif [[ "${#NODES[@]}" -gt 0 ]]; then + synced=0 + for i in "${NODES[@]}"; do + tm="$(v4l2-ctl -d "/dev/video$i" -C trig_mode 2>/dev/null | sed -n 's/^trig_mode:[[:space:]]*//p')" + case "$tm" in + 0) ok "video$i trig_mode=0 (free run)" ;; + "") info "video$i has no trig_mode control" ;; + *) warn "video$i trig_mode=$tm (external trigger)"; synced=1 ;; + esac + done + [[ "$synced" -eq 0 ]] || hint "No J19 pin 2<->4 strap? These will never produce a frame. Fix: setup_host.sh --free-run" +fi + +# --- Argus ------------------------------------------------------------------ +step "Argus" +if [[ -S /tmp/argus_socket ]]; then + ok "/tmp/argus_socket present" +else + if [[ "$CONTEXT" == container ]]; then + fail "/tmp/argus_socket not visible in this container" \ + "Bind-mount it: -v /tmp/argus_socket:/tmp/argus_socket (see setup_container.sh)" + else + fail "/tmp/argus_socket missing" "sudo systemctl restart nvargus-daemon" + fi +fi +if find /usr/lib -maxdepth 3 -name 'libnvargus_socketclient.so*' 2>/dev/null | grep -q .; then + ok "libnvargus_socketclient.so present" +else + fail "libnvargus_socketclient.so not found" "Install the L4T Argus runtime (nvidia-l4t-camera)." +fi +if ARGUS_INC="$(find_argus_include)" && [[ -n "$ARGUS_INC" ]]; then + ok "Argus headers: $ARGUS_INC" +else + warn "Argus headers not found — needed only to build the camera_viz argus source" + hint "Host: sudo apt-get install nvidia-l4t-jetson-multimedia-api" +fi + +# --- Summary ---------------------------------------------------------------- +printf '\n' +if [[ "$FAILED" -eq 0 ]]; then + printf '%s%s All checks passed.%s\n' "$C_GREEN" "$C_BOLD" "$C_RESET" +else + printf '%s%s Some checks failed — see the actions above.%s\n' "$C_RED" "$C_BOLD" "$C_RESET" +fi +exit "$FAILED"