From a734692b6090e7fab2ab096cee2c5cd589a9ff5a Mon Sep 17 00:00:00 2001 From: cesar Date: Tue, 8 Sep 2026 09:10:53 -0300 Subject: [PATCH 01/10] update the build to dora 1.0 and a shell-free build command MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit dora 1.0 runs a node's build command directly rather than through a shell, so the mkdir/cd/&& sequence in dataflow.yml could not work. Move it to scripts/build.sh and invoke that instead. Default CMAKE_BUILD_TYPE to Release so DoraTargets.cmake picks the release cargo profile and the binary does not link debug dora bindings. Pin the dataflow source with tag: — 1.0 replaced git_checkout with the mutually exclusive tag/rev/branch. Co-Authored-By: Claude Opus 5 Claude-Session: https://claude.ai/code/session_01Y7g2cBJ5e3r15ZbJ9WZn8c --- AGENTS.md | 11 +++++++---- CMakeLists.txt | 6 ++++++ Dockerfile | 2 +- README.md | 15 ++++++++++++--- dataflow.yml | 4 ++-- scripts/build.sh | 14 ++++++++++++++ 6 files changed, 42 insertions(+), 10 deletions(-) create mode 100755 scripts/build.sh diff --git a/AGENTS.md b/AGENTS.md index 310a548..b5c9da8 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -9,12 +9,14 @@ A dora-rs C++ node that interfaces with ZED stereo cameras. On each tick it grab ## Build ```bash -mkdir -p build && cd build && cmake .. && make install +scripts/build.sh ``` -The binary is installed to `bin/dora_zed` relative to the repo root. +The binary is installed to `bin/dora_zed` relative to the repo root. The script is also +the dataflow's `build:` command: dora 1.0 executes build commands directly rather than +through a shell, so `cd`, `&&` and command substitution do not work there. -**Requirements:** CMake 3.21+, ZED SDK 5.x (at `/usr/local/zed`), CUDA Toolkit, ONNX Runtime 1.20+ (system install), Rust stable toolchain (for dora-rs C++ bindings). +**Requirements:** dora 1.0+ (verified with `dora-cli 1.0.1`), CMake 3.21+, ZED SDK 5.x (at `/usr/local/zed`), CUDA Toolkit, ONNX Runtime 1.20+ (system install), Rust stable toolchain (for dora-rs C++ bindings). The first build clones dora-rs from GitHub and compiles `dora-node-api-cxx` via Cargo. This takes a while. Subsequent builds are incremental. If `DORA_ROOT_DIR` is set to a local dora checkout, it builds against that instead. @@ -40,7 +42,8 @@ Camera and detection behavior is configured entirely through environment variabl - **`src/engines/yolo_onnx.h/cc`** — `engines::YoloOnnx` / `YoloOnnxConfig`: closed-vocabulary detection for any single-input YOLO ONNX model (YOLOv8, YOLOv11, YOLOv5, etc.). Auto-detects output layout (`NC_FIRST` / `NC_LAST` / `NC_LAST_OBJ`) from tensor shape; override with `ZED_ONNX_OUTPUT_FORMAT`. Labels loaded from `ZED_ONNX_LABELS` or `ZED_ONNX_LABELS_FILE`. - **`src/main.cc`** — Dora event loop. Selects engine via `ZED_ENGINE` env var (`yolo_onnx`/`onnx` or `native`), holds a `std::unique_ptr`. Parses `ZED_PC_MODE` (`FULL`/`HALF`/`ROI`) once before the loop. On any input event (intended to be driven by a tick timer; the id is not inspected): calls `camera.grab()` and `detector->detect()`, then sends `image_bgr`, `point_cloud`, `camera_info`, and `objects` outputs. Point cloud output varies by mode: `FULL` sends the raw `sl::Mat` pointer (no copy); `HALF` downsamples by 2 in each dimension; `ROI` emits only valid points within detected bounding boxes as a flat `[N, 4]` array. Handles reconnection on grab failure. - **`DoraTargets.cmake`** — Handles building or fetching dora-rs and generating the CXX bridge files. Two paths: local (`DORA_ROOT_DIR`) or git clone. Produces `dora_node_api_cxx` library and the `node_bridge.cc` glue. -- **`dataflow.yml`** — Example dora dataflow definition using the `git:` source format. +- **`scripts/build.sh`** — Configure/compile/install in one shell-free-callable step. Referenced by `dataflow.yml`, the `Dockerfile` and the README. +- **`dataflow.yml`** — Example dora dataflow definition using the `git:` source format, pinned with `tag:` — dora 1.0 pins a source with the mutually exclusive `tag:`, `rev:` or `branch:`. - **`Dockerfile`** — Ubuntu 24.04 x86-64 build environment (CUDA 12.8, ZED SDK 5.2, ONNX Runtime, Rust). No ARM64/Jetson image; build natively there. No camera needed; run the compile with `--gpus all`. Sets `NVIDIA_DRIVER_CAPABILITIES=compute,utility,video` — the `video` capability is what makes the container runtime mount NVDEC/NVENC, which `libsl_zed.so` links against. ONNX Runtime's CMake package files are relocated to `/usr/local/lib/cmake` because Ubuntu's multiarch CMake does not search `lib64`. Rust lives at `/usr/local/{rustup,cargo}` rather than `/root` so the build can run as the invoking user. `docker build -t dora-zed-build . && docker run --rm --gpus all --user "$(id -u):$(id -g)" -v "$PWD:/src" -v dora-zed-cargo:/usr/local/cargo/registry dora-zed-build`. - **`scripts/export_yolo.py`** — PEP 723 uv script. Exports a standard YOLOv11 model to ONNX for closed-vocabulary detection. Run: `uv run scripts/export_yolo.py --model n --output-dir models/`. - **`src/test_yolo_onnx.cc`** — Standalone C++ test binary (`bin/test_yolo_onnx`). Exercises `YoloOnnx` with an image (default `test/assets/yolo.png`). No dora and no live camera required. Uses the same env vars as the main binary. Saves an annotated result image alongside the input. diff --git a/CMakeLists.txt b/CMakeLists.txt index 71f3cd6..3baf07b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,6 +4,12 @@ project(dora_zed VERSION 0.1.0 LANGUAGES C CXX) set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_FLAGS "-fPIC") +# DoraTargets.cmake picks the cargo profile from this, so an unset build type +# would link debug dora bindings into an otherwise unoptimized binary. +if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) + set(CMAKE_BUILD_TYPE Release CACHE STRING "Build type" FORCE) +endif() + include(DoraTargets.cmake) set(ZED_DIR "/usr/local/zed" CACHE PATH "Path to the ZED SDK") diff --git a/Dockerfile b/Dockerfile index 2e30c15..b7c0016 100644 --- a/Dockerfile +++ b/Dockerfile @@ -72,4 +72,4 @@ RUN curl -fSL -o /tmp/ort.tgz \ ENV NVIDIA_DRIVER_CAPABILITIES=compute,utility,video WORKDIR /src -CMD ["bash", "-lc", "mkdir -p build && cd build && cmake .. && make -j$(nproc) install"] +CMD ["scripts/build.sh"] diff --git a/README.md b/README.md index 2bfe4cf..8a5c825 100644 --- a/README.md +++ b/README.md @@ -10,8 +10,8 @@ Add to your dataflow: nodes: - id: zed-camera git: ssh://git@gitea.liria.cl:5001/liria/dora-zed.git - git_checkout: v0.1.0 - build: mkdir -p build && cd build && cmake .. && make install + tag: v0.2.0 + build: scripts/build.sh path: bin/dora_zed inputs: tick: dora/timer/millis/100 @@ -28,6 +28,7 @@ See [`dataflow.yml`](dataflow.yml) for a complete runnable example with a camera | Dependency | Version | Notes | |---|---|---| +| [dora-rs](https://github.com/dora-rs/dora) | 1.0+ | Verified against `dora-cli 1.0.1`; the C++ bindings are built from source at configure time | | [CMake](https://cmake.org/download/) | 3.21+ | | | [ZED SDK](https://www.stereolabs.com/developers/release) | 5.x | Bundles OpenCV; default path `/usr/local/zed` | | [CUDA Toolkit](https://developer.nvidia.com/cuda-downloads) | — | Required by ZED SDK | @@ -51,11 +52,17 @@ See [`dataflow.yml`](dataflow.yml) for a complete runnable example with a camera ```bash git clone ssh://git@gitea.liria.cl:5001/liria/dora-zed.git cd dora-zed -mkdir -p build && cd build && cmake .. && make install +scripts/build.sh ``` Binary installed to `bin/dora_zed`. +`scripts/build.sh` is also the `build:` command in the dataflow. dora 1.0 runs build +commands directly rather than through a shell, so `cd`, `&&` and command substitution are +not available there; keeping the sequence in one script is what makes the dataflow build +work. It configures a `Release` build, which is also what links the optimized dora +bindings. + ### Containerized build [`Dockerfile`](Dockerfile) provides the full toolchain — CUDA, ZED SDK, ONNX Runtime and Rust — on Ubuntu 24.04, the newest release Stereolabs ships a ZED SDK for. No camera is needed. @@ -66,6 +73,8 @@ docker run --rm --gpus all --user "$(id -u):$(id -g)" \ -v "$PWD:/src" -v dora-zed-cargo:/usr/local/cargo/registry dora-zed-build ``` +The container runs the same `scripts/build.sh`. + The first run clones dora-rs and compiles it with Cargo; the named volume keeps that cache across runs. `--user` keeps `build/` and `bin/` owned by you rather than root. diff --git a/dataflow.yml b/dataflow.yml index 80dcf14..5d7d4c4 100644 --- a/dataflow.yml +++ b/dataflow.yml @@ -1,8 +1,8 @@ nodes: - id: zed-camera git: ssh://git@gitea.liria.cl:5001/liria/dora-zed.git - git_checkout: v0.1.0 - build: mkdir -p build && cd build && cmake .. && make install + tag: v0.2.0 + build: scripts/build.sh path: bin/dora_zed inputs: tick: dora/timer/millis/100 diff --git a/scripts/build.sh b/scripts/build.sh new file mode 100755 index 0000000..8eb753a --- /dev/null +++ b/scripts/build.sh @@ -0,0 +1,14 @@ +#!/usr/bin/env bash +# Configure, compile and install the node into bin/. +# +# dora 1.0 runs a node's `build:` command directly rather than through a shell, +# so `cd`, `&&` and command substitution are not available there. The whole +# sequence lives here instead, and the dataflow only invokes this script. +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +REPO_DIR="$(dirname "$SCRIPT_DIR")" +BUILD_DIR="${REPO_DIR}/build" + +cmake -S "$REPO_DIR" -B "$BUILD_DIR" -DCMAKE_BUILD_TYPE=Release +cmake --build "$BUILD_DIR" --target install --parallel "$(nproc)" -- 2.54.0 From 126dbf4f321161e03c250fc23a9fb60ac3748c99 Mon Sep 17 00:00:00 2001 From: cesar Date: Tue, 8 Sep 2026 09:12:40 -0300 Subject: [PATCH 02/10] remove detection from the node MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit dora-zed publishes camera data only. The two engines under src/engines/, the objects output, the ZED_ENGINE/ZED_DETECTION_*/ZED_ONNX_* variables and the ZED_PC_MODE=ROI crop are gone, along with the standalone engine test and the model export script. This drops the unconditional ONNX Runtime requirement from CMakeLists, the Dockerfile and the install scripts, so the node now configures on a machine with no ONNX Runtime — which is what blocked the Jetson build, since there is no official ARM64 GPU binary. zed_native is dropped rather than kept: it is bound to the live camera handle and cannot run in a node that receives images over a dataflow. The removed code remains reachable at tag v0.1.0. Co-Authored-By: Claude Opus 5 Claude-Session: https://claude.ai/code/session_01Y7g2cBJ5e3r15ZbJ9WZn8c --- AGENTS.md | 21 +- CMakeLists.txt | 31 +-- Dockerfile | 19 -- README.md | 84 +------- scripts/export_yolo.py | 111 ----------- scripts/install_linux_arm.sh | 15 +- scripts/install_linux_x64.sh | 30 +-- scripts/install_windows.ps1 | 22 --- src/engines/engine.h | 27 --- src/engines/yolo_helpers.h | 136 ------------- src/engines/yolo_onnx.cc | 369 ----------------------------------- src/engines/yolo_onnx.h | 48 ----- src/engines/zed_native.cc | 99 ---------- src/engines/zed_native.h | 34 ---- src/main.cc | 101 +--------- src/test_yolo_onnx.cc | 113 ----------- test/assets/.gitkeep | 0 17 files changed, 18 insertions(+), 1242 deletions(-) delete mode 100644 scripts/export_yolo.py delete mode 100644 src/engines/engine.h delete mode 100644 src/engines/yolo_helpers.h delete mode 100644 src/engines/yolo_onnx.cc delete mode 100644 src/engines/yolo_onnx.h delete mode 100644 src/engines/zed_native.cc delete mode 100644 src/engines/zed_native.h delete mode 100644 src/test_yolo_onnx.cc delete mode 100644 test/assets/.gitkeep diff --git a/AGENTS.md b/AGENTS.md index b5c9da8..f37926f 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -4,7 +4,9 @@ This file provides guidance to AI coding assistants when working with code in th ## What this is -A dora-rs C++ node that interfaces with ZED stereo cameras. On each tick it grabs a frame, runs object detection, and publishes four outputs over the dora dataflow: a BGR image, an XYZRGBA point cloud, camera intrinsics, and detected objects with 3D positions. Camera management lives in `src/zed/`, detection engines in `src/engines/`, and the dora event loop in `src/main.cc`. +A dora-rs C++ node that interfaces with ZED stereo cameras. On each tick it grabs a frame and publishes it over the dora dataflow: a BGR image, an XYZRGBA point cloud and camera intrinsics. Camera management lives in `src/zed/` and the dora event loop in `src/main.cc`. + +The node is a camera source only. It ran inference in-process through `v0.1.0`, under `src/engines/`; that was removed so the node has no inference dependency and builds on any platform the ZED SDK supports. The removed code remains reachable at tag `v0.1.0`. ## Build @@ -16,7 +18,7 @@ The binary is installed to `bin/dora_zed` relative to the repo root. The script the dataflow's `build:` command: dora 1.0 executes build commands directly rather than through a shell, so `cd`, `&&` and command substitution do not work there. -**Requirements:** dora 1.0+ (verified with `dora-cli 1.0.1`), CMake 3.21+, ZED SDK 5.x (at `/usr/local/zed`), CUDA Toolkit, ONNX Runtime 1.20+ (system install), Rust stable toolchain (for dora-rs C++ bindings). +**Requirements:** dora 1.0+ (verified with `dora-cli 1.0.1`), CMake 3.21+, ZED SDK 5.x (at `/usr/local/zed`), CUDA Toolkit, Rust stable toolchain (for dora-rs C++ bindings). The first build clones dora-rs from GitHub and compiles `dora-node-api-cxx` via Cargo. This takes a while. Subsequent builds are incremental. If `DORA_ROOT_DIR` is set to a local dora checkout, it builds against that instead. @@ -29,30 +31,23 @@ dora up dora start dataflow.yml ``` -Camera and detection behavior is configured entirely through environment variables (see README.md for the full list: `ZED_RESOLUTION`, `ZED_FPS`, `ZED_DEPTH_MODE`, `ZED_DETECTION_MODEL`, etc.). +Camera behavior is configured entirely through environment variables (see README.md for the full list: `ZED_RESOLUTION`, `ZED_FPS`, `ZED_DEPTH_MODE`, etc.). ## Architecture - **`src/util/env.h`** — Shared inline env-variable parsing helpers (`get_env_int`, `get_env_str`, `get_env_bool`, `get_env_float`). - **`src/zed/camera.h`** — `zed::Config`, `zed::Calibration`, and `zed::Camera` class. Camera wraps the ZED SDK lifecycle: open (with retry loop), warmup, grab (image + point cloud), calibration, and close. Exposes `handle()` for engine access. - **`src/zed/camera.cc`** — Implementation. SDK enum parsers live in an anonymous namespace. -- **`src/engines/engine.h`** — `engines::DetectedObject` struct and `engines::Engine` abstract base class (`enable`, `detect`, `objects`, `label_names`). -- **`src/engines/zed_native.h/cc`** — `engines::ZedNative`: wraps ZED SDK's TensorRT-optimized object detection module. -- **`src/engines/yolo_helpers.h`** — Header-only shared helpers used by the ONNX engine: `RawDetection`, `nms()`, `letterbox()`, `fuse_bbox()`, `backproject()`. Lives in `engines::detail` namespace. -- **`src/engines/yolo_onnx.h/cc`** — `engines::YoloOnnx` / `YoloOnnxConfig`: closed-vocabulary detection for any single-input YOLO ONNX model (YOLOv8, YOLOv11, YOLOv5, etc.). Auto-detects output layout (`NC_FIRST` / `NC_LAST` / `NC_LAST_OBJ`) from tensor shape; override with `ZED_ONNX_OUTPUT_FORMAT`. Labels loaded from `ZED_ONNX_LABELS` or `ZED_ONNX_LABELS_FILE`. -- **`src/main.cc`** — Dora event loop. Selects engine via `ZED_ENGINE` env var (`yolo_onnx`/`onnx` or `native`), holds a `std::unique_ptr`. Parses `ZED_PC_MODE` (`FULL`/`HALF`/`ROI`) once before the loop. On any input event (intended to be driven by a tick timer; the id is not inspected): calls `camera.grab()` and `detector->detect()`, then sends `image_bgr`, `point_cloud`, `camera_info`, and `objects` outputs. Point cloud output varies by mode: `FULL` sends the raw `sl::Mat` pointer (no copy); `HALF` downsamples by 2 in each dimension; `ROI` emits only valid points within detected bounding boxes as a flat `[N, 4]` array. Handles reconnection on grab failure. +- **`src/main.cc`** — Dora event loop. Parses `ZED_PC_MODE` (`FULL`/`HALF`) once before the loop. On any input event (intended to be driven by a tick timer; the id is not inspected): calls `camera.grab()`, then sends `image_bgr`, `point_cloud` and `camera_info`. Point cloud output varies by mode: `FULL` sends the raw `sl::Mat` pointer (no copy); `HALF` downsamples by 2 in each dimension. Handles reconnection on grab failure. - **`DoraTargets.cmake`** — Handles building or fetching dora-rs and generating the CXX bridge files. Two paths: local (`DORA_ROOT_DIR`) or git clone. Produces `dora_node_api_cxx` library and the `node_bridge.cc` glue. - **`scripts/build.sh`** — Configure/compile/install in one shell-free-callable step. Referenced by `dataflow.yml`, the `Dockerfile` and the README. - **`dataflow.yml`** — Example dora dataflow definition using the `git:` source format, pinned with `tag:` — dora 1.0 pins a source with the mutually exclusive `tag:`, `rev:` or `branch:`. -- **`Dockerfile`** — Ubuntu 24.04 x86-64 build environment (CUDA 12.8, ZED SDK 5.2, ONNX Runtime, Rust). No ARM64/Jetson image; build natively there. No camera needed; run the compile with `--gpus all`. Sets `NVIDIA_DRIVER_CAPABILITIES=compute,utility,video` — the `video` capability is what makes the container runtime mount NVDEC/NVENC, which `libsl_zed.so` links against. ONNX Runtime's CMake package files are relocated to `/usr/local/lib/cmake` because Ubuntu's multiarch CMake does not search `lib64`. Rust lives at `/usr/local/{rustup,cargo}` rather than `/root` so the build can run as the invoking user. `docker build -t dora-zed-build . && docker run --rm --gpus all --user "$(id -u):$(id -g)" -v "$PWD:/src" -v dora-zed-cargo:/usr/local/cargo/registry dora-zed-build`. -- **`scripts/export_yolo.py`** — PEP 723 uv script. Exports a standard YOLOv11 model to ONNX for closed-vocabulary detection. Run: `uv run scripts/export_yolo.py --model n --output-dir models/`. -- **`src/test_yolo_onnx.cc`** — Standalone C++ test binary (`bin/test_yolo_onnx`). Exercises `YoloOnnx` with an image (default `test/assets/yolo.png`). No dora and no live camera required. Uses the same env vars as the main binary. Saves an annotated result image alongside the input. +- **`Dockerfile`** — Ubuntu 24.04 x86-64 build environment (CUDA 12.8, ZED SDK 5.2, Rust). No ARM64/Jetson image; build natively there. No camera needed; run the compile with `--gpus all`. Sets `NVIDIA_DRIVER_CAPABILITIES=compute,utility,video` — the `video` capability is what makes the container runtime mount NVDEC/NVENC, which `libsl_zed.so` links against. Rust lives at `/usr/local/{rustup,cargo}` rather than `/root` so the build can run as the invoking user. `docker build -t dora-zed-build . && docker run --rm --gpus all --user "$(id -u):$(id -g)" -v "$PWD:/src" -v dora-zed-cargo:/usr/local/cargo/registry dora-zed-build`. ## Key APIs - **dora C++ API** (``): `init_dora_node()`, `dora_node.events->next()`, `event_type()`, `send_output_with_metadata()`, `new_metadata()`. -- **ZED SDK** (``): `sl::Camera`, `sl::InitParameters`, `sl::RuntimeParameters`, `sl::Mat`, `sl::ObjectDetectionParameters`, `sl::Objects`. -- **ONNX Runtime** (``): `Ort::Env`, `Ort::Session`, `Ort::SessionOptions`, `Ort::Value`. Used by the `yolo_onnx` engine. +- **ZED SDK** (``): `sl::Camera`, `sl::InitParameters`, `sl::RuntimeParameters`, `sl::Mat`. - Output data is sent as raw byte slices (`rust::Slice`) with typed metadata (shape, dtype, encoding). ## Tools diff --git a/CMakeLists.txt b/CMakeLists.txt index 3baf07b..77fa223 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.21) -project(dora_zed VERSION 0.1.0 LANGUAGES C CXX) +project(dora_zed VERSION 0.2.0 LANGUAGES C CXX) set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_FLAGS "-fPIC") @@ -19,18 +19,12 @@ find_package(zed REQUIRED) find_package(OpenCV REQUIRED) -find_package(onnxruntime REQUIRED) -set(ONNXRUNTIME_INCLUDE_DIRS "") # propagated via imported target -set(ONNXRUNTIME_LIBRARIES onnxruntime::onnxruntime) - link_directories(${dora_link_dirs}) link_directories(${ZED_LIBRARY_DIR}) add_executable(dora_zed src/main.cc src/zed/camera.cc - src/engines/zed_native.cc - src/engines/yolo_onnx.cc ${node_bridge} ) add_dependencies(dora_zed Dora_cxx) @@ -44,29 +38,8 @@ target_include_directories( ${ZED_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} ${CUDAToolkit_INCLUDE_DIRS} - ${ONNXRUNTIME_INCLUDE_DIRS} ) -target_link_libraries(dora_zed dora_node_api_cxx ${ZED_LIBRARIES} ${OpenCV_LIBS} ${ONNXRUNTIME_LIBRARIES}) +target_link_libraries(dora_zed dora_node_api_cxx ${ZED_LIBRARIES} ${OpenCV_LIBS}) install(TARGETS dora_zed DESTINATION ${CMAKE_CURRENT_SOURCE_DIR}/bin) - -# Standalone detector tests — no dora, no live camera required - -add_executable(test_yolo_onnx - src/test_yolo_onnx.cc - src/engines/yolo_onnx.cc -) -target_include_directories(test_yolo_onnx PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/src - ${ZED_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} - ${CUDAToolkit_INCLUDE_DIRS} - ${ONNXRUNTIME_INCLUDE_DIRS} -) -target_link_libraries(test_yolo_onnx PRIVATE - ${ZED_LIBRARIES} - ${OpenCV_LIBS} - ${ONNXRUNTIME_LIBRARIES} -) -install(TARGETS test_yolo_onnx DESTINATION ${CMAKE_CURRENT_SOURCE_DIR}/bin) diff --git a/Dockerfile b/Dockerfile index b7c0016..437d4bc 100644 --- a/Dockerfile +++ b/Dockerfile @@ -21,7 +21,6 @@ FROM nvidia/cuda:12.8.1-devel-ubuntu24.04 ARG ZED_VERSION=5.2 -ARG ORT_VERSION=1.24.4 ENV DEBIAN_FRONTEND=noninteractive @@ -51,24 +50,6 @@ RUN curl -fSL -o /tmp/ZED_SDK.run \ && chmod -R a+rX /usr/local/zed \ && rm -f /tmp/ZED_SDK.run -# The tarball puts its CMake package files under lib/, but onnxruntimeTargets.cmake -# resolves the library as /lib64. Ubuntu is multiarch, so CMake does not -# search lib64 for packages and ld.so does not search /usr/local/lib64 at all: -# keep the libraries in lib64, move only the package files to lib/cmake, and add -# lib64 to the linker path. -RUN curl -fSL -o /tmp/ort.tgz \ - "https://github.com/microsoft/onnxruntime/releases/download/v${ORT_VERSION}/onnxruntime-linux-x64-gpu-${ORT_VERSION}.tgz" \ - && tar -xzf /tmp/ort.tgz -C /tmp \ - && mkdir -p /usr/local/include/onnxruntime /usr/local/lib64 \ - && cp -r /tmp/onnxruntime-linux-x64-gpu-${ORT_VERSION}/include/* /usr/local/include/onnxruntime/ \ - && cp -r /tmp/onnxruntime-linux-x64-gpu-${ORT_VERSION}/lib/* /usr/local/lib64/ \ - && mkdir -p /usr/local/lib/cmake \ - && mv /usr/local/lib64/cmake/onnxruntime /usr/local/lib/cmake/ \ - && rmdir /usr/local/lib64/cmake \ - && echo /usr/local/lib64 > /etc/ld.so.conf.d/onnxruntime.conf \ - && ldconfig \ - && rm -rf /tmp/ort.tgz /tmp/onnxruntime-linux-x64-gpu-${ORT_VERSION} - ENV NVIDIA_DRIVER_CAPABILITIES=compute,utility,video WORKDIR /src diff --git a/README.md b/README.md index 8a5c825..80a5c08 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # dora-zed -Dora node for ZED stereo cameras. Publishes BGR images, point clouds, camera intrinsics, and 3D object detections. +Dora node for ZED stereo cameras. Publishes BGR images, point clouds and camera intrinsics. ## Usage @@ -19,7 +19,6 @@ nodes: - image_bgr - point_cloud - camera_info - - objects ``` See [`dataflow.yml`](dataflow.yml) for a complete runnable example with a camera display node. @@ -33,7 +32,6 @@ See [`dataflow.yml`](dataflow.yml) for a complete runnable example with a camera | [ZED SDK](https://www.stereolabs.com/developers/release) | 5.x | Bundles OpenCV; default path `/usr/local/zed` | | [CUDA Toolkit](https://developer.nvidia.com/cuda-downloads) | — | Required by ZED SDK | | [Rust toolchain](https://rustup.rs) | stable | Builds dora-rs C++ bindings | -| [ONNX Runtime](https://github.com/microsoft/onnxruntime/releases) | 1.20+ | Required at configure time; used at runtime only by `yolo_onnx` | ### Installation scripts @@ -43,9 +41,7 @@ See [`dataflow.yml`](dataflow.yml) for a complete runnable example with a camera | NVIDIA Jetson (ARM64) | `sudo scripts/install_linux_arm.sh` | | Windows x64 (PowerShell admin) | `scripts/install_windows.ps1` | -> **Jetson:** No official ONNX Runtime GPU binary for ARM64. It is still needed to configure the build; only the `yolo_onnx` engine uses it at runtime. Requires [building from source](https://onnxruntime.ai/docs/build/eps.html#tensorrt). -> -> **Windows:** Install ZED SDK manually from [stereolabs.com](https://www.stereolabs.com/developers/release), then pass the ONNX Runtime path: `cmake .. -DCMAKE_PREFIX_PATH=C:\onnxruntime` +> **Windows:** Install the ZED SDK manually from [stereolabs.com](https://www.stereolabs.com/developers/release). ## Local build @@ -65,7 +61,7 @@ bindings. ### Containerized build -[`Dockerfile`](Dockerfile) provides the full toolchain — CUDA, ZED SDK, ONNX Runtime and Rust — on Ubuntu 24.04, the newest release Stereolabs ships a ZED SDK for. No camera is needed. +[`Dockerfile`](Dockerfile) provides the full toolchain — CUDA, ZED SDK and Rust — on Ubuntu 24.04, the newest release Stereolabs ships a ZED SDK for. No camera is needed. ```bash docker build -t dora-zed-build . @@ -106,75 +102,18 @@ All behavior is controlled via environment variables. | `ZED_WARMUP_FRAMES` | `30` | Frames to discard on startup | | `ZED_RETRY_INTERVAL_SECONDS` | `5` | Reconnection interval (s) | -### Detection engine - -Set `ZED_ENGINE` to select the detection backend: - -| `ZED_ENGINE` | Custom model? | Classes | -|---|---|---| -| `native` (default) | No — ZED SDK built-in TensorRT | Fixed: 7 categories | -| `yolo_onnx` (alias: `onnx`) | Yes — any single-input YOLO ONNX (v5/v8/v11) | Fixed labels from file or env var | - -#### `native` engine - -Uses ZED SDK's built-in TensorRT models — no model files to download or configure. `ZED_DETECTION_MODEL` selects the speed/accuracy variant, not a file path. - -Detects 7 fixed categories: PERSON, VEHICLE, BAG, ANIMAL, ELECTRONICS, FRUIT_VEGETABLE, SPORT. - -| Variable | Default | Description | -|---|---|---| -| `ZED_DETECTION_MODEL` | `MULTI_CLASS_BOX_MEDIUM` | `MULTI_CLASS_BOX_FAST`, `MULTI_CLASS_BOX_MEDIUM`, or `MULTI_CLASS_BOX_ACCURATE` | -| `ZED_DETECTION_CONFIDENCE` | `40` | Confidence threshold (0–100) | -| `ZED_DETECTION_TRACKING` | `false` | Enable object tracking | - -#### `yolo_onnx` engine (closed-vocabulary) - -For any single-input YOLO ONNX model with fixed class names (YOLOv8, YOLOv11, YOLOv5, custom-trained). - -Export a YOLOv11 model once: - -```bash -uv run scripts/export_yolo.py --model n --output-dir models/ -``` - -Then run: - -```bash -ZED_ENGINE=yolo_onnx \ -ZED_ONNX_MODEL_PATH=models/yolo11n_i640.onnx \ -ZED_ONNX_LABELS_FILE=models/coco80.txt \ -dora start dataflow.yml -``` - -**Required:** - -| Variable | Description | -|---|---| -| `ZED_ONNX_MODEL_PATH` | Path to YOLO ONNX model | -| `ZED_ONNX_LABELS` or `ZED_ONNX_LABELS_FILE` | Class names (inline CSV or one-per-line file) | - -**Optional:** - -| Variable | Default | Description | -|---|---|---| -| `ZED_ONNX_CONFIDENCE` | `0.25` | Confidence threshold (0.0–1.0) | -| `ZED_ONNX_NMS_THRESHOLD` | `0.45` | NMS IoU threshold | -| `ZED_ONNX_INPUT_SIZE` | `640` | Model input size (px) | -| `ZED_ONNX_OUTPUT_FORMAT` | `auto` | Output layout: `auto`, `nc_first`/`yolov8`/`yolov11`, `nc_last`, `nc_last_obj`/`yolov5`. `auto` detects from tensor shape. | - ### Point cloud | `ZED_PC_MODE` | Shape | Description | |---|---|---| | `FULL` (default) | `[H, W, 4]` | Full resolution grid (~14.7 MB/frame at 720p) | | `HALF` | `[H/2, W/2, 4]` | Stride-2 downsample (~4× smaller) | -| `ROI` | `[N, 4]` | Valid points within detected bounding boxes only | ## Inputs | Input | Description | |---|---| -| `tick` | Triggers a camera grab and detection. Connect to `dora/timer/millis/N`. | +| `tick` | Triggers a camera grab. Connect to `dora/timer/millis/N`. | ## Outputs @@ -183,18 +122,3 @@ dora start dataflow.yml | `image_bgr` | `uint8 [H, W, 3]` | BGR image. Metadata: `shape`, `encoding=bgr8`, intrinsics, `timestamp_ns`. | | `point_cloud` | `float32 [*, 4]` | XYZRGBA point cloud. Shape depends on `ZED_PC_MODE`. Metadata: `units=mm`, intrinsics. | | `camera_info` | `float32 [4]` | `[fx, fy, cx, cy]`. Metadata: `distortion` coefficients. | -| `objects` | `float32 [N, 37]` | Detected objects. See below. | - -### `objects` layout - -Each of the N rows contains 37 floats: - -| Index | Field | Description | -|---|---|---| -| 0 | `label_id` | Class index | -| 1 | `confidence` | Score (0–100) | -| 2–4 | `x, y, z` | 3D center (mm) | -| 5–12 | `bb2d` | 2D bounding box: 4 corners × (x, y) px | -| 13–36 | `bb3d` | 3D bounding box: 8 corners × (x, y, z) mm | - -Metadata includes `engine`, `label_names`, `image_width`, `image_height`, and `timestamp_ns`. An empty array `[0, 37]` is sent when no objects are detected. diff --git a/scripts/export_yolo.py b/scripts/export_yolo.py deleted file mode 100644 index 8e1c0b3..0000000 --- a/scripts/export_yolo.py +++ /dev/null @@ -1,111 +0,0 @@ -# /// script -# requires-python = ">=3.10" -# dependencies = ["ultralytics>=8.3"] -# /// -"""Export a standard YOLOv11 model to ONNX for closed-vocabulary detection. - -The exported model has one input: - images (1, 3, imgsz, imgsz) float32 - -Class names are fixed at training time. Provide them at runtime via -ZED_ONNX_LABELS or ZED_ONNX_LABELS_FILE. - -Usage: - uv run scripts/export_yolo.py - uv run scripts/export_yolo.py --model s --output-dir models/ - uv run scripts/export_yolo.py --model m --imgsz 1280 -""" - -import argparse -import shutil -import sys -from pathlib import Path - -MODELS = { - "n": "yolo11n", - "s": "yolo11s", - "m": "yolo11m", - "l": "yolo11l", - "x": "yolo11x", -} - - -def parse_args(): - p = argparse.ArgumentParser( - description="Export YOLOv11 to ONNX for closed-vocabulary detection" - ) - p.add_argument( - "--model", "-m", - choices=list(MODELS), - default="n", - help="Model size: n (~6 MB), s (~18 MB), m (~38 MB), l (~49 MB), x (~109 MB). Default: n", - ) - p.add_argument( - "--imgsz", - type=int, - default=640, - help="Input image size in pixels (square). Default: 640", - ) - p.add_argument( - "--output-dir", "-o", - default=".", - help="Output directory for the .onnx file. Default: current directory", - ) - return p.parse_args() - - -def verify_output(onnx_path: Path): - try: - import onnx - model = onnx.load(str(onnx_path)) - inputs = [(i.name, [d.dim_value for d in i.type.tensor_type.shape.dim]) - for i in model.graph.input] - outputs = [(o.name, [d.dim_value for d in o.type.tensor_type.shape.dim]) - for o in model.graph.output] - print(f" Inputs: {inputs}") - print(f" Outputs: {outputs}") - except ImportError: - print(" (install 'onnx' to verify model structure)") - - -def main(): - args = parse_args() - - try: - from ultralytics import YOLO - except ImportError: - print("Error: ultralytics not installed. Run: uv run scripts/export_yolo.py", file=sys.stderr) - sys.exit(1) - - model_name = MODELS[args.model] - out_dir = Path(args.output_dir) - out_dir.mkdir(parents=True, exist_ok=True) - out_name = f"{model_name}_i{args.imgsz}.onnx" - dst = out_dir / out_name - - print(f"Model: {model_name}.pt") - print(f"Input size: {args.imgsz}") - print(f"Output: {dst}") - print() - - model = YOLO(f"{model_name}.pt") - exported = Path(model.export(format="onnx", imgsz=args.imgsz)) - - if exported.resolve() != dst.resolve(): - shutil.move(str(exported), str(dst)) - - print(f"\nVerifying exported model...") - verify_output(dst) - - print(f"\nExported: {dst.resolve()}") - print(f"\nRun with (COCO labels example):") - print(f" ZED_ENGINE=onnx \\") - print(f" ZED_ONNX_MODEL_PATH={dst.resolve()} \\") - print(f" ZED_ONNX_LABELS_FILE=models/coco80.txt \\") - print(f" dora start dataflow.yml") - print(f"\nOr with inline labels:") - print(f" ZED_ONNX_LABELS=\"person,car,chair\" \\") - - -if __name__ == "__main__": - main() diff --git a/scripts/install_linux_arm.sh b/scripts/install_linux_arm.sh index f6d6afd..14a9016 100755 --- a/scripts/install_linux_arm.sh +++ b/scripts/install_linux_arm.sh @@ -9,11 +9,6 @@ # JetPack 6.2.2 (L4T 36.5) # JetPack 7.0 (L4T 38.2) — Jetson Thor # JetPack 7.1 (L4T 38.4) -# -# ONNX Runtime: Microsoft does not publish official GPU binaries for ARM64. -# ONNX Runtime is still needed to configure the build; only the yolo_onnx -# engine uses it at runtime. Build it from source: -# https://onnxruntime.ai/docs/build/eps.html#tensorrt set -euo pipefail ZED_VERSION="5.2" @@ -63,13 +58,5 @@ else sudo "${TMP_DIR}/ZED_SDK.run" -- silent skip_python skip_tools fi -# --- ONNX Runtime --- -echo "" -echo ">>> ONNX Runtime (ARM64 GPU): no official pre-built binary available." -echo " Needed to configure the build; only yolo_onnx uses it at runtime." -echo " Build from source:" -echo " https://onnxruntime.ai/docs/build/eps.html#tensorrt" -echo "" - -echo ">>> Done. You can now run: mkdir -p build && cd build && cmake .. && make install" +echo ">>> Done. You can now run: scripts/build.sh" echo ">>> Downloaded files are in ${TMP_DIR}" diff --git a/scripts/install_linux_x64.sh b/scripts/install_linux_x64.sh index ff05d0a..ed1e03d 100755 --- a/scripts/install_linux_x64.sh +++ b/scripts/install_linux_x64.sh @@ -6,7 +6,6 @@ set -euo pipefail ZED_VERSION="5.2" -ORT_VERSION="1.24.4" SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" TMP_DIR="${SCRIPT_DIR}/../tmp" @@ -34,32 +33,5 @@ else sudo "${TMP_DIR}/ZED_SDK.run" -- silent skip_python skip_tools fi -# --- ONNX Runtime (C++ GPU, installs to /usr/local) --- -if [ -f /usr/local/lib64/libonnxruntime.so ] && [ -d /usr/local/lib/cmake/onnxruntime ]; then - echo ">>> ONNX Runtime already installed — skipping." -else - echo ">>> Downloading ONNX Runtime ${ORT_VERSION} GPU..." - download \ - "https://github.com/microsoft/onnxruntime/releases/download/v${ORT_VERSION}/onnxruntime-linux-x64-gpu-${ORT_VERSION}.tgz" \ - "${TMP_DIR}/onnxruntime-linux-x64-gpu-${ORT_VERSION}.tgz" - echo ">>> Extracting..." - tar -xzf "${TMP_DIR}/onnxruntime-linux-x64-gpu-${ORT_VERSION}.tgz" -C "${TMP_DIR}" - echo ">>> Installing ONNX Runtime to /usr/local..." - sudo mkdir -p /usr/local/include/onnxruntime - sudo cp -r "${TMP_DIR}/onnxruntime-linux-x64-gpu-${ORT_VERSION}/include/"* /usr/local/include/onnxruntime/ - sudo mkdir -p /usr/local/lib64 - sudo cp -r "${TMP_DIR}/onnxruntime-linux-x64-gpu-${ORT_VERSION}/lib/"* /usr/local/lib64/ - # onnxruntimeTargets.cmake resolves the library as /lib64, but Ubuntu - # is multiarch: CMake does not search lib64 for packages and ld.so does not - # search /usr/local/lib64 at all. Move only the package files to lib/cmake - # and put lib64 on the linker path. - sudo mkdir -p /usr/local/lib/cmake - sudo rm -rf /usr/local/lib/cmake/onnxruntime - sudo mv /usr/local/lib64/cmake/onnxruntime /usr/local/lib/cmake/ - sudo rmdir /usr/local/lib64/cmake - echo /usr/local/lib64 | sudo tee /etc/ld.so.conf.d/onnxruntime.conf > /dev/null - sudo ldconfig -fi - -echo ">>> Done. You can now run: mkdir -p build && cd build && cmake .. && make install" +echo ">>> Done. You can now run: scripts/build.sh" echo ">>> Downloaded files are in ${TMP_DIR}" diff --git a/scripts/install_windows.ps1 b/scripts/install_windows.ps1 index 8f8979e..5a10cec 100755 --- a/scripts/install_windows.ps1 +++ b/scripts/install_windows.ps1 @@ -10,10 +10,6 @@ $ErrorActionPreference = "Stop" $ZED_INSTALL_DIR = "C:\Program Files (x86)\ZED SDK" -$ORT_VERSION = "1.24.4" -$ORT_NAME = "onnxruntime-win-x64-gpu_cuda13-$ORT_VERSION" -$ORT_URL = "https://github.com/microsoft/onnxruntime/releases/download/v$ORT_VERSION/$ORT_NAME.zip" -$ORT_DEST = "C:\onnxruntime" # --- ZED SDK --- if (Test-Path $ZED_INSTALL_DIR) { @@ -25,23 +21,5 @@ if (Test-Path $ZED_INSTALL_DIR) { Write-Host "" } -# --- ONNX Runtime --- -if (Test-Path "$ORT_DEST\lib\onnxruntime.dll") { - Write-Host ">>> ONNX Runtime already installed at '$ORT_DEST' — skipping." -} else { - Write-Host ">>> Downloading ONNX Runtime $ORT_VERSION GPU (CUDA 13)..." - $tmp = "$env:TEMP\$ORT_NAME.zip" - Invoke-WebRequest -Uri $ORT_URL -OutFile $tmp - - Write-Host ">>> Extracting to $ORT_DEST..." - Expand-Archive -Path $tmp -DestinationPath $env:TEMP -Force - if (Test-Path $ORT_DEST) { Remove-Item $ORT_DEST -Recurse -Force } - Move-Item "$env:TEMP\$ORT_NAME" $ORT_DEST - Remove-Item $tmp - - Write-Host ">>> ONNX Runtime installed to $ORT_DEST" - Write-Host " Pass it to CMake: cmake .. -DCMAKE_PREFIX_PATH=C:\onnxruntime" -} - Write-Host "" Write-Host ">>> Done." diff --git a/src/engines/engine.h b/src/engines/engine.h deleted file mode 100644 index eb3f07a..0000000 --- a/src/engines/engine.h +++ /dev/null @@ -1,27 +0,0 @@ -#pragma once - -#include -#include -#include - -namespace engines { - -struct DetectedObject { - int label_id; - float confidence; - float position[3]; // center x, y, z (mm) - float bounding_box_2d[8]; // 4 corners × 2 coords (pixels) - float bounding_box_3d[24]; // 8 corners × 3 coords (mm) -}; - -class Engine { -public: - virtual ~Engine() = default; - - virtual void enable(sl::Camera& camera) = 0; - virtual void detect(sl::Camera& camera) = 0; - virtual const std::vector& objects() const = 0; - virtual std::string label_names() const = 0; -}; - -} // namespace engines diff --git a/src/engines/yolo_helpers.h b/src/engines/yolo_helpers.h deleted file mode 100644 index 1f4645e..0000000 --- a/src/engines/yolo_helpers.h +++ /dev/null @@ -1,136 +0,0 @@ -#pragma once - -#include -#include - -#include -#include -#include - -namespace engines::detail { - -// --- NMS --- - -struct RawDetection { - float x1, y1, x2, y2; - float score; - int class_id; -}; - -inline float iou(const RawDetection& a, const RawDetection& b) { - float inter_x1 = std::max(a.x1, b.x1); - float inter_y1 = std::max(a.y1, b.y1); - float inter_x2 = std::min(a.x2, b.x2); - float inter_y2 = std::min(a.y2, b.y2); - float inter_w = std::max(0.0f, inter_x2 - inter_x1); - float inter_h = std::max(0.0f, inter_y2 - inter_y1); - float inter_area = inter_w * inter_h; - float area_a = (a.x2 - a.x1) * (a.y2 - a.y1); - float area_b = (b.x2 - b.x1) * (b.y2 - b.y1); - float union_area = area_a + area_b - inter_area; - return union_area > 0.0f ? inter_area / union_area : 0.0f; -} - -inline std::vector nms(std::vector dets, float threshold) { - std::sort(dets.begin(), dets.end(), - [](const RawDetection& a, const RawDetection& b) { - return a.score > b.score; - }); - std::vector suppressed(dets.size(), false); - std::vector result; - for (size_t i = 0; i < dets.size(); i++) { - if (suppressed[i]) continue; - result.push_back(dets[i]); - for (size_t j = i + 1; j < dets.size(); j++) { - if (!suppressed[j] && iou(dets[i], dets[j]) > threshold) - suppressed[j] = true; - } - } - return result; -} - -// --- Letterbox resize --- - -inline cv::Mat letterbox(const cv::Mat& src, int size, - float& scale, float& pad_x, float& pad_y) { - float scale_w = static_cast(size) / src.cols; - float scale_h = static_cast(size) / src.rows; - scale = std::min(scale_w, scale_h); - int new_w = static_cast(std::round(src.cols * scale)); - int new_h = static_cast(std::round(src.rows * scale)); - pad_x = (size - new_w) / 2.0f; - pad_y = (size - new_h) / 2.0f; - - cv::Mat resized; - cv::resize(src, resized, {new_w, new_h}); - - cv::Mat canvas(size, size, CV_8UC3, cv::Scalar(114, 114, 114)); - resized.copyTo(canvas(cv::Rect( - static_cast(pad_x), static_cast(pad_y), new_w, new_h))); - return canvas; -} - -// --- Point cloud fusion --- - -struct FusionResult { - float cx, cy, cz; - float depth_near, depth_far; - bool valid = false; -}; - -inline FusionResult fuse_bbox(const sl::Mat& pc, int x1, int y1, int x2, int y2) { - FusionResult result{}; - int w = static_cast(pc.getWidth()); - int h = static_cast(pc.getHeight()); - x1 = std::max(x1, 0); - y1 = std::max(y1, 0); - x2 = std::min(x2, w - 1); - y2 = std::min(y2, h - 1); - - std::vector xs, ys, zs; - xs.reserve((x2 - x1) * (y2 - y1)); - ys.reserve(xs.capacity()); - zs.reserve(xs.capacity()); - - for (int y = y1; y <= y2; y++) { - for (int x = x1; x <= x2; x++) { - sl::float4 pt; - pc.getValue(x, y, &pt); - if (!std::isnan(pt.x) && !std::isnan(pt.y) && !std::isnan(pt.z) && - !std::isinf(pt.x) && !std::isinf(pt.y) && !std::isinf(pt.z)) { - xs.push_back(pt.x); - ys.push_back(pt.y); - zs.push_back(pt.z); - } - } - } - - if (xs.empty()) return result; - - std::sort(xs.begin(), xs.end()); - std::sort(ys.begin(), ys.end()); - std::sort(zs.begin(), zs.end()); - - size_t n = xs.size(); - size_t mid = n / 2; - size_t p25 = n / 4; - size_t p75 = 3 * n / 4; - - result.cx = xs[mid]; - result.cy = ys[mid]; - result.cz = zs[mid]; - result.depth_near = zs[p25]; - result.depth_far = zs[p75]; - result.valid = true; - return result; -} - -inline void backproject(float u, float v, float z, - float fx, float fy, float cx, float cy, - float& X, float& Y, float& Z) { - Z = z; - X = (u - cx) * z / fx; - Y = (v - cy) * z / fy; -} - -} // namespace engines::detail diff --git a/src/engines/yolo_onnx.cc b/src/engines/yolo_onnx.cc deleted file mode 100644 index f170edd..0000000 --- a/src/engines/yolo_onnx.cc +++ /dev/null @@ -1,369 +0,0 @@ -#include "engines/yolo_onnx.h" -#include "engines/yolo_helpers.h" -#include "util/env.h" - -#include -#include -#include - -#include -#include -#include -#include -#include - -namespace engines { - -using namespace detail; - -// --- Impl --- - -struct YoloOnnx::Impl { - Ort::Env env{ORT_LOGGING_LEVEL_WARNING, "yolo_onnx"}; - Ort::SessionOptions session_options; - std::unique_ptr session; - Ort::AllocatorWithDefaultOptions allocator; - - std::string input_name; - std::string output_name; - int num_classes = 0; - YoloOutputLayout resolved_layout = YoloOutputLayout::NC_FIRST; - - std::vector run_inference(const YoloOnnxConfig& config, - const cv::Mat& image_bgr); -}; - -// --- Impl::run_inference --- - -std::vector YoloOnnx::Impl::run_inference( - const YoloOnnxConfig& config, const cv::Mat& image_bgr) -{ - if (!session) return {}; - - int orig_w = image_bgr.cols; - int orig_h = image_bgr.rows; - - cv::Mat rgb; - cv::cvtColor(image_bgr, rgb, cv::COLOR_BGR2RGB); - - float scale, pad_x, pad_y; - cv::Mat lb = letterbox(rgb, config.input_size, scale, pad_x, pad_y); - - cv::Mat lb_float; - lb.convertTo(lb_float, CV_32F, 1.0f / 255.0f); - - int sz = config.input_size; - std::vector input_data(3 * sz * sz); - for (int c = 0; c < 3; c++) { - for (int h = 0; h < sz; h++) { - for (int w = 0; w < sz; w++) { - input_data[c * sz * sz + h * sz + w] = - lb_float.at(h, w)[c]; - } - } - } - - auto mem_info = Ort::MemoryInfo::CreateCpu( - OrtAllocatorType::OrtArenaAllocator, OrtMemTypeDefault); - - std::vector image_shape{1, 3, sz, sz}; - Ort::Value image_tensor = Ort::Value::CreateTensor( - mem_info, input_data.data(), input_data.size(), - image_shape.data(), image_shape.size()); - - const char* input_name_cstr = input_name.c_str(); - const char* output_name_cstr = output_name.c_str(); - - std::vector outputs; - try { - outputs = session->Run( - Ort::RunOptions{nullptr}, - &input_name_cstr, &image_tensor, 1, - &output_name_cstr, 1); - } catch (const Ort::Exception& e) { - std::cerr << "YoloOnnx: inference failed: " << e.what() << std::endl; - return {}; - } - - auto out_shape = outputs[0].GetTensorTypeAndShapeInfo().GetShape(); - if (out_shape.size() < 3) return {}; - - const float* out = outputs[0].GetTensorData(); - int nc = num_classes; - if (nc <= 0) return {}; - - std::vector candidates; - - if (resolved_layout == YoloOutputLayout::NC_FIRST) { - // (1, 4+N, anchors) — YOLOv8, YOLOv11 - int num_anchors = static_cast(out_shape[2]); - for (int a = 0; a < num_anchors; a++) { - float cx_lb = out[0 * num_anchors + a]; - float cy_lb = out[1 * num_anchors + a]; - float w_lb = out[2 * num_anchors + a]; - float h_lb = out[3 * num_anchors + a]; - - float best_score = 0.0f; - int best_class = 0; - for (int c = 0; c < nc; c++) { - float s = out[(4 + c) * num_anchors + a]; - if (s > best_score) { best_score = s; best_class = c; } - } - if (best_score < config.confidence_threshold) continue; - - float x1_lb = cx_lb - w_lb / 2.0f; - float y1_lb = cy_lb - h_lb / 2.0f; - float x2_lb = cx_lb + w_lb / 2.0f; - float y2_lb = cy_lb + h_lb / 2.0f; - - float x1 = std::max(0.0f, std::min((x1_lb - pad_x) / scale, static_cast(orig_w - 1))); - float y1 = std::max(0.0f, std::min((y1_lb - pad_y) / scale, static_cast(orig_h - 1))); - float x2 = std::max(0.0f, std::min((x2_lb - pad_x) / scale, static_cast(orig_w - 1))); - float y2 = std::max(0.0f, std::min((y2_lb - pad_y) / scale, static_cast(orig_h - 1))); - candidates.push_back({x1, y1, x2, y2, best_score, best_class}); - } - } else { - // (1, anchors, 4+N) or (1, anchors, 5+N) - int num_anchors = static_cast(out_shape[1]); - int row_stride = static_cast(out_shape[2]); - int cls_offset = (resolved_layout == YoloOutputLayout::NC_LAST_OBJ) ? 5 : 4; - - for (int a = 0; a < num_anchors; a++) { - const float* row = out + a * row_stride; - float cx_lb = row[0], cy_lb = row[1], w_lb = row[2], h_lb = row[3]; - float obj_conf = (resolved_layout == YoloOutputLayout::NC_LAST_OBJ) ? row[4] : 1.0f; - - float best_score = 0.0f; - int best_class = 0; - for (int c = 0; c < nc; c++) { - float s = obj_conf * row[cls_offset + c]; - if (s > best_score) { best_score = s; best_class = c; } - } - if (best_score < config.confidence_threshold) continue; - - float x1_lb = cx_lb - w_lb / 2.0f; - float y1_lb = cy_lb - h_lb / 2.0f; - float x2_lb = cx_lb + w_lb / 2.0f; - float y2_lb = cy_lb + h_lb / 2.0f; - - float x1 = std::max(0.0f, std::min((x1_lb - pad_x) / scale, static_cast(orig_w - 1))); - float y1 = std::max(0.0f, std::min((y1_lb - pad_y) / scale, static_cast(orig_h - 1))); - float x2 = std::max(0.0f, std::min((x2_lb - pad_x) / scale, static_cast(orig_w - 1))); - float y2 = std::max(0.0f, std::min((y2_lb - pad_y) / scale, static_cast(orig_h - 1))); - candidates.push_back({x1, y1, x2, y2, best_score, best_class}); - } - } - - return nms(std::move(candidates), config.nms_threshold); -} - -// --- Config --- - -YoloOnnxConfig YoloOnnxConfig::from_env() { - YoloOnnxConfig c; - c.model_path = util::get_env_str("ZED_ONNX_MODEL_PATH", c.model_path); - c.confidence_threshold = util::get_env_float("ZED_ONNX_CONFIDENCE", c.confidence_threshold); - c.nms_threshold = util::get_env_float("ZED_ONNX_NMS_THRESHOLD", c.nms_threshold); - c.input_size = util::get_env_int("ZED_ONNX_INPUT_SIZE", c.input_size); - c.labels = util::get_env_str("ZED_ONNX_LABELS", c.labels); - c.labels_file = util::get_env_str("ZED_ONNX_LABELS_FILE", c.labels_file); - - std::string fmt = util::get_env_str("ZED_ONNX_OUTPUT_FORMAT", ""); - if (fmt == "nc_first" || fmt == "yolov8" || fmt == "yolov11") - c.output_layout = YoloOutputLayout::NC_FIRST; - else if (fmt == "nc_last") - c.output_layout = YoloOutputLayout::NC_LAST; - else if (fmt == "nc_last_obj" || fmt == "yolov5") - c.output_layout = YoloOutputLayout::NC_LAST_OBJ; - - return c; -} - -// --- YoloOnnx --- - -YoloOnnx::YoloOnnx(const YoloOnnxConfig& config) - : config_(config), impl_(std::make_unique()) {} - -YoloOnnx::~YoloOnnx() = default; - -void YoloOnnx::enable(sl::Camera& /* camera */) { - if (config_.model_path.empty()) { - std::cerr << "YoloOnnx: ZED_ONNX_MODEL_PATH is not set." << std::endl; - return; - } - - OrtCUDAProviderOptions cuda_opts{}; - cuda_opts.device_id = 0; - try { - impl_->session_options.AppendExecutionProvider_CUDA(cuda_opts); - } catch (const Ort::Exception& e) { - std::cerr << "YoloOnnx: CUDA unavailable (" << e.what() << "), using CPU." << std::endl; - } - impl_->session_options.SetGraphOptimizationLevel(GraphOptimizationLevel::ORT_ENABLE_ALL); - - std::cout << "YoloOnnx: loading model (" << config_.model_path << ")..." << std::endl; - try { - impl_->session = std::make_unique( - impl_->env, config_.model_path.c_str(), impl_->session_options); - } catch (const Ort::Exception& e) { - std::cerr << "YoloOnnx: failed to load model: " << e.what() << std::endl; - return; - } - - impl_->input_name = impl_->session->GetInputNameAllocated(0, impl_->allocator).get(); - impl_->output_name = impl_->session->GetOutputNameAllocated(0, impl_->allocator).get(); - - size_t num_inputs = impl_->session->GetInputCount(); - if (num_inputs >= 2) { - std::cerr << "YoloOnnx: model has " << num_inputs << " inputs — " - "this engine only supports single-input models." - << std::endl; - } - - auto out_info = impl_->session->GetOutputTypeInfo(0).GetTensorTypeAndShapeInfo(); - auto shape = out_info.GetShape(); - if (shape.size() == 3) { - int64_t dim1 = shape[1]; - int64_t dim2 = shape[2]; - - YoloOutputLayout resolved; - if (config_.output_layout == YoloOutputLayout::AUTO) { - // dim1 < dim2 → NC_FIRST (e.g. 84 vs 8400 for YOLOv8/11) - // dim1 > dim2 → NC_LAST_OBJ (e.g. 25200 vs 85 for YOLOv5) - resolved = (dim1 < dim2) ? YoloOutputLayout::NC_FIRST - : YoloOutputLayout::NC_LAST_OBJ; - } else { - resolved = config_.output_layout; - } - impl_->resolved_layout = resolved; - - if (resolved == YoloOutputLayout::NC_FIRST) { - impl_->num_classes = static_cast(dim1) - 4; - } else if (resolved == YoloOutputLayout::NC_LAST) { - impl_->num_classes = static_cast(dim2) - 4; - } else { // NC_LAST_OBJ - impl_->num_classes = static_cast(dim2) - 5; - } - } - - if (!config_.labels_file.empty() && config_.labels.empty()) { - std::ifstream lf(config_.labels_file); - if (!lf) { - std::cerr << "YoloOnnx: cannot open ZED_ONNX_LABELS_FILE: " - << config_.labels_file << std::endl; - } else { - std::string line, joined; - while (std::getline(lf, line)) { - size_t l = line.find_first_not_of(" \t\r"); - size_t r = line.find_last_not_of(" \t\r"); - if (l == std::string::npos) continue; - if (!joined.empty()) joined += ','; - joined += line.substr(l, r - l + 1); - } - if (!joined.empty()) { - config_.labels = joined; - std::cout << "YoloOnnx: loaded labels from file (" - << config_.labels_file << ")" << std::endl; - } - } - } - - std::cout << "YoloOnnx: model loaded." - << " Classes=" << impl_->num_classes - << " Layout=" << (impl_->resolved_layout == YoloOutputLayout::NC_FIRST ? "NC_FIRST" - : impl_->resolved_layout == YoloOutputLayout::NC_LAST ? "NC_LAST" - : "NC_LAST_OBJ") - << std::endl; -} - -void YoloOnnx::detect(sl::Camera& camera) { - detected_.clear(); - if (!impl_->session) return; - - sl::Mat img_mat; - camera.retrieveImage(img_mat, sl::VIEW::LEFT); - - sl::Mat pc_mat; - camera.retrieveMeasure(pc_mat, sl::MEASURE::XYZRGBA); - - sl::CameraInformation info = camera.getCameraInformation(); - auto calib = info.camera_configuration.calibration_parameters.left_cam; - float fx = calib.fx, fy = calib.fy, cx = calib.cx, cy = calib.cy; - - int orig_w = static_cast(img_mat.getWidth()); - int orig_h = static_cast(img_mat.getHeight()); - - cv::Mat rgba(orig_h, orig_w, CV_8UC4, img_mat.getPtr()); - cv::Mat bgr; - cv::cvtColor(rgba, bgr, cv::COLOR_RGBA2BGR); - - auto kept = impl_->run_inference(config_, bgr); - - for (const auto& det : kept) { - FusionResult fusion = fuse_bbox(pc_mat, - static_cast(det.x1), static_cast(det.y1), - static_cast(det.x2), static_cast(det.y2)); - - DetectedObject obj{}; - obj.label_id = det.class_id; - obj.confidence = det.score * 100.0f; - - if (fusion.valid) { - obj.position[0] = fusion.cx; - obj.position[1] = fusion.cy; - obj.position[2] = fusion.cz; - } - - obj.bounding_box_2d[0] = det.x1; obj.bounding_box_2d[1] = det.y1; - obj.bounding_box_2d[2] = det.x2; obj.bounding_box_2d[3] = det.y1; - obj.bounding_box_2d[4] = det.x2; obj.bounding_box_2d[5] = det.y2; - obj.bounding_box_2d[6] = det.x1; obj.bounding_box_2d[7] = det.y2; - - if (fusion.valid) { - float corners_u[4] = {det.x1, det.x2, det.x2, det.x1}; - float corners_v[4] = {det.y1, det.y1, det.y2, det.y2}; - float depths[2] = {fusion.depth_near, fusion.depth_far}; - for (int face = 0; face < 2; face++) { - for (int c = 0; c < 4; c++) { - float X, Y, Z; - backproject(corners_u[c], corners_v[c], depths[face], - fx, fy, cx, cy, X, Y, Z); - int idx = (face * 4 + c) * 3; - obj.bounding_box_3d[idx + 0] = X; - obj.bounding_box_3d[idx + 1] = Y; - obj.bounding_box_3d[idx + 2] = Z; - } - } - } - - detected_.push_back(obj); - } -} - -void YoloOnnx::detect(const cv::Mat& image_bgr) { - detected_.clear(); - if (!impl_->session) return; - - auto kept = impl_->run_inference(config_, image_bgr); - - for (const auto& det : kept) { - DetectedObject obj{}; - obj.label_id = det.class_id; - obj.confidence = det.score * 100.0f; - obj.bounding_box_2d[0] = det.x1; obj.bounding_box_2d[1] = det.y1; - obj.bounding_box_2d[2] = det.x2; obj.bounding_box_2d[3] = det.y1; - obj.bounding_box_2d[4] = det.x2; obj.bounding_box_2d[5] = det.y2; - obj.bounding_box_2d[6] = det.x1; obj.bounding_box_2d[7] = det.y2; - detected_.push_back(obj); - } -} - -const std::vector& YoloOnnx::objects() const { - return detected_; -} - -std::string YoloOnnx::label_names() const { - return config_.labels; -} - -} // namespace engines diff --git a/src/engines/yolo_onnx.h b/src/engines/yolo_onnx.h deleted file mode 100644 index 8dec6b4..0000000 --- a/src/engines/yolo_onnx.h +++ /dev/null @@ -1,48 +0,0 @@ -#pragma once - -#include "engines/engine.h" - -#include -#include -#include - -namespace engines { - -enum class YoloOutputLayout { - AUTO, // detect from output tensor shape at load time - NC_FIRST, // (1, 4+N, anchors) — YOLOv8, YOLOv11 - NC_LAST, // (1, anchors, 4+N) — rows-first, no objectness - NC_LAST_OBJ // (1, anchors, 5+N) — YOLOv5, rows-first with objectness -}; - -struct YoloOnnxConfig { - std::string model_path; // ZED_ONNX_MODEL_PATH (required) - float confidence_threshold = 0.25f; // ZED_ONNX_CONFIDENCE - float nms_threshold = 0.45f; // ZED_ONNX_NMS_THRESHOLD - int input_size = 640; // ZED_ONNX_INPUT_SIZE - std::string labels; // ZED_ONNX_LABELS (comma-separated) - std::string labels_file; // ZED_ONNX_LABELS_FILE (one class name per line) - YoloOutputLayout output_layout = YoloOutputLayout::AUTO; - - static YoloOnnxConfig from_env(); -}; - -class YoloOnnx : public Engine { -public: - explicit YoloOnnx(const YoloOnnxConfig& config); - ~YoloOnnx() override; - - void enable(sl::Camera& camera) override; - void detect(sl::Camera& camera) override; - void detect(const cv::Mat& image_bgr); // image-only; no 3D fusion - const std::vector& objects() const override; - std::string label_names() const override; - -private: - YoloOnnxConfig config_; - struct Impl; - std::unique_ptr impl_; - std::vector detected_; -}; - -} // namespace engines diff --git a/src/engines/zed_native.cc b/src/engines/zed_native.cc deleted file mode 100644 index efe6600..0000000 --- a/src/engines/zed_native.cc +++ /dev/null @@ -1,99 +0,0 @@ -#include "engines/zed_native.h" -#include "util/env.h" - -#include -#include - -namespace { - -sl::OBJECT_DETECTION_MODEL parse_detection_model(const std::string& model) { - if (model == "MULTI_CLASS_BOX_FAST") { - return sl::OBJECT_DETECTION_MODEL::MULTI_CLASS_BOX_FAST; - } - if (model == "MULTI_CLASS_BOX_ACCURATE") { - return sl::OBJECT_DETECTION_MODEL::MULTI_CLASS_BOX_ACCURATE; - } - return sl::OBJECT_DETECTION_MODEL::MULTI_CLASS_BOX_MEDIUM; -} - -} // namespace - -namespace engines { - -DetectionConfig DetectionConfig::from_env() { - DetectionConfig c; - c.model = util::get_env_str("ZED_DETECTION_MODEL", c.model); - c.confidence_threshold = util::get_env_float("ZED_DETECTION_CONFIDENCE", c.confidence_threshold); - c.tracking = util::get_env_bool("ZED_DETECTION_TRACKING", c.tracking); - return c; -} - -ZedNative::ZedNative(const DetectionConfig& config) : config_(config) { - det_params_.detection_model = parse_detection_model(config_.model); - det_params_.enable_tracking = config_.tracking; - det_params_.enable_segmentation = false; - - det_rt_params_.detection_confidence_threshold = config_.confidence_threshold; -} - -void ZedNative::enable(sl::Camera& camera) { - if (config_.tracking) { - sl::PositionalTrackingParameters tracking_params; - sl::ERROR_CODE err = camera.enablePositionalTracking(tracking_params); - if (err != sl::ERROR_CODE::SUCCESS) { - std::cerr << "Failed to enable positional tracking: " - << sl::toString(err) << std::endl; - } - } - - std::cout << "Object Detection: loading model (" << config_.model << ")..." << std::endl; - sl::ERROR_CODE err = camera.enableObjectDetection(det_params_); - if (err != sl::ERROR_CODE::SUCCESS) { - std::cerr << "Failed to enable object detection: " - << sl::toString(err) << std::endl; - return; - } - std::cout << "Object Detection: model loaded." << std::endl; -} - -void ZedNative::detect(sl::Camera& camera) { - detected_.clear(); - - sl::ERROR_CODE err = camera.retrieveObjects(raw_objects_, det_rt_params_); - if (err != sl::ERROR_CODE::SUCCESS) { - return; - } - - for (const auto& obj : raw_objects_.object_list) { - DetectedObject det{}; - det.label_id = static_cast(obj.label); - det.confidence = obj.confidence; - - det.position[0] = obj.position.x; - det.position[1] = obj.position.y; - det.position[2] = obj.position.z; - - for (int i = 0; i < 4 && i < static_cast(obj.bounding_box_2d.size()); i++) { - det.bounding_box_2d[i * 2 + 0] = obj.bounding_box_2d[i].x; - det.bounding_box_2d[i * 2 + 1] = obj.bounding_box_2d[i].y; - } - - for (int i = 0; i < 8 && i < static_cast(obj.bounding_box.size()); i++) { - det.bounding_box_3d[i * 3 + 0] = obj.bounding_box[i].x; - det.bounding_box_3d[i * 3 + 1] = obj.bounding_box[i].y; - det.bounding_box_3d[i * 3 + 2] = obj.bounding_box[i].z; - } - - detected_.push_back(det); - } -} - -const std::vector& ZedNative::objects() const { - return detected_; -} - -std::string ZedNative::label_names() const { - return "PERSON,VEHICLE,BAG,ANIMAL,ELECTRONICS,FRUIT_VEGETABLE,SPORT"; -} - -} // namespace engines diff --git a/src/engines/zed_native.h b/src/engines/zed_native.h deleted file mode 100644 index 45df940..0000000 --- a/src/engines/zed_native.h +++ /dev/null @@ -1,34 +0,0 @@ -#pragma once - -#include "engines/engine.h" - -#include - -namespace engines { - -struct DetectionConfig { - std::string model = "MULTI_CLASS_BOX_MEDIUM"; - float confidence_threshold = 40.0f; - bool tracking = false; - - static DetectionConfig from_env(); -}; - -class ZedNative : public Engine { -public: - explicit ZedNative(const DetectionConfig& config); - - void enable(sl::Camera& camera) override; - void detect(sl::Camera& camera) override; - const std::vector& objects() const override; - std::string label_names() const override; - -private: - DetectionConfig config_; - sl::ObjectDetectionParameters det_params_; - sl::ObjectDetectionRuntimeParameters det_rt_params_; - sl::Objects raw_objects_; - std::vector detected_; -}; - -} // namespace engines diff --git a/src/main.cc b/src/main.cc index c06c814..8bb8600 100644 --- a/src/main.cc +++ b/src/main.cc @@ -1,6 +1,3 @@ -#include "engines/engine.h" -#include "engines/yolo_onnx.h" -#include "engines/zed_native.h" #include "util/env.h" #include "zed/camera.h" @@ -8,18 +5,14 @@ #include #include -#include #include #include -#include #include #include namespace { -constexpr int FIELDS_PER_OBJECT = 37; - -enum class PcMode { FULL, HALF, ROI }; +enum class PcMode { FULL, HALF }; rust::Vec make_shape(int height, int width, int channels) { rust::Vec shape; @@ -45,22 +38,6 @@ rust::Vec make_vector_int(std::initializer_list values) { return out; } -std::vector serialize_objects(const std::vector& objects) { - std::vector buffer(objects.size() * FIELDS_PER_OBJECT); - for (size_t i = 0; i < objects.size(); i++) { - float* row = buffer.data() + i * FIELDS_PER_OBJECT; - const auto& obj = objects[i]; - row[0] = static_cast(obj.label_id); - row[1] = obj.confidence; - row[2] = obj.position[0]; - row[3] = obj.position[1]; - row[4] = obj.position[2]; - std::memcpy(row + 5, obj.bounding_box_2d, sizeof(obj.bounding_box_2d)); - std::memcpy(row + 13, obj.bounding_box_3d, sizeof(obj.bounding_box_3d)); - } - return buffer; -} - } // namespace int main() { @@ -72,21 +49,8 @@ int main() { camera.warmup(); auto calib = camera.calibration(); - std::string engine_name = util::get_env_str("ZED_ENGINE", "native"); - std::unique_ptr detector; - if (engine_name == "yolo_onnx" || engine_name == "onnx") { - auto cfg = engines::YoloOnnxConfig::from_env(); - detector = std::make_unique(cfg); - } else { - auto cfg = engines::DetectionConfig::from_env(); - detector = std::make_unique(cfg); - } - detector->enable(camera.handle()); - std::string pc_mode_str = util::get_env_str("ZED_PC_MODE", "FULL"); - PcMode pc_mode = (pc_mode_str == "HALF") ? PcMode::HALF - : (pc_mode_str == "ROI") ? PcMode::ROI - : PcMode::FULL; + PcMode pc_mode = (pc_mode_str == "HALF") ? PcMode::HALF : PcMode::FULL; int64_t frame_id = 0; while (true) { @@ -106,12 +70,9 @@ int main() { camera.close(); camera.open(); calib = camera.calibration(); - detector->enable(camera.handle()); continue; } - detector->detect(camera.handle()); - sl::Mat& image_mat = camera.image(); sl::Mat& point_cloud_mat = camera.point_cloud(); @@ -185,30 +146,6 @@ int main() { pc_metadata->set_list_int("shape", make_shape(hh, hw, 4)); pc_data_ptr = reinterpret_cast(pc_buf.data()); pc_data_size = pc_buf.size() * sizeof(float); - } else if (pc_mode == PcMode::ROI) { - const auto& objs = detector->objects(); - for (const auto& obj : objs) { - int x1 = std::max(0, static_cast(obj.bounding_box_2d[0])); - int y1 = std::max(0, static_cast(obj.bounding_box_2d[1])); - int x2 = std::min(pc_width - 1, static_cast(obj.bounding_box_2d[4])); - int y2 = std::min(pc_height - 1, static_cast(obj.bounding_box_2d[5])); - for (int y = y1; y <= y2; y++) { - for (int x = x1; x <= x2; x++) { - sl::float4 pt; - point_cloud_mat.getValue(x, y, &pt); - if (!std::isnan(pt.x) && !std::isinf(pt.x)) { - pc_buf.push_back(pt.x); pc_buf.push_back(pt.y); - pc_buf.push_back(pt.z); pc_buf.push_back(pt.w); - } - } - } - } - int N = static_cast(pc_buf.size() / 4); - pc_metadata->set_string("layout", "NC"); - pc_metadata->set_string("mode", "roi"); - pc_metadata->set_list_int("shape", make_vector_int({N, 4})); - pc_data_ptr = reinterpret_cast(pc_buf.data()); - pc_data_size = pc_buf.size() * sizeof(float); } else { // FULL — send raw sl::Mat pointer directly (no copy) pc_data_size = static_cast(pc_width * pc_height * sizeof(sl::float4)); @@ -240,40 +177,6 @@ int main() { if (!std::string(cam_result.error).empty()) { std::cerr << "Error sending camera_info: " << std::string(cam_result.error) << std::endl; } - - // --- objects --- - const auto& objects = detector->objects(); - auto obj_buffer = serialize_objects(objects); - int num_objects = static_cast(objects.size()); - - const size_t obj_size = obj_buffer.size() * sizeof(float); - rust::Slice obj_slice{ - reinterpret_cast(obj_buffer.data()), obj_size}; - - auto obj_metadata = new_metadata(); - obj_metadata->set_string("dtype", "float32"); - obj_metadata->set_string("layout", "RC"); - obj_metadata->set_string("units", "mm"); - obj_metadata->set_list_int("shape", make_vector_int({num_objects, FIELDS_PER_OBJECT})); - obj_metadata->set_string("fields", - "label_id,confidence,x,y,z," - "bb2d_x0,bb2d_y0,bb2d_x1,bb2d_y1,bb2d_x2,bb2d_y2,bb2d_x3,bb2d_y3," - "bb3d_x0,bb3d_y0,bb3d_z0,bb3d_x1,bb3d_y1,bb3d_z1," - "bb3d_x2,bb3d_y2,bb3d_z2,bb3d_x3,bb3d_y3,bb3d_z3," - "bb3d_x4,bb3d_y4,bb3d_z4,bb3d_x5,bb3d_y5,bb3d_z5," - "bb3d_x6,bb3d_y6,bb3d_z6,bb3d_x7,bb3d_y7,bb3d_z7"); - obj_metadata->set_string("label_names", detector->label_names()); - obj_metadata->set_string("engine", engine_name); - obj_metadata->set_int("frame_id", current_frame_id); - obj_metadata->set_int("timestamp_ns", timestamp_ns); - obj_metadata->set_int("image_width", width); - obj_metadata->set_int("image_height", height); - - auto obj_result = send_output_with_metadata( - dora_node.send_output, "objects", obj_slice, std::move(obj_metadata)); - if (!std::string(obj_result.error).empty()) { - std::cerr << "Error sending objects: " << std::string(obj_result.error) << std::endl; - } } return 0; diff --git a/src/test_yolo_onnx.cc b/src/test_yolo_onnx.cc deleted file mode 100644 index e1439d1..0000000 --- a/src/test_yolo_onnx.cc +++ /dev/null @@ -1,113 +0,0 @@ -// Standalone test for the YoloOnnx (closed-vocabulary) detection engine. -// No dora framework, no live ZED camera — loads an image from disk. -// -// Usage: -// bin/test_yolo_onnx [image_path] -// image_path default: test/assets/yolo.png -// -// Required env vars: -// ZED_ONNX_MODEL_PATH path to YOLO ONNX model -// -// Optional env vars: -// ZED_ONNX_LABELS comma-separated class names -// ZED_ONNX_LABELS_FILE path to newline-separated class names file -// ZED_ONNX_CONFIDENCE detection threshold (default 0.25) -// ZED_ONNX_NMS_THRESHOLD NMS IoU threshold (default 0.45) -// ZED_ONNX_INPUT_SIZE model input size (default 640) -// ZED_ONNX_OUTPUT_FORMAT nc_first|nc_last|nc_last_obj|yolov5|yolov8|yolov11|auto - -#include "engines/yolo_onnx.h" -#include "util/env.h" - -#include -#include - -#include -#include -#include -#include -#include - -namespace fs = std::filesystem; - -namespace { - -std::vector parse_csv(const std::string& s) { - std::vector out; - std::istringstream ss(s); - std::string tok; - while (std::getline(ss, tok, ',')) { - size_t l = tok.find_first_not_of(" \t"); - size_t r = tok.find_last_not_of(" \t"); - if (l != std::string::npos) - out.push_back(tok.substr(l, r - l + 1)); - } - return out; -} - -void draw_detections(cv::Mat& img, - const std::vector& objects, - const std::vector& labels) { - for (const auto& obj : objects) { - int x1 = static_cast(obj.bounding_box_2d[0]); - int y1 = static_cast(obj.bounding_box_2d[1]); - int x2 = static_cast(obj.bounding_box_2d[4]); - int y2 = static_cast(obj.bounding_box_2d[5]); - - std::string name = (obj.label_id >= 0 && obj.label_id < static_cast(labels.size())) - ? labels[obj.label_id] : std::to_string(obj.label_id); - - char buf[64]; - std::snprintf(buf, sizeof(buf), " %.2f", obj.confidence / 100.0f); - - cv::rectangle(img, {x1, y1}, {x2, y2}, {0, 200, 0}, 2); - cv::putText(img, name + buf, {x1, std::max(y1 - 6, 14)}, - cv::FONT_HERSHEY_SIMPLEX, 0.55, {0, 200, 0}, 2, cv::LINE_AA); - } -} - -} // namespace - -int main(int argc, char* argv[]) { - std::string image_path = (argc > 1) ? argv[1] : "test/assets/yolo.png"; - - auto config = engines::YoloOnnxConfig::from_env(); - - sl::Camera dummy_cam; - engines::YoloOnnx engine(config); - engine.enable(dummy_cam); - - cv::Mat img = cv::imread(image_path); - if (img.empty()) { - std::cerr << "Error: cannot read image: " << image_path << std::endl; - return 1; - } - - engine.detect(img); - const auto& objects = engine.objects(); - auto labels = parse_csv(engine.label_names()); - - std::cout << "[" << image_path << "] " << objects.size() << " detection(s):" << std::endl; - for (const auto& obj : objects) { - std::string name = (obj.label_id >= 0 && obj.label_id < static_cast(labels.size())) - ? labels[obj.label_id] : std::to_string(obj.label_id); - std::cout << " " << name - << " conf=" << obj.confidence / 100.0f - << " bbox=[" - << obj.bounding_box_2d[0] << "," << obj.bounding_box_2d[1] - << " -> " - << obj.bounding_box_2d[4] << "," << obj.bounding_box_2d[5] - << "]" << std::endl; - } - - cv::Mat annotated = img.clone(); - draw_detections(annotated, objects, labels); - - fs::path stem = fs::path(image_path).stem(); - fs::path parent = fs::path(image_path).parent_path(); - std::string result_path = (parent / (stem.string() + "_result.png")).string(); - cv::imwrite(result_path, annotated); - std::cout << "Saved: " << result_path << std::endl; - - return 0; -} diff --git a/test/assets/.gitkeep b/test/assets/.gitkeep deleted file mode 100644 index e69de29..0000000 -- 2.54.0 From dc7f9f5501ac4ef506a58f32e97b23713d258a82 Mon Sep 17 00:00:00 2001 From: cesar Date: Tue, 8 Sep 2026 09:15:47 -0300 Subject: [PATCH 03/10] fix frame timestamps to use a comparable epoch MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit steady_clock's epoch is unspecified and typically counts from process start or boot, so the published timestamp_ns could not be compared against another node's timestamps, wall clock or a recording — and it recorded when the node processed the frame rather than when the camera captured it. Publish two keys on every output instead: device_timestamp_ns from sl::Camera::getTimestamp(TIME_REFERENCE::IMAGE), and host_timestamp_ns from system_clock. Both are Unix-epoch nanoseconds, set as dora timestamp parameters. The SDK clock source is pinned to SYSTEM_CLOCK at construction, since it is process-wide and decides the epoch of the device value. Both are kept because the wall clock can step when NTP synchronises after boot on a Jetson, while the device timestamp is what two nodes reading the same frame can agree on. Co-Authored-By: Claude Opus 5 Claude-Session: https://claude.ai/code/session_01Y7g2cBJ5e3r15ZbJ9WZn8c --- AGENTS.md | 2 +- README.md | 27 ++++++++++++++++++++++--- src/main.cc | 50 +++++++++++++++++++++++++++++++---------------- src/zed/camera.cc | 10 ++++++++++ src/zed/camera.h | 2 ++ 5 files changed, 70 insertions(+), 21 deletions(-) diff --git a/AGENTS.md b/AGENTS.md index f37926f..457edd5 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -38,7 +38,7 @@ Camera behavior is configured entirely through environment variables (see README - **`src/util/env.h`** — Shared inline env-variable parsing helpers (`get_env_int`, `get_env_str`, `get_env_bool`, `get_env_float`). - **`src/zed/camera.h`** — `zed::Config`, `zed::Calibration`, and `zed::Camera` class. Camera wraps the ZED SDK lifecycle: open (with retry loop), warmup, grab (image + point cloud), calibration, and close. Exposes `handle()` for engine access. - **`src/zed/camera.cc`** — Implementation. SDK enum parsers live in an anonymous namespace. -- **`src/main.cc`** — Dora event loop. Parses `ZED_PC_MODE` (`FULL`/`HALF`) once before the loop. On any input event (intended to be driven by a tick timer; the id is not inspected): calls `camera.grab()`, then sends `image_bgr`, `point_cloud` and `camera_info`. Point cloud output varies by mode: `FULL` sends the raw `sl::Mat` pointer (no copy); `HALF` downsamples by 2 in each dimension. Handles reconnection on grab failure. +- **`src/main.cc`** — Dora event loop. Parses `ZED_PC_MODE` (`FULL`/`HALF`) once before the loop. On any input event (intended to be driven by a tick timer; the id is not inspected): calls `camera.grab()`, then sends `image_bgr`, `point_cloud` and `camera_info`. `set_intrinsics` and `set_frame_stamps` stamp every output of a frame with the same intrinsics, `frame_id` and the two timestamps (`device_timestamp_ns`, `host_timestamp_ns`; both Unix-epoch nanoseconds). Point cloud output varies by mode: `FULL` sends the raw `sl::Mat` pointer (no copy); `HALF` downsamples by 2 in each dimension. Handles reconnection on grab failure. - **`DoraTargets.cmake`** — Handles building or fetching dora-rs and generating the CXX bridge files. Two paths: local (`DORA_ROOT_DIR`) or git clone. Produces `dora_node_api_cxx` library and the `node_bridge.cc` glue. - **`scripts/build.sh`** — Configure/compile/install in one shell-free-callable step. Referenced by `dataflow.yml`, the `Dockerfile` and the README. - **`dataflow.yml`** — Example dora dataflow definition using the `git:` source format, pinned with `tag:` — dora 1.0 pins a source with the mutually exclusive `tag:`, `rev:` or `branch:`. diff --git a/README.md b/README.md index 80a5c08..2b6cde3 100644 --- a/README.md +++ b/README.md @@ -119,6 +119,27 @@ All behavior is controlled via environment variables. | Output | Type | Description | |---|---|---| -| `image_bgr` | `uint8 [H, W, 3]` | BGR image. Metadata: `shape`, `encoding=bgr8`, intrinsics, `timestamp_ns`. | -| `point_cloud` | `float32 [*, 4]` | XYZRGBA point cloud. Shape depends on `ZED_PC_MODE`. Metadata: `units=mm`, intrinsics. | -| `camera_info` | `float32 [4]` | `[fx, fy, cx, cy]`. Metadata: `distortion` coefficients. | +| `image_bgr` | `uint8 [H, W, 3]` | BGR image. Metadata: `shape`, `encoding=bgr8`, intrinsics, timestamps. | +| `point_cloud` | `float32 [*, 4]` | XYZRGBA point cloud. Shape depends on `ZED_PC_MODE`. Metadata: `units=mm`, intrinsics, timestamps. | +| `camera_info` | `float32 [4]` | `[fx, fy, cx, cy]`. Metadata: `distortion` coefficients, timestamps. | + +### Timestamps + +Every output carries both clocks, as dora `timestamp` metadata parameters: + +| Key | Epoch | Unit | Source | +|---|---|---|---| +| `device_timestamp_ns` | Unix epoch (UTC) | nanoseconds | `sl::Camera::getTimestamp(TIME_REFERENCE::IMAGE)` — when the camera captured the frame | +| `host_timestamp_ns` | Unix epoch (UTC) | nanoseconds | `std::chrono::system_clock` on the host — when this node published the frame | + +Both are needed. The device timestamp is the capture instant and is the value two +nodes reading the same frame agree on; the host timestamp is the one the rest of the +dataflow and the session recorder share. On a Jetson the wall clock can step when NTP +synchronises after boot, so the device timestamp is recorded alongside the host one +rather than in place of it. + +The node calls `sl::setTimestampClock(SYSTEM_CLOCK)` before opening the camera, which +is what makes the device timestamp a Unix-epoch value rather than an SDK-default clock. + +Every output for a frame also carries the same `frame_id`, an integer counter starting +at 0. diff --git a/src/main.cc b/src/main.cc index 8bb8600..edb3ea6 100644 --- a/src/main.cc +++ b/src/main.cc @@ -38,6 +38,32 @@ rust::Vec make_vector_int(std::initializer_list values) { return out; } +void set_intrinsics(Metadata& metadata, const zed::Calibration& calib) { + metadata.set_float("fx", calib.fx); + metadata.set_float("fy", calib.fy); + metadata.set_float("cx", calib.cx); + metadata.set_float("cy", calib.cy); + metadata.set_list_float("distortion", make_distortion(calib.distortion)); +} + +// Both clocks are needed. The device timestamp is the instant the frame was +// captured and is the only value two nodes reading the same frame can agree on. +// The host timestamp is what the rest of the dataflow and the session recorder +// share, and on a Jetson it can step when NTP synchronises after boot — which is +// exactly why the device timestamp is recorded alongside it rather than instead +// of it. +void set_frame_stamps(Metadata& metadata, int64_t frame_id, int64_t device_ns, int64_t host_ns) { + metadata.set_int("frame_id", frame_id); + metadata.set_timestamp("device_timestamp_ns", device_ns); + metadata.set_timestamp("host_timestamp_ns", host_ns); +} + +int64_t host_timestamp_ns() { + return std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()) + .count(); +} + } // namespace int main() { @@ -80,11 +106,8 @@ int main() { int height = image_mat.getHeight(); int64_t current_frame_id = frame_id++; - int64_t timestamp_ns = static_cast( - std::chrono::duration_cast( - std::chrono::steady_clock::now().time_since_epoch() - ).count() - ); + int64_t device_ns = camera.device_timestamp_ns(); + int64_t host_ns = host_timestamp_ns(); // --- image_bgr --- cv::Mat image_rgba(height, width, CV_8UC4, image_mat.getPtr()); @@ -100,13 +123,8 @@ int main() { image_metadata->set_string("encoding", "bgr8"); image_metadata->set_string("layout", "HWC"); image_metadata->set_list_int("shape", make_shape(height, width, 3)); - image_metadata->set_float("fx", calib.fx); - image_metadata->set_float("fy", calib.fy); - image_metadata->set_float("cx", calib.cx); - image_metadata->set_float("cy", calib.cy); - image_metadata->set_list_float("distortion", make_distortion(calib.distortion)); - image_metadata->set_int("frame_id", current_frame_id); - image_metadata->set_int("timestamp_ns", timestamp_ns); + set_intrinsics(*image_metadata, calib); + set_frame_stamps(*image_metadata, current_frame_id, device_ns, host_ns); auto image_result = send_output_with_metadata( dora_node.send_output, "image_bgr", image_slice, std::move(image_metadata)); @@ -125,11 +143,8 @@ int main() { pc_metadata->set_string("dtype", "float32"); pc_metadata->set_string("channels", "XYZRGBA"); pc_metadata->set_string("units", "mm"); - pc_metadata->set_float("fx", calib.fx); - pc_metadata->set_float("fy", calib.fy); - pc_metadata->set_float("cx", calib.cx); - pc_metadata->set_float("cy", calib.cy); - pc_metadata->set_list_float("distortion", make_distortion(calib.distortion)); + set_intrinsics(*pc_metadata, calib); + set_frame_stamps(*pc_metadata, current_frame_id, device_ns, host_ns); if (pc_mode == PcMode::HALF) { int hw = pc_width / 2, hh = pc_height / 2; @@ -171,6 +186,7 @@ int main() { cam_metadata->set_string("layout", "C"); cam_metadata->set_list_int("shape", make_vector_int({4})); cam_metadata->set_list_float("distortion", make_distortion(calib.distortion)); + set_frame_stamps(*cam_metadata, current_frame_id, device_ns, host_ns); auto cam_result = send_output_with_metadata( dora_node.send_output, "camera_info", cam_slice, std::move(cam_metadata)); diff --git a/src/zed/camera.cc b/src/zed/camera.cc index ecaaa9b..c89b502 100644 --- a/src/zed/camera.cc +++ b/src/zed/camera.cc @@ -71,6 +71,11 @@ Config Config::from_env() { } Camera::Camera(const Config& config) : config_(config) { + // Process-wide, and must be set before any camera is opened. Without it the + // epoch of the timestamps published alongside every frame depends on the SDK + // default rather than on anything this node states. + sl::setTimestampClock(sl::TIMESTAMP_CLOCK::SYSTEM_CLOCK); + init_params_.camera_resolution = parse_resolution(config_.resolution); init_params_.camera_fps = config_.fps; init_params_.depth_mode = parse_depth_mode(config_.depth_mode); @@ -135,6 +140,11 @@ Calibration Camera::calibration() const { return result; } +int64_t Camera::device_timestamp_ns() { + return static_cast( + zed_.getTimestamp(sl::TIME_REFERENCE::IMAGE).getNanoseconds()); +} + sl::Mat& Camera::image() { return image_mat_; } diff --git a/src/zed/camera.h b/src/zed/camera.h index 2b2f2a4..9223562 100644 --- a/src/zed/camera.h +++ b/src/zed/camera.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include @@ -35,6 +36,7 @@ public: void warmup(); bool grab(); Calibration calibration() const; + int64_t device_timestamp_ns(); sl::Mat& image(); sl::Mat& point_cloud(); sl::Camera& handle(); -- 2.54.0 From 15134dc12ec5716857a0aa14c381aa7b13364b97 Mon Sep 17 00:00:00 2001 From: cesar Date: Tue, 8 Sep 2026 09:17:36 -0300 Subject: [PATCH 04/10] add a depth output MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Publish sl::MEASURE::DEPTH as float32 [H, W] millimetres on every frame that carries image_bgr, with the intrinsics, frame_id and timestamps the other outputs carry. The map is copied out rather than published in place: sl::Mat rows can be padded, so the buffer is not necessarily H * W floats. inf and nan are cleared to 0 in the same pass, matching the reference driver — a hole is meaningful and a consumer cannot back-project a non-finite value. Consumers that need 3D can now back-project depth with the published intrinsics instead of receiving a point cloud. Also drops the stale objects entry from the dataflow outputs, missed when detection was removed. Co-Authored-By: Claude Opus 5 Claude-Session: https://claude.ai/code/session_01Y7g2cBJ5e3r15ZbJ9WZn8c --- AGENTS.md | 4 ++-- README.md | 5 +++++ dataflow.yml | 4 +++- src/main.cc | 47 +++++++++++++++++++++++++++++++++++++++++++++++ src/zed/camera.cc | 5 +++++ src/zed/camera.h | 2 ++ 6 files changed, 64 insertions(+), 3 deletions(-) diff --git a/AGENTS.md b/AGENTS.md index 457edd5..833131f 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -36,9 +36,9 @@ Camera behavior is configured entirely through environment variables (see README ## Architecture - **`src/util/env.h`** — Shared inline env-variable parsing helpers (`get_env_int`, `get_env_str`, `get_env_bool`, `get_env_float`). -- **`src/zed/camera.h`** — `zed::Config`, `zed::Calibration`, and `zed::Camera` class. Camera wraps the ZED SDK lifecycle: open (with retry loop), warmup, grab (image + point cloud), calibration, and close. Exposes `handle()` for engine access. +- **`src/zed/camera.h`** — `zed::Config`, `zed::Calibration`, and `zed::Camera` class. Camera wraps the ZED SDK lifecycle: open (with retry loop), warmup, grab (image + depth + point cloud), calibration, device timestamp, and close. Exposes `handle()` for engine access. - **`src/zed/camera.cc`** — Implementation. SDK enum parsers live in an anonymous namespace. -- **`src/main.cc`** — Dora event loop. Parses `ZED_PC_MODE` (`FULL`/`HALF`) once before the loop. On any input event (intended to be driven by a tick timer; the id is not inspected): calls `camera.grab()`, then sends `image_bgr`, `point_cloud` and `camera_info`. `set_intrinsics` and `set_frame_stamps` stamp every output of a frame with the same intrinsics, `frame_id` and the two timestamps (`device_timestamp_ns`, `host_timestamp_ns`; both Unix-epoch nanoseconds). Point cloud output varies by mode: `FULL` sends the raw `sl::Mat` pointer (no copy); `HALF` downsamples by 2 in each dimension. Handles reconnection on grab failure. +- **`src/main.cc`** — Dora event loop. Parses `ZED_PC_MODE` (`FULL`/`HALF`) once before the loop. On any input event (intended to be driven by a tick timer; the id is not inspected): calls `camera.grab()`, then sends `image_bgr`, `depth`, `point_cloud` and `camera_info`. `pack_depth` copies the depth map into a tightly packed `[H, W]` float buffer (sl::Mat rows can be padded) and clears `inf`/`nan` to `0`. `set_intrinsics` and `set_frame_stamps` stamp every output of a frame with the same intrinsics, `frame_id` and the two timestamps (`device_timestamp_ns`, `host_timestamp_ns`; both Unix-epoch nanoseconds). Point cloud output varies by mode: `FULL` sends the raw `sl::Mat` pointer (no copy); `HALF` downsamples by 2 in each dimension. Handles reconnection on grab failure. - **`DoraTargets.cmake`** — Handles building or fetching dora-rs and generating the CXX bridge files. Two paths: local (`DORA_ROOT_DIR`) or git clone. Produces `dora_node_api_cxx` library and the `node_bridge.cc` glue. - **`scripts/build.sh`** — Configure/compile/install in one shell-free-callable step. Referenced by `dataflow.yml`, the `Dockerfile` and the README. - **`dataflow.yml`** — Example dora dataflow definition using the `git:` source format, pinned with `tag:` — dora 1.0 pins a source with the mutually exclusive `tag:`, `rev:` or `branch:`. diff --git a/README.md b/README.md index 2b6cde3..1e585cb 100644 --- a/README.md +++ b/README.md @@ -17,6 +17,7 @@ nodes: tick: dora/timer/millis/100 outputs: - image_bgr + - depth - point_cloud - camera_info ``` @@ -120,9 +121,13 @@ All behavior is controlled via environment variables. | Output | Type | Description | |---|---|---| | `image_bgr` | `uint8 [H, W, 3]` | BGR image. Metadata: `shape`, `encoding=bgr8`, intrinsics, timestamps. | +| `depth` | `float32 [H, W]` | Per-pixel depth in millimetres, published on every frame `image_bgr` is. Metadata: `dtype`, `layout=HW`, `shape`, `units=mm`, intrinsics, timestamps. `H` and `W` match `image_bgr` for the same `frame_id`. Missing depth is `0` — the payload contains no `inf` and no `nan`. Holes are not interpolated unless `ZED_DEPTH_FILL` is set. | | `point_cloud` | `float32 [*, 4]` | XYZRGBA point cloud. Shape depends on `ZED_PC_MODE`. Metadata: `units=mm`, intrinsics, timestamps. | | `camera_info` | `float32 [4]` | `[fx, fy, cx, cy]`. Metadata: `distortion` coefficients, timestamps. | +Consumers that need 3D back-project `depth` with the published intrinsics rather than +consuming `point_cloud`; the point cloud is not a prerequisite for 3D. + ### Timestamps Every output carries both clocks, as dora `timestamp` metadata parameters: diff --git a/dataflow.yml b/dataflow.yml index 5d7d4c4..df9c3e9 100644 --- a/dataflow.yml +++ b/dataflow.yml @@ -8,7 +8,9 @@ nodes: tick: dora/timer/millis/100 outputs: - image_bgr - - objects + - depth + - point_cloud + - camera_info - id: plot git: https://github.com/dora-rs/dora-hub.git diff --git a/src/main.cc b/src/main.cc index edb3ea6..9cfe22e 100644 --- a/src/main.cc +++ b/src/main.cc @@ -5,6 +5,7 @@ #include #include +#include #include #include #include @@ -58,6 +59,29 @@ void set_frame_stamps(Metadata& metadata, int64_t frame_id, int64_t device_ns, i metadata.set_timestamp("host_timestamp_ns", host_ns); } +// Copy the depth map out into a tightly packed [H, W] buffer of millimetres. +// +// The rows of an sl::Mat can be padded, so the buffer is not necessarily H * W +// floats and cannot be published in place. inf and nan mark depth the SDK could +// not measure; they are cleared to 0 here, as the reference driver does, because +// a hole is meaningful and a consumer cannot back-project a non-finite value. +std::vector pack_depth(sl::Mat& depth_mat) { + const int height = depth_mat.getHeight(); + const int width = depth_mat.getWidth(); + const size_t step = depth_mat.getStep(sl::MEM::CPU); + const float* src = depth_mat.getPtr(); + + std::vector out(static_cast(height) * static_cast(width)); + for (int y = 0; y < height; y++) { + const float* row = src + static_cast(y) * step; + float* dst = out.data() + static_cast(y) * static_cast(width); + for (int x = 0; x < width; x++) { + dst[x] = std::isfinite(row[x]) ? row[x] : 0.0f; + } + } + return out; +} + int64_t host_timestamp_ns() { return std::chrono::duration_cast( std::chrono::system_clock::now().time_since_epoch()) @@ -100,6 +124,7 @@ int main() { } sl::Mat& image_mat = camera.image(); + sl::Mat& depth_mat = camera.depth(); sl::Mat& point_cloud_mat = camera.point_cloud(); int width = image_mat.getWidth(); @@ -132,6 +157,28 @@ int main() { std::cerr << "Error sending image_bgr: " << std::string(image_result.error) << std::endl; } + // --- depth --- + int depth_width = depth_mat.getWidth(); + int depth_height = depth_mat.getHeight(); + + std::vector depth_buf = pack_depth(depth_mat); + rust::Slice depth_slice{ + reinterpret_cast(depth_buf.data()), depth_buf.size() * sizeof(float)}; + + auto depth_metadata = new_metadata(); + depth_metadata->set_string("dtype", "float32"); + depth_metadata->set_string("layout", "HW"); + depth_metadata->set_string("units", "mm"); + depth_metadata->set_list_int("shape", make_vector_int({depth_height, depth_width})); + set_intrinsics(*depth_metadata, calib); + set_frame_stamps(*depth_metadata, current_frame_id, device_ns, host_ns); + + auto depth_result = send_output_with_metadata( + dora_node.send_output, "depth", depth_slice, std::move(depth_metadata)); + if (!std::string(depth_result.error).empty()) { + std::cerr << "Error sending depth: " << std::string(depth_result.error) << std::endl; + } + // --- point_cloud --- int pc_width = point_cloud_mat.getWidth(); int pc_height = point_cloud_mat.getHeight(); diff --git a/src/zed/camera.cc b/src/zed/camera.cc index c89b502..45fbe78 100644 --- a/src/zed/camera.cc +++ b/src/zed/camera.cc @@ -119,6 +119,7 @@ bool Camera::grab() { return false; } zed_.retrieveImage(image_mat_, sl::VIEW::LEFT); + zed_.retrieveMeasure(depth_mat_, sl::MEASURE::DEPTH); zed_.retrieveMeasure(point_cloud_mat_, sl::MEASURE::XYZRGBA); return true; } @@ -149,6 +150,10 @@ sl::Mat& Camera::image() { return image_mat_; } +sl::Mat& Camera::depth() { + return depth_mat_; +} + sl::Mat& Camera::point_cloud() { return point_cloud_mat_; } diff --git a/src/zed/camera.h b/src/zed/camera.h index 9223562..240299d 100644 --- a/src/zed/camera.h +++ b/src/zed/camera.h @@ -38,6 +38,7 @@ public: Calibration calibration() const; int64_t device_timestamp_ns(); sl::Mat& image(); + sl::Mat& depth(); sl::Mat& point_cloud(); sl::Camera& handle(); @@ -47,6 +48,7 @@ private: sl::InitParameters init_params_; sl::RuntimeParameters runtime_params_; sl::Mat image_mat_; + sl::Mat depth_mat_; sl::Mat point_cloud_mat_; }; -- 2.54.0 From 617b8ceb5021c15468f84fc5a461eda1e558a78e Mon Sep 17 00:00:00 2001 From: cesar Date: Tue, 8 Sep 2026 09:20:01 -0300 Subject: [PATCH 05/10] add depth configuration parity with the reference driver parse_depth_mode returned NEURAL for any unrecognised string, so a typo silently changed depth quality, and NEURAL_PLUS and NEURAL_LIGHT could not be selected at all. All six modes are now reachable and an unknown name exits non-zero listing the accepted values. Parsing moves into Config::from_env() and main reads the configuration before init_dora_node(), so a bad value fails with its own message instead of after the node has registered with the dataflow. Add ZED_CONFIDENCE_THRESHOLD (60) and ZED_TEXTURE_CONFIDENCE_THRESHOLD (100), which were never set, and change the defaults to 1080 / 200 mm / 600 mm to match the reference driver perception was tuned against. The resolved settings are logged at startup. Co-Authored-By: Claude Opus 5 Claude-Session: https://claude.ai/code/session_01Y7g2cBJ5e3r15ZbJ9WZn8c --- AGENTS.md | 2 +- README.md | 16 +++++++++++----- src/main.cc | 6 ++++-- src/zed/camera.cc | 49 +++++++++++++++++++++++++++++++++++++---------- src/zed/camera.h | 10 ++++++---- 5 files changed, 61 insertions(+), 22 deletions(-) diff --git a/AGENTS.md b/AGENTS.md index 833131f..6a4f960 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -37,7 +37,7 @@ Camera behavior is configured entirely through environment variables (see README - **`src/util/env.h`** — Shared inline env-variable parsing helpers (`get_env_int`, `get_env_str`, `get_env_bool`, `get_env_float`). - **`src/zed/camera.h`** — `zed::Config`, `zed::Calibration`, and `zed::Camera` class. Camera wraps the ZED SDK lifecycle: open (with retry loop), warmup, grab (image + depth + point cloud), calibration, device timestamp, and close. Exposes `handle()` for engine access. -- **`src/zed/camera.cc`** — Implementation. SDK enum parsers live in an anonymous namespace. +- **`src/zed/camera.cc`** — Implementation. SDK enum parsers live in an anonymous namespace. `parse_depth_mode` runs in `Config::from_env()`, not in the constructor, so an unrecognised `ZED_DEPTH_MODE` exits before the node joins the dataflow rather than falling back to a default. - **`src/main.cc`** — Dora event loop. Parses `ZED_PC_MODE` (`FULL`/`HALF`) once before the loop. On any input event (intended to be driven by a tick timer; the id is not inspected): calls `camera.grab()`, then sends `image_bgr`, `depth`, `point_cloud` and `camera_info`. `pack_depth` copies the depth map into a tightly packed `[H, W]` float buffer (sl::Mat rows can be padded) and clears `inf`/`nan` to `0`. `set_intrinsics` and `set_frame_stamps` stamp every output of a frame with the same intrinsics, `frame_id` and the two timestamps (`device_timestamp_ns`, `host_timestamp_ns`; both Unix-epoch nanoseconds). Point cloud output varies by mode: `FULL` sends the raw `sl::Mat` pointer (no copy); `HALF` downsamples by 2 in each dimension. Handles reconnection on grab failure. - **`DoraTargets.cmake`** — Handles building or fetching dora-rs and generating the CXX bridge files. Two paths: local (`DORA_ROOT_DIR`) or git clone. Produces `dora_node_api_cxx` library and the `node_bridge.cc` glue. - **`scripts/build.sh`** — Configure/compile/install in one shell-free-callable step. Referenced by `dataflow.yml`, the `Dockerfile` and the README. diff --git a/README.md b/README.md index 1e585cb..65f6d9b 100644 --- a/README.md +++ b/README.md @@ -93,16 +93,22 @@ All behavior is controlled via environment variables. | Variable | Default | Description | |---|---|---| -| `ZED_RESOLUTION` | `720` | `720`, `1080`, or `2000` | +| `ZED_RESOLUTION` | `1080` | `720`, `1080`, or `2000` | | `ZED_FPS` | `15` | Frame rate | -| `ZED_DEPTH_MODE` | `NEURAL` | `PERFORMANCE`, `QUALITY`, `NEURAL`, or `NONE` | -| `ZED_DEPTH_MIN_MM` | `10` | Minimum depth (mm) | -| `ZED_DEPTH_MAX_MM` | `500` | Maximum depth (mm) | -| `ZED_DEPTH_FILL` | `false` | Enable depth fill mode | +| `ZED_DEPTH_MODE` | `NEURAL` | `NEURAL`, `NEURAL_PLUS`, `NEURAL_LIGHT`, `PERFORMANCE`, `QUALITY`, or `NONE`. An unrecognised value is a startup error, not a fallback. | +| `ZED_DEPTH_MIN_MM` | `200` | Minimum depth (mm) | +| `ZED_DEPTH_MAX_MM` | `600` | Maximum depth (mm) | +| `ZED_CONFIDENCE_THRESHOLD` | `60` | Reject depth less certain than this (1–100, lower is stricter) | +| `ZED_TEXTURE_CONFIDENCE_THRESHOLD` | `100` | Reject depth on surfaces less textured than this (1–100) | +| `ZED_DEPTH_FILL` | `false` | Interpolate depth over holes. Overrides both confidence thresholds. | | `ZED_FLIP` | `ON` | `ON` or `OFF` | | `ZED_WARMUP_FRAMES` | `30` | Frames to discard on startup | | `ZED_RETRY_INTERVAL_SECONDS` | `5` | Reconnection interval (s) | +The resolved depth mode, both confidence thresholds and the depth range are logged at +startup. The defaults match the reference camera driver the perception stack was tuned +against, so depth quality on the robot matches what it expects. + ### Point cloud | `ZED_PC_MODE` | Shape | Description | diff --git a/src/main.cc b/src/main.cc index 9cfe22e..26d545c 100644 --- a/src/main.cc +++ b/src/main.cc @@ -91,9 +91,11 @@ int64_t host_timestamp_ns() { } // namespace int main() { - auto dora_node = init_dora_node(); - + // Read the environment before joining the dataflow, so a bad configuration + // fails with its own message rather than after the node has registered. auto config = zed::Config::from_env(); + + auto dora_node = init_dora_node(); zed::Camera camera(config); camera.open(); camera.warmup(); diff --git a/src/zed/camera.cc b/src/zed/camera.cc index 45fbe78..15bd821 100644 --- a/src/zed/camera.cc +++ b/src/zed/camera.cc @@ -2,10 +2,12 @@ #include "util/env.h" #include +#include #include #include #include #include +#include namespace { @@ -21,17 +23,30 @@ sl::RESOLUTION parse_resolution(int resolution) { } } +const std::pair DEPTH_MODES[] = { + {"NEURAL", sl::DEPTH_MODE::NEURAL}, + {"NEURAL_PLUS", sl::DEPTH_MODE::NEURAL_PLUS}, + {"NEURAL_LIGHT", sl::DEPTH_MODE::NEURAL_LIGHT}, + {"PERFORMANCE", sl::DEPTH_MODE::PERFORMANCE}, + {"QUALITY", sl::DEPTH_MODE::QUALITY}, + {"NONE", sl::DEPTH_MODE::NONE}, +}; + +// A typo here silently changes depth quality if it falls back to a default, and +// the resulting depth is wrong in a way that only shows up downstream, so an +// unrecognised name stops the node instead. sl::DEPTH_MODE parse_depth_mode(const std::string& mode) { - if (mode == "PERFORMANCE") { - return sl::DEPTH_MODE::PERFORMANCE; + for (const auto& entry : DEPTH_MODES) { + if (mode == entry.first) { + return entry.second; + } } - if (mode == "QUALITY") { - return sl::DEPTH_MODE::QUALITY; + std::cerr << "Unknown ZED_DEPTH_MODE '" << mode << "'. Accepted values:"; + for (const auto& entry : DEPTH_MODES) { + std::cerr << " " << entry.first; } - if (mode == "NONE") { - return sl::DEPTH_MODE::NONE; - } - return sl::DEPTH_MODE::NEURAL; + std::cerr << std::endl; + std::exit(1); } sl::FLIP_MODE parse_flip(const std::string& mode) { @@ -59,10 +74,13 @@ Config Config::from_env() { Config c; c.resolution = util::get_env_int("ZED_RESOLUTION", c.resolution); c.fps = util::get_env_int("ZED_FPS", c.fps); - c.depth_mode = util::get_env_str("ZED_DEPTH_MODE", c.depth_mode); + c.depth_mode = parse_depth_mode(util::get_env_str("ZED_DEPTH_MODE", "NEURAL")); c.depth_min_mm = util::get_env_int("ZED_DEPTH_MIN_MM", c.depth_min_mm); c.depth_max_mm = util::get_env_int("ZED_DEPTH_MAX_MM", c.depth_max_mm); c.depth_fill = util::get_env_bool("ZED_DEPTH_FILL", c.depth_fill); + c.confidence_threshold = util::get_env_int("ZED_CONFIDENCE_THRESHOLD", c.confidence_threshold); + c.texture_confidence_threshold = + util::get_env_int("ZED_TEXTURE_CONFIDENCE_THRESHOLD", c.texture_confidence_threshold); c.flip = util::get_env_str("ZED_FLIP", c.flip); c.warmup_frames = util::get_env_int("ZED_WARMUP_FRAMES", c.warmup_frames); int retry_seconds = util::get_env_int("ZED_RETRY_INTERVAL_SECONDS", 5); @@ -78,7 +96,7 @@ Camera::Camera(const Config& config) : config_(config) { init_params_.camera_resolution = parse_resolution(config_.resolution); init_params_.camera_fps = config_.fps; - init_params_.depth_mode = parse_depth_mode(config_.depth_mode); + init_params_.depth_mode = config_.depth_mode; init_params_.coordinate_units = sl::UNIT::MILLIMETER; init_params_.camera_image_flip = parse_flip(config_.flip); init_params_.depth_minimum_distance = config_.depth_min_mm; @@ -86,6 +104,17 @@ Camera::Camera(const Config& config) : config_(config) { runtime_params_.enable_depth = true; runtime_params_.enable_fill_mode = config_.depth_fill; + // The two thresholds discard the depth the SDK is least certain of, which is + // the depth at object edges: those pixels straddle the object and whatever is + // behind it, and belong to neither. + runtime_params_.confidence_threshold = config_.confidence_threshold; + runtime_params_.texture_confidence_threshold = config_.texture_confidence_threshold; + + std::cout << "ZED depth: mode=" << sl::toString(init_params_.depth_mode) + << ", range=" << config_.depth_min_mm << "-" << config_.depth_max_mm << " mm" + << ", confidence=" << config_.confidence_threshold + << ", texture_confidence=" << config_.texture_confidence_threshold + << ", fill=" << (config_.depth_fill ? "on" : "off") << std::endl; } Camera::~Camera() { diff --git a/src/zed/camera.h b/src/zed/camera.h index 240299d..beec2dd 100644 --- a/src/zed/camera.h +++ b/src/zed/camera.h @@ -8,12 +8,14 @@ namespace zed { struct Config { - int resolution = 720; + int resolution = 1080; int fps = 15; - std::string depth_mode = "NEURAL"; - int depth_min_mm = 10; - int depth_max_mm = 500; + sl::DEPTH_MODE depth_mode = sl::DEPTH_MODE::NEURAL; + int depth_min_mm = 200; + int depth_max_mm = 600; bool depth_fill = false; + int confidence_threshold = 60; + int texture_confidence_threshold = 100; std::string flip = "ON"; int warmup_frames = 30; int retry_interval_ms = 5000; -- 2.54.0 From 42c2b791fd11d2b0b726bf0c728dc96842ab5b78 Mon Sep 17 00:00:00 2001 From: cesar Date: Tue, 8 Sep 2026 09:21:11 -0300 Subject: [PATCH 06/10] make point_cloud optional and publish camera_info once per session ZED_POINT_CLOUD, off by default, gates both retrieveMeasure(XYZRGBA) and the point_cloud output; when off the measure is not retrieved at all. At 1080p a full cloud is ~14.7 MB per frame and costs capture time whether or not anything consumes it, and consumers that need 3D can back-project depth instead. camera_info now goes out once per camera session rather than on every frame: fx, fy, cx, cy and the distortion coefficients are fixed for a given camera and resolution, so repeating them per frame adds no information. It rides the first frame after the camera opens, and again after a reconnection, so it still carries that frame's timestamps. Co-Authored-By: Claude Opus 5 Claude-Session: https://claude.ai/code/session_01Y7g2cBJ5e3r15ZbJ9WZn8c --- AGENTS.md | 2 +- README.md | 16 +++++-- src/main.cc | 114 +++++++++++++++++++++++++--------------------- src/zed/camera.cc | 7 ++- src/zed/camera.h | 1 + 5 files changed, 84 insertions(+), 56 deletions(-) diff --git a/AGENTS.md b/AGENTS.md index 6a4f960..ff129ca 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -38,7 +38,7 @@ Camera behavior is configured entirely through environment variables (see README - **`src/util/env.h`** — Shared inline env-variable parsing helpers (`get_env_int`, `get_env_str`, `get_env_bool`, `get_env_float`). - **`src/zed/camera.h`** — `zed::Config`, `zed::Calibration`, and `zed::Camera` class. Camera wraps the ZED SDK lifecycle: open (with retry loop), warmup, grab (image + depth + point cloud), calibration, device timestamp, and close. Exposes `handle()` for engine access. - **`src/zed/camera.cc`** — Implementation. SDK enum parsers live in an anonymous namespace. `parse_depth_mode` runs in `Config::from_env()`, not in the constructor, so an unrecognised `ZED_DEPTH_MODE` exits before the node joins the dataflow rather than falling back to a default. -- **`src/main.cc`** — Dora event loop. Parses `ZED_PC_MODE` (`FULL`/`HALF`) once before the loop. On any input event (intended to be driven by a tick timer; the id is not inspected): calls `camera.grab()`, then sends `image_bgr`, `depth`, `point_cloud` and `camera_info`. `pack_depth` copies the depth map into a tightly packed `[H, W]` float buffer (sl::Mat rows can be padded) and clears `inf`/`nan` to `0`. `set_intrinsics` and `set_frame_stamps` stamp every output of a frame with the same intrinsics, `frame_id` and the two timestamps (`device_timestamp_ns`, `host_timestamp_ns`; both Unix-epoch nanoseconds). Point cloud output varies by mode: `FULL` sends the raw `sl::Mat` pointer (no copy); `HALF` downsamples by 2 in each dimension. Handles reconnection on grab failure. +- **`src/main.cc`** — Dora event loop. Parses `ZED_PC_MODE` (`FULL`/`HALF`) once before the loop. On any input event (intended to be driven by a tick timer; the id is not inspected): calls `camera.grab()`, then sends `image_bgr` and `depth`, plus `point_cloud` when `ZED_POINT_CLOUD` is on and `camera_info` on the first frame of a camera session (`camera_info_pending`, re-armed on reconnection). `pack_depth` copies the depth map into a tightly packed `[H, W]` float buffer (sl::Mat rows can be padded) and clears `inf`/`nan` to `0`. `set_intrinsics` and `set_frame_stamps` stamp every output of a frame with the same intrinsics, `frame_id` and the two timestamps (`device_timestamp_ns`, `host_timestamp_ns`; both Unix-epoch nanoseconds). Point cloud output varies by mode: `FULL` sends the raw `sl::Mat` pointer (no copy); `HALF` downsamples by 2 in each dimension. Handles reconnection on grab failure. - **`DoraTargets.cmake`** — Handles building or fetching dora-rs and generating the CXX bridge files. Two paths: local (`DORA_ROOT_DIR`) or git clone. Produces `dora_node_api_cxx` library and the `node_bridge.cc` glue. - **`scripts/build.sh`** — Configure/compile/install in one shell-free-callable step. Referenced by `dataflow.yml`, the `Dockerfile` and the README. - **`dataflow.yml`** — Example dora dataflow definition using the `git:` source format, pinned with `tag:` — dora 1.0 pins a source with the mutually exclusive `tag:`, `rev:` or `branch:`. diff --git a/README.md b/README.md index 65f6d9b..fa7bfbc 100644 --- a/README.md +++ b/README.md @@ -111,9 +111,19 @@ against, so depth quality on the robot matches what it expects. ### Point cloud +Off by default. With `ZED_POINT_CLOUD` unset the XYZRGBA measure is not retrieved at all +and no `point_cloud` output is published — at 1080p a full cloud is roughly 14.7 MB per +frame, and retrieving it costs capture time whether or not anything consumes it. +Consumers that need 3D back-project `depth` with the published intrinsics instead. + +| Variable | Default | Description | +|---|---|---| +| `ZED_POINT_CLOUD` | `false` | Retrieve and publish the point cloud | +| `ZED_PC_MODE` | `FULL` | Only applies when `ZED_POINT_CLOUD` is on | + | `ZED_PC_MODE` | Shape | Description | |---|---|---| -| `FULL` (default) | `[H, W, 4]` | Full resolution grid (~14.7 MB/frame at 720p) | +| `FULL` (default) | `[H, W, 4]` | Full resolution grid (~14.7 MB/frame at 1080p) | | `HALF` | `[H/2, W/2, 4]` | Stride-2 downsample (~4× smaller) | ## Inputs @@ -128,8 +138,8 @@ against, so depth quality on the robot matches what it expects. |---|---|---| | `image_bgr` | `uint8 [H, W, 3]` | BGR image. Metadata: `shape`, `encoding=bgr8`, intrinsics, timestamps. | | `depth` | `float32 [H, W]` | Per-pixel depth in millimetres, published on every frame `image_bgr` is. Metadata: `dtype`, `layout=HW`, `shape`, `units=mm`, intrinsics, timestamps. `H` and `W` match `image_bgr` for the same `frame_id`. Missing depth is `0` — the payload contains no `inf` and no `nan`. Holes are not interpolated unless `ZED_DEPTH_FILL` is set. | -| `point_cloud` | `float32 [*, 4]` | XYZRGBA point cloud. Shape depends on `ZED_PC_MODE`. Metadata: `units=mm`, intrinsics, timestamps. | -| `camera_info` | `float32 [4]` | `[fx, fy, cx, cy]`. Metadata: `distortion` coefficients, timestamps. | +| `point_cloud` | `float32 [*, 4]` | XYZRGBA point cloud, only when `ZED_POINT_CLOUD` is on. Shape depends on `ZED_PC_MODE`. Metadata: `units=mm`, intrinsics, timestamps. | +| `camera_info` | `float32 [4]` | `[fx, fy, cx, cy]`. Metadata: `distortion` coefficients, timestamps. Published **once per camera session** — with the first frame after the camera opens, and again after a reconnection — not on every frame, since the intrinsics are fixed for a given camera and resolution. | Consumers that need 3D back-project `depth` with the published intrinsics rather than consuming `point_cloud`; the point cloud is not a prerequisite for 3D. diff --git a/src/main.cc b/src/main.cc index 26d545c..00880da 100644 --- a/src/main.cc +++ b/src/main.cc @@ -104,6 +104,12 @@ int main() { std::string pc_mode_str = util::get_env_str("ZED_PC_MODE", "FULL"); PcMode pc_mode = (pc_mode_str == "HALF") ? PcMode::HALF : PcMode::FULL; + // fx, fy, cx, cy and the distortion coefficients are fixed for a given camera + // and resolution, so they are published once per camera session rather than on + // every frame. It goes out with the first frame of the session so that it + // carries that frame's timestamps like every other output. + bool camera_info_pending = true; + int64_t frame_id = 0; while (true) { auto event = dora_node.events->next(); @@ -122,12 +128,12 @@ int main() { camera.close(); camera.open(); calib = camera.calibration(); + camera_info_pending = true; continue; } sl::Mat& image_mat = camera.image(); sl::Mat& depth_mat = camera.depth(); - sl::Mat& point_cloud_mat = camera.point_cloud(); int width = image_mat.getWidth(); int height = image_mat.getHeight(); @@ -181,66 +187,72 @@ int main() { std::cerr << "Error sending depth: " << std::string(depth_result.error) << std::endl; } - // --- point_cloud --- - int pc_width = point_cloud_mat.getWidth(); - int pc_height = point_cloud_mat.getHeight(); + if (config.point_cloud) { + // --- point_cloud --- + sl::Mat& point_cloud_mat = camera.point_cloud(); + int pc_width = point_cloud_mat.getWidth(); + int pc_height = point_cloud_mat.getHeight(); - std::vector pc_buf; - const uint8_t* pc_data_ptr = nullptr; - size_t pc_data_size = 0; - auto pc_metadata = new_metadata(); - pc_metadata->set_string("dtype", "float32"); - pc_metadata->set_string("channels", "XYZRGBA"); - pc_metadata->set_string("units", "mm"); - set_intrinsics(*pc_metadata, calib); - set_frame_stamps(*pc_metadata, current_frame_id, device_ns, host_ns); + std::vector pc_buf; + const uint8_t* pc_data_ptr = nullptr; + size_t pc_data_size = 0; + auto pc_metadata = new_metadata(); + pc_metadata->set_string("dtype", "float32"); + pc_metadata->set_string("channels", "XYZRGBA"); + pc_metadata->set_string("units", "mm"); + set_intrinsics(*pc_metadata, calib); + set_frame_stamps(*pc_metadata, current_frame_id, device_ns, host_ns); - if (pc_mode == PcMode::HALF) { - int hw = pc_width / 2, hh = pc_height / 2; - pc_buf.resize(static_cast(hh * hw * 4)); - const sl::float4* src = point_cloud_mat.getPtr(); - for (int y = 0; y < hh; y++) { - for (int x = 0; x < hw; x++) { - const sl::float4& pt = src[(y * 2) * pc_width + (x * 2)]; - float* dst = pc_buf.data() + (y * hw + x) * 4; - dst[0] = pt.x; dst[1] = pt.y; dst[2] = pt.z; dst[3] = pt.w; + if (pc_mode == PcMode::HALF) { + int hw = pc_width / 2, hh = pc_height / 2; + pc_buf.resize(static_cast(hh * hw * 4)); + const sl::float4* src = point_cloud_mat.getPtr(); + for (int y = 0; y < hh; y++) { + for (int x = 0; x < hw; x++) { + const sl::float4& pt = src[(y * 2) * pc_width + (x * 2)]; + float* dst = pc_buf.data() + (y * hw + x) * 4; + dst[0] = pt.x; dst[1] = pt.y; dst[2] = pt.z; dst[3] = pt.w; + } } + pc_metadata->set_string("layout", "HWC"); + pc_metadata->set_list_int("shape", make_shape(hh, hw, 4)); + pc_data_ptr = reinterpret_cast(pc_buf.data()); + pc_data_size = pc_buf.size() * sizeof(float); + } else { + // FULL — send raw sl::Mat pointer directly (no copy) + pc_data_size = static_cast(pc_width * pc_height * sizeof(sl::float4)); + pc_data_ptr = reinterpret_cast(point_cloud_mat.getPtr()); + pc_metadata->set_string("layout", "HWC"); + pc_metadata->set_list_int("shape", make_shape(pc_height, pc_width, 4)); } - pc_metadata->set_string("layout", "HWC"); - pc_metadata->set_list_int("shape", make_shape(hh, hw, 4)); - pc_data_ptr = reinterpret_cast(pc_buf.data()); - pc_data_size = pc_buf.size() * sizeof(float); - } else { - // FULL — send raw sl::Mat pointer directly (no copy) - pc_data_size = static_cast(pc_width * pc_height * sizeof(sl::float4)); - pc_data_ptr = reinterpret_cast(point_cloud_mat.getPtr()); - pc_metadata->set_string("layout", "HWC"); - pc_metadata->set_list_int("shape", make_shape(pc_height, pc_width, 4)); - } - rust::Slice pc_slice{pc_data_ptr, pc_data_size}; - auto pc_result = send_output_with_metadata( - dora_node.send_output, "point_cloud", pc_slice, std::move(pc_metadata)); - if (!std::string(pc_result.error).empty()) { - std::cerr << "Error sending point_cloud: " << std::string(pc_result.error) << std::endl; + rust::Slice pc_slice{pc_data_ptr, pc_data_size}; + auto pc_result = send_output_with_metadata( + dora_node.send_output, "point_cloud", pc_slice, std::move(pc_metadata)); + if (!std::string(pc_result.error).empty()) { + std::cerr << "Error sending point_cloud: " << std::string(pc_result.error) << std::endl; + } } // --- camera_info --- - float camera_data[4] = {calib.fx, calib.fy, calib.cx, calib.cy}; - rust::Slice cam_slice{ - reinterpret_cast(camera_data), sizeof(camera_data)}; + if (camera_info_pending) { + float camera_data[4] = {calib.fx, calib.fy, calib.cx, calib.cy}; + rust::Slice cam_slice{ + reinterpret_cast(camera_data), sizeof(camera_data)}; - auto cam_metadata = new_metadata(); - cam_metadata->set_string("dtype", "float32"); - cam_metadata->set_string("layout", "C"); - cam_metadata->set_list_int("shape", make_vector_int({4})); - cam_metadata->set_list_float("distortion", make_distortion(calib.distortion)); - set_frame_stamps(*cam_metadata, current_frame_id, device_ns, host_ns); + auto cam_metadata = new_metadata(); + cam_metadata->set_string("dtype", "float32"); + cam_metadata->set_string("layout", "C"); + cam_metadata->set_list_int("shape", make_vector_int({4})); + cam_metadata->set_list_float("distortion", make_distortion(calib.distortion)); + set_frame_stamps(*cam_metadata, current_frame_id, device_ns, host_ns); - auto cam_result = send_output_with_metadata( - dora_node.send_output, "camera_info", cam_slice, std::move(cam_metadata)); - if (!std::string(cam_result.error).empty()) { - std::cerr << "Error sending camera_info: " << std::string(cam_result.error) << std::endl; + auto cam_result = send_output_with_metadata( + dora_node.send_output, "camera_info", cam_slice, std::move(cam_metadata)); + if (!std::string(cam_result.error).empty()) { + std::cerr << "Error sending camera_info: " << std::string(cam_result.error) << std::endl; + } + camera_info_pending = false; } } diff --git a/src/zed/camera.cc b/src/zed/camera.cc index 15bd821..ed52202 100644 --- a/src/zed/camera.cc +++ b/src/zed/camera.cc @@ -78,6 +78,7 @@ Config Config::from_env() { c.depth_min_mm = util::get_env_int("ZED_DEPTH_MIN_MM", c.depth_min_mm); c.depth_max_mm = util::get_env_int("ZED_DEPTH_MAX_MM", c.depth_max_mm); c.depth_fill = util::get_env_bool("ZED_DEPTH_FILL", c.depth_fill); + c.point_cloud = util::get_env_bool("ZED_POINT_CLOUD", c.point_cloud); c.confidence_threshold = util::get_env_int("ZED_CONFIDENCE_THRESHOLD", c.confidence_threshold); c.texture_confidence_threshold = util::get_env_int("ZED_TEXTURE_CONFIDENCE_THRESHOLD", c.texture_confidence_threshold); @@ -149,7 +150,11 @@ bool Camera::grab() { } zed_.retrieveImage(image_mat_, sl::VIEW::LEFT); zed_.retrieveMeasure(depth_mat_, sl::MEASURE::DEPTH); - zed_.retrieveMeasure(point_cloud_mat_, sl::MEASURE::XYZRGBA); + // At 1080p a full XYZRGBA cloud is ~14.7 MB per frame, and retrieving it + // costs capture time whether or not anything consumes it. + if (config_.point_cloud) { + zed_.retrieveMeasure(point_cloud_mat_, sl::MEASURE::XYZRGBA); + } return true; } diff --git a/src/zed/camera.h b/src/zed/camera.h index beec2dd..598dc48 100644 --- a/src/zed/camera.h +++ b/src/zed/camera.h @@ -14,6 +14,7 @@ struct Config { int depth_min_mm = 200; int depth_max_mm = 600; bool depth_fill = false; + bool point_cloud = false; int confidence_threshold = 60; int texture_confidence_threshold = 100; std::string flip = "ON"; -- 2.54.0 From d8956f269da4ba3ad5056dd872bbed1a47d3e965 Mon Sep 17 00:00:00 2001 From: cesar Date: Tue, 8 Sep 2026 09:26:17 -0300 Subject: [PATCH 07/10] fix camera recovery blocking the dora event loop Camera::open() retried forever inside a fixed sleep, so a camera that did not come back left the node stuck there: it stopped reading dora events and no longer honoured Stop or AllInputsClosed. Recovery is now a state machine over ticks. open() makes one attempt and returns; the loop decides when to try again from a 1/2/5/10 s backoff deadline, and nothing sleeps, so events keep being read while the camera is down. The first attempt closes the handle; past that the device is wedged and the node escalates to sl::Camera::reboot(), guarded to the ZED 2 and newer since it does nothing on the ZED and ZED Mini. Reopening pins the serial so recovery gets the same physical camera back. Set async_grab_camera_recovery so a link hiccup returns CAMERA_REBOOTING immediately instead of blocking the grab for ~17 s, and enable_image_validity_ check so the SDK flags frames a stalled camera hands back looking valid. Those transient errors no longer drop the handle, since the SDK is already reconnecting it. An all-black frame now counts as a failed grab rather than being published, and a camera delivering nothing usable for 5 s (30 s while the SDK reports it is reconnecting) triggers recovery. A new camera_state output publishes opening/streaming/recovering so a downstream node can tell "no frame yet" from "camera lost". ZED_RETRY_INTERVAL_SECONDS is replaced by the backoff. Co-Authored-By: Claude Opus 5 Claude-Session: https://claude.ai/code/session_01Y7g2cBJ5e3r15ZbJ9WZn8c --- AGENTS.md | 4 +- README.md | 32 +++++++++- dataflow.yml | 1 + src/main.cc | 146 +++++++++++++++++++++++++++++++++++++++++++--- src/zed/camera.cc | 133 +++++++++++++++++++++++++++++++++++------ src/zed/camera.h | 22 +++++-- 6 files changed, 307 insertions(+), 31 deletions(-) diff --git a/AGENTS.md b/AGENTS.md index ff129ca..c4f26b4 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -36,9 +36,9 @@ Camera behavior is configured entirely through environment variables (see README ## Architecture - **`src/util/env.h`** — Shared inline env-variable parsing helpers (`get_env_int`, `get_env_str`, `get_env_bool`, `get_env_float`). -- **`src/zed/camera.h`** — `zed::Config`, `zed::Calibration`, and `zed::Camera` class. Camera wraps the ZED SDK lifecycle: open (with retry loop), warmup, grab (image + depth + point cloud), calibration, device timestamp, and close. Exposes `handle()` for engine access. +- **`src/zed/camera.h`** — `zed::Config`, `zed::Calibration`, and `zed::Camera` class. Camera wraps the ZED SDK lifecycle: `open()` (one attempt, returns success), warmup, grab (image + depth + point cloud), calibration, device timestamp, blank-frame check, `close()` and `reset()` (close plus `sl::Camera::reboot()` where the model allows it). Exposes `handle()` for engine access. - **`src/zed/camera.cc`** — Implementation. SDK enum parsers live in an anonymous namespace. `parse_depth_mode` runs in `Config::from_env()`, not in the constructor, so an unrecognised `ZED_DEPTH_MODE` exits before the node joins the dataflow rather than falling back to a default. -- **`src/main.cc`** — Dora event loop. Parses `ZED_PC_MODE` (`FULL`/`HALF`) once before the loop. On any input event (intended to be driven by a tick timer; the id is not inspected): calls `camera.grab()`, then sends `image_bgr` and `depth`, plus `point_cloud` when `ZED_POINT_CLOUD` is on and `camera_info` on the first frame of a camera session (`camera_info_pending`, re-armed on reconnection). `pack_depth` copies the depth map into a tightly packed `[H, W]` float buffer (sl::Mat rows can be padded) and clears `inf`/`nan` to `0`. `set_intrinsics` and `set_frame_stamps` stamp every output of a frame with the same intrinsics, `frame_id` and the two timestamps (`device_timestamp_ns`, `host_timestamp_ns`; both Unix-epoch nanoseconds). Point cloud output varies by mode: `FULL` sends the raw `sl::Mat` pointer (no copy); `HALF` downsamples by 2 in each dimension. Handles reconnection on grab failure. +- **`src/main.cc`** — Dora event loop. Parses `ZED_PC_MODE` (`FULL`/`HALF`) once before the loop. On any input event (intended to be driven by a tick timer; the id is not inspected): calls `camera.grab()`, then sends `image_bgr` and `depth`, plus `point_cloud` when `ZED_POINT_CLOUD` is on and `camera_info` on the first frame of a camera session (`camera_info_pending`, re-armed on reconnection). `pack_depth` copies the depth map into a tightly packed `[H, W]` float buffer (sl::Mat rows can be padded) and clears `inf`/`nan` to `0`. `set_intrinsics` and `set_frame_stamps` stamp every output of a frame with the same intrinsics, `frame_id` and the two timestamps (`device_timestamp_ns`, `host_timestamp_ns`; both Unix-epoch nanoseconds). Point cloud output varies by mode: `FULL` sends the raw `sl::Mat` pointer (no copy); `HALF` downsamples by 2 in each dimension. Recovery is a state machine over ticks rather than a blocking retry: the camera is opened from inside the loop, `begin_recovery` closes (first attempt) or resets (later ones) and sets a backoff deadline, and `camera_state` is published on each transition. Nothing sleeps, so `Stop` is honoured while the camera is down. - **`DoraTargets.cmake`** — Handles building or fetching dora-rs and generating the CXX bridge files. Two paths: local (`DORA_ROOT_DIR`) or git clone. Produces `dora_node_api_cxx` library and the `node_bridge.cc` glue. - **`scripts/build.sh`** — Configure/compile/install in one shell-free-callable step. Referenced by `dataflow.yml`, the `Dockerfile` and the README. - **`dataflow.yml`** — Example dora dataflow definition using the `git:` source format, pinned with `tag:` — dora 1.0 pins a source with the mutually exclusive `tag:`, `rev:` or `branch:`. diff --git a/README.md b/README.md index fa7bfbc..b634abe 100644 --- a/README.md +++ b/README.md @@ -20,6 +20,7 @@ nodes: - depth - point_cloud - camera_info + - camera_state ``` See [`dataflow.yml`](dataflow.yml) for a complete runnable example with a camera display node. @@ -103,7 +104,6 @@ All behavior is controlled via environment variables. | `ZED_DEPTH_FILL` | `false` | Interpolate depth over holes. Overrides both confidence thresholds. | | `ZED_FLIP` | `ON` | `ON` or `OFF` | | `ZED_WARMUP_FRAMES` | `30` | Frames to discard on startup | -| `ZED_RETRY_INTERVAL_SECONDS` | `5` | Reconnection interval (s) | The resolved depth mode, both confidence thresholds and the depth range are logged at startup. The defaults match the reference camera driver the perception stack was tuned @@ -140,6 +140,36 @@ Consumers that need 3D back-project `depth` with the published intrinsics instea | `depth` | `float32 [H, W]` | Per-pixel depth in millimetres, published on every frame `image_bgr` is. Metadata: `dtype`, `layout=HW`, `shape`, `units=mm`, intrinsics, timestamps. `H` and `W` match `image_bgr` for the same `frame_id`. Missing depth is `0` — the payload contains no `inf` and no `nan`. Holes are not interpolated unless `ZED_DEPTH_FILL` is set. | | `point_cloud` | `float32 [*, 4]` | XYZRGBA point cloud, only when `ZED_POINT_CLOUD` is on. Shape depends on `ZED_PC_MODE`. Metadata: `units=mm`, intrinsics, timestamps. | | `camera_info` | `float32 [4]` | `[fx, fy, cx, cy]`. Metadata: `distortion` coefficients, timestamps. Published **once per camera session** — with the first frame after the camera opens, and again after a reconnection — not on every frame, since the intrinsics are fixed for a given camera and resolution. | +| `camera_state` | `string` | UTF-8 state name, published on each change. Metadata: `dtype=string`, `encoding=utf8`, `host_timestamp_ns`. | + +### Camera state and recovery + +`camera_state` lets a downstream node tell a camera that has not delivered its first +frame yet from one that has been lost: + +| State | Meaning | +|---|---| +| `opening` | The camera has not delivered a usable frame yet in this run | +| `streaming` | Frames are being published | +| `recovering` | The camera was streaming and has stopped; the node is trying to bring it back | + +Recovery runs inside the dora event loop rather than in a retry loop of its own, so the +node keeps reading events — and honours `Stop` — while the camera is down. Reopening +never blocks the loop between attempts: the tick timer paces them, and a backoff deadline +of 1, 2, 5 then 10 seconds decides which ticks actually try. + +The node treats an all-black frame as a failed grab: a camera whose link has dropped does +not necessarily fail its grabs, and can keep reporting success while handing back a +uniform frame. A camera that delivers no usable frame for 5 seconds is recovered — 30 +seconds if the SDK reports it is reconnecting the link itself, since taking the handle +away mid-reconnect aborts a recovery that was already under way. + +The first recovery attempt closes the handle. Past that the device itself is wedged, and +the node escalates to `sl::Camera::reboot()`, which resets it over USB without unplugging +it. That call reaches the ZED 2 and newer only; on the first-generation ZED and the ZED +Mini the node closes the handle and lets the backoff give the endpoint time to clear. +Every attempt and every escalation is logged. + Consumers that need 3D back-project `depth` with the published intrinsics rather than consuming `point_cloud`; the point cloud is not a prerequisite for 3D. diff --git a/dataflow.yml b/dataflow.yml index df9c3e9..f03c9db 100644 --- a/dataflow.yml +++ b/dataflow.yml @@ -11,6 +11,7 @@ nodes: - depth - point_cloud - camera_info + - camera_state - id: plot git: https://github.com/dora-rs/dora-hub.git diff --git a/src/main.cc b/src/main.cc index 00880da..94dbfee 100644 --- a/src/main.cc +++ b/src/main.cc @@ -4,10 +4,12 @@ #include #include +#include #include #include #include #include +#include #include #include @@ -15,6 +17,28 @@ namespace { enum class PcMode { FULL, HALF }; +using Clock = std::chrono::steady_clock; + +// Going this long without a usable frame means the camera is wedged rather than +// dropping the odd frame. Measured in elapsed time rather than in failed grabs, +// because a stalling camera can spend seconds inside a single grab. +constexpr double STALL_TIMEOUT_S = 5.0; + +// How long the SDK is left to reconnect the camera on its own before the loop +// stops waiting on it and rebuilds the camera anyway. A driver that says it is +// recovering is usually right, but it has no way to tell us it has given up. +constexpr double DRIVER_RECOVERY_TIMEOUT_S = 30.0; + +// How long to wait before each reopen: one entry per attempt, the last repeated +// for every attempt beyond it. +constexpr int REOPEN_BACKOFF_S[] = {1, 2, 5, 10}; + +// Published on the camera_state output so a downstream node can tell a camera +// that has not delivered its first frame yet from one that has been lost. +constexpr const char* STATE_OPENING = "opening"; +constexpr const char* STATE_STREAMING = "streaming"; +constexpr const char* STATE_RECOVERING = "recovering"; + rust::Vec make_shape(int height, int width, int channels) { rust::Vec shape; shape.push_back(height); @@ -82,6 +106,35 @@ std::vector pack_depth(sl::Mat& depth_mat) { return out; } +std::chrono::seconds reopen_backoff(int attempt) { + const size_t index = + std::min(static_cast(attempt), std::size(REOPEN_BACKOFF_S)) - 1; + return std::chrono::seconds(REOPEN_BACKOFF_S[index]); +} + +double seconds_since(Clock::time_point since) { + return std::chrono::duration(Clock::now() - since).count(); +} + +// Release the camera and schedule the next open attempt. The first attempt only +// closes the handle, which is enough when the driver rather than the device lost +// its way; past that the device itself is wedged and nothing short of resetting +// it will bring the frames back. +// +// The wait is a deadline rather than a sleep: the loop returns to dora between +// attempts, so Stop is still honoured while the camera is down. +void begin_recovery(zed::Camera& camera, int attempt, Clock::time_point& next_attempt_at) { + if (attempt <= 1) { + camera.close(); + } else { + camera.reset(); + } + const auto wait = reopen_backoff(attempt); + std::cerr << "recovering camera (attempt " << attempt << "); next open in " << wait.count() + << "s" << std::endl; + next_attempt_at = Clock::now() + wait; +} + int64_t host_timestamp_ns() { return std::chrono::duration_cast( std::chrono::system_clock::now().time_since_epoch()) @@ -97,9 +150,29 @@ int main() { auto dora_node = init_dora_node(); zed::Camera camera(config); - camera.open(); - camera.warmup(); - auto calib = camera.calibration(); + zed::Calibration calib; + + std::string camera_state; + auto publish_camera_state = [&](const char* state) { + if (camera_state == state) { + return; + } + camera_state = state; + rust::Slice state_slice{ + reinterpret_cast(camera_state.data()), camera_state.size()}; + + auto metadata = new_metadata(); + metadata->set_string("dtype", "string"); + metadata->set_string("encoding", "utf8"); + metadata->set_timestamp("host_timestamp_ns", host_timestamp_ns()); + + auto result = send_output_with_metadata( + dora_node.send_output, "camera_state", state_slice, std::move(metadata)); + if (!std::string(result.error).empty()) { + std::cerr << "Error sending camera_state: " << std::string(result.error) << std::endl; + } + std::cout << "camera state: " << camera_state << std::endl; + }; std::string pc_mode_str = util::get_env_str("ZED_PC_MODE", "FULL"); PcMode pc_mode = (pc_mode_str == "HALF") ? PcMode::HALF : PcMode::FULL; @@ -110,6 +183,16 @@ int main() { // carries that frame's timestamps like every other output. bool camera_info_pending = true; + // The camera is opened from inside the event loop, one attempt per tick, so + // the node keeps reading dora events while the camera is unavailable. There + // is no retry sleep: the tick timer paces the attempts, and the backoff + // deadline decides which ticks actually try. + int recovery_attempts = 0; + bool streamed_once = false; + bool stall_reported = false; + auto next_open_attempt_at = Clock::now(); + auto last_good_frame_at = Clock::now(); + int64_t frame_id = 0; while (true) { auto event = dora_node.events->next(); @@ -123,15 +206,64 @@ int main() { continue; } - if (!camera.grab()) { - std::cerr << "Grab failed; attempting to reconnect..." << std::endl; - camera.close(); - camera.open(); + const char* down_state = streamed_once ? STATE_RECOVERING : STATE_OPENING; + + if (!camera.is_open()) { + publish_camera_state(down_state); + if (Clock::now() < next_open_attempt_at) { + continue; + } + if (!camera.open()) { + recovery_attempts++; + begin_recovery(camera, recovery_attempts, next_open_attempt_at); + continue; + } + camera.warmup(); calib = camera.calibration(); camera_info_pending = true; + // Opening the ZED takes seconds. Start the stall clock from here so + // that cost is never mistaken for a stalled stream. + last_good_frame_at = Clock::now(); + // recovery_attempts is deliberately not cleared here. A wedged device + // can open and still deliver nothing; clearing on open would restart + // the escalation ladder every cycle and never reach the reset. continue; } + // A blank frame counts as a failed grab: a camera whose link has dropped + // does not necessarily fail its grabs, it can keep reporting success and + // hand back a uniform frame with an empty depth map. + if (!camera.grab() || camera.image_is_blank()) { + publish_camera_state(down_state); + const double stalled_for = seconds_since(last_good_frame_at); + // The SDK holds the camera open while it reconnects a dropped link. + // Taking the handle away mid-reconnect aborts that and leaves us to + // redo it the slow way, so it gets longer than a plain stall. + const double patience = + camera.recovering() ? DRIVER_RECOVERY_TIMEOUT_S : STALL_TIMEOUT_S; + if (stalled_for < patience) { + continue; + } + if (!stall_reported) { + stall_reported = true; + std::cerr << "camera delivered no usable frame for " << stalled_for << "s" + << std::endl; + } + recovery_attempts++; + begin_recovery(camera, recovery_attempts, next_open_attempt_at); + continue; + } + + if (stall_reported) { + std::cout << "camera recovered after " << recovery_attempts << " attempt(s)" + << std::endl; + stall_reported = false; + } + last_good_frame_at = Clock::now(); + recovery_attempts = 0; + streamed_once = true; + publish_camera_state(STATE_STREAMING); + sl::Mat& image_mat = camera.image(); sl::Mat& depth_mat = camera.depth(); diff --git a/src/zed/camera.cc b/src/zed/camera.cc index ed52202..1e3b31d 100644 --- a/src/zed/camera.cc +++ b/src/zed/camera.cc @@ -1,12 +1,11 @@ #include "zed/camera.h" #include "util/env.h" -#include +#include #include #include #include #include -#include #include namespace { @@ -56,6 +55,24 @@ sl::FLIP_MODE parse_flip(const std::string& mode) { return sl::FLIP_MODE::ON; } +// A blank frame is checked on a strided sample rather than the full image, which +// is just as conclusive and costs nothing at frame rate. +constexpr int BLANK_FRAME_STRIDE = 16; + +// sl::Camera::reboot() reaches the ZED 2 and newer. On the first-generation ZED +// and the ZED Mini it returns without touching the device, so calling it there +// only costs time and fills the log. +bool model_supports_reboot(sl::MODEL model) { + return model != sl::MODEL::ZED && model != sl::MODEL::ZED_M; +} + +// The SDK reconnects a camera whose link hiccuped by itself. Dropping the handle +// over one of these takes the recovery away from the SDK and hands it to us, +// which is slower and, on a device the kernel is about to suspend, often fatal. +bool is_transient_grab_error(sl::ERROR_CODE err) { + return err == sl::ERROR_CODE::CAMERA_REBOOTING || err == sl::ERROR_CODE::CORRUPTED_FRAME; +} + void list_available_cameras() { auto devices = sl::Camera::getDeviceList(); std::cout << "Detected ZED cameras: " << devices.size() << std::endl; @@ -84,8 +101,6 @@ Config Config::from_env() { util::get_env_int("ZED_TEXTURE_CONFIDENCE_THRESHOLD", c.texture_confidence_threshold); c.flip = util::get_env_str("ZED_FLIP", c.flip); c.warmup_frames = util::get_env_int("ZED_WARMUP_FRAMES", c.warmup_frames); - int retry_seconds = util::get_env_int("ZED_RETRY_INTERVAL_SECONDS", 5); - c.retry_interval_ms = retry_seconds * 1000; return c; } @@ -102,6 +117,13 @@ Camera::Camera(const Config& config) : config_(config) { init_params_.camera_image_flip = parse_flip(config_.flip); init_params_.depth_minimum_distance = config_.depth_min_mm; init_params_.depth_maximum_distance = config_.depth_max_mm; + // Recover a hiccuping link inside the SDK rather than out here. Without this + // a grab blocks for the roughly 17 s the SDK spends reconnecting, freezing the + // dora event loop; with it the grab returns CAMERA_REBOOTING straight away and + // the SDK reconnects in the background, holding the device open meanwhile. + init_params_.async_grab_camera_recovery = true; + // Let the SDK flag the frames a stalled camera hands back looking valid. + init_params_.enable_image_validity_check = 1; runtime_params_.enable_depth = true; runtime_params_.enable_fill_mode = config_.depth_fill; @@ -116,28 +138,64 @@ Camera::Camera(const Config& config) : config_(config) { << ", confidence=" << config_.confidence_threshold << ", texture_confidence=" << config_.texture_confidence_threshold << ", fill=" << (config_.depth_fill ? "on" : "off") << std::endl; + + list_available_cameras(); } Camera::~Camera() { close(); } -void Camera::open() { - list_available_cameras(); - while (true) { - sl::ERROR_CODE err = zed_.open(init_params_); - if (err == sl::ERROR_CODE::SUCCESS) { - return; - } +bool Camera::open() { + sl::ERROR_CODE err = zed_.open(init_params_); + if (err != sl::ERROR_CODE::SUCCESS) { std::cerr << "Failed to open ZED camera: " << sl::toString(err) << std::endl; - std::this_thread::sleep_for(std::chrono::milliseconds(config_.retry_interval_ms)); + return false; } + recovering_ = false; + + sl::CameraInformation info = zed_.getCameraInformation(); + serial_number_ = info.serial_number; + model_ = info.camera_model; + // After the first open, reopen the same physical camera. Recovery runs while + // the device may be mid-re-enumeration, and picking whichever ZED answers + // first is not the same thing as picking ours. + init_params_.input.setFromSerialNumber(serial_number_); + + std::cout << "ZED camera opened (" << sl::toString(model_) << ", SN " << serial_number_ << ")" + << std::endl; + return true; } void Camera::close() { + recovering_ = false; zed_.close(); } +void Camera::reset() { + close(); + if (serial_number_ == 0 || !model_supports_reboot(model_)) { + return; + } + // The ZED can stop delivering frames while staying enumerated: the video + // endpoint halts, grabs fail, and reopening returns CAMERA FAILED TO SETUP + // because the endpoint is still halted. A device reset clears that, and + // reboot() is the SDK's way to ask for one without unplugging the camera. + // The device re-enumerates afterwards, so the caller's backoff before the + // next open is what lets it settle. + sl::ERROR_CODE err = sl::Camera::reboot(static_cast(serial_number_), true); + std::cout << "reset ZED camera (SN " << serial_number_ << "): " << sl::toString(err) + << std::endl; +} + +bool Camera::is_open() const { + return zed_.isOpened(); +} + +bool Camera::recovering() const { + return recovering_; +} + void Camera::warmup() { for (int i = 0; i < config_.warmup_frames; i++) { zed_.grab(runtime_params_); @@ -145,9 +203,27 @@ void Camera::warmup() { } bool Camera::grab() { - if (zed_.grab(runtime_params_) != sl::ERROR_CODE::SUCCESS) { + sl::ERROR_CODE err = zed_.grab(runtime_params_); + if (err != sl::ERROR_CODE::SUCCESS) { + if (is_transient_grab_error(err)) { + // Every grab repeats this for as long as the reconnect takes, so only + // the first one is worth a line in the log. + if (!recovering_) { + recovering_ = true; + std::cerr << "ZED camera link dropped (" << sl::toString(err) + << "); the SDK is reconnecting it" << std::endl; + } + } else { + recovering_ = false; + std::cerr << "Failed to grab frame: " << sl::toString(err) << std::endl; + } return false; } + if (recovering_) { + recovering_ = false; + std::cout << "ZED camera link restored" << std::endl; + } + zed_.retrieveImage(image_mat_, sl::VIEW::LEFT); zed_.retrieveMeasure(depth_mat_, sl::MEASURE::DEPTH); // At 1080p a full XYZRGBA cloud is ~14.7 MB per frame, and retrieving it @@ -175,6 +251,33 @@ Calibration Camera::calibration() const { return result; } +bool Camera::image_is_blank() { + const int height = image_mat_.getHeight(); + const int width = image_mat_.getWidth(); + if (height == 0 || width == 0) { + return true; + } + + const size_t step = image_mat_.getStep(sl::MEM::CPU); + const sl::uchar4* src = image_mat_.getPtr(); + + unsigned char lowest[3] = {255, 255, 255}; + unsigned char highest[3] = {0, 0, 0}; + for (int y = 0; y < height; y += BLANK_FRAME_STRIDE) { + const sl::uchar4* row = src + static_cast(y) * step; + for (int x = 0; x < width; x += BLANK_FRAME_STRIDE) { + const unsigned char channels[3] = {row[x].x, row[x].y, row[x].z}; + for (int c = 0; c < 3; c++) { + lowest[c] = std::min(lowest[c], channels[c]); + highest[c] = std::max(highest[c], channels[c]); + } + } + } + // Sensor noise makes a single-colour frame impossible for a live camera, so + // this tells a dead device apart from any real scene. + return lowest[0] == highest[0] && lowest[1] == highest[1] && lowest[2] == highest[2]; +} + int64_t Camera::device_timestamp_ns() { return static_cast( zed_.getTimestamp(sl::TIME_REFERENCE::IMAGE).getNanoseconds()); @@ -192,8 +295,4 @@ sl::Mat& Camera::point_cloud() { return point_cloud_mat_; } -sl::Camera& Camera::handle() { - return zed_; -} - } // namespace zed diff --git a/src/zed/camera.h b/src/zed/camera.h index 598dc48..f13be90 100644 --- a/src/zed/camera.h +++ b/src/zed/camera.h @@ -19,13 +19,12 @@ struct Config { int texture_confidence_threshold = 100; std::string flip = "ON"; int warmup_frames = 30; - int retry_interval_ms = 5000; static Config from_env(); }; struct Calibration { - float fx, fy, cx, cy; + float fx = 0, fy = 0, cx = 0, cy = 0; std::vector distortion; }; @@ -34,16 +33,26 @@ public: explicit Camera(const Config& config); ~Camera(); - void open(); + // One attempt, returning whether it succeeded. The caller decides when to try + // again, so a camera that is down never blocks the dora event loop. + bool open(); void close(); + // Close the handle and reset the device over USB, where the model allows it. + void reset(); void warmup(); bool grab(); + bool is_open() const; + // True while the SDK is reconnecting the camera after a link hiccup. + bool recovering() const; + // True when the last grabbed frame carries no picture at all. A camera whose + // link has dropped does not necessarily fail its grabs: it can keep reporting + // success and hand back a uniform frame. + bool image_is_blank(); Calibration calibration() const; int64_t device_timestamp_ns(); sl::Mat& image(); sl::Mat& depth(); sl::Mat& point_cloud(); - sl::Camera& handle(); private: Config config_; @@ -53,6 +62,11 @@ private: sl::Mat image_mat_; sl::Mat depth_mat_; sl::Mat point_cloud_mat_; + // Read off the device once it is open and kept afterwards: resetting a wedged + // camera needs the serial and the model, and by then the handle is gone. + unsigned int serial_number_ = 0; + sl::MODEL model_ = sl::MODEL::LAST; + bool recovering_ = false; }; } // namespace zed -- 2.54.0 From ee48948de944d41c827dba6d8bb04d84b345f45f Mon Sep 17 00:00:00 2001 From: cesar Date: Tue, 8 Sep 2026 09:34:12 -0300 Subject: [PATCH 08/10] add a dora-node.yml manifest and a dora validate CI gate No source package is shared between node repositories, so the manifest and its type URNs are the only contract a consumer has. Declaring them makes a wiring mistake fail at validate time with the producer and consumer URNs, instead of at runtime. Every payload this node sends is a raw buffer whose shape and dtype live in the metadata rather than an Arrow struct, so the URNs name the element type: UInt8 for image_bgr, Float32 for depth, camera_info and point_cloud, String for camera_state. tick is UInt64, matching how dora types timer inputs. dataflow.yml now builds the node from the checkout with path: + build:. A git:-sourced node does not pick up an adjacent manifest, so validating the previous form checked nothing; with path: the CLI reports "injected 6 contract type(s)" and a deliberately mismatched annotation exits non-zero under strict_types. The README keeps the tag: form for consumers. The workflow needs no camera and no ZED SDK, since validation never runs the node. Also ignores the git/ and out/ working directories dora build creates, and drops the ignore rules for the deleted test assets. Co-Authored-By: Claude Opus 5 Claude-Session: https://claude.ai/code/session_01Y7g2cBJ5e3r15ZbJ9WZn8c --- .gitea/workflows/validate.yml | 32 ++++++++++++++++++++++++++++ .gitignore | 7 ++++--- AGENTS.md | 4 +++- README.md | 28 ++++++++++++++++++++++++- dataflow.yml | 9 +++++--- dora-node.yml | 39 +++++++++++++++++++++++++++++++++++ 6 files changed, 111 insertions(+), 8 deletions(-) create mode 100644 .gitea/workflows/validate.yml create mode 100644 dora-node.yml diff --git a/.gitea/workflows/validate.yml b/.gitea/workflows/validate.yml new file mode 100644 index 0000000..9fc8ce3 --- /dev/null +++ b/.gitea/workflows/validate.yml @@ -0,0 +1,32 @@ +# Checks the dataflow against the typed contract in dora-node.yml. This runs no +# node, so it needs neither a camera nor the ZED SDK. +name: validate + +on: [push, pull_request] + +env: + DORA_VERSION: v1.0.1 + DORA_TARGET: x86_64-unknown-linux-gnu + +jobs: + validate: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + + - name: Install the dora CLI + run: | + curl -fsSL -o dora-cli.tar.gz \ + "https://github.com/dora-rs/dora/releases/download/${DORA_VERSION}/dora-cli-${DORA_TARGET}.tar.gz" + tar xzf dora-cli.tar.gz + mkdir -p "$HOME/.local/bin" + install -m 755 "dora-cli-${DORA_TARGET}/dora" "$HOME/.local/bin/dora" + echo "$HOME/.local/bin" >> "$GITHUB_PATH" + rm -rf dora-cli.tar.gz "dora-cli-${DORA_TARGET}" + + - run: dora --version + + # strict_types: true in dataflow.yml turns a type warning into a non-zero + # exit, so a mismatched annotation fails here rather than at runtime. + - name: Validate the dataflow + run: dora validate dataflow.yml diff --git a/.gitignore b/.gitignore index d3aa2de..94824ed 100644 --- a/.gitignore +++ b/.gitignore @@ -1,6 +1,7 @@ build/ bin/ tmp/ -test/assets/*.png -test/assets/*.jpg -test/assets/*_result.* + +# dora build working directories +git/ +out/ diff --git a/AGENTS.md b/AGENTS.md index c4f26b4..1c14d96 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -41,7 +41,9 @@ Camera behavior is configured entirely through environment variables (see README - **`src/main.cc`** — Dora event loop. Parses `ZED_PC_MODE` (`FULL`/`HALF`) once before the loop. On any input event (intended to be driven by a tick timer; the id is not inspected): calls `camera.grab()`, then sends `image_bgr` and `depth`, plus `point_cloud` when `ZED_POINT_CLOUD` is on and `camera_info` on the first frame of a camera session (`camera_info_pending`, re-armed on reconnection). `pack_depth` copies the depth map into a tightly packed `[H, W]` float buffer (sl::Mat rows can be padded) and clears `inf`/`nan` to `0`. `set_intrinsics` and `set_frame_stamps` stamp every output of a frame with the same intrinsics, `frame_id` and the two timestamps (`device_timestamp_ns`, `host_timestamp_ns`; both Unix-epoch nanoseconds). Point cloud output varies by mode: `FULL` sends the raw `sl::Mat` pointer (no copy); `HALF` downsamples by 2 in each dimension. Recovery is a state machine over ticks rather than a blocking retry: the camera is opened from inside the loop, `begin_recovery` closes (first attempt) or resets (later ones) and sets a backoff deadline, and `camera_state` is published on each transition. Nothing sleeps, so `Stop` is honoured while the camera is down. - **`DoraTargets.cmake`** — Handles building or fetching dora-rs and generating the CXX bridge files. Two paths: local (`DORA_ROOT_DIR`) or git clone. Produces `dora_node_api_cxx` library and the `node_bridge.cc` glue. - **`scripts/build.sh`** — Configure/compile/install in one shell-free-callable step. Referenced by `dataflow.yml`, the `Dockerfile` and the README. -- **`dataflow.yml`** — Example dora dataflow definition using the `git:` source format, pinned with `tag:` — dora 1.0 pins a source with the mutually exclusive `tag:`, `rev:` or `branch:`. +- **`dora-node.yml`** — Node manifest (`apiVersion: 1`): typed input/output contract, entrypoint, build command. `dora validate` injects these types into any dataflow that reaches the node by `path:`; a `git:`-sourced node does **not** pick it up, which is why `dataflow.yml` builds from the checkout. Type URNs name the element type (`std/core/v1/UInt8`, `.../Float32`, `.../String`) because every payload is a raw buffer described by its metadata, not an Arrow struct. +- **`dataflow.yml`** — Example dora dataflow, built from this checkout with `path:` + `build:` and `strict_types: true`. A consumer wires the node from a tag instead (see README) — dora 1.0 pins a source with the mutually exclusive `tag:`, `rev:` or `branch:`. +- **`.gitea/workflows/validate.yml`** — CI gate running `dora validate dataflow.yml` on push and pull request. Needs no camera and no ZED SDK. - **`Dockerfile`** — Ubuntu 24.04 x86-64 build environment (CUDA 12.8, ZED SDK 5.2, Rust). No ARM64/Jetson image; build natively there. No camera needed; run the compile with `--gpus all`. Sets `NVIDIA_DRIVER_CAPABILITIES=compute,utility,video` — the `video` capability is what makes the container runtime mount NVDEC/NVENC, which `libsl_zed.so` links against. Rust lives at `/usr/local/{rustup,cargo}` rather than `/root` so the build can run as the invoking user. `docker build -t dora-zed-build . && docker run --rm --gpus all --user "$(id -u):$(id -g)" -v "$PWD:/src" -v dora-zed-cargo:/usr/local/cargo/registry dora-zed-build`. ## Key APIs diff --git a/README.md b/README.md index b634abe..62facbe 100644 --- a/README.md +++ b/README.md @@ -23,7 +23,33 @@ nodes: - camera_state ``` -See [`dataflow.yml`](dataflow.yml) for a complete runnable example with a camera display node. +See [`dataflow.yml`](dataflow.yml) for a complete runnable example with a camera display +node. That file builds the node from the checkout (`path:` + `build:`) rather than from a +tag, which is what lets `dora validate` pick up the manifest described below. + +### Node manifest + +[`dora-node.yml`](dora-node.yml) declares the node's inputs and outputs with type URNs. +No source package is shared between node repositories, so it is the only contract a +consumer has, and it is what makes a wiring mistake fail at build time instead of at +runtime: + +``` +$ dora validate dataflow.yml + node "zed-camera": injected 6 contract type(s) from .../dora-node.yml +All type annotations OK. +``` + +Types are `std/core/v1/UInt8` for `image_bgr`, `std/core/v1/Float32` for `depth`, +`camera_info` and `point_cloud`, and `std/core/v1/String` for `camera_state`; every +payload is a raw buffer whose shape and dtype are in the metadata, so the URN names the +element type rather than a struct. The `tick` input is `std/core/v1/UInt64`, matching how +dora types timer inputs. + +`strict_types: true` in `dataflow.yml` makes a type warning a non-zero exit. +[`.gitea/workflows/validate.yml`](.gitea/workflows/validate.yml) runs +`dora validate dataflow.yml` on every push and pull request; it needs no camera and no +ZED SDK, since validation never runs the node. ## Dependencies diff --git a/dataflow.yml b/dataflow.yml index f03c9db..28b6a9f 100644 --- a/dataflow.yml +++ b/dataflow.yml @@ -1,9 +1,12 @@ +# The repository's own example: builds and runs the node from this checkout, so +# `dora validate` picks up the typed contract in dora-node.yml next to it. +# A consumer wires the node from a tag instead — see README.md. +strict_types: true + nodes: - id: zed-camera - git: ssh://git@gitea.liria.cl:5001/liria/dora-zed.git - tag: v0.2.0 - build: scripts/build.sh path: bin/dora_zed + build: scripts/build.sh inputs: tick: dora/timer/millis/100 outputs: diff --git a/dora-node.yml b/dora-node.yml new file mode 100644 index 0000000..336e30a --- /dev/null +++ b/dora-node.yml @@ -0,0 +1,39 @@ +# Node manifest: the typed contract a consumer wires against. +# +# No source package is shared between node repositories, so this file and its +# type URNs are the only contract a consumer has. `dora validate` checks a +# dataflow against it without running the node, so CI needs no camera and no +# ZED SDK. +apiVersion: 1 +name: dora-zed +namespace: liria +description: ZED stereo camera source publishing a BGR image, a depth map, intrinsics and an optional point cloud +categories: [sensor] +keywords: [zed, stereolabs, camera, depth, stereo, jetson] + +runtime: cpp +entrypoint: bin/dora_zed +build: scripts/build.sh + +inputs: + tick: + type: std/core/v1/UInt64 + required: true + description: Each tick triggers one camera grab. Connect to dora/timer/millis/N. + +outputs: + image_bgr: + type: std/core/v1/UInt8 + description: BGR image bytes, [H, W, 3]. Shape and encoding are in the metadata. + depth: + type: std/core/v1/Float32 + description: Per-pixel depth in millimetres, [H, W]. Missing depth is 0, never inf or nan. + camera_info: + type: std/core/v1/Float32 + description: Intrinsics [fx, fy, cx, cy], distortion in the metadata. Published once per camera session. + point_cloud: + type: std/core/v1/Float32 + description: XYZRGBA point cloud. Published only when ZED_POINT_CLOUD is on. + camera_state: + type: std/core/v1/String + description: One of opening, streaming, recovering. Published on every change. -- 2.54.0 From 2f55ca3f744a4a41919fc9154db073af97d79a15 Mon Sep 17 00:00:00 2001 From: cesar Date: Tue, 8 Sep 2026 09:35:09 -0300 Subject: [PATCH 09/10] docs: correct the output summaries and drop a duplicated paragraph The README and AGENTS intros still described the v0.1.0 output set. Also removes a copy of the point-cloud rationale that ended up orphaned after the recovery section landed above it, and narrows the timestamp section to the frame outputs, since camera_state carries only the host clock. Co-Authored-By: Claude Opus 5 Claude-Session: https://claude.ai/code/session_01Y7g2cBJ5e3r15ZbJ9WZn8c --- AGENTS.md | 2 +- README.md | 10 ++++------ 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/AGENTS.md b/AGENTS.md index 1c14d96..550a7c4 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -4,7 +4,7 @@ This file provides guidance to AI coding assistants when working with code in th ## What this is -A dora-rs C++ node that interfaces with ZED stereo cameras. On each tick it grabs a frame and publishes it over the dora dataflow: a BGR image, an XYZRGBA point cloud and camera intrinsics. Camera management lives in `src/zed/` and the dora event loop in `src/main.cc`. +A dora-rs C++ node that interfaces with ZED stereo cameras. On each tick it grabs a frame and publishes it over the dora dataflow: a BGR image and a depth map every frame, camera intrinsics once per camera session, a camera state on every transition, and an XYZRGBA point cloud when `ZED_POINT_CLOUD` is on. Camera management lives in `src/zed/` and the dora event loop in `src/main.cc`. The node is a camera source only. It ran inference in-process through `v0.1.0`, under `src/engines/`; that was removed so the node has no inference dependency and builds on any platform the ZED SDK supports. The removed code remains reachable at tag `v0.1.0`. diff --git a/README.md b/README.md index 62facbe..598da51 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # dora-zed -Dora node for ZED stereo cameras. Publishes BGR images, point clouds and camera intrinsics. +Dora node for ZED stereo cameras. Publishes a BGR image and a depth map on every frame, plus camera intrinsics, a camera state, and an optional point cloud. ## Usage @@ -196,13 +196,11 @@ it. That call reaches the ZED 2 and newer only; on the first-generation ZED and Mini the node closes the handle and lets the backoff give the endpoint time to clear. Every attempt and every escalation is logged. - -Consumers that need 3D back-project `depth` with the published intrinsics rather than -consuming `point_cloud`; the point cloud is not a prerequisite for 3D. - ### Timestamps -Every output carries both clocks, as dora `timestamp` metadata parameters: +Every frame output — `image_bgr`, `depth`, `point_cloud` and `camera_info` — carries both +clocks, as dora `timestamp` metadata parameters. `camera_state` carries +`host_timestamp_ns` only, since it is not tied to a frame. | Key | Epoch | Unit | Source | |---|---|---|---| -- 2.54.0 From 5822a870bbb27c4e14402913c0ea12bb6d35dc7a Mon Sep 17 00:00:00 2001 From: cesar Date: Tue, 8 Sep 2026 12:04:35 -0300 Subject: [PATCH 10/10] fix the camera blocking the dora event loop by moving it to its own thread Hardware testing showed the tick-driven recovery state machine did not work. async_grab_camera_recovery does not make grab() prompt on a disconnected ZED-M under SDK 5.3: grabs took ~3 s each while the link was down and then blocked indefinitely. The event loop stopped turning, so Stop went unanswered, the 30 s patience timer and the backoff were never reached, and replugging the camera did nothing. Measured: 122 s frozen, then dora stop needed 10 s and a forced kill. Move everything that can block onto a capture thread. zed::Capture owns the camera and runs open, warmup, grab, blank and stall detection, backoff and escalation; it packs each frame into owned buffers and hands the newest one to the reader by swapping under a mutex. The dora event loop now only drains that buffer and publishes, so it never calls into the SDK and stays responsive however long the SDK blocks. The newest frame wins and uncollected frames are dropped, which is what a consumer wants from a camera source. Swapping rather than assigning returns the previous frame's buffers to the capture thread, so steady state does not reallocate. The point cloud is now copied out of the SDK buffer rather than published in place, since the reader is another thread. On exit the loop waits 2 s for the thread and then _Exit()s if it is still inside the SDK, so Stop is always answered promptly. This also removes the blocking warmup from the event loop: it stays a blocking burst, which is harmless on the capture thread. Verified on a ZED-M: unplug leaves the node responsive and logs the backoff 1, 2, 5 then 10 s; replug gives "camera recovered after 13 attempt(s)" and publishing resumes without restarting the dataflow; dora stop with the camera unplugged returns in 0.70 s with the thread joining gracefully. Co-Authored-By: Claude Opus 5 Claude-Session: https://claude.ai/code/session_01Y7g2cBJ5e3r15ZbJ9WZn8c --- AGENTS.md | 3 +- CMakeLists.txt | 1 + README.md | 18 ++- src/main.cc | 363 ++++++++++----------------------------------- src/zed/camera.cc | 1 + src/zed/camera.h | 3 + src/zed/capture.cc | 302 +++++++++++++++++++++++++++++++++++++ src/zed/capture.h | 94 ++++++++++++ 8 files changed, 498 insertions(+), 287 deletions(-) create mode 100644 src/zed/capture.cc create mode 100644 src/zed/capture.h diff --git a/AGENTS.md b/AGENTS.md index 550a7c4..a6a0cea 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -37,8 +37,9 @@ Camera behavior is configured entirely through environment variables (see README - **`src/util/env.h`** — Shared inline env-variable parsing helpers (`get_env_int`, `get_env_str`, `get_env_bool`, `get_env_float`). - **`src/zed/camera.h`** — `zed::Config`, `zed::Calibration`, and `zed::Camera` class. Camera wraps the ZED SDK lifecycle: `open()` (one attempt, returns success), warmup, grab (image + depth + point cloud), calibration, device timestamp, blank-frame check, `close()` and `reset()` (close plus `sl::Camera::reboot()` where the model allows it). Exposes `handle()` for engine access. +- **`src/zed/capture.h/cc`** — `zed::Capture` and `zed::Frame`. Owns the camera on a dedicated thread: open, warmup, grab, blank/stall detection, backoff and escalation all live here, along with the packing of every payload into owned buffers. The newest frame is handed to the reader by swapping under a mutex (`take_frame`), which also recycles the previous frame's buffers so steady-state capture does not reallocate. **Nothing outside this file may call the ZED SDK** — it blocks unboundedly on a disconnected camera, which is what froze the node when the loop drove the camera directly. - **`src/zed/camera.cc`** — Implementation. SDK enum parsers live in an anonymous namespace. `parse_depth_mode` runs in `Config::from_env()`, not in the constructor, so an unrecognised `ZED_DEPTH_MODE` exits before the node joins the dataflow rather than falling back to a default. -- **`src/main.cc`** — Dora event loop. Parses `ZED_PC_MODE` (`FULL`/`HALF`) once before the loop. On any input event (intended to be driven by a tick timer; the id is not inspected): calls `camera.grab()`, then sends `image_bgr` and `depth`, plus `point_cloud` when `ZED_POINT_CLOUD` is on and `camera_info` on the first frame of a camera session (`camera_info_pending`, re-armed on reconnection). `pack_depth` copies the depth map into a tightly packed `[H, W]` float buffer (sl::Mat rows can be padded) and clears `inf`/`nan` to `0`. `set_intrinsics` and `set_frame_stamps` stamp every output of a frame with the same intrinsics, `frame_id` and the two timestamps (`device_timestamp_ns`, `host_timestamp_ns`; both Unix-epoch nanoseconds). Point cloud output varies by mode: `FULL` sends the raw `sl::Mat` pointer (no copy); `HALF` downsamples by 2 in each dimension. Recovery is a state machine over ticks rather than a blocking retry: the camera is opened from inside the loop, `begin_recovery` closes (first attempt) or resets (later ones) and sets a backoff deadline, and `camera_state` is published on each transition. Nothing sleeps, so `Stop` is honoured while the camera is down. +- **`src/main.cc`** — Dora event loop and output publishing only. On any input event (intended to be driven by a tick timer; the id is not inspected): calls `camera.grab()`, then sends `image_bgr` and `depth`, plus `point_cloud` when `ZED_POINT_CLOUD` is on and `camera_info` on the first frame of a camera session (`camera_info_pending`, re-armed on reconnection). `pack_depth` copies the depth map into a tightly packed `[H, W]` float buffer (sl::Mat rows can be padded) and clears `inf`/`nan` to `0`. `set_intrinsics` and `set_frame_stamps` stamp every output of a frame with the same intrinsics, `frame_id` and the two timestamps (`device_timestamp_ns`, `host_timestamp_ns`; both Unix-epoch nanoseconds). Point cloud output varies by mode: `FULL` sends the raw `sl::Mat` pointer (no copy); `HALF` downsamples by 2 in each dimension. The loop never touches the SDK: it drains `take_state_change` and `take_frame` from the capture thread and publishes. On exit it calls `capture.stop()` and waits 2 s for the thread; if the thread is still inside a blocking SDK call it gives up and `_Exit(0)`s rather than let dora time out and kill the node. - **`DoraTargets.cmake`** — Handles building or fetching dora-rs and generating the CXX bridge files. Two paths: local (`DORA_ROOT_DIR`) or git clone. Produces `dora_node_api_cxx` library and the `node_bridge.cc` glue. - **`scripts/build.sh`** — Configure/compile/install in one shell-free-callable step. Referenced by `dataflow.yml`, the `Dockerfile` and the README. - **`dora-node.yml`** — Node manifest (`apiVersion: 1`): typed input/output contract, entrypoint, build command. `dora validate` injects these types into any dataflow that reaches the node by `path:`; a `git:`-sourced node does **not** pick it up, which is why `dataflow.yml` builds from the checkout. Type URNs name the element type (`std/core/v1/UInt8`, `.../Float32`, `.../String`) because every payload is a raw buffer described by its metadata, not an Arrow struct. diff --git a/CMakeLists.txt b/CMakeLists.txt index 77fa223..0fbd2c8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -25,6 +25,7 @@ link_directories(${ZED_LIBRARY_DIR}) add_executable(dora_zed src/main.cc src/zed/camera.cc + src/zed/capture.cc ${node_bridge} ) add_dependencies(dora_zed Dora_cxx) diff --git a/README.md b/README.md index 598da51..9b7650a 100644 --- a/README.md +++ b/README.md @@ -152,6 +152,10 @@ Consumers that need 3D back-project `depth` with the published intrinsics instea | `FULL` (default) | `[H, W, 4]` | Full resolution grid (~14.7 MB/frame at 1080p) | | `HALF` | `[H/2, W/2, 4]` | Stride-2 downsample (~4× smaller) | +The cloud is copied out of the SDK buffer, since the capture thread and the event loop are +different threads. That copy is the main reason to leave `ZED_POINT_CLOUD` off unless a +consumer needs it. + ## Inputs | Input | Description | @@ -179,10 +183,16 @@ frame yet from one that has been lost: | `streaming` | Frames are being published | | `recovering` | The camera was streaming and has stopped; the node is trying to bring it back | -Recovery runs inside the dora event loop rather than in a retry loop of its own, so the -node keeps reading events — and honours `Stop` — while the camera is down. Reopening -never blocks the loop between attempts: the tick timer paces them, and a backoff deadline -of 1, 2, 5 then 10 seconds decides which ticks actually try. +The camera runs on its own thread. The ZED SDK can block for an unbounded time inside +`grab()` and `open()` when the camera is disconnected — measured blocking indefinitely on +a ZED-M even with `async_grab_camera_recovery` set — so driving it from the dora event +loop would freeze the node: it would stop reading events, leaving `Stop` unanswered and +the recovery logic itself unreachable. The event loop only reads the newest frame the +capture thread has already produced, and therefore never calls into the SDK. + +Reopening is paced by a backoff of 1, 2, 5 then 10 seconds, the last repeated. Frames that +are not collected before the next one arrives are dropped: a consumer always gets the +newest frame rather than a queue of stale ones. The node treats an all-black frame as a failed grab: a camera whose link has dropped does not necessarily fail its grabs, and can keep reporting success while handing back a diff --git a/src/main.cc b/src/main.cc index 94dbfee..e9cd256 100644 --- a/src/main.cc +++ b/src/main.cc @@ -1,50 +1,22 @@ -#include "util/env.h" -#include "zed/camera.h" +#include "zed/capture.h" #include -#include -#include #include -#include +#include #include #include -#include #include #include namespace { -enum class PcMode { FULL, HALF }; - -using Clock = std::chrono::steady_clock; - -// Going this long without a usable frame means the camera is wedged rather than -// dropping the odd frame. Measured in elapsed time rather than in failed grabs, -// because a stalling camera can spend seconds inside a single grab. -constexpr double STALL_TIMEOUT_S = 5.0; - -// How long the SDK is left to reconnect the camera on its own before the loop -// stops waiting on it and rebuilds the camera anyway. A driver that says it is -// recovering is usually right, but it has no way to tell us it has given up. -constexpr double DRIVER_RECOVERY_TIMEOUT_S = 30.0; - -// How long to wait before each reopen: one entry per attempt, the last repeated -// for every attempt beyond it. -constexpr int REOPEN_BACKOFF_S[] = {1, 2, 5, 10}; - -// Published on the camera_state output so a downstream node can tell a camera -// that has not delivered its first frame yet from one that has been lost. -constexpr const char* STATE_OPENING = "opening"; -constexpr const char* STATE_STREAMING = "streaming"; -constexpr const char* STATE_RECOVERING = "recovering"; - -rust::Vec make_shape(int height, int width, int channels) { - rust::Vec shape; - shape.push_back(height); - shape.push_back(width); - shape.push_back(channels); - return shape; +rust::Vec make_vector_int(std::initializer_list values) { + rust::Vec out; + for (int64_t v : values) { + out.push_back(v); + } + return out; } rust::Vec make_distortion(const std::vector& distortion) { @@ -55,14 +27,6 @@ rust::Vec make_distortion(const std::vector& distortion) { return values; } -rust::Vec make_vector_int(std::initializer_list values) { - rust::Vec out; - for (int64_t v : values) { - out.push_back(v); - } - return out; -} - void set_intrinsics(Metadata& metadata, const zed::Calibration& calib) { metadata.set_float("fx", calib.fx); metadata.set_float("fy", calib.fy); @@ -74,65 +38,19 @@ void set_intrinsics(Metadata& metadata, const zed::Calibration& calib) { // Both clocks are needed. The device timestamp is the instant the frame was // captured and is the only value two nodes reading the same frame can agree on. // The host timestamp is what the rest of the dataflow and the session recorder -// share, and on a Jetson it can step when NTP synchronises after boot — which is +// share, and on a Jetson it can step when NTP synchronises after boot - which is // exactly why the device timestamp is recorded alongside it rather than instead // of it. -void set_frame_stamps(Metadata& metadata, int64_t frame_id, int64_t device_ns, int64_t host_ns) { - metadata.set_int("frame_id", frame_id); - metadata.set_timestamp("device_timestamp_ns", device_ns); - metadata.set_timestamp("host_timestamp_ns", host_ns); +void set_frame_stamps(Metadata& metadata, const zed::Frame& frame) { + metadata.set_int("frame_id", frame.frame_id); + metadata.set_timestamp("device_timestamp_ns", frame.device_ns); + metadata.set_timestamp("host_timestamp_ns", frame.host_ns); } -// Copy the depth map out into a tightly packed [H, W] buffer of millimetres. -// -// The rows of an sl::Mat can be padded, so the buffer is not necessarily H * W -// floats and cannot be published in place. inf and nan mark depth the SDK could -// not measure; they are cleared to 0 here, as the reference driver does, because -// a hole is meaningful and a consumer cannot back-project a non-finite value. -std::vector pack_depth(sl::Mat& depth_mat) { - const int height = depth_mat.getHeight(); - const int width = depth_mat.getWidth(); - const size_t step = depth_mat.getStep(sl::MEM::CPU); - const float* src = depth_mat.getPtr(); - - std::vector out(static_cast(height) * static_cast(width)); - for (int y = 0; y < height; y++) { - const float* row = src + static_cast(y) * step; - float* dst = out.data() + static_cast(y) * static_cast(width); - for (int x = 0; x < width; x++) { - dst[x] = std::isfinite(row[x]) ? row[x] : 0.0f; - } +void report(const char* output, const DoraResult& result) { + if (!std::string(result.error).empty()) { + std::cerr << "Error sending " << output << ": " << std::string(result.error) << std::endl; } - return out; -} - -std::chrono::seconds reopen_backoff(int attempt) { - const size_t index = - std::min(static_cast(attempt), std::size(REOPEN_BACKOFF_S)) - 1; - return std::chrono::seconds(REOPEN_BACKOFF_S[index]); -} - -double seconds_since(Clock::time_point since) { - return std::chrono::duration(Clock::now() - since).count(); -} - -// Release the camera and schedule the next open attempt. The first attempt only -// closes the handle, which is enough when the driver rather than the device lost -// its way; past that the device itself is wedged and nothing short of resetting -// it will bring the frames back. -// -// The wait is a deadline rather than a sleep: the loop returns to dora between -// attempts, so Stop is still honoured while the camera is down. -void begin_recovery(zed::Camera& camera, int attempt, Clock::time_point& next_attempt_at) { - if (attempt <= 1) { - camera.close(); - } else { - camera.reset(); - } - const auto wait = reopen_backoff(attempt); - std::cerr << "recovering camera (attempt " << attempt << "); next open in " << wait.count() - << "s" << std::endl; - next_attempt_at = Clock::now() + wait; } int64_t host_timestamp_ns() { @@ -149,51 +67,15 @@ int main() { auto config = zed::Config::from_env(); auto dora_node = init_dora_node(); - zed::Camera camera(config); - zed::Calibration calib; - std::string camera_state; - auto publish_camera_state = [&](const char* state) { - if (camera_state == state) { - return; - } - camera_state = state; - rust::Slice state_slice{ - reinterpret_cast(camera_state.data()), camera_state.size()}; + // The camera runs on its own thread. The SDK can block for an unbounded time + // inside grab() and open() when the camera is disconnected, so nothing below + // may call into it: this loop only reads what the capture thread has already + // produced, which is what keeps Stop answerable while the camera is down. + zed::Capture capture(config); + capture.start(); - auto metadata = new_metadata(); - metadata->set_string("dtype", "string"); - metadata->set_string("encoding", "utf8"); - metadata->set_timestamp("host_timestamp_ns", host_timestamp_ns()); - - auto result = send_output_with_metadata( - dora_node.send_output, "camera_state", state_slice, std::move(metadata)); - if (!std::string(result.error).empty()) { - std::cerr << "Error sending camera_state: " << std::string(result.error) << std::endl; - } - std::cout << "camera state: " << camera_state << std::endl; - }; - - std::string pc_mode_str = util::get_env_str("ZED_PC_MODE", "FULL"); - PcMode pc_mode = (pc_mode_str == "HALF") ? PcMode::HALF : PcMode::FULL; - - // fx, fy, cx, cy and the distortion coefficients are fixed for a given camera - // and resolution, so they are published once per camera session rather than on - // every frame. It goes out with the first frame of the session so that it - // carries that frame's timestamps like every other output. - bool camera_info_pending = true; - - // The camera is opened from inside the event loop, one attempt per tick, so - // the node keeps reading dora events while the camera is unavailable. There - // is no retry sleep: the tick timer paces the attempts, and the backoff - // deadline decides which ticks actually try. - int recovery_attempts = 0; - bool streamed_once = false; - bool stall_reported = false; - auto next_open_attempt_at = Clock::now(); - auto last_good_frame_at = Clock::now(); - - int64_t frame_id = 0; + zed::Frame frame; while (true) { auto event = dora_node.events->next(); auto type = event_type(event); @@ -206,169 +88,81 @@ int main() { continue; } - const char* down_state = streamed_once ? STATE_RECOVERING : STATE_OPENING; + std::string state; + if (capture.take_state_change(state)) { + rust::Slice state_slice{ + reinterpret_cast(state.data()), state.size()}; - if (!camera.is_open()) { - publish_camera_state(down_state); - if (Clock::now() < next_open_attempt_at) { - continue; - } - if (!camera.open()) { - recovery_attempts++; - begin_recovery(camera, recovery_attempts, next_open_attempt_at); - continue; - } - camera.warmup(); - calib = camera.calibration(); - camera_info_pending = true; - // Opening the ZED takes seconds. Start the stall clock from here so - // that cost is never mistaken for a stalled stream. - last_good_frame_at = Clock::now(); - // recovery_attempts is deliberately not cleared here. A wedged device - // can open and still deliver nothing; clearing on open would restart - // the escalation ladder every cycle and never reach the reset. + auto state_metadata = new_metadata(); + state_metadata->set_string("dtype", "string"); + state_metadata->set_string("encoding", "utf8"); + state_metadata->set_timestamp("host_timestamp_ns", host_timestamp_ns()); + + report("camera_state", + send_output_with_metadata(dora_node.send_output, "camera_state", state_slice, + std::move(state_metadata))); + } + + // No new frame since the last tick: the camera is slower than the timer, + // or it is down and the capture thread is recovering it. + if (!capture.take_frame(frame)) { continue; } - // A blank frame counts as a failed grab: a camera whose link has dropped - // does not necessarily fail its grabs, it can keep reporting success and - // hand back a uniform frame with an empty depth map. - if (!camera.grab() || camera.image_is_blank()) { - publish_camera_state(down_state); - const double stalled_for = seconds_since(last_good_frame_at); - // The SDK holds the camera open while it reconnects a dropped link. - // Taking the handle away mid-reconnect aborts that and leaves us to - // redo it the slow way, so it gets longer than a plain stall. - const double patience = - camera.recovering() ? DRIVER_RECOVERY_TIMEOUT_S : STALL_TIMEOUT_S; - if (stalled_for < patience) { - continue; - } - if (!stall_reported) { - stall_reported = true; - std::cerr << "camera delivered no usable frame for " << stalled_for << "s" - << std::endl; - } - recovery_attempts++; - begin_recovery(camera, recovery_attempts, next_open_attempt_at); - continue; - } - - if (stall_reported) { - std::cout << "camera recovered after " << recovery_attempts << " attempt(s)" - << std::endl; - stall_reported = false; - } - last_good_frame_at = Clock::now(); - recovery_attempts = 0; - streamed_once = true; - publish_camera_state(STATE_STREAMING); - - sl::Mat& image_mat = camera.image(); - sl::Mat& depth_mat = camera.depth(); - - int width = image_mat.getWidth(); - int height = image_mat.getHeight(); - - int64_t current_frame_id = frame_id++; - int64_t device_ns = camera.device_timestamp_ns(); - int64_t host_ns = host_timestamp_ns(); - // --- image_bgr --- - cv::Mat image_rgba(height, width, CV_8UC4, image_mat.getPtr()); - cv::Mat image_bgr; - cv::cvtColor(image_rgba, image_bgr, cv::COLOR_BGRA2BGR); - - const auto image_size = static_cast(image_bgr.total() * image_bgr.elemSize()); - rust::Slice image_slice{ - reinterpret_cast(image_bgr.data), image_size}; + rust::Slice image_slice{frame.image_bgr.data(), frame.image_bgr.size()}; auto image_metadata = new_metadata(); image_metadata->set_string("dtype", "uint8"); image_metadata->set_string("encoding", "bgr8"); image_metadata->set_string("layout", "HWC"); - image_metadata->set_list_int("shape", make_shape(height, width, 3)); - set_intrinsics(*image_metadata, calib); - set_frame_stamps(*image_metadata, current_frame_id, device_ns, host_ns); + image_metadata->set_list_int("shape", make_vector_int({frame.height, frame.width, 3})); + set_intrinsics(*image_metadata, frame.calib); + set_frame_stamps(*image_metadata, frame); - auto image_result = send_output_with_metadata( - dora_node.send_output, "image_bgr", image_slice, std::move(image_metadata)); - if (!std::string(image_result.error).empty()) { - std::cerr << "Error sending image_bgr: " << std::string(image_result.error) << std::endl; - } + report("image_bgr", send_output_with_metadata(dora_node.send_output, "image_bgr", + image_slice, std::move(image_metadata))); // --- depth --- - int depth_width = depth_mat.getWidth(); - int depth_height = depth_mat.getHeight(); - - std::vector depth_buf = pack_depth(depth_mat); rust::Slice depth_slice{ - reinterpret_cast(depth_buf.data()), depth_buf.size() * sizeof(float)}; + reinterpret_cast(frame.depth.data()), + frame.depth.size() * sizeof(float)}; auto depth_metadata = new_metadata(); depth_metadata->set_string("dtype", "float32"); depth_metadata->set_string("layout", "HW"); depth_metadata->set_string("units", "mm"); - depth_metadata->set_list_int("shape", make_vector_int({depth_height, depth_width})); - set_intrinsics(*depth_metadata, calib); - set_frame_stamps(*depth_metadata, current_frame_id, device_ns, host_ns); + depth_metadata->set_list_int("shape", make_vector_int({frame.height, frame.width})); + set_intrinsics(*depth_metadata, frame.calib); + set_frame_stamps(*depth_metadata, frame); - auto depth_result = send_output_with_metadata( - dora_node.send_output, "depth", depth_slice, std::move(depth_metadata)); - if (!std::string(depth_result.error).empty()) { - std::cerr << "Error sending depth: " << std::string(depth_result.error) << std::endl; - } + report("depth", send_output_with_metadata(dora_node.send_output, "depth", depth_slice, + std::move(depth_metadata))); - if (config.point_cloud) { - // --- point_cloud --- - sl::Mat& point_cloud_mat = camera.point_cloud(); - int pc_width = point_cloud_mat.getWidth(); - int pc_height = point_cloud_mat.getHeight(); + // --- point_cloud --- + if (frame.has_point_cloud) { + rust::Slice pc_slice{ + reinterpret_cast(frame.point_cloud.data()), + frame.point_cloud.size() * sizeof(float)}; - std::vector pc_buf; - const uint8_t* pc_data_ptr = nullptr; - size_t pc_data_size = 0; auto pc_metadata = new_metadata(); pc_metadata->set_string("dtype", "float32"); pc_metadata->set_string("channels", "XYZRGBA"); pc_metadata->set_string("units", "mm"); - set_intrinsics(*pc_metadata, calib); - set_frame_stamps(*pc_metadata, current_frame_id, device_ns, host_ns); + pc_metadata->set_string("layout", "HWC"); + pc_metadata->set_list_int("shape", + make_vector_int({frame.pc_height, frame.pc_width, 4})); + set_intrinsics(*pc_metadata, frame.calib); + set_frame_stamps(*pc_metadata, frame); - if (pc_mode == PcMode::HALF) { - int hw = pc_width / 2, hh = pc_height / 2; - pc_buf.resize(static_cast(hh * hw * 4)); - const sl::float4* src = point_cloud_mat.getPtr(); - for (int y = 0; y < hh; y++) { - for (int x = 0; x < hw; x++) { - const sl::float4& pt = src[(y * 2) * pc_width + (x * 2)]; - float* dst = pc_buf.data() + (y * hw + x) * 4; - dst[0] = pt.x; dst[1] = pt.y; dst[2] = pt.z; dst[3] = pt.w; - } - } - pc_metadata->set_string("layout", "HWC"); - pc_metadata->set_list_int("shape", make_shape(hh, hw, 4)); - pc_data_ptr = reinterpret_cast(pc_buf.data()); - pc_data_size = pc_buf.size() * sizeof(float); - } else { - // FULL — send raw sl::Mat pointer directly (no copy) - pc_data_size = static_cast(pc_width * pc_height * sizeof(sl::float4)); - pc_data_ptr = reinterpret_cast(point_cloud_mat.getPtr()); - pc_metadata->set_string("layout", "HWC"); - pc_metadata->set_list_int("shape", make_shape(pc_height, pc_width, 4)); - } - - rust::Slice pc_slice{pc_data_ptr, pc_data_size}; - auto pc_result = send_output_with_metadata( - dora_node.send_output, "point_cloud", pc_slice, std::move(pc_metadata)); - if (!std::string(pc_result.error).empty()) { - std::cerr << "Error sending point_cloud: " << std::string(pc_result.error) << std::endl; - } + report("point_cloud", send_output_with_metadata(dora_node.send_output, "point_cloud", + pc_slice, std::move(pc_metadata))); } // --- camera_info --- - if (camera_info_pending) { - float camera_data[4] = {calib.fx, calib.fy, calib.cx, calib.cy}; + if (frame.camera_info_first) { + float camera_data[4] = {frame.calib.fx, frame.calib.fy, frame.calib.cx, + frame.calib.cy}; rust::Slice cam_slice{ reinterpret_cast(camera_data), sizeof(camera_data)}; @@ -376,17 +170,22 @@ int main() { cam_metadata->set_string("dtype", "float32"); cam_metadata->set_string("layout", "C"); cam_metadata->set_list_int("shape", make_vector_int({4})); - cam_metadata->set_list_float("distortion", make_distortion(calib.distortion)); - set_frame_stamps(*cam_metadata, current_frame_id, device_ns, host_ns); + cam_metadata->set_list_float("distortion", make_distortion(frame.calib.distortion)); + set_frame_stamps(*cam_metadata, frame); - auto cam_result = send_output_with_metadata( - dora_node.send_output, "camera_info", cam_slice, std::move(cam_metadata)); - if (!std::string(cam_result.error).empty()) { - std::cerr << "Error sending camera_info: " << std::string(cam_result.error) << std::endl; - } - camera_info_pending = false; + report("camera_info", send_output_with_metadata(dora_node.send_output, "camera_info", + cam_slice, std::move(cam_metadata))); } } + capture.stop(); + if (!capture.wait_exit(std::chrono::seconds(2))) { + // The capture thread is inside a blocking SDK call, where nothing can + // reach it. Leave rather than let dora wait out its grace period and kill + // the node: the frames are gone either way, and Stop has been answered. + std::cerr << "capture thread still in the SDK; exiting anyway" << std::endl; + std::cout.flush(); + std::_Exit(0); + } return 0; } diff --git a/src/zed/camera.cc b/src/zed/camera.cc index 1e3b31d..e104c68 100644 --- a/src/zed/camera.cc +++ b/src/zed/camera.cc @@ -96,6 +96,7 @@ Config Config::from_env() { c.depth_max_mm = util::get_env_int("ZED_DEPTH_MAX_MM", c.depth_max_mm); c.depth_fill = util::get_env_bool("ZED_DEPTH_FILL", c.depth_fill); c.point_cloud = util::get_env_bool("ZED_POINT_CLOUD", c.point_cloud); + c.pc_mode = util::get_env_str("ZED_PC_MODE", "FULL") == "HALF" ? PcMode::HALF : PcMode::FULL; c.confidence_threshold = util::get_env_int("ZED_CONFIDENCE_THRESHOLD", c.confidence_threshold); c.texture_confidence_threshold = util::get_env_int("ZED_TEXTURE_CONFIDENCE_THRESHOLD", c.texture_confidence_threshold); diff --git a/src/zed/camera.h b/src/zed/camera.h index f13be90..6235694 100644 --- a/src/zed/camera.h +++ b/src/zed/camera.h @@ -7,6 +7,8 @@ namespace zed { +enum class PcMode { FULL, HALF }; + struct Config { int resolution = 1080; int fps = 15; @@ -15,6 +17,7 @@ struct Config { int depth_max_mm = 600; bool depth_fill = false; bool point_cloud = false; + PcMode pc_mode = PcMode::FULL; int confidence_threshold = 60; int texture_confidence_threshold = 100; std::string flip = "ON"; diff --git a/src/zed/capture.cc b/src/zed/capture.cc new file mode 100644 index 0000000..f90c74d --- /dev/null +++ b/src/zed/capture.cc @@ -0,0 +1,302 @@ +#include "zed/capture.h" + +#include + +#include +#include +#include +#include + +namespace { + +using Clock = std::chrono::steady_clock; + +// Going this long without a usable frame means the camera is wedged rather than +// dropping the odd frame. Measured in elapsed time rather than in failed grabs, +// because a stalling camera can spend seconds inside a single grab. +constexpr double STALL_TIMEOUT_S = 5.0; + +// How long the SDK is left to reconnect the camera on its own before the loop +// stops waiting on it and rebuilds the camera anyway. A driver that says it is +// recovering is usually right, but it has no way to tell us it has given up. +constexpr double DRIVER_RECOVERY_TIMEOUT_S = 30.0; + +// How long to wait between grabs while the camera is not delivering. +constexpr std::chrono::milliseconds GRAB_RETRY{500}; + +// How long to wait before each reopen: one entry per attempt, the last repeated +// for every attempt beyond it. +constexpr int REOPEN_BACKOFF_S[] = {1, 2, 5, 10}; + +std::chrono::seconds reopen_backoff(int attempt) { + const size_t index = std::min(static_cast(attempt), std::size(REOPEN_BACKOFF_S)) - 1; + return std::chrono::seconds(REOPEN_BACKOFF_S[index]); +} + +double seconds_since(Clock::time_point since) { + return std::chrono::duration(Clock::now() - since).count(); +} + +int64_t host_timestamp_ns() { + return std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()) + .count(); +} + +// Copy the depth map into a tightly packed [H, W] buffer of millimetres. +// +// The rows of an sl::Mat can be padded, so the buffer is not necessarily H * W +// floats. inf and nan mark depth the SDK could not measure; they are cleared to +// 0 here, as the reference driver does, because a hole is meaningful and a +// consumer cannot back-project a non-finite value. +void pack_depth(sl::Mat& depth_mat, std::vector& out) { + const int height = depth_mat.getHeight(); + const int width = depth_mat.getWidth(); + const size_t step = depth_mat.getStep(sl::MEM::CPU); + const float* src = depth_mat.getPtr(); + + out.resize(static_cast(height) * static_cast(width)); + for (int y = 0; y < height; y++) { + const float* row = src + static_cast(y) * step; + float* dst = out.data() + static_cast(y) * static_cast(width); + for (int x = 0; x < width; x++) { + dst[x] = std::isfinite(row[x]) ? row[x] : 0.0f; + } + } +} + +} // namespace + +namespace zed { + +Capture::Capture(const Config& config) : config_(config), camera_(config) {} + +Capture::~Capture() { + stop(); + if (thread_.joinable()) { + // The thread may still be inside the SDK. Detaching leaks it for the few + // moments the process has left, which is preferable to blocking here. + if (wait_exit(std::chrono::seconds(1))) { + thread_.join(); + } else { + thread_.detach(); + } + } +} + +void Capture::start() { + { + std::lock_guard lock(mutex_); + running_ = true; + } + thread_ = std::thread(&Capture::run, this); +} + +void Capture::stop() { + std::lock_guard lock(mutex_); + running_ = false; + wake_.notify_all(); +} + +bool Capture::wait_exit(std::chrono::milliseconds timeout) { + std::unique_lock lock(mutex_); + return exited_.wait_for(lock, timeout, [this] { return finished_; }); +} + +bool Capture::take_frame(Frame& out) { + std::lock_guard lock(mutex_); + if (!has_frame_) { + return false; + } + std::swap(out, latest_); + has_frame_ = false; + return true; +} + +bool Capture::take_state_change(std::string& out) { + std::lock_guard lock(mutex_); + if (!state_changed_) { + return false; + } + out = state_; + state_changed_ = false; + return true; +} + +void Capture::set_state(const char* state) { + std::lock_guard lock(mutex_); + if (state_ == state) { + return; + } + state_ = state; + state_changed_ = true; + std::cout << "camera state: " << state_ << std::endl; +} + +void Capture::sleep_interruptible(std::chrono::milliseconds duration) { + std::unique_lock lock(mutex_); + wake_.wait_for(lock, duration, [this] { return !running_; }); +} + +void Capture::begin_recovery(int attempt) { + // The first attempt only closes the handle, which is enough when the driver + // rather than the device lost its way; past that the device itself is wedged + // and nothing short of resetting it will bring the frames back. + if (attempt <= 1) { + camera_.close(); + } else { + camera_.reset(); + } + const auto wait = reopen_backoff(attempt); + std::cerr << "recovering camera (attempt " << attempt << "); next open in " << wait.count() + << "s" << std::endl; + sleep_interruptible(std::chrono::duration_cast(wait)); +} + +void Capture::pack_frame(Frame& frame, int64_t frame_id, bool camera_info_first) { + sl::Mat& image_mat = camera_.image(); + const int width = image_mat.getWidth(); + const int height = image_mat.getHeight(); + + frame.frame_id = frame_id; + frame.device_ns = camera_.device_timestamp_ns(); + frame.host_ns = host_timestamp_ns(); + frame.width = width; + frame.height = height; + frame.calib = camera_.calibration(); + frame.camera_info_first = camera_info_first; + + cv::Mat image_rgba(height, width, CV_8UC4, image_mat.getPtr(), + image_mat.getStep(sl::MEM::CPU) * sizeof(sl::uchar4)); + frame.image_bgr.resize(static_cast(height) * width * 3); + cv::Mat image_bgr(height, width, CV_8UC3, frame.image_bgr.data()); + cv::cvtColor(image_rgba, image_bgr, cv::COLOR_BGRA2BGR); + + pack_depth(camera_.depth(), frame.depth); + + frame.has_point_cloud = config_.point_cloud; + if (!frame.has_point_cloud) { + return; + } + + // Unlike the image and depth, the cloud used to be published straight from + // the SDK buffer. It has to be copied now that the reader is another thread. + sl::Mat& pc_mat = camera_.point_cloud(); + const int pc_width = pc_mat.getWidth(); + const int pc_height = pc_mat.getHeight(); + const size_t pc_step = pc_mat.getStep(sl::MEM::CPU); + const sl::float4* src = pc_mat.getPtr(); + + const int stride = config_.pc_mode == PcMode::HALF ? 2 : 1; + frame.pc_width = pc_width / stride; + frame.pc_height = pc_height / stride; + frame.point_cloud.resize(static_cast(frame.pc_height) * frame.pc_width * 4); + + for (int y = 0; y < frame.pc_height; y++) { + const sl::float4* row = src + static_cast(y * stride) * pc_step; + float* dst = frame.point_cloud.data() + static_cast(y) * frame.pc_width * 4; + for (int x = 0; x < frame.pc_width; x++) { + const sl::float4& point = row[x * stride]; + dst[x * 4 + 0] = point.x; + dst[x * 4 + 1] = point.y; + dst[x * 4 + 2] = point.z; + dst[x * 4 + 3] = point.w; + } + } +} + +void Capture::run() { + int recovery_attempts = 0; + bool streamed_once = false; + bool stall_reported = false; + bool camera_info_pending = false; + auto last_good_frame_at = Clock::now(); + int64_t frame_id = 0; + + set_state("opening"); + + while (true) { + { + std::lock_guard lock(mutex_); + if (!running_) { + break; + } + } + + const char* down_state = streamed_once ? "recovering" : "opening"; + + if (!camera_.is_open()) { + set_state(down_state); + if (!camera_.open()) { + recovery_attempts++; + begin_recovery(recovery_attempts); + continue; + } + // Discard the first frames so auto-exposure settles. This blocks, and + // on this thread that is fine. + camera_.warmup(); + camera_info_pending = true; + // Opening the ZED takes seconds. Start the stall clock from here so + // that cost is never mistaken for a stalled stream. + last_good_frame_at = Clock::now(); + // recovery_attempts is deliberately not cleared here. A wedged device + // can open and still deliver nothing; clearing on open would restart + // the escalation ladder every cycle and never reach the reset. + continue; + } + + // A blank frame counts as a failed grab: a camera whose link has dropped + // does not necessarily fail its grabs, it can keep reporting success and + // hand back a uniform frame with an empty depth map. + if (!camera_.grab() || camera_.image_is_blank()) { + set_state(down_state); + const double stalled_for = seconds_since(last_good_frame_at); + // The SDK holds the camera open while it reconnects a dropped link. + // Taking the handle away mid-reconnect aborts that and leaves us to + // redo it the slow way, so it gets longer than a plain stall. + const double patience = + camera_.recovering() ? DRIVER_RECOVERY_TIMEOUT_S : STALL_TIMEOUT_S; + if (stalled_for < patience) { + sleep_interruptible(GRAB_RETRY); + continue; + } + if (!stall_reported) { + stall_reported = true; + std::cerr << "camera delivered no usable frame for " << stalled_for << "s" + << std::endl; + } + recovery_attempts++; + begin_recovery(recovery_attempts); + continue; + } + + if (stall_reported) { + std::cout << "camera recovered after " << recovery_attempts << " attempt(s)" + << std::endl; + stall_reported = false; + } + last_good_frame_at = Clock::now(); + recovery_attempts = 0; + streamed_once = true; + + pack_frame(scratch_, frame_id++, camera_info_pending); + camera_info_pending = false; + { + std::lock_guard lock(mutex_); + // Swapping rather than assigning hands the previous frame's buffers + // back, so steady-state capture does not reallocate. + std::swap(latest_, scratch_); + has_frame_ = true; + } + set_state("streaming"); + } + + camera_.close(); + { + std::lock_guard lock(mutex_); + finished_ = true; + exited_.notify_all(); + } +} + +} // namespace zed diff --git a/src/zed/capture.h b/src/zed/capture.h new file mode 100644 index 0000000..a41305c --- /dev/null +++ b/src/zed/capture.h @@ -0,0 +1,94 @@ +#pragma once + +#include "zed/camera.h" + +#include +#include +#include +#include +#include +#include + +namespace zed { + +// One captured frame, owning its buffers. +// +// The SDK reuses its own buffers on the next grab, so nothing here can point +// into an sl::Mat: the capture thread copies every payload out before handing +// the frame over. +struct Frame { + int64_t frame_id = 0; + int64_t device_ns = 0; + int64_t host_ns = 0; + int width = 0; + int height = 0; + std::vector image_bgr; // height * width * 3 + std::vector depth; // height * width, millimetres + bool has_point_cloud = false; + int pc_width = 0; + int pc_height = 0; + std::vector point_cloud; // pc_height * pc_width * 4 + Calibration calib; + // Set on the first frame of a camera session, so camera_info is published + // once per session rather than per frame. + bool camera_info_first = false; +}; + +// Owns the camera on its own thread and hands the newest frame to the reader. +// +// The ZED SDK can block for an unbounded time inside grab() and open() — a +// disconnected camera has been measured blocking indefinitely, even with +// InitParameters::async_grab_camera_recovery set. Driving the camera from the +// dora event loop therefore froze the node: it stopped reading events, so Stop +// went unanswered and the recovery logic itself was never reached. Everything +// that can block lives on this thread instead, and the event loop only ever +// touches the buffer below. +class Capture { +public: + explicit Capture(const Config& config); + ~Capture(); + + void start(); + // Ask the thread to finish. Returns immediately: the thread may be inside a + // blocking SDK call and cannot be interrupted there. + void stop(); + // Wait for the thread to actually leave, up to timeout. False means it is + // still stuck in the SDK. + bool wait_exit(std::chrono::milliseconds timeout); + + // Move the newest frame out, if one arrived since the last call. Frames that + // are not collected are overwritten: the reader always wants the latest. + bool take_frame(Frame& out); + // Report the camera state, if it changed since the last call. + bool take_state_change(std::string& out); + +private: + void run(); + void set_state(const char* state); + // Release the camera and wait out the backoff for this attempt. The wait is + // interruptible, so stop() is honoured while the camera is down. + void begin_recovery(int attempt); + void sleep_interruptible(std::chrono::milliseconds duration); + void pack_frame(Frame& frame, int64_t frame_id, bool camera_info_first); + + Config config_; + Camera camera_; + std::thread thread_; + + std::mutex mutex_; + std::condition_variable wake_; + std::condition_variable exited_; + bool running_ = false; + bool finished_ = false; + + Frame latest_; + // Holds the previously published frame once the reader has taken the newest + // one, so its buffers are reused instead of reallocated every frame. + Frame scratch_; + bool has_frame_ = false; + + std::string state_; + bool state_changed_ = false; +}; + +} // namespace zed -- 2.54.0