From 46fb7bb11b1eaa5eed0b5f1c99226b758ae58db8 Mon Sep 17 00:00:00 2001 From: Jiwen Cai Date: Sat, 22 Aug 2026 00:32:44 +0000 Subject: [PATCH] sensing: add the SENSING SG10A GMSL camera plugin camera_plugin_sensing captures the Astra S56C through libargus on an AGX Orin and either encodes H.264 on the Jetson V4L2 M2M engine -- Jetson ships no libnvidia-encode -- or serves frames to another process as CUDA memory, which camera_viz consumes as `type: cuda_ipc`. The shape follows the OAK plugin: repeatable --add-stream, raw Annex-B H.264 per sensor, per-frame metadata to MCAP or an OpenXR schema push. Bring-up is two-sided and neither half can do the other's job, hence setup_host.sh for the kernel modules, the device-tree overlay and the POC/PWM writes, and setup_container.sh for the Argus socket and build headers. The recurring failure they exist to stop: the vendor install.sh never copies the sensor .ko files into /lib/modules, so /dev/video* is empty after every reboot. Two traps fail misleadingly and are called out in code and docs. Legacy CUDA IPC returns cudaSuccess on Tegra and only breaks in the far process, so the transport uses the VMM API over SCM_RIGHTS. And GLVND picks the EGL vendor from DISPLAY, so losing the probe to Mesa surfaces as EGL_NO_STREAM_KHR out of Argus; main() clears DISPLAY before the first EGL call. 1080p60 sustained, ~1 ms producer to consumer on an AGX Orin. Setup, recording and live streaming are documented in docs/source/device/sensing.rst. Signed-off-by: Jiwen Cai --- CMakeLists.txt | 5 + docs/source/device/sensing.rst | 357 +++++++ docs/source/index.rst | 1 + examples/camera_viz/README.md | 42 + examples/camera_viz/configs/cuda_ipc.yaml | 44 + examples/camera_viz/sources/__init__.py | 20 +- examples/camera_viz/sources/_cuda_vmm.py | 195 ++++ examples/camera_viz/sources/cuda_ipc.py | 415 +++++++++ .../camera_viz/tests/test_cuda_ipc_source.py | 253 +++++ src/core/schema/fbs/sensing.fbs | 31 + src/plugins/sensing/CMakeLists.txt | 227 +++++ src/plugins/sensing/README.md | 318 +++++++ src/plugins/sensing/_common.sh | 142 +++ src/plugins/sensing/configs/argus_s56c.yaml | 71 ++ src/plugins/sensing/core/argus_camera.cpp | 869 ++++++++++++++++++ src/plugins/sensing/core/argus_camera.hpp | 143 +++ .../sensing/core/cuda_ipc_protocol.hpp | 79 ++ .../sensing/core/cuda_ipc_publisher.cpp | 366 ++++++++ .../sensing/core/cuda_ipc_publisher.hpp | 125 +++ src/plugins/sensing/core/frame_sink.cpp | 212 +++++ src/plugins/sensing/core/frame_sink.hpp | 68 ++ src/plugins/sensing/core/jetson_encoder.cpp | 260 ++++++ src/plugins/sensing/core/jetson_encoder.hpp | 71 ++ src/plugins/sensing/core/rawdata_writer.cpp | 51 + src/plugins/sensing/core/rawdata_writer.hpp | 44 + src/plugins/sensing/core/rgba_to_nv12.cu | 103 +++ src/plugins/sensing/core/rgba_to_nv12.cuh | 36 + src/plugins/sensing/core/sensing_camera.cpp | 182 ++++ src/plugins/sensing/core/sensing_camera.hpp | 78 ++ src/plugins/sensing/core/sensing_types.hpp | 63 ++ src/plugins/sensing/core/yuv_to_rgba.cu | 214 +++++ src/plugins/sensing/core/yuv_to_rgba.cuh | 46 + src/plugins/sensing/main.cpp | 274 ++++++ src/plugins/sensing/plugin.yaml | 11 + .../sensing/scripts/sensing-camera.service.in | 24 + src/plugins/sensing/scripts/sensing-load.sh | 70 ++ src/plugins/sensing/setup.sh | 43 + src/plugins/sensing/setup_container.sh | 197 ++++ src/plugins/sensing/setup_host.sh | 175 ++++ src/plugins/sensing/tools/ipc_testsrc.cu | 196 ++++ src/plugins/sensing/verify.sh | 148 +++ 41 files changed, 6268 insertions(+), 1 deletion(-) create mode 100644 docs/source/device/sensing.rst create mode 100644 examples/camera_viz/configs/cuda_ipc.yaml create mode 100644 examples/camera_viz/sources/_cuda_vmm.py create mode 100644 examples/camera_viz/sources/cuda_ipc.py create mode 100644 examples/camera_viz/tests/test_cuda_ipc_source.py create mode 100644 src/core/schema/fbs/sensing.fbs create mode 100644 src/plugins/sensing/CMakeLists.txt create mode 100644 src/plugins/sensing/README.md create mode 100644 src/plugins/sensing/_common.sh create mode 100644 src/plugins/sensing/configs/argus_s56c.yaml create mode 100644 src/plugins/sensing/core/argus_camera.cpp create mode 100644 src/plugins/sensing/core/argus_camera.hpp create mode 100644 src/plugins/sensing/core/cuda_ipc_protocol.hpp create mode 100644 src/plugins/sensing/core/cuda_ipc_publisher.cpp create mode 100644 src/plugins/sensing/core/cuda_ipc_publisher.hpp create mode 100644 src/plugins/sensing/core/frame_sink.cpp create mode 100644 src/plugins/sensing/core/frame_sink.hpp create mode 100644 src/plugins/sensing/core/jetson_encoder.cpp create mode 100644 src/plugins/sensing/core/jetson_encoder.hpp create mode 100644 src/plugins/sensing/core/rawdata_writer.cpp create mode 100644 src/plugins/sensing/core/rawdata_writer.hpp create mode 100644 src/plugins/sensing/core/rgba_to_nv12.cu create mode 100644 src/plugins/sensing/core/rgba_to_nv12.cuh create mode 100644 src/plugins/sensing/core/sensing_camera.cpp create mode 100644 src/plugins/sensing/core/sensing_camera.hpp create mode 100644 src/plugins/sensing/core/sensing_types.hpp create mode 100644 src/plugins/sensing/core/yuv_to_rgba.cu create mode 100644 src/plugins/sensing/core/yuv_to_rgba.cuh create mode 100644 src/plugins/sensing/main.cpp create mode 100644 src/plugins/sensing/plugin.yaml create mode 100644 src/plugins/sensing/scripts/sensing-camera.service.in create mode 100755 src/plugins/sensing/scripts/sensing-load.sh create mode 100755 src/plugins/sensing/setup.sh create mode 100755 src/plugins/sensing/setup_container.sh create mode 100755 src/plugins/sensing/setup_host.sh create mode 100644 src/plugins/sensing/tools/ipc_testsrc.cu create mode 100755 src/plugins/sensing/verify.sh 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"