diff --git a/CMakeLists.txt b/CMakeLists.txt index f98d794ba..9e1d6b0cf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -63,7 +63,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)" OFF) +option(BUILD_PLUGIN_SENSING "Build SENSING GMSL camera plugin (Jetson only; requires SIPL + 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) diff --git a/docs/source/device/sensing.rst b/docs/source/device/sensing.rst index aa4038f71..5e4b45329 100644 --- a/docs/source/device/sensing.rst +++ b/docs/source/device/sensing.rst @@ -5,9 +5,10 @@ SENSING GMSL Camera =================== Bring-up for SENSING GMSL2 cameras on a Jetson AGX Orin, where capture goes -through **SIPL** rather than Argus or V4L2. This page covers provisioning the -rig and confirming it streams with the vendor's own tool; scripts live in -:code-file:`src/plugins/sensing`. +through **SIPL** rather than Argus or V4L2, and ``camera_plugin_sensing``, the +C++ plugin that captures from them — encoding H.264 on the Jetson's V4L2 engine +or handing frames to another process as CUDA device memory. Source and scripts +live in :code-file:`src/plugins/sensing`. .. contents:: On this page :local: @@ -385,6 +386,151 @@ capture: nvsipl_query -t query/sg8a_agth_g2a/shw5g.json -l # config names nvsipl_query -t query/sg8a_agth_g2a/shw5g.json -c SHW5G_2 +Building and running +-------------------- + +.. code-block:: bash + + cmake -B build -DBUILD_PLUGIN_SENSING=ON + cmake --build build --target camera_plugin_sensing + +The platform config is vendored at ``src/plugins/sensing/configs/shw5g.json`` +and resolved relative to the executable, so the plugin runs without the vendor +package on disk; ``--platform-config=PATH`` points it at a newer vendor drop. + +.. note:: + + The published package has **no** ``SHW5G_2`` config — only the mixed + populations. SENSING sends ``shw5g.json`` directly to customers with a + 2× SHW5G rig, so the vendored copy is the only one a fresh checkout has, and + ``setup.sh`` drops it into the fetched package for ``nvsipl_camera``. + +Start by asking the platform config what exists. This needs no hardware — the +query API only parses the driver database and the JSON, so it works with the +cameras unplugged: + +.. code-block:: bash + + ./build/src/plugins/sensing/app/camera_plugin_sensing --list-sensors + +.. code-block:: text + + SHW5G_2: 2 sensor(s) + sensor=0 SHW5G 2560x1984 @ 60 fps + sensor=1 SHW5G 2560x1984 @ 60 fps + +.. warning:: + + ``sensor=N`` is the **SIPL pipeline index**, and for ``SHW5G_2`` it is not + the number the JSON appears to name. The GMSL link indices, the CSI virtual + channels and the JSON's ``sensorInfo.id`` are all **2 and 3**; the pipeline + indices are **0 and 1**. Always take the number from ``--list-sensors``. + + In the vendor's ``S56C_1_SHF3L_2`` config the two coincide, which is exactly + why this is worth stating. + +Geometry comes from the platform config +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +There is no ``--width``, ``--height``, ``--fps`` or ``--sensor-mode``. SIPL has +no runtime mode index; resolution and frame rate are properties of the virtual +channel in the platform config, and the plugin sizes its buffers from what the +query reports. + +Encoding at 5 MP60 +~~~~~~~~~~~~~~~~~~ + +Two sensors at 2560×1984 @ 60 is 4.9× the pixel rate the 1080p30 defaults were +chosen for, so the codec settings moved with it: + +.. list-table:: + :widths: 25 20 55 + :header-rows: 1 + + * - Setting + - Value + - Why + * - level + - **5.2** + - 1,190,400 MB/s exceeds Level 5.1's 983,040 by 21% + * - ``--bitrate`` + - 40 Mbps + - 0.13 bits/pixel; 20 Mbps was 0.066 + * - ``--peak-bitrate`` + - 60 Mbps + - VBR ceiling; pass ``0`` to select CBR + +80 Mbps for the pair, about 36 GB/hour. + +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. + +.. code-block:: bash + + # terminal 1 — producer + ./build/src/plugins/sensing/app/camera_plugin_sensing \ + --add-stream=sensor=0,ipc=/tmp/sensing0.sock \ + --add-stream=sensor=1,ipc=/tmp/sensing1.sock + + # 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: left + type: cuda_ipc + socket: /tmp/sensing0.sock # must match the producer's ipc= path + width: 2560 # must match what the producer serves + height: 1984 + +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. + +Pairing the two eyes +~~~~~~~~~~~~~~~~~~~~ + +Both sensors are driven from one deserializer fsync generator, and every frame +carries SIPL's ``frameCaptureTSC`` on a timebase shared across the whole rig. +It is recorded in the MCAP metadata as ``capture_tsc_ns``. + +.. important:: + + Pair on ``capture_tsc_ns``, not on the wrapper timestamps. The wrapper stamps + are ``CLOCK_MONOTONIC`` taken after per-sensor conversion, so they carry each + sensor's own queueing jitter; the TSC does not. + +Testing without a camera +~~~~~~~~~~~~~~~~~~~~~~~~ + +``sensing_ipc_testsrc`` publishes an animated pattern over the same protocol. It +needs CUDA only — no SIPL, 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/tools/sensing_ipc_testsrc --socket=/tmp/sensing0.sock \ + --width=2560 --height=1984 --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. + Troubleshooting --------------- @@ -402,8 +548,12 @@ Troubleshooting - drivers were never installed — re-run ``setup.sh`` * - ``Failed to open file .../shw5g.json`` - the fetched package ships no ``SHW5G_2`` config; ``setup.sh`` drops one in + * - ``sensor=N is not a pipeline in ''`` + - using the link index or the JSON id; run ``--list-sensors`` * - capture hangs, no frames, no error - another SIPL client holds the hardware — stop ``nvsipl_camera`` + * - ``ISP0 reconciled to colour standard ...`` + - the ISP refused BT.601; see the note in ``sipl_camera.cpp`` * - ``Could not get EglImage from fd`` / ``Failed to create EGLImage`` - ``DISPLAY`` names an X server Tegra EGL cannot drive; see below @@ -421,4 +571,10 @@ The container defaults to ``DISPLAY=:99``, so anything that renders needs: .. code-block:: bash - export DISPLAY=:1 # nvsipl_camera --egl-display, or any viewer + export DISPLAY=:1 # nvsipl_camera --egl-display, camera_viz, any viewer + +``camera_plugin_sensing`` is unaffected: it never renders and calls +``unsetenv("DISPLAY")`` before its first EGL call, which selects the Tegra +driver regardless. Do not remove that because the capture path "has no +display" — ``NvBufSurfaceMapEglImage()`` is how a SIPL buffer reaches CUDA, so +EGL is on the critical path even headless. diff --git a/examples/camera_viz/README.md b/examples/camera_viz/README.md index dfa9dc57c..5bd4939d1 100644 --- a/examples/camera_viz/README.md +++ b/examples/camera_viz/README.md @@ -21,7 +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. See [below](#cuda_ipc-frames-from-another-process) | +| `cuda_ipc` | RGBA8 frames mapped straight out of another process's CUDA memory — no encode, no host copy. Pairs with the [sensing plugin](../../docs/source/device/sensing.rst); 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`. @@ -164,9 +164,18 @@ 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. -Any process can be the producer — the wire format is a 24-byte message and a -CUDA VMM handle over `SCM_RIGHTS`. An animated test pattern ships with it, so -the consumer can be developed and tested with no camera and no capture SDK: +Producer is the sensing plugin. Geometry comes from its platform config, not +from flags, and `sensor=` is the **SIPL pipeline index** — take it from +`--list-sensors`, not from the vendor JSON, where the same cameras are numbered +differently: + +```bash +camera_plugin_sensing --add-stream=sensor=0,ipc=/tmp/s0.sock \ + --add-stream=sensor=1,ipc=/tmp/s1.sock +``` + +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 diff --git a/src/core/schema/fbs/sensing.fbs b/src/core/schema/fbs/sensing.fbs new file mode 100644 index 000000000..5d782f304 --- /dev/null +++ b/src/core/schema/fbs/sensing.fbs @@ -0,0 +1,39 @@ +// 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 SIPL pipeline index rather than an enum: the populated +// set varies with the platform config the rig is running. +// All fields are always present when the parent Tracked/Record wrapper's data is non-null. +table FrameMetadataSensing { + // SIPL pipeline index, as INvSIPLCameraQuery reports it for the active + // platform config. This is NOT the GMSL link index, the CSI virtual channel, + // or the JSON sensorInfo.id -- for SHW5G_2 those are 2 and 3 while this is + // 0 and 1. + sensor_id: uint32 (id: 0); + + // Per-stream counter of records emitted for this sensor. Not a SIPL capture + // index: a stream with an encoder counts bitstream units, one per captured + // frame, and an ipc-only stream counts published frames. + sequence_number: uint64 (id: 1); + + // SIPL frameCaptureTSC: capture time on the Tegra TSC timebase, which every + // sensor on the rig shares because the deserializer drives them from one + // fsync generator. This is the field that lets a consumer pair the two eyes + // of a stereo rig; the wrapper timestamps cannot, since they are stamped + // after per-sensor conversion. Zero when the encoder dropped the frame this + // record's stamp came from. + capture_tsc_ns: uint64 (id: 2); +} + +// MCAP recording wrapper for FrameMetadataSensing. +table FrameMetadataSensingRecord { + data: FrameMetadataSensing (id: 0); + timestamp: DeviceDataTimestamp (id: 1); +} + +root_type FrameMetadataSensingRecord; diff --git a/src/plugins/CMakeLists.txt b/src/plugins/CMakeLists.txt index eee6873f3..490b09e04 100644 --- a/src/plugins/CMakeLists.txt +++ b/src/plugins/CMakeLists.txt @@ -35,7 +35,7 @@ endif() if(BUILD_PLUGIN_WUJI_GLOVE) add_subdirectory(wuji_glove) endif() -# Gated: Jetson only -- the Tegra CUDA and NVENC entry points are L4T-only. +# Gated: Jetson only -- SIPL and the Tegra NVENC entry point are L4T-only. if(BUILD_PLUGIN_SENSING) add_subdirectory(sensing) endif() diff --git a/src/plugins/sensing/CMakeLists.txt b/src/plugins/sensing/CMakeLists.txt index 81b2a1fa5..a168711c0 100644 --- a/src/plugins/sensing/CMakeLists.txt +++ b/src/plugins/sensing/CMakeLists.txt @@ -2,15 +2,21 @@ # SPDX-License-Identifier: Apache-2.0 # ============================================================================== -# SENSING camera plugin — CUDA IPC transport +# SENSING Camera Plugin (Jetson SIPL capture + V4L2 H.264 encode) # ============================================================================== -# Two leaf targets, one per directory (cmake/cmake-structure.md rule 1): -# cuda_ipc/ sensing_cuda_ipc — publisher + wire format, CUDA only -# tools/ sensing_ipc_testsrc — synthetic producer for the same protocol +# Jetson-only: SIPL and the V4L2 M2M encoder exist only on L4T. # -# Neither needs a camera, a capture SDK or an encoder: the transport is CUDA and -# a Unix socket. Keep the capture-side SDK checks in the directories that need -# them so this half stays buildable on its own. +# Four leaf targets, one per directory (cmake/cmake-structure.md rule 1): +# cuda_ipc/ sensing_cuda_ipc — CUDA-only IPC publisher + wire format +# core/ sensing_core — SIPL capture, encoder, sinks +# app/ camera_plugin_sensing +# tools/ sensing_ipc_testsrc — links cuda_ipc only, no SIPL, no encoder +# +# The Jetson SDK checks below gate the whole plugin, tools/ included. +# +# Prerequisites: +# src/plugins/sensing/setup_container.sh # fetches the Multimedia API, +# # locates jetson_sipl_api # # Configure with: # cmake -B build -DBUILD_PLUGIN_SENSING=ON @@ -25,5 +31,147 @@ endif() find_package(CUDAToolkit REQUIRED) +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() + +# ============================================================================== +# SIPL +# ============================================================================== +# Headers ship in Jetson_SIPL_API_R_aarch64.tbz2, published alongside the +# L4T release. Not in any apt pool, so setup_container.sh fetches it by URL. +if(NOT SIPL_API_DIR) + find_path(SIPL_API_DIR sipl/include/NvSIPLCamera.hpp + PATHS "/usr/src/jetson_sipl_api" + "/opt/nvidia/jetson_sipl_api" + "$ENV{SIPL_API_DIR}" + ) +endif() + +if(NOT SIPL_API_DIR) + message(FATAL_ERROR + "================================================================================\n" + "SENSING camera plugin requires the Jetson SIPL API headers.\n" + "\n" + "Run the container setup, which fetches them:\n" + " src/plugins/sensing/setup_container.sh\n" + "\n" + "Or point CMake at an existing copy:\n" + " cmake -B build -DSIPL_API_DIR=/path/to/jetson_sipl_api\n" + "\n" + "Or disable this plugin:\n" + " cmake -B build -DBUILD_PLUGIN_SENSING=OFF\n" + "================================================================================\n" + ) +endif() + +set(SIPL_INCLUDE_DIRS + "${SIPL_API_DIR}/sipl/include" + "${SIPL_API_DIR}/sipl/include/query/include" + "${SIPL_API_DIR}/sipl/include/nvsci" +) +message(STATUS "SENSING plugin: SIPL=${SIPL_API_DIR}") + +# L4T ships no .so dev symlinks for these, hence the explicit sonames. +foreach(_lib nvsipl nvsipl_query nvscibuf nvscisync nvbufsurface_nvsci) + string(TOUPPER "${_lib}" _upper) + find_library(${_upper}_LIBRARY + NAMES ${_lib} lib${_lib}.so.1 lib${_lib}.so.1.0.0 + PATHS ${_tegra_lib_dirs} + ) + if(NOT ${_upper}_LIBRARY) + message(FATAL_ERROR + "lib${_lib} not found. It is supplied by the L4T BSP and, in a container, by the " + "NVIDIA container runtime. Run src/plugins/sensing/verify.sh to see what is missing.") + endif() + list(APPEND SIPL_LIBRARIES ${${_upper}_LIBRARY}) +endforeach() + +# EGL stays on the capture path: NvBufSurfaceMapEglImage is how a SIPL +# NvSciBufObj becomes a CUeglFrame. Interop path B in the design notes would +# remove this; path A does not. +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" + "setup_container.sh fetches the version matching /etc/nv_tegra_release:\n" + " src/plugins/sensing/setup_container.sh\n" + "\n" + "Or point CMake at an existing tree:\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}") + +# No libnvjpeg here on purpose: on R39 libnvbufsurface has no libjpeg +# dependency at all (readelf -d lists only EGL, cuda, dl, pthread, rt, c), and +# the tegra lib dir ships no libnvjpeg to link against. + +# ============================================================================== +# Targets +# ============================================================================== +message(STATUS "Building SENSING camera plugin") + add_subdirectory(cuda_ipc) +add_subdirectory(core) +add_subdirectory(app) add_subdirectory(tools) + +install(FILES + "${CMAKE_CURRENT_SOURCE_DIR}/plugin.yaml" + "${CMAKE_CURRENT_SOURCE_DIR}/README.md" + DESTINATION plugins/sensing_camera +) diff --git a/src/plugins/sensing/app/CMakeLists.txt b/src/plugins/sensing/app/CMakeLists.txt new file mode 100644 index 000000000..4d94874f1 --- /dev/null +++ b/src/plugins/sensing/app/CMakeLists.txt @@ -0,0 +1,25 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +add_executable(camera_plugin_sensing main.cpp) + +set_target_properties(camera_plugin_sensing PROPERTIES INSTALL_RPATH "$ORIGIN") + +target_link_libraries(camera_plugin_sensing PRIVATE sensing::core) + +# The plugin resolves configs/shw5g.json relative to its own executable, so the +# config has to sit beside the binary in the build tree as well as the install +# tree -- otherwise it only works after `cmake --install`. +add_custom_command(TARGET camera_plugin_sensing POST_BUILD + COMMAND ${CMAKE_COMMAND} -E copy_directory + "${CMAKE_CURRENT_SOURCE_DIR}/../configs" + "$/configs" + COMMENT "Staging SIPL platform configs beside camera_plugin_sensing" +) + +install(TARGETS camera_plugin_sensing + RUNTIME DESTINATION plugins/sensing_camera +) +install(DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/../configs/" + DESTINATION plugins/sensing_camera/configs +) diff --git a/src/plugins/sensing/app/main.cpp b/src/plugins/sensing/app/main.cpp new file mode 100644 index 000000000..6f64f0c19 --- /dev/null +++ b/src/plugins/sensing/app/main.cpp @@ -0,0 +1,366 @@ +// 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 "core/sipl_camera.hpp" + +#include +#include +#include +#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); + } +} + +// ============================================================================= +// Argument parsers +// ============================================================================= + +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; +} + +/// "0x0000 0x1100" -> {0x0000, 0x1100}. Same spelling as nvsipl_camera -m, so a +/// working vendor command line can be copied across verbatim. +static std::vector parse_link_masks(const std::string& arg) +{ + std::vector masks; + std::istringstream ss(arg); + std::string token; + while (ss >> token) + { + masks.push_back(static_cast(std::stoul(token, nullptr, 0))); + } + if (masks.empty()) + throw std::runtime_error("--link-masks needs at least one mask, e.g. --link-masks='0x0000 0x1100'"); + return masks; +} + +/// The vendored platform config, resolved next to the executable so it works +/// from the build tree and the install tree alike, whatever the cwd. Empty when +/// it is not there, which leaves --platform-config required. +static std::string default_platform_config() +{ + char buf[PATH_MAX]; + const ssize_t n = ::readlink("/proc/self/exe", buf, sizeof(buf) - 1); + if (n <= 0) + return {}; + buf[n] = '\0'; + const std::string exe(buf); + const auto slash = exe.rfind('/'); + if (slash == std::string::npos) + return {}; + const std::string candidate = exe.substr(0, slash) + "/configs/shw5g.json"; + return ::access(candidate.c_str(), R_OK) == 0 ? candidate : std::string{}; +} + +// ============================================================================= +// 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: SIPL pipeline index, as --list-sensors reports it. This is NOT\n" + << " the GMSL link index or the JSON sensorInfo.id.\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" + << "\nPlatform (resolution and frame rate come from here, not from flags):\n" + << " --platform-config=PATH platform JSON (default: configs/shw5g.json beside this\n" + << " binary; point it at the vendor package to test a\n" + << " newer driver drop)\n" + << " --config-name=NAME named config inside it (default: SHW5G_2)\n" + << " --link-masks='M M' one mask per deserializer (default: '0x0000 0x1100')\n" + << " --nito-dir=DIR ISP tuning directory (default: /var/nvidia/nvcam/settings/sipl)\n" + << " --list-sensors print what the platform config resolves to, then exit\n" + << "\nEncoding:\n" + << " --bitrate=N H.264 average bitrate in bps (default: 40000000)\n" + << " --peak-bitrate=N VBR ceiling in bps; 0 selects CBR (default: 60000000)\n" + << " --gop=N IDR period in frames (default: fps*5)\n" + << "\nGeneral:\n" + << " --gpu-id=N CUDA device index (default: 0)\n" + << " --isp0-buffers=N ISP0 buffers per sensor (default: 6)\n" + << " --full-range Treat luma as full range instead of broadcast range\n" + << " --swap-uv Swap the chroma planes\n" + << " --mcap-filename=PATH Record per-frame metadata to an MCAP file\n" + << " --help Show this help message\n" + << "\nExamples:\n" + << " " << program_name << " --list-sensors\n" + << " " << program_name << " --add-stream=sensor=0,ipc=/tmp/sensing0.sock \\\n" + << " --add-stream=sensor=1,ipc=/tmp/sensing1.sock\n"; +} + +// ============================================================================= +// Main +// ============================================================================= + +int main(int argc, char** argv) +try +{ + SensingConfig camera_config; + std::map stream_map; + std::string mcap_filename; + bool list_sensors = false; + + 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)); + // Overwriting would silently drop the earlier stream's output= or + // ipc=; both belong in one --add-stream for a given sensor. + if (!stream_map.emplace(cfg.sensor_id, cfg).second) + { + throw std::runtime_error("sensor " + std::to_string(cfg.sensor_id) + + " given twice; put output= and ipc= in a single --add-stream"); + } + } + else if (arg.find("--platform-config=") == 0) + { + camera_config.platform_config_json = arg.substr(18); + } + else if (arg.find("--config-name=") == 0) + { + camera_config.platform_config_name = arg.substr(14); + } + else if (arg.find("--link-masks=") == 0) + { + camera_config.link_masks = parse_link_masks(arg.substr(13)); + } + else if (arg.find("--nito-dir=") == 0) + { + camera_config.nito_dir = arg.substr(11); + } + else if (arg == "--list-sensors") + { + list_sensors = true; + } + else if (arg.find("--bitrate=") == 0) + { + camera_config.bitrate_bps = static_cast(std::stoul(arg.substr(10))); + } + else if (arg.find("--peak-bitrate=") == 0) + { + camera_config.peak_bitrate_bps = static_cast(std::stoul(arg.substr(15))); + } + 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.find("--isp0-buffers=") == 0) + { + camera_config.isp0_buffers = static_cast(std::stoul(arg.substr(15))); + } + else if (arg == "--full-range") + { + camera_config.full_range = true; + } + else if (arg == "--swap-uv") + { + camera_config.swap_uv = true; + } + 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 (camera_config.platform_config_json.empty()) + { + camera_config.platform_config_json = default_platform_config(); + } + if (camera_config.platform_config_json.empty()) + { + std::cerr << "Error: no platform config. configs/shw5g.json was not found beside this\n" + "binary, so pass --platform-config=PATH. SIPL cannot enumerate sensors\n" + "without one." + << std::endl; + print_usage(argv[0]); + return 1; + } + + // Query is hardware-free -- it only parses the driver database and the JSON + // -- so --list-sensors works on a rig whose cameras are unplugged, and the + // poll interval below can be derived before anything is opened. + const auto sensors = SiplCamera::query(camera_config.platform_config_json, + camera_config.platform_config_name, camera_config.link_masks); + + if (list_sensors) + { + std::cout << camera_config.platform_config_name << ": " << sensors.size() << " sensor(s)\n"; + for (const auto& s : sensors) + { + std::cout << " sensor=" << s.id << " " << s.name << " " << s.width << "x" << s.height << " @ " + << s.fps << " fps\n"; + } + return 0; + } + + 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: + // NvBufSurfaceMapEglImage() is how a SIPL NvSciBufObj reaches CUDA, and + // libnvbufsurface resolves its own display via + // eglGetDisplay(EGL_DEFAULT_DISPLAY). GLVND hands out Mesa's EGL rather than + // Tegra's whenever DISPLAY names an X server Tegra EGL cannot drive (Xvfb, + // X11 forwarding), and that choice cannot be made per-call -- DISPLAY 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 (SIPL)" << std::endl; + std::cout << "============================================================" << std::endl; + + SensingCamera camera(camera_config, stream_configs, create_frame_sink(stream_configs, 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(); + + // SiplCamera::latest() is a non-blocking mailbox read, so poll at roughly + // twice the fastest sensor's frame rate rather than spinning a core flat + // out. Capture itself runs on SIPL's own threads and is unaffected. + double fastest_fps = 30.0; + for (const auto& s : sensors) + { + fastest_fps = std::max(fastest_fps, s.fps); + } + const auto poll_interval = std::chrono::microseconds(static_cast(500'000.0 / fastest_fps)); + + 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/core/CMakeLists.txt b/src/plugins/sensing/core/CMakeLists.txt new file mode 100644 index 000000000..3b4c856fd --- /dev/null +++ b/src/plugins/sensing/core/CMakeLists.txt @@ -0,0 +1,57 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# SIPL capture, the V4L2 M2M encoder wrapper, and the output sinks. Everything +# that needs the Jetson SDKs lives here, so tools/ stays buildable without them. +add_library(sensing_core STATIC + sensing_camera.cpp + frame_sink.cpp + rawdata_writer.cpp + sipl_camera.cpp + yuv_to_rgba.cu + jetson_encoder.cpp + rgba_to_nv12.cu + ${JETSON_MMAPI_SOURCES} +) +add_library(sensing::core ALIAS sensing_core) + +# RESOLVE_DEVICE_SYMBOLS as well as SEPARABLE_COMPILATION: without it the +# device-link step only happens for consumers in a directory where CUDA is an +# enabled language, so linking sensing_core from tests/ fails on +# __cudaRegisterLinkedBinary_*. Resolving here makes the archive self-contained. +set_target_properties(sensing_core PROPERTIES + CUDA_SEPARABLE_COMPILATION ON + CUDA_RESOLVE_DEVICE_SYMBOLS ON +) + +target_include_directories(sensing_core + PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/.. + PRIVATE + # sipl_camera.hpp pimpls the SIPL and NvSci types away, so these stay + # private: they drag in NvMedia and X11-style macros that collide with + # mcap's StatusCode, which is what sensing_types.hpp exists to avoid. + ${SIPL_INCLUDE_DIRS} + ${EGL_INCLUDE_DIRS} + ${JETSON_MMAPI_DIR}/include +) + +# The Multimedia API reference sources carry legacy warnings. +target_compile_options(sensing_core PRIVATE + $<$:-Wno-reorder -Wno-uninitialized -Wno-unused-variable + -Wno-unused-parameter -Wno-missing-field-initializers -Wno-sign-compare> +) + +target_link_libraries(sensing_core + PUBLIC + # sensing_types.hpp is public and includes the generated schema header. + sensing::cuda_ipc + isaacteleop_schema + PRIVATE + ${SIPL_LIBRARIES} + ${EGL_LIBRARIES} + ${NVBUFSURFACE_LIBRARY} + ${V4L2_LIBRARY} + mcap::mcap + oxr::oxr_utils +) diff --git a/src/plugins/sensing/core/frame_sink.cpp b/src/plugins/sensing/core/frame_sink.cpp new file mode 100644 index 000000000..370b5ae26 --- /dev/null +++ b/src/plugins/sensing/core/frame_sink.cpp @@ -0,0 +1,154 @@ +// 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 + +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 no H.264 to write. SensingCamera still calls + // on_frame for it, so its metadata is recorded. + 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); +} + +// ============================================================================= +// 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& mcap_filename) +{ + std::unique_ptr pusher; + 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..76c40cccd --- /dev/null +++ b/src/plugins/sensing/core/frame_sink.hpp @@ -0,0 +1,64 @@ +// 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. + * + * McapMetadataPusher is the only implementation; the interface stays because + * src/plugins/oak/core/frame_sink.hpp has the same shape and the two are + * candidates to hoist into plugin_utils together. + */ +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. + * + * Writes raw H.264 per sensor that asked for it, and optionally delegates to an + * IMetadataPusher. A stream with no output path still reaches on_frame, so its + * metadata is recorded even though it has no bitstream. + */ +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 Create a FrameSink, attaching an MCAP pusher when a filename is given. + */ +std::unique_ptr create_frame_sink(const std::vector& streams, 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..39b6a9c73 --- /dev/null +++ b/src/plugins/sensing/core/jetson_encoder.cpp @@ -0,0 +1,290 @@ +// 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); + // V4L2 M2M copies the OUTPUT buffer's timestamp onto the CAPTURE + // buffer it produces, so this unit carries its own frame's stamp + // rather than whatever was submitted most recently. + EncodedUnit unit; + unit.data.assign(data, data + buffer->planes[0].bytesused); + unit.timestamp_ns = static_cast(v4l2_buf->timestamp.tv_sec) * 1000000000LL + + static_cast(v4l2_buf->timestamp.tv_usec) * 1000LL; + std::lock_guard lock(self->mutex); + self->ready.push_back(std::move(unit)); + } + + // 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"); + + // Level is not inferred reliably, and 2560x1984@60 is 1,190,400 macroblocks + // per second against Level 5.1's ceiling of 983,040 -- 21% over. An unset + // or too-low level encodes fine here and then gets refused downstream. + check(enc->setLevel(V4L2_MPEG_VIDEO_H264_LEVEL_5_2), "setLevel"); + + if (config.peak_bitrate_bps > config.bitrate_bps) + { + check(enc->setRateControlMode(V4L2_MPEG_VIDEO_BITRATE_MODE_VBR), "setRateControlMode"); + check(enc->setPeakBitrate(config.peak_bitrate_bps), "setPeakBitrate"); + } + else + { + 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, int64_t timestamp_ns) +{ + 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; + + // Microsecond granularity is all a timeval carries; that is well below the + // frame interval and keeps each unit attributed to its own capture. + v4l2_buf.timestamp.tv_sec = static_cast(timestamp_ns / 1000000000LL); + v4l2_buf.timestamp.tv_usec = static_cast((timestamp_ns % 1000000000LL) / 1000LL); + + check(enc->output_plane.qBuffer(v4l2_buf, nullptr), "output qBuffer"); + return true; +} + +EncodedUnit 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; +} + +EncodedUnit 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); + + EncodedUnit out; + std::lock_guard lock(m_impl->mutex); + // The tail is concatenated, so it carries the stamp of the last unit in it. + for (auto& chunk : m_impl->ready) + { + out.data.insert(out.data.end(), chunk.data.begin(), chunk.data.end()); + out.timestamp_ns = chunk.timestamp_ns; + } + 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..da3742ca7 --- /dev/null +++ b/src/plugins/sensing/core/jetson_encoder.hpp @@ -0,0 +1,88 @@ +// 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 = 40'000'000; + /// VBR ceiling; 0 keeps CBR. The H.264 output is a recording, not a + /// fixed-rate transport, so CBR would pad static scenes and starve motion. + uint32_t peak_bitrate_bps = 60'000'000; + uint32_t fps = 30; + /// IDR period in frames; 0 defers to fps*5. + uint32_t gop = 0; + bool full_range = false; +}; + +/// One encoded Annex-B unit and the capture stamp of the frame it came from. +struct EncodedUnit +{ + std::vector data; + int64_t timestamp_ns = 0; + + bool empty() const + { + return data.empty(); + } +}; + +/** + * @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. + * @param timestamp_ns Capture stamp, carried on the V4L2 output buffer and + * returned by poll() on the unit this frame produces. + * @return false when no input buffer is free (frame dropped). + */ + bool submit(uintptr_t rgba_device_ptr, std::size_t row_pitch_bytes, int64_t timestamp_ns); + + /** @brief Take any completed unit; empty during encoder warmup. */ + EncodedUnit poll(); + + /** @brief Signal EOS and drain the remaining bitstream. */ + EncodedUnit 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..a296be618 --- /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 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..e90788c46 --- /dev/null +++ b/src/plugins/sensing/core/sensing_camera.cpp @@ -0,0 +1,239 @@ +// 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 +#include +#include + +namespace plugins +{ +namespace sensing +{ + +namespace +{ + +SiplConfig make_sipl_config(const SensingConfig& config) +{ + SiplConfig sipl{}; + sipl.platform_config_json = config.platform_config_json; + sipl.platform_config_name = config.platform_config_name; + sipl.link_masks = config.link_masks; + sipl.nito_dir = config.nito_dir; + sipl.gpu_id = config.gpu_id; + sipl.full_range = config.full_range; + sipl.swap_uv = config.swap_uv; + sipl.isp0_buffers = config.isp0_buffers; + return sipl; +} + +EncoderConfig make_encoder_config(const SensingConfig& config, const SensorInfo& sensor) +{ + EncoderConfig encoder{}; + encoder.width = sensor.width; + encoder.height = sensor.height; + encoder.bitrate_bps = config.bitrate_bps; + encoder.peak_bitrate_bps = config.peak_bitrate_bps; + encoder.fps = static_cast(sensor.fps + 0.5); + 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_camera = std::make_unique(make_sipl_config(config)); + const auto& sensors = m_camera->sensors(); + + m_streams.reserve(streams.size()); + for (const auto& stream_config : streams) + { + const auto found = std::find_if(sensors.begin(), sensors.end(), [&](const SensorInfo& s) { + return s.id == stream_config.sensor_id; + }); + if (found == sensors.end()) + { + std::ostringstream oss; + oss << "sensor=" << stream_config.sensor_id << " is not a pipeline in '" + << config.platform_config_name << "'. Available:"; + for (const auto& s : sensors) + oss << ' ' << s.id; + oss << ". Note this is the SIPL pipeline index, not the GMSL link index."; + throw std::runtime_error(oss.str()); + } + + Stream stream; + stream.sensor_id = found->id; + stream.width = found->width; + stream.height = found->height; + + if (!stream_config.output_path.empty()) + stream.encoder = std::make_unique(make_encoder_config(config, *found)); + + if (!stream_config.ipc_socket_path.empty()) + { + CudaIpcConfig ipc{}; + ipc.socket_path = stream_config.ipc_socket_path; + ipc.width = found->width; + ipc.height = found->height; + ipc.sensor_id = found->id; + ipc.gpu_id = config.gpu_id; + stream.publisher = std::make_unique(ipc); + } + + m_streams.push_back(std::move(stream)); + + std::cout << "Sensor " << found->id << " (" << found->name << "): " << found->width << "x" << found->height + << " @ " << found->fps << " fps" << std::endl; + } + + m_camera->start(); +} + +SensingCamera::~SensingCamera() +{ + if (m_camera) + m_camera->stop(); +} + +int64_t SensingCamera::take_capture_tsc(Stream& stream, int64_t timestamp_ns) +{ + // The encoder returns units in submission order, so the match is at the + // front. Anything older than the match was dropped by the encoder and is + // discarded with it, which is what keeps this bounded. + while (!stream.pending_stamps.empty()) + { + auto entry = stream.pending_stamps.front(); + stream.pending_stamps.pop_front(); + if (entry.first == timestamp_ns) + return entry.second; + } + return 0; +} + +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 = m_camera->latest(stream.sensor_id); + if (view.has_value() && (!stream.have_last_sequence || view->sequence != stream.last_sequence)) + { + if (stream.have_last_sequence && view->sequence > stream.last_sequence + 1) + stream.missed_captures += view->sequence - stream.last_sequence - 1; + stream.last_sequence = view->sequence; + stream.have_last_sequence = true; + + // 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->ptr, view->pitch, view->timestamp_ns); + + if (stream.encoder) + { + if (stream.encoder->submit(view->ptr, view->pitch, static_cast(view->timestamp_ns))) + { + stream.pending_stamps.emplace_back(static_cast(view->timestamp_ns), + static_cast(view->capture_tsc_ns)); + } + else + { + ++stream.encoder_drops; + } + } + else + { + // No encoder means nothing would ever reach the sink, so an + // ipc-only stream emits its metadata here instead. + dispatch(stream, {}, static_cast(view->timestamp_ns), + static_cast(view->capture_tsc_ns)); + } + } + + // Submission and output are decoupled: the V4L2 encoder needs several + // input frames before it emits the first unit, so drain independently. + // Each unit carries its own frame's stamp back from the output plane. + if (stream.encoder) + { + for (auto unit = stream.encoder->poll(); !unit.empty(); unit = stream.encoder->poll()) + { + const int64_t tsc = take_capture_tsc(stream, unit.timestamp_ns); + dispatch(stream, std::move(unit.data), unit.timestamp_ns, tsc); + } + } + } +} + +void SensingCamera::flush() +{ + for (auto& stream : m_streams) + { + if (!stream.encoder) + continue; + auto unit = stream.encoder->end_of_stream(); + if (!unit.empty()) + { + const int64_t tsc = take_capture_tsc(stream, unit.timestamp_ns); + dispatch(stream, std::move(unit.data), unit.timestamp_ns, tsc); + } + } +} + +void SensingCamera::dispatch(Stream& stream, std::vector h264, int64_t timestamp_ns, int64_t capture_tsc_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; + frame.metadata.capture_tsc_ns = static_cast(capture_tsc_ns); + + // CLOCK_MONOTONIC, stamped at YUV->RGBA conversion, so it includes capture + // and convert latency. The device clock behind it is SIPL's frameCaptureTSC, + // on a timebase every sensor on the rig shares -- that is the one a consumer + // must pair the two eyes on. + frame.sample_time_local_common_clock_ns = timestamp_ns; + frame.sample_time_raw_device_clock_ns = capture_tsc_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.missed_captures) + std::cout << " | " << stream.missed_captures << " captures missed"; + if (stream.encoder_drops) + std::cout << " | " << stream.encoder_drops << " encoder drops"; + 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..71b47d527 --- /dev/null +++ b/src/plugins/sensing/core/sensing_camera.hpp @@ -0,0 +1,91 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "jetson_encoder.hpp" +#include "sensing_types.hpp" +#include "sipl_camera.hpp" + +#include + +#include +#include +#include +#include + +namespace plugins +{ +namespace sensing +{ + +class FrameSink; + +/** + * @brief Multi-sensor SENSING camera manager. + * + * One SiplCamera owns every sensor, because INvSIPLCamera::GetInstance() is + * process-wide -- there is no per-sensor capture object to hold. 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. + * + * Geometry is per sensor and comes from the platform config, not from the CLI. + */ +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; + uint32_t width = 0; + uint32_t height = 0; + /// 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; + /// SIPL publish counter of the frame already handled; skips re-reads. + uint64_t last_sequence = 0; + bool have_last_sequence = false; + uint64_t frame_count = 0; + /// Frames the encoder refused because no input buffer was free. + uint64_t encoder_drops = 0; + /// Captures the poll loop never saw, from gaps in the SIPL sequence. + uint64_t missed_captures = 0; + /// (monotonic, TSC) for frames submitted to the encoder but not yet + /// emitted. The encoder returns the monotonic stamp it was given, which + /// is how a delayed unit recovers the capture TSC of its own frame + /// rather than of whatever frame happens to be current. + std::deque> pending_stamps; + }; + + void dispatch(Stream& stream, std::vector h264, int64_t timestamp_ns, int64_t capture_tsc_ns); + int64_t take_capture_tsc(Stream& stream, int64_t timestamp_ns); + + SensingConfig m_config; + std::unique_ptr m_camera; + 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..b2f29a92f --- /dev/null +++ b/src/plugins/sensing/core/sensing_types.hpp @@ -0,0 +1,78 @@ +// 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 SIPL, NvSci 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 +{ + /// SIPL pipeline index, validated against the platform config at startup. + 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 +{ + /// Vendor platform config JSON. Required -- SIPL cannot enumerate without + /// one, and there is no sensible default path. + std::string platform_config_json; + /// Named config inside it. + std::string platform_config_name = "SHW5G_2"; + /// One mask per deserializer, in transport order. + std::vector link_masks{ 0x0000, 0x1100 }; + std::string nito_dir = "/var/nvidia/nvcam/settings/sipl"; + int gpu_id = 0; + + /// 0.13 bits/pixel at 2560x1984@60. The old 20 Mbps default was chosen for + /// 1080p30 and carries 4.9x the pixels at the same rate. + uint32_t bitrate_bps = 40'000'000; + /// VBR ceiling. Ignored under CBR. + uint32_t peak_bitrate_bps = 60'000'000; + /// IDR period in frames; 0 defers to the encoder default of fps*5. + uint32_t gop = 0; + + bool full_range = false; + /// Defaults off: set_sipl_buf_attributes() requests the ISP0 surface + /// order explicitly, so it does not need discovering from the frame. + bool swap_uv = false; + /// ISP0 buffers per sensor, ~6 MB each at 2560x1984 NV12. + uint32_t isp0_buffers = 6; +}; + +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; + /// SIPL frameCaptureTSC. Shared across sensors, unlike the host stamp. + int64_t sample_time_raw_device_clock_ns = 0; +}; + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/sipl_camera.cpp b/src/plugins/sensing/core/sipl_camera.cpp new file mode 100644 index 000000000..5b43d2028 --- /dev/null +++ b/src/plugins/sensing/core/sipl_camera.cpp @@ -0,0 +1,1218 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 +// +// Portions adapted from NVIDIA's sipl_coe_unit_sample +// (/usr/src/jetson_multimedia_api/samples/unittest_samples/), BSD-3-Clause, +// Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. + +#include "sipl_camera.hpp" + +#include "yuv_to_rgba.cuh" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace sensing +{ +namespace +{ + +using nvsipl::INvSIPLCamera; +using nvsipl::INvSIPLCameraQuery; +using nvsipl::INvSIPLClient; +using nvsipl::NVSIPL_STATUS_OK; +using nvsipl::SIPLStatus; + +constexpr auto kIsp0 = INvSIPLClient::ConsumerDesc::OutputType::ISP0; +constexpr auto kIcp = INvSIPLClient::ConsumerDesc::OutputType::ICP; + +/// SIPL allocates and registers ICP capture buffers unconditionally, even when +/// only an ISP output is consumed -- CNvSIPLMaster::AllocateBuffers hardcodes +/// the ICP slot to true. Skip them and RegisterImages fails with a bare +/// NVSIPL_STATUS_ERROR that names nothing. +constexpr uint32_t kIcpBuffers = 6; + +uint64_t monotonic_ns() +{ + return static_cast( + std::chrono::duration_cast(std::chrono::steady_clock::now().time_since_epoch()) + .count()); +} + +/// SIPL reports almost every failure as NVSIPL_STATUS_ERROR (10), which says +/// nothing on its own -- the reason only appears in its own log, above a +/// threshold that defaults to silent. SENSING_SIPL_TRACE=1..4 raises it +/// (1=error, 2=warn, 3=info, 4=debug). +void init_sipl_trace() +{ + const char* level = std::getenv("SENSING_SIPL_TRACE"); + if (level == nullptr) + { + return; + } + const int value = std::atoi(level); + if (value <= 0) + { + return; + } + auto* trace = nvsipl::INvSIPLTrace::GetInstance(); + if (trace != nullptr) + { + trace->SetLevel(static_cast(value)); + } +} + +void check_sipl(SIPLStatus status, const char* what) +{ + if (status != NVSIPL_STATUS_OK) + { + std::ostringstream oss; + oss << what << " failed with SIPLStatus " << static_cast(status); + throw std::runtime_error(oss.str()); + } +} + +void check_sci(NvSciError err, const char* what) +{ + if (err != NvSciError_Success) + { + std::ostringstream oss; + oss << what << " failed with NvSciError " << static_cast(err); + throw std::runtime_error(oss.str()); + } +} + +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()); +} + +cudaTextureObject_t texture_for_array(CUarray array); + +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()); + } +} + +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; +} + +/** + * @brief Buffer attributes for the ISP0 output. + * + * Block-linear and REC709_ER are what the ISP produces, not what we would + * prefer. Neither is negotiable: GetImageAttributes rejects an attribute list + * asking for pitch-linear or BT.601 with NVSIPL_STATUS_BAD_ARGUMENT. So the + * conversion adapts to the ISP rather than the other way round -- convert() + * takes the CUDA-array path, and yuv_to_rgba.cu is called with bt709=true. + * `nvsipl_camera -v 3` prints the same thing: "YUV 420 SEMI-PLANAR UINT8 BL + * REC_709ER". + */ +NvSciError set_sipl_buf_attributes(NvSciBufAttrList attr_list) +{ + NvSciBufType buf_type = NvSciBufType_Image; + NvSciBufAttrValAccessPerm access_perm = NvSciBufAccessPerm_ReadWrite; + // Must be true, and it is not about us wanting CPU access. NvmmImageFill- + // NvSciBufAttrList sets NeedCpuAccess=true on the NvBufSurface list, so + // reconciliation ORs it in; if the producer list says false, SIPL's + // GetVerifiedBufferAttributes sees a producer/reconciled mismatch and + // RegisterImages fails with a bare NVSIPL_STATUS_ERROR. + bool cpu_access = true; + bool cpu_cache = true; + NvSciBufSurfMemLayout mem_layout = NvSciSurfMemLayout_SemiPlanar; + NvSciBufAttrValImageLayoutType layout_type = NvSciBufImage_BlockLinearType; + NvSciBufSurfType surf_type = NvSciSurfType_YUV; + NvSciBufSurfSampleType sample_type = NvSciSurfSampleType_420; + NvSciBufSurfBPC surf_bpc = NvSciSurfBPC_8; + NvSciBufSurfComponentOrder comp_order = NvSciSurfComponentOrder_YUV; + NvSciBufAttrValColorStd color_std[] = { NvSciColorStd_REC709_ER }; + + NvSciBufAttrKeyValuePair kvp[] = { + { NvSciBufGeneralAttrKey_Types, &buf_type, sizeof(buf_type) }, + { NvSciBufGeneralAttrKey_RequiredPerm, &access_perm, sizeof(access_perm) }, + { NvSciBufGeneralAttrKey_NeedCpuAccess, &cpu_access, sizeof(cpu_access) }, + { NvSciBufGeneralAttrKey_EnableCpuCache, &cpu_cache, sizeof(cpu_cache) }, + { NvSciBufImageAttrKey_Layout, &layout_type, sizeof(layout_type) }, + { NvSciBufImageAttrKey_SurfType, &surf_type, sizeof(surf_type) }, + { NvSciBufImageAttrKey_SurfMemLayout, &mem_layout, sizeof(mem_layout) }, + { NvSciBufImageAttrKey_SurfSampleType, &sample_type, sizeof(sample_type) }, + { NvSciBufImageAttrKey_SurfBPC, &surf_bpc, sizeof(surf_bpc) }, + { NvSciBufImageAttrKey_SurfComponentOrder, &comp_order, sizeof(comp_order) }, + { NvSciBufImageAttrKey_SurfColorStd, &color_std, sizeof(color_std) }, + }; + + return NvSciBufAttrListSetAttrs(attr_list, kvp, sizeof(kvp) / sizeof(kvp[0])); +} + + +/// Fill a second attribute list with the NvBufSurface attributes matching the +/// SIPL list, so the allocated buffer satisfies both consumers. Without it the +/// buffer is allocated for SIPL alone and RegisterImages rejects it with +/// NVSIPL_STATUS_INVALID_STATE -- a status whose only documented meaning is +/// "pipeline had seen an init error", which sends you looking in the wrong +/// place entirely. +/// +/// Adapted from SetNvBufNvSciBufAttributes in NVIDIA's NvNvSciBufHelper.cpp +/// (BSD-3-Clause). Its colour table has exactly two entries, both REC709_ER; +/// that is the whole set this ISP path can produce. +NvSciError set_nvbuf_attributes(NvSciBufAttrList nvbuf_attrs, NvSciBufAttrList sipl_attrs, int gpu_id) +{ + NvSciBufAttrKeyValuePair img[] = { + { NvSciBufImageAttrKey_SurfWidthBase, nullptr, 0 }, + { NvSciBufImageAttrKey_SurfHeightBase, nullptr, 0 }, + { NvSciBufImageAttrKey_SurfSampleType, nullptr, 0 }, + { NvSciBufImageAttrKey_SurfColorStd, nullptr, 0 }, + { NvSciBufImageAttrKey_SurfMemLayout, nullptr, 0 }, + { NvSciBufImageAttrKey_SurfBPC, nullptr, 0 }, + { NvSciBufImageAttrKey_Layout, nullptr, 0 }, + }; + const NvSciError err = NvSciBufAttrListGetAttrs(sipl_attrs, img, sizeof(img) / sizeof(img[0])); + if (err != NvSciError_Success) + { + return err; + } + + const auto width = *static_cast(img[0].value); + const auto height = *static_cast(img[1].value); + const auto sample = *static_cast(img[2].value); + const auto color = *static_cast(img[3].value); + const auto mem = *static_cast(img[4].value); + const auto bpc = *static_cast(img[5].value); + const auto layout = *static_cast(img[6].value); + + NvBufSurfaceColorFormat pixfmt; + if (color == NvSciColorStd_REC709_ER && sample == NvSciSurfSampleType_420 && bpc == NvSciSurfBPC_8 && + mem == NvSciSurfMemLayout_SemiPlanar) + { + pixfmt = NVBUF_COLOR_FORMAT_NV12_709_ER; + } + else if (color == NvSciColorStd_REC709_ER && sample == NvSciSurfSampleType_420 && + bpc == NvSciSurfBPC_8 && mem == NvSciSurfMemLayout_Planar) + { + pixfmt = NVBUF_COLOR_FORMAT_YUV420_709_ER; + } + else + { + std::cerr << "[sipl] no NvBufSurface colour format for colourStd=" << static_cast(color) + << " sample=" << static_cast(sample) << " bpc=" << static_cast(bpc) + << " memLayout=" << static_cast(mem) << std::endl; + return NvSciError_BadParameter; + } + + NvmmImageParams params{}; + params.gpuId = static_cast(gpu_id); + params.width = width; + params.height = height; + params.colorFormat = pixfmt; + params.layout = (layout == NvSciBufImage_PitchLinearType) ? NVBUF_LAYOUT_PITCH : NVBUF_LAYOUT_BLOCK_LINEAR; + params.displayscanformat = NVBUF_DISPLAYSCANFORMAT_PROGRESSIVE; + params.isProtected = false; + + return (NvmmImageFillNvSciBufAttrList(¶ms, nvbuf_attrs) == 0) ? NvSciError_Success + : NvSciError_BadParameter; +} + +/// Read back what reconciliation settled on, and report whether it is extended +/// range. _ER is full range (0-255), _SR is studio range (16-235); decoding one +/// as the other subtracts a black level that is not there and crushes shadows, +/// which on a dim scene loses most of the image. Returns true for full range. +bool check_color_std(NvSciBufAttrList reconciled) +{ + NvSciBufAttrKeyValuePair query{ NvSciBufImageAttrKey_SurfColorStd, nullptr, 0 }; + if (NvSciBufAttrListGetAttrs(reconciled, &query, 1) != NvSciError_Success || query.value == nullptr) + { + std::cerr << "[sipl] warning: could not read back SurfColorStd; assuming BT.709 extended range" + << std::endl; + return true; + } + const auto* value = static_cast(query.value); + if (*value != NvSciColorStd_REC709_ER && *value != NvSciColorStd_REC709_SR) + { + std::ostringstream oss; + oss << "ISP0 reconciled to colour standard " << static_cast(*value) + << "; yuv_to_rgba.cu implements BT.601 and BT.709 only, and decoding anything else as" + " 709 would be a silent hue error"; + throw std::runtime_error(oss.str()); + } + return *value == NvSciColorStd_REC709_ER; +} + +std::vector load_nito(const std::string& dir, const std::string& module) +{ + const std::string path = dir + "/" + module + ".nito"; + std::ifstream in(path, std::ios::binary | std::ios::ate); + if (!in) + { + throw std::runtime_error("cannot open NITO file " + path + + " (the ISP has no tuning; run the vendor install.sh on the host)"); + } + const auto size = static_cast(in.tellg()); + in.seekg(0); + std::vector blob(static_cast(size)); + if (!in.read(reinterpret_cast(blob.data()), size)) + { + throw std::runtime_error("short read on NITO file " + path); + } + return blob; +} + +/// Walk a SensorSystemConfig into flat SensorInfo records. The nesting is +/// module -> variant -> sensorConfigs -> variant, and `id` is the pipeline +/// index SetPipelineCfg wants. +std::vector flatten(const nvsipl::sensorconfig::SensorSystemConfig& cfg) +{ + std::vector out; + for (const auto& module : cfg.modules) + { + std::visit( + [&](const auto& mod) { + for (const auto& sensor_variant : mod.sensorConfigs) + { + std::visit( + [&](const auto& sensor) { + SensorInfo info; + info.id = sensor.id; + info.name = module.name; + if (!sensor.vcInfoList.empty()) + { + const auto& vc = sensor.vcInfoList.front(); + info.width = vc.resolution.width; + info.height = vc.resolution.height; + info.fps = static_cast(vc.fps); + } + out.push_back(std::move(info)); + }, + sensor_variant); + } + }, + module.moduleType); + } + return out; +} + +} // namespace + +// ============================================================================= +// Impl +// ============================================================================= + +struct SiplCamera::Impl +{ + /// One ISP0 buffer, imported into CUDA once at registration. Nothing here + /// is re-created per frame -- that is the whole point of registering. + struct Slot + { + NvSciBufObj buf = nullptr; + NvBufSurface* surface = nullptr; + CUgraphicsResource resource = nullptr; + CUeglFrame egl_frame{}; + }; + + struct DeviceBuffer + { + uint8_t* ptr = nullptr; + size_t pitch = 0; + }; + + struct Sensor + { + SensorInfo info; + nvsipl::NvSIPLPipelineConfiguration pipeline_config{}; + nvsipl::NvSIPLPipelineQueues queues{}; + + /// From the reconciled surface, not from config: the ISP decides. + bool full_range = true; + NvSciBufAttrList buf_attrs = nullptr; + std::vector buf_objects; + /// ICP pool. Never read -- allocated only because SIPL requires it. + NvSciBufAttrList icp_attrs = nullptr; + std::vector icp_buf_objects; + std::vector slots; + NvSciSyncObj sync_obj = nullptr; + + /// Triple-buffered RGBA8 mailbox. A reader leases the published slot + /// until its next latest() call, so the producer never overwrites it. + std::array buffers{}; + mutable std::mutex publish_mutex; + int publish_idx = -1; + int lease_idx = -1; + uint64_t published_sequence = 0; + uint64_t consumed_sequence = 0; + uint64_t published_timestamp_ns = 0; + uint64_t published_capture_tsc_ns = 0; + + std::thread frame_thread; + std::thread event_thread; + }; + + explicit Impl(const SiplConfig& cfg) : config(cfg) {} + + SiplConfig config; + + NvSciBufModule sci_buf_module = nullptr; + NvSciSyncModule sci_sync_module = nullptr; + NvSciSyncCpuWaitContext cpu_wait_context = nullptr; + + CUdevice cu_device = 0; + CUcontext cu_context = nullptr; + bool cu_context_retained = false; + CUstream convert_stream = nullptr; + + std::unique_ptr camera; + std::unique_ptr query_api; + std::unique_ptr system_config; + + std::vector sensor_infos; + std::vector> sensors; + + std::atomic running{ false }; + std::atomic failed{ false }; + mutable std::mutex error_mutex; + std::string failure_message; + + Sensor* find(uint32_t sensor_id) + { + for (auto& s : sensors) + { + if (s->info.id == sensor_id) + { + return s.get(); + } + } + return nullptr; + } + + void set_failure(const std::string& message) + { + std::lock_guard guard(error_mutex); + if (failure_message.empty()) + { + failure_message = message; + } + failed.store(true); + } + + void throw_if_failed() const + { + if (!failed.load()) + { + return; + } + std::lock_guard guard(error_mutex); + throw std::runtime_error(failure_message.empty() ? "SIPL capture failed" : failure_message); + } + + void init_cuda(); + void init_nvsci(); + void configure(); + void allocate_buffers(Sensor& sensor); + void allocate_icp_buffers(Sensor& sensor); + void register_buffers(Sensor& sensor); + void allocate_sync(Sensor& sensor); + void register_sync(Sensor& sensor); + void register_nito(Sensor& sensor); + void frame_loop(Sensor& sensor); + void event_loop(Sensor& sensor); + void convert(Sensor& sensor, const Slot& slot, uint32_t write_idx); + void publish(Sensor& sensor, uint32_t write_idx, uint64_t timestamp_ns, uint64_t capture_tsc_ns); + uint32_t pick_write_index(const Sensor& sensor) const; + void cleanup(); +}; + +void SiplCamera::Impl::init_cuda() +{ + 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"); +} + +void SiplCamera::Impl::init_nvsci() +{ + check_sci(NvSciBufModuleOpen(&sci_buf_module), "NvSciBufModuleOpen"); + check_sci(NvSciSyncModuleOpen(&sci_sync_module), "NvSciSyncModuleOpen"); + check_sci(NvSciSyncCpuWaitContextAlloc(sci_sync_module, &cpu_wait_context), "NvSciSyncCpuWaitContextAlloc"); +} + +void SiplCamera::Impl::configure() +{ + init_sipl_trace(); + query_api = INvSIPLCameraQuery::GetInstance(); + if (!query_api) + { + throw std::runtime_error("INvSIPLCameraQuery::GetInstance returned null"); + } + // Loads the UDDF driver .so libs out of /usr/lib/nvsipl_drv. + check_sipl(query_api->ParseDatabase(), "ParseDatabase"); + check_sipl(query_api->ParseJsonFile(config.platform_config_json), "ParseJsonFile"); + + system_config = std::make_unique(); + check_sipl(query_api->GetSensorSystemConfig(config.platform_config_name, *system_config), + "GetSensorSystemConfig"); + if (config.link_masks.empty()) + { + throw std::runtime_error("link masks are required for a GMSL platform config"); + } + check_sipl(query_api->ApplyMask(*system_config, config.link_masks), "ApplyMask"); + if (system_config->modules.empty()) + { + throw std::runtime_error("no modules left in '" + config.platform_config_name + + "' after applying the link masks -- check the mask against the config"); + } + + sensor_infos = flatten(*system_config); + if (sensor_infos.empty()) + { + throw std::runtime_error("platform config '" + config.platform_config_name + "' declares no sensors"); + } + + camera = INvSIPLCamera::GetInstance(); + if (!camera) + { + throw std::runtime_error("INvSIPLCamera::GetInstance returned null"); + } + check_sipl(camera->SetPlatformCfg(*system_config), "SetPlatformCfg"); + + for (const auto& info : sensor_infos) + { + auto sensor = std::make_unique(); + sensor->info = info; + // ISP0 only. ICP would double the capture bandwidth for a raw stream + // nothing here consumes, and ISP1/ISP2 are a downscale we do not want. + sensor->pipeline_config.captureOutputRequested = false; + sensor->pipeline_config.isp0OutputRequested = true; + sensor->pipeline_config.isp1OutputRequested = false; + sensor->pipeline_config.isp2OutputRequested = false; + // Defaults to false, i.e. subframe ENABLED, which needs a sliceCount we + // do not set. NVIDIA's own samples disable it unless asked for. + sensor->pipeline_config.disableSubframe = true; + sensor->pipeline_config.bufferCfg.maxIsp0BufferCount = config.isp0_buffers; + check_sipl(camera->SetPipelineCfg(info.id, sensor->pipeline_config, sensor->queues), "SetPipelineCfg"); + sensors.push_back(std::move(sensor)); + } + + // Order is not ours to choose. Everything that *describes* a pipeline -- + // GetImageAttributes and FillNvSciSyncAttrList -- must happen before Init(); + // everything that *binds* a resource to it -- RegisterImages and + // RegisterNvSciSyncObj -- must happen after. Get it wrong and Init() + // still returns OK, but the pipeline is left in an error state and the + // first Register* call returns NVSIPL_STATUS_INVALID_STATE with no clue + // why. Verified against a -v 4 trace of NVIDIA's own nvsipl_camera. + for (auto& sensor : sensors) + { + allocate_icp_buffers(*sensor); + allocate_buffers(*sensor); + allocate_sync(*sensor); + } + + check_sipl(camera->Init(), "INvSIPLCamera::Init"); + + for (auto& sensor : sensors) + { + register_buffers(*sensor); + register_sync(*sensor); + register_nito(*sensor); + } +} + +void SiplCamera::Impl::allocate_buffers(Sensor& sensor) +{ + NvSciBufAttrList sipl_attrs = nullptr; + NvSciBufAttrList nvbuf_attrs = nullptr; + NvSciBufAttrList conflict = nullptr; + check_sci(NvSciBufAttrListCreate(sci_buf_module, &sipl_attrs), "NvSciBufAttrListCreate sipl"); + check_sci(NvSciBufAttrListCreate(sci_buf_module, &nvbuf_attrs), "NvSciBufAttrListCreate nvbuf"); + check_sci(set_sipl_buf_attributes(sipl_attrs), "NvSciBufAttrListSetAttrs"); + check_sipl(camera->GetImageAttributes(sensor.info.id, kIsp0, sipl_attrs), "GetImageAttributes"); + + // Both consumers must be described before allocation: SIPL writes the + // surface, NvBufSurface reads it on the way to CUDA. Reconciling the SIPL + // list alone yields a buffer RegisterImages will not accept. + check_sci(set_nvbuf_attributes(nvbuf_attrs, sipl_attrs, config.gpu_id), "set_nvbuf_attributes"); + + NvSciBufAttrList unreconciled[] = { sipl_attrs, nvbuf_attrs }; + check_sci(NvSciBufAttrListReconcile(unreconciled, 2U, &sensor.buf_attrs, &conflict), + "NvSciBufAttrListReconcile"); + NvSciBufAttrListFree(sipl_attrs); + NvSciBufAttrListFree(nvbuf_attrs); + if (conflict) + { + NvSciBufAttrListFree(conflict); + } + sensor.full_range = check_color_std(sensor.buf_attrs); + + sensor.slots.resize(config.isp0_buffers); + for (uint32_t i = 0; i < config.isp0_buffers; ++i) + { + Slot& slot = sensor.slots[i]; + check_sci(NvSciBufObjAlloc(sensor.buf_attrs, &slot.buf), "NvSciBufObjAlloc"); + sensor.buf_objects.push_back(slot.buf); + + if (NvmmNvSciBufToNvBufSurface(slot.buf, &slot.surface) != 0 || slot.surface == nullptr) + { + throw std::runtime_error("NvmmNvSciBufToNvBufSurface failed"); + } + slot.surface->numFilled = 1; + + } + + // RGBA8 destination slots. Allocated from the reported geometry, never from + // a CLI flag -- see "Geometry comes from the query" in the plan. + for (auto& dest : sensor.buffers) + { + void* ptr = nullptr; + size_t pitch = 0; + check_runtime(cudaMallocPitch(&ptr, &pitch, static_cast(sensor.info.width) * 4, + sensor.info.height), + "cudaMallocPitch"); + dest.ptr = static_cast(ptr); + dest.pitch = pitch; + } +} + +void SiplCamera::Impl::allocate_icp_buffers(Sensor& sensor) +{ + NvSciBufAttrList attrs = nullptr; + NvSciBufAttrList conflict = nullptr; + check_sci(NvSciBufAttrListCreate(sci_buf_module, &attrs), "NvSciBufAttrListCreate icp"); + + // Only the type and permissions: GetImageAttributes fills the raw format in. + NvSciBufType buf_type = NvSciBufType_Image; + NvSciBufAttrValAccessPerm perm = NvSciBufAccessPerm_ReadWrite; + NvSciBufAttrKeyValuePair kvp[] = { + { NvSciBufGeneralAttrKey_Types, &buf_type, sizeof(buf_type) }, + { NvSciBufGeneralAttrKey_RequiredPerm, &perm, sizeof(perm) }, + }; + check_sci(NvSciBufAttrListSetAttrs(attrs, kvp, 2), "NvSciBufAttrListSetAttrs icp"); + check_sipl(camera->GetImageAttributes(sensor.info.id, kIcp, attrs), "GetImageAttributes ICP"); + + NvSciBufAttrList unreconciled[] = { attrs }; + check_sci(NvSciBufAttrListReconcile(unreconciled, 1U, &sensor.icp_attrs, &conflict), + "NvSciBufAttrListReconcile icp"); + NvSciBufAttrListFree(attrs); + if (conflict) + { + NvSciBufAttrListFree(conflict); + } + + for (uint32_t i = 0; i < kIcpBuffers; ++i) + { + NvSciBufObj obj = nullptr; + check_sci(NvSciBufObjAlloc(sensor.icp_attrs, &obj), "NvSciBufObjAlloc icp"); + sensor.icp_buf_objects.push_back(obj); + } +} + +void SiplCamera::Impl::register_buffers(Sensor& sensor) +{ + check_sipl(camera->RegisterImages(sensor.info.id, kIcp, sensor.icp_buf_objects), "RegisterImages ICP"); + check_sipl(camera->RegisterImages(sensor.info.id, kIsp0, sensor.buf_objects), "RegisterImages ISP0"); + + // Import into CUDA only once SIPL owns the buffers. Doing it at allocation + // time hands SIPL a surface that already has an EGL image bound to it. + // Registering per frame instead would put an EGL round trip on the 60 Hz + // path for no benefit. + for (auto& slot : sensor.slots) + { + if (NvBufSurfaceMapEglImage(slot.surface, 0) != 0) + { + throw std::runtime_error("NvBufSurfaceMapEglImage failed"); + } + check_cuda(cuGraphicsEGLRegisterImage(&slot.resource, slot.surface->surfaceList[0].mappedAddr.eglImage, + CU_GRAPHICS_REGISTER_FLAGS_READ_ONLY), + "cuGraphicsEGLRegisterImage"); + check_cuda(cuGraphicsResourceGetMappedEglFrame(&slot.egl_frame, slot.resource, 0, 0), + "cuGraphicsResourceGetMappedEglFrame"); + } +} + +void SiplCamera::Impl::allocate_sync(Sensor& sensor) +{ + NvSciSyncAttrList signaler = nullptr; + NvSciSyncAttrList waiter = nullptr; + NvSciSyncAttrList reconciled = nullptr; + NvSciSyncAttrList conflict = nullptr; + + check_sci(NvSciSyncAttrListCreate(sci_sync_module, &signaler), "NvSciSyncAttrListCreate signaler"); + check_sci(NvSciSyncAttrListCreate(sci_sync_module, &waiter), "NvSciSyncAttrListCreate waiter"); + + // We wait on the CPU before handing the surface to the conversion kernel. + NvSciSyncAccessPerm perm = NvSciSyncAccessPerm_WaitOnly; + bool cpu_waiter = true; + NvSciSyncAttrKeyValuePair kvp[] = { + { NvSciSyncAttrKey_NeedCpuAccess, &cpu_waiter, sizeof(cpu_waiter) }, + { NvSciSyncAttrKey_RequiredPerm, &perm, sizeof(perm) }, + }; + check_sci(NvSciSyncAttrListSetAttrs(waiter, kvp, 2), "NvSciSyncAttrListSetAttrs"); + check_sipl(camera->FillNvSciSyncAttrList(sensor.info.id, kIsp0, signaler, nvsipl::SIPL_SIGNALER), + "FillNvSciSyncAttrList"); + + NvSciSyncAttrList unreconciled[] = { signaler, waiter }; + check_sci(NvSciSyncAttrListReconcile(unreconciled, 2U, &reconciled, &conflict), + "NvSciSyncAttrListReconcile"); + check_sci(NvSciSyncObjAlloc(reconciled, &sensor.sync_obj), "NvSciSyncObjAlloc"); + NvSciSyncAttrListFree(signaler); + NvSciSyncAttrListFree(waiter); + if (reconciled) + { + NvSciSyncAttrListFree(reconciled); + } + if (conflict) + { + NvSciSyncAttrListFree(conflict); + } +} + +void SiplCamera::Impl::register_sync(Sensor& sensor) +{ + check_sipl(camera->RegisterNvSciSyncObj(sensor.info.id, kIsp0, nvsipl::NVSIPL_EOFSYNCOBJ, sensor.sync_obj), + "RegisterNvSciSyncObj"); +} + +void SiplCamera::Impl::register_nito(Sensor& sensor) +{ + auto blob = load_nito(config.nito_dir, sensor.info.name); + nvsipl::ISiplControlAuto* auto_control = nullptr; + check_sipl(camera->RegisterAutoControlPlugin(sensor.info.id, nvsipl::NV_PLUGIN, auto_control, blob), + "RegisterAutoControlPlugin"); +} + +void SiplCamera::Impl::convert(Sensor& sensor, const Slot& slot, uint32_t write_idx) +{ + const CUeglFrame& frame = slot.egl_frame; + DeviceBuffer& dest = sensor.buffers[write_idx]; + + if (frame.planeCount < 2) + { + throw std::runtime_error("ISP0 surface has fewer than two planes"); + } + + // Requested SemiPlanar YUV, so this is NV12 unless swap_uv flips it, and + // REC709_ER, so the kernel gets bt709=true. + const YuvLayout layout = config.swap_uv ? YuvLayout::YVU420SemiPlanar : YuvLayout::YUV420SemiPlanar; + constexpr bool kBt709 = true; + + if (frame.frameType == CU_EGL_FRAME_TYPE_PITCH) + { + const auto* y_plane = static_cast(frame.frame.pPitch[0]); + const auto* uv_plane = static_cast(frame.frame.pPitch[1]); + launch_yuv420_pitch_to_rgba(y_plane, uv_plane, nullptr, static_cast(frame.pitch), + static_cast(frame.pitch), 0, sensor.info.width, sensor.info.height, + dest.ptr, static_cast(dest.pitch), layout, sensor.full_range, kBt709, + reinterpret_cast(convert_stream)); + } + else if (frame.frameType == CU_EGL_FRAME_TYPE_ARRAY) + { + // Block-linear: CUDA exposes the planes as arrays, so the kernel reads + // them through texture objects. Those are built per frame because the + // CUarray handles come from the per-frame CUeglFrame; the expensive + // part, cuGraphicsEGLRegisterImage, is done once per buffer. + cudaTextureObject_t y_tex = texture_for_array(frame.frame.pArray[0]); + cudaTextureObject_t uv_tex = texture_for_array(frame.frame.pArray[1]); + launch_yuv420_array_to_rgba(y_tex, uv_tex, 0, sensor.info.width, sensor.info.height, dest.ptr, + static_cast(dest.pitch), layout, sensor.full_range, kBt709, + reinterpret_cast(convert_stream)); + check_cuda(cuStreamSynchronize(convert_stream), "cuStreamSynchronize"); + check_runtime(cudaDestroyTextureObject(y_tex), "cudaDestroyTextureObject"); + check_runtime(cudaDestroyTextureObject(uv_tex), "cudaDestroyTextureObject"); + check_runtime(cudaGetLastError(), "SIPL YUV to RGBA kernel"); + return; + } + else + { + throw std::runtime_error("unsupported CUDA EGL frame type from ISP0"); + } + + check_cuda(cuStreamSynchronize(convert_stream), "cuStreamSynchronize"); + check_runtime(cudaGetLastError(), "SIPL YUV to RGBA kernel"); +} + +void SiplCamera::Impl::frame_loop(Sensor& sensor) +{ + try + { + check_cuda(cuCtxSetCurrent(cu_context), "cuCtxSetCurrent frame_loop"); + while (running.load()) + { + INvSIPLClient::INvSIPLBuffer* buffer = nullptr; + const SIPLStatus status = + sensor.queues.isp0CompletionQueue->Get(buffer, config.frame_timeout_ms * 1000U); + if (status == nvsipl::NVSIPL_STATUS_TIMED_OUT) + { + continue; + } + if (status == nvsipl::NVSIPL_STATUS_EOF) + { + break; + } + check_sipl(status, "isp0CompletionQueue->Get"); + if (!buffer) + { + continue; + } + + auto* nvm = dynamic_cast(buffer); + if (!nvm) + { + buffer->Release(); + throw std::runtime_error("ISP0 buffer is not an INvSIPLNvMBuffer"); + } + + // The ISP is still writing until this fence clears. + NvSciSyncFence fence = NvSciSyncFenceInitializer; + check_sipl(nvm->GetEOFNvSciSyncFence(&fence), "GetEOFNvSciSyncFence"); + const NvSciError wait = + NvSciSyncFenceWait(&fence, cpu_wait_context, config.frame_timeout_ms * 1000UL); + NvSciSyncFenceClear(&fence); + check_sci(wait, "NvSciSyncFenceWait"); + + const NvSciBufObj arrived = nvm->GetNvSciBufImage(); + const Slot* slot = nullptr; + for (const auto& candidate : sensor.slots) + { + if (candidate.buf == arrived) + { + slot = &candidate; + break; + } + } + if (!slot) + { + buffer->Release(); + throw std::runtime_error("ISP0 delivered a buffer this process never registered"); + } + + // The TSC is what makes the two eyes pairable; do not substitute a + // host clock, which carries this sensor's own queueing jitter. + const auto& meta = nvm->GetImageData(); + const uint64_t capture_tsc_ns = meta.frameCaptureTSC; + // SENSING_AE_PROBE=1 logs the sensor's exposure and gain once a + // second. AE failures are invisible in the frame rate and hard to + // judge by eye, and this is the only place the numbers surface. + if (std::getenv("SENSING_AE_PROBE") != nullptr) + { + static thread_local uint64_t probe_n = 0; + if ((probe_n++ % 60) == 0) + { + std::cerr << "[ae] sensor " << sensor.info.id << " numExp=" << meta.numExposures + << " expValid=" << static_cast(meta.sensorExpInfo.expTimeValid) + << " exp[0]=" << meta.sensorExpInfo.exposureTime[0] << "s" + << " gainValid=" << static_cast(meta.sensorExpInfo.gainValid) + << " gain[0]=" << meta.sensorExpInfo.sensorGain[0] << std::endl; + } + } + + const uint32_t write_idx = pick_write_index(sensor); + convert(sensor, *slot, write_idx); + if (std::getenv("SENSING_LUMA_PROBE") != nullptr) + { + static thread_local uint64_t luma_n = 0; + if ((luma_n++ % 60) == 0) + { + // Mean over one row out of every 64, straight off the RGBA + // the consumer receives -- the only number that says what + // the operator actually sees. + const auto& dst = sensor.buffers[write_idx]; + const uint32_t step = 64; + std::vector row(dst.pitch); + double sum = 0.0; + uint64_t n = 0; + for (uint32_t y = 0; y < sensor.info.height; y += step) + { + if (cudaMemcpy(row.data(), dst.ptr + static_cast(y) * dst.pitch, dst.pitch, + cudaMemcpyDeviceToHost) != cudaSuccess) + break; + for (uint32_t x = 0; x < sensor.info.width; ++x) + { + sum += 0.2126 * row[x * 4 + 0] + 0.7152 * row[x * 4 + 1] + 0.0722 * row[x * 4 + 2]; + ++n; + } + } + if (n) + std::cerr << "[luma] sensor " << sensor.info.id << " mean=" << (sum / n) << "/255" + << std::endl; + } + } + buffer->Release(); + publish(sensor, write_idx, monotonic_ns(), capture_tsc_ns); + } + } + catch (const std::exception& e) + { + std::ostringstream oss; + oss << "SIPL sensor " << sensor.info.id << " capture error: " << e.what(); + std::cerr << "[sipl] " << oss.str() << std::endl; + set_failure(oss.str()); + running.store(false); + } +} + +void SiplCamera::Impl::event_loop(Sensor& sensor) +{ + // A GMSL link drop arrives here and nowhere else. Without this drain the + // symptom is a silent stall on the frame queue. + while (running.load()) + { + nvsipl::NvSIPLPipelineNotifier::NotificationData data{}; + const SIPLStatus status = sensor.queues.notificationQueue->Get(data, config.frame_timeout_ms * 1000U); + if (status != NVSIPL_STATUS_OK) + { + continue; + } + using Notif = nvsipl::NvSIPLPipelineNotifier; + switch (data.eNotifType) + { + case Notif::NOTIF_ERROR_DESERIALIZER_FAILURE: + case Notif::NOTIF_ERROR_SERIALIZER_FAILURE: + case Notif::NOTIF_ERROR_SENSOR_FAILURE: + case Notif::NOTIF_ERROR_INTERNAL_FAILURE: + { + std::ostringstream oss; + oss << "SIPL sensor " << sensor.info.id << " pipeline error, notification " + << static_cast(data.eNotifType); + std::cerr << "[sipl] " << oss.str() << std::endl; + set_failure(oss.str()); + running.store(false); + break; + } + default: + break; + } + } +} + +void SiplCamera::Impl::publish(Sensor& sensor, uint32_t write_idx, uint64_t timestamp_ns, uint64_t capture_tsc_ns) +{ + std::lock_guard guard(sensor.publish_mutex); + sensor.publish_idx = static_cast(write_idx); + sensor.published_timestamp_ns = timestamp_ns; + sensor.published_capture_tsc_ns = capture_tsc_ns; + ++sensor.published_sequence; +} + +uint32_t SiplCamera::Impl::pick_write_index(const Sensor& sensor) const +{ + std::lock_guard guard(sensor.publish_mutex); + if (sensor.publish_idx < 0) + { + return 0; + } + // Skip the published slot and any slot still leased to a reader. With three + // slots one is always free, so this cannot spin. + uint32_t idx = static_cast((sensor.publish_idx + 1) % 3); + if (static_cast(idx) == sensor.lease_idx) + { + idx = (idx + 1) % 3; + } + return idx; +} + +void SiplCamera::Impl::cleanup() +{ + running.store(false); + for (auto& sensor : sensors) + { + if (sensor->frame_thread.joinable()) + { + sensor->frame_thread.join(); + } + if (sensor->event_thread.joinable()) + { + sensor->event_thread.join(); + } + } + if (camera) + { + camera->Stop(); + camera->Deinit(); + } + + for (auto& sensor : sensors) + { + for (auto& slot : sensor->slots) + { + if (slot.resource) + { + cuGraphicsUnregisterResource(slot.resource); + } + if (slot.surface) + { + NvBufSurfaceUnMapEglImage(slot.surface, 0); + NvBufSurfaceDestroy(slot.surface); + } + if (slot.buf) + { + NvSciBufObjFree(slot.buf); + } + } + sensor->slots.clear(); + sensor->buf_objects.clear(); + for (auto& obj : sensor->icp_buf_objects) + { + NvSciBufObjFree(obj); + } + sensor->icp_buf_objects.clear(); + if (sensor->icp_attrs) + { + NvSciBufAttrListFree(sensor->icp_attrs); + sensor->icp_attrs = nullptr; + } + if (sensor->buf_attrs) + { + NvSciBufAttrListFree(sensor->buf_attrs); + sensor->buf_attrs = nullptr; + } + if (sensor->sync_obj) + { + NvSciSyncObjFree(sensor->sync_obj); + sensor->sync_obj = nullptr; + } + for (auto& dest : sensor->buffers) + { + if (dest.ptr) + { + cudaFree(dest.ptr); + dest.ptr = nullptr; + } + } + } + + camera.reset(); + query_api.reset(); + + if (cpu_wait_context) + { + NvSciSyncCpuWaitContextFree(cpu_wait_context); + cpu_wait_context = nullptr; + } + if (sci_sync_module) + { + NvSciSyncModuleClose(sci_sync_module); + sci_sync_module = nullptr; + } + if (sci_buf_module) + { + NvSciBufModuleClose(sci_buf_module); + sci_buf_module = nullptr; + } + if (convert_stream) + { + cuStreamDestroy(convert_stream); + convert_stream = nullptr; + } + if (cu_context_retained) + { + cuDevicePrimaryCtxRelease(cu_device); + cu_context_retained = false; + cu_context = nullptr; + } +} + +// ============================================================================= +// SiplCamera +// ============================================================================= + +SiplCamera::SiplCamera(const SiplConfig& config) : m_impl(std::make_unique(config)) +{ + if (config.platform_config_json.empty() || config.platform_config_name.empty()) + { + throw std::invalid_argument("SiplConfig needs both platform_config_json and platform_config_name"); + } + try + { + m_impl->init_cuda(); + m_impl->init_nvsci(); + m_impl->configure(); + } + catch (...) + { + m_impl->cleanup(); + throw; + } +} + +SiplCamera::~SiplCamera() +{ + m_impl->cleanup(); +} + +std::vector SiplCamera::query(const std::string& platform_config_json, + const std::string& platform_config_name, + const std::vector& link_masks) +{ + auto api = INvSIPLCameraQuery::GetInstance(); + if (!api) + { + throw std::runtime_error("INvSIPLCameraQuery::GetInstance returned null"); + } + check_sipl(api->ParseDatabase(), "ParseDatabase"); + check_sipl(api->ParseJsonFile(platform_config_json), "ParseJsonFile"); + + nvsipl::sensorconfig::SensorSystemConfig cfg; + check_sipl(api->GetSensorSystemConfig(platform_config_name, cfg), "GetSensorSystemConfig"); + if (!link_masks.empty()) + { + check_sipl(api->ApplyMask(cfg, link_masks), "ApplyMask"); + } + return flatten(cfg); +} + +const std::vector& SiplCamera::sensors() const +{ + return m_impl->sensor_infos; +} + +void SiplCamera::start() +{ + if (m_impl->running.load()) + { + return; + } + m_impl->running.store(true); + for (auto& sensor : m_impl->sensors) + { + sensor->frame_thread = std::thread([this, s = sensor.get()] { m_impl->frame_loop(*s); }); + sensor->event_thread = std::thread([this, s = sensor.get()] { m_impl->event_loop(*s); }); + } + try + { + check_sipl(m_impl->camera->Start(), "INvSIPLCamera::Start"); + } + catch (...) + { + m_impl->running.store(false); + for (auto& sensor : m_impl->sensors) + { + if (sensor->frame_thread.joinable()) + { + sensor->frame_thread.join(); + } + if (sensor->event_thread.joinable()) + { + sensor->event_thread.join(); + } + } + throw; + } +} + +void SiplCamera::stop() +{ + m_impl->running.store(false); + for (auto& sensor : m_impl->sensors) + { + if (sensor->frame_thread.joinable()) + { + sensor->frame_thread.join(); + } + if (sensor->event_thread.joinable()) + { + sensor->event_thread.join(); + } + } + if (m_impl->camera) + { + m_impl->camera->Stop(); + } +} + +std::optional SiplCamera::latest(uint32_t sensor_id) +{ + m_impl->throw_if_failed(); + Impl::Sensor* sensor = m_impl->find(sensor_id); + if (!sensor) + { + throw std::runtime_error("no SIPL pipeline with index " + std::to_string(sensor_id)); + } + + std::lock_guard guard(sensor->publish_mutex); + if (sensor->publish_idx < 0 || sensor->consumed_sequence == sensor->published_sequence) + { + return std::nullopt; + } + sensor->consumed_sequence = sensor->published_sequence; + + // The caller now holds this slot; pick_write_index() skips it until the + // next latest() moves the lease on. + sensor->lease_idx = sensor->publish_idx; + + FrameView view; + view.ptr = reinterpret_cast(sensor->buffers[sensor->publish_idx].ptr); + view.pitch = sensor->buffers[sensor->publish_idx].pitch; + view.width = sensor->info.width; + view.height = sensor->info.height; + view.timestamp_ns = sensor->published_timestamp_ns; + view.capture_tsc_ns = sensor->published_capture_tsc_ns; + view.sequence = sensor->published_sequence; + return view; +} + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/sipl_camera.hpp b/src/plugins/sensing/core/sipl_camera.hpp new file mode 100644 index 000000000..56eb64afe --- /dev/null +++ b/src/plugins/sensing/core/sipl_camera.hpp @@ -0,0 +1,115 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 +// +// SIPL capture for the SENSING SG8A carrier: every sensor in one platform +// config, ISP0 YUV out, converted to RGBA8 on the GPU. +// +// One instance owns every sensor. This is not a style choice -- +// INvSIPLCamera::GetInstance() hands out a single process-wide object, so a +// per-sensor camera class is not expressible. Do not reintroduce one. +// +// The SIPL and NvSci headers are pimpl'd away because they drag in NvMedia and +// X11-style macros that collide with mcap's StatusCode; keeping them out of +// this header is what lets frame_sink.cpp include both. + +#pragma once + +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace sensing +{ + +struct SiplConfig +{ + /// Vendor platform config, e.g. /query/sg8a_agth_g2a/shw5g.json. + std::string platform_config_json; + /// Named config inside it, e.g. "SHW5G_2". + std::string platform_config_name; + /// One mask per deserializer, in transport order. Required for GMSL. + std::vector link_masks; + /// Directory holding .nito; the ISP has no tuning without it. + std::string nito_dir = "/var/nvidia/nvcam/settings/sipl"; + int gpu_id = 0; + /// Overrides the range implied by the reconciled colour standard. + bool full_range = false; + /// Swap the chroma planes. Defaults off: the ISP0 surface order is + /// requested explicitly, so it does not need discovering from the frame. + bool swap_uv = false; + /// ISP0 buffers per sensor. Six is the SIPL default; more absorbs a slow + /// consumer at the cost of 6 MB each at 2560x1984 NV12. + uint32_t isp0_buffers = 6; + uint32_t frame_timeout_ms = 1000; +}; + +/// One sensor as the platform config describes it. `id` is the SIPL pipeline +/// index, which is NOT the GMSL link index, the CSI virtual channel, or the +/// JSON sensorInfo.id -- for SHW5G_2 those are 2 and 3 while this is 0 and 1. +struct SensorInfo +{ + uint32_t id = 0; + std::string name; + uint32_t width = 0; + uint32_t height = 0; + double fps = 0.0; +}; + +/** + * @brief Borrowed view of the most recently converted frame. + * + * `ptr` addresses one of three producer-owned slots and stays valid until the + * second subsequent latest() call for the same sensor, at which point the + * producer may overwrite it. Copy out before then; do not cache a FrameView + * across update() ticks. + */ +struct FrameView +{ + uintptr_t ptr = 0; + size_t pitch = 0; + uint32_t width = 0; + uint32_t height = 0; + /// CLOCK_MONOTONIC, stamped at YUV->RGBA conversion. Round-trips through + /// the encoder, so it is what identifies a unit on the way back out. + uint64_t timestamp_ns = 0; + /// Capture time on the TSC timebase, shared by every sensor on the rig. + /// This is what makes the two eyes pairable; a host clock is not. + uint64_t capture_tsc_ns = 0; + uint64_t sequence = 0; +}; + +class SiplCamera +{ +public: + explicit SiplCamera(const SiplConfig& config); + ~SiplCamera(); + + SiplCamera(const SiplCamera&) = delete; + SiplCamera& operator=(const SiplCamera&) = delete; + + /// Enumerate a platform config without touching the hardware. Cheap: the + /// query API only parses the driver database and the JSON. + static std::vector query(const std::string& platform_config_json, + const std::string& platform_config_name, + const std::vector& link_masks); + + /// Sensors this instance configured, in pipeline-index order. + const std::vector& sensors() const; + + void start(); + void stop(); + + /// Non-blocking mailbox read. nullopt when no new frame has arrived. + std::optional latest(uint32_t sensor_id); + +private: + struct Impl; + std::unique_ptr m_impl; +}; + +} // 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..9a48f6809 --- /dev/null +++ b/src/plugins/sensing/core/yuv_to_rgba.cu @@ -0,0 +1,234 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 +// +// YUV420 -> RGBA8 conversion for the SIPL ISP0 output. +// +// BT.601 and BT.709, selected per call. SIPL's ISP0 output on this rig is +// REC709_ER and the request is not negotiable -- GetImageAttributes rejects a +// BT.601 attribute list with NVSIPL_STATUS_BAD_ARGUMENT -- so the 709 matrix is +// the one the SIPL path actually uses. rgba_to_nv12.cu on the encode side is +// still 601, so an encode round trip is not identity; that only matters if the +// H.264 output is ever compared against the RGBA it came from. + +#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, + bool bt709, + unsigned char& r, + unsigned char& g, + unsigned char& b) +{ + // Full range uses Y as-is; limited range expands 16..235 first. + const float yf = full_range ? static_cast(Y) : (static_cast(Y) - 16.f) * 1.16438f; + const float u = static_cast(Cb) - 128.f; + const float v = static_cast(Cr) - 128.f; + + // Limited-range coefficients already fold in the 255/219 luma gain applied + // above, so they differ from the full-range pair by more than rounding. + float kr, kgu, kgv, kb; + if (bt709) + { + if (full_range) { kr = 1.5748f; kgu = 0.18733f; kgv = 0.46813f; kb = 1.8556f; } + else { kr = 1.79274f; kgu = 0.21325f; kgv = 0.53291f; kb = 2.11240f; } + } + else + { + if (full_range) { kr = 1.402f; kgu = 0.344136f; kgv = 0.714136f; kb = 1.772f; } + else { kr = 1.596f; kgu = 0.391f; kgv = 0.813f; kb = 2.018f; } + } + + r = clamp_u8(yf + kr * v); + g = clamp_u8(yf - kgu * u - kgv * v); + b = clamp_u8(yf + kb * u); +} + +__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, + int bt709_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 == plugins::sensing::YuvLayout::YUV420SemiPlanar || layout == plugins::sensing::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 == plugins::sensing::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 == plugins::sensing::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, bt709_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, + int bt709_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 == plugins::sensing::YuvLayout::YUV420SemiPlanar || layout == plugins::sensing::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 == plugins::sensing::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 == plugins::sensing::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, bt709_value != 0, rgba_out[idx + 0], rgba_out[idx + 1], + rgba_out[idx + 2]); + rgba_out[idx + 3] = 255; +} + +} // namespace + +namespace plugins +{ +namespace sensing +{ + +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, + bool bt709, + 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, bt709 ? 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, + bool bt709, + 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, bt709 ? 1 : 0); +} + +} // namespace sensing +} // namespace plugins 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..364520252 --- /dev/null +++ b/src/plugins/sensing/core/yuv_to_rgba.cuh @@ -0,0 +1,53 @@ +// 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 +{ + +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, + /// BT.709 matrix instead of BT.601. + bool bt709, + 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, + /// BT.709 matrix instead of BT.601. + bool bt709, + cudaStream_t stream); + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/plugin.yaml b/src/plugins/sensing/plugin.yaml new file mode 100644 index 000000000..a4ea26d11 --- /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 (SIPL 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/tests/cpp/plugins/CMakeLists.txt b/tests/cpp/plugins/CMakeLists.txt index 6ad14ea6e..e0400dd4e 100644 --- a/tests/cpp/plugins/CMakeLists.txt +++ b/tests/cpp/plugins/CMakeLists.txt @@ -6,3 +6,7 @@ add_subdirectory(plugin_utils) if(BUILD_PLUGIN_OGLO) add_subdirectory(oglo_tactile) endif() + +if(BUILD_PLUGIN_SENSING) + add_subdirectory(sensing) +endif() diff --git a/tests/cpp/plugins/sensing/CMakeLists.txt b/tests/cpp/plugins/sensing/CMakeLists.txt new file mode 100644 index 000000000..97d52c4ef --- /dev/null +++ b/tests/cpp/plugins/sensing/CMakeLists.txt @@ -0,0 +1,14 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# Query-only: needs the vendor JSON on disk but no camera, and links +# sensing_core purely for SiplCamera::query. sipl_camera.hpp is pimpl'd, so this +# target never sees a SIPL or NvSci header. +add_executable(test_sipl_query test_sipl_query.cpp) +target_link_libraries(test_sipl_query PRIVATE sensing::core) +# The in-tree config, so this runs unconditionally -- a test that skips when the +# vendor package is absent is a test that does not exist in CI. +target_compile_definitions(test_sipl_query PRIVATE + SENSING_PLATFORM_CONFIG="${CMAKE_SOURCE_DIR}/src/plugins/sensing/configs/shw5g.json" +) +add_test(NAME sipl_query COMMAND test_sipl_query) diff --git a/tests/cpp/plugins/sensing/test_sipl_query.cpp b/tests/cpp/plugins/sensing/test_sipl_query.cpp new file mode 100644 index 000000000..9cab4165d --- /dev/null +++ b/tests/cpp/plugins/sensing/test_sipl_query.cpp @@ -0,0 +1,88 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// The SIPL query API resolves a vendor platform config to pipeline indices, and +// those indices are NOT the numbers the JSON appears to name. For SHW5G_2 the +// GMSL link indices, the CSI virtual channels and the JSON sensorInfo.id are all +// 2 and 3, while the pipeline indices SetPipelineCfg and --add-stream sensor=N +// take are 0 and 1. In S56C_1_SHF3L_2 they coincide, so this is exactly the kind +// of renumbering that hides until someone points a camera at the wrong eye. +// +// No hardware: the query API only parses the UDDF driver database and the JSON, +// so this runs on a rig with the cameras unplugged. The config is the in-tree +// copy, so it does not need the vendor package either. + +#include "core/sipl_camera.hpp" + +#include +#include +#include +#include + +#ifndef SENSING_PLATFORM_CONFIG +#error "SENSING_PLATFORM_CONFIG must be defined by the build" +#endif + +using namespace plugins::sensing; + +namespace +{ + +int g_failures = 0; + +#define CHECK(cond, ...) \ + do \ + { \ + if (!(cond)) \ + { \ + std::printf("FAIL %s:%d: ", __FILE__, __LINE__); \ + std::printf(__VA_ARGS__); \ + std::printf("\n"); \ + ++g_failures; \ + } \ + } while (0) + +} // namespace + +int main() +{ + const std::string config = SENSING_PLATFORM_CONFIG; + std::printf("platform config: %s\n", config.c_str()); + + const std::vector masks{ 0x0000, 0x1100 }; + const auto sensors = SiplCamera::query(config, "SHW5G_2", masks); + + CHECK(sensors.size() == 2, "expected 2 sensors in SHW5G_2, got %zu", sensors.size()); + if (sensors.size() != 2) + { + return 1; + } + + // The load-bearing assertion. If the query ever starts echoing the JSON's + // sensorInfo.id instead of renumbering, this catches it here rather than as + // a swapped or missing eye at runtime. + CHECK(sensors[0].id == 0, "first pipeline index is %u, expected 0", sensors[0].id); + CHECK(sensors[1].id == 1, "second pipeline index is %u, expected 1", sensors[1].id); + + for (const auto& s : sensors) + { + std::printf(" sensor=%u %s %ux%u @ %.2f fps\n", s.id, s.name.c_str(), s.width, s.height, s.fps); + CHECK(s.width == 2560, "sensor %u width is %u, expected 2560", s.id, s.width); + CHECK(s.height == 1984, "sensor %u height is %u, expected 1984", s.id, s.height); + CHECK(s.fps > 59.0 && s.fps < 61.0, "sensor %u fps is %.2f, expected 60", s.id, s.fps); + CHECK(!s.name.empty(), "sensor %u has an empty module name", s.id); + } + + // An empty mask must yield no modules rather than silently falling back to + // every link on the deserializer. + const auto none = SiplCamera::query(config, "SHW5G_2", { 0x0000, 0x0000 }); + CHECK(none.empty(), "masking every link left %zu sensor(s)", none.size()); + + if (g_failures == 0) + { + std::printf("PASS\n"); + return 0; + } + std::printf("%d check(s) failed\n", g_failures); + return 1; +}