add dora C++ node for ZED stereo camera capture

Implements a dora-rs node that captures frames from ZED cameras
and publishes BGR images, XYZRGBA point clouds, and camera
intrinsics. Includes CMake build system with automatic dora-rs
C++ bindings, dataflow definition, and documentation.

.gitignore

@@ -0,0 +1,2 @@
+build/
+bin/

CMakeLists.txt

@@ -0,0 +1,34 @@
+cmake_minimum_required(VERSION 3.21)
+project(dora_zed LANGUAGES C CXX)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_FLAGS "-fPIC")
+
+include(DoraTargets.cmake)
+
+set(ZED_DIR "/usr/local/zed" CACHE PATH "Path to the ZED SDK")
+set(ZED_PATH ${ZED_DIR})
+find_package(CUDAToolkit REQUIRED)
+find_package(zed REQUIRED)
+
+find_package(OpenCV REQUIRED)
+
+link_directories(${dora_link_dirs})
+link_directories(${ZED_LIBRARY_DIR})
+
+add_executable(dora_zed main.cc ${node_bridge})
+add_dependencies(dora_zed Dora_cxx)
+
+target_include_directories(
+    dora_zed
+    PRIVATE
+        ${dora_cxx_include_dir}
+        ${dora_c_include_dir}
+        ${ZED_INCLUDE_DIRS}
+        ${OpenCV_INCLUDE_DIRS}
+        ${CUDAToolkit_INCLUDE_DIRS}
+)
+
+target_link_libraries(dora_zed dora_node_api_cxx ${ZED_LIBRARIES} ${OpenCV_LIBS})
+
+install(TARGETS dora_zed DESTINATION ${CMAKE_CURRENT_SOURCE_DIR}/bin)

DoraTargets.cmake

@@ -0,0 +1,97 @@
+set(DORA_ROOT_DIR "" CACHE FILEPATH "Path to the root of dora")
+
+set(dora_c_include_dir "${CMAKE_CURRENT_BINARY_DIR}/include/c")
+
+set(dora_cxx_include_dir "${CMAKE_CURRENT_BINARY_DIR}/include/cxx")
+set(node_bridge "${CMAKE_CURRENT_BINARY_DIR}/node_bridge.cc")
+
+if(DORA_ROOT_DIR AND EXISTS "${DORA_ROOT_DIR}/Cargo.toml")
+    include(ExternalProject)
+    ExternalProject_Add(Dora
+        SOURCE_DIR ${DORA_ROOT_DIR}
+        BUILD_IN_SOURCE True
+        CONFIGURE_COMMAND ""
+        BUILD_COMMAND
+            cargo build
+            --package dora-node-api-c
+            &&
+            cargo build
+            --package dora-node-api-cxx
+        INSTALL_COMMAND ""
+    )
+
+    add_custom_command(OUTPUT ${node_bridge} ${dora_cxx_include_dir} ${dora_c_include_dir}
+        WORKING_DIRECTORY ${DORA_ROOT_DIR}
+        DEPENDS Dora
+        COMMAND
+            mkdir ${dora_cxx_include_dir} -p
+            &&
+            mkdir ${CMAKE_CURRENT_BINARY_DIR}/include/c -p
+            &&
+            cp target/cxxbridge/dora-node-api-cxx/src/lib.rs.cc ${node_bridge}
+            &&
+            cp target/cxxbridge/dora-node-api-cxx/src/lib.rs.h ${dora_cxx_include_dir}/dora-node-api.h
+            &&
+            cp apis/c/node ${CMAKE_CURRENT_BINARY_DIR}/include/c -r
+    )
+
+    add_custom_target(Dora_c DEPENDS ${dora_c_include_dir})
+    add_custom_target(Dora_cxx DEPENDS ${node_bridge} ${dora_cxx_include_dir})
+    set(dora_link_dirs ${DORA_ROOT_DIR}/target/debug)
+else()
+    include(ExternalProject)
+    set(DORA_GIT_TAG "main")
+    find_program(DORA_CLI dora)
+    if(DORA_CLI)
+        execute_process(
+            COMMAND ${DORA_CLI} --version
+            RESULT_VARIABLE _dora_version_result
+            OUTPUT_VARIABLE _dora_version_out
+            ERROR_VARIABLE _dora_version_err
+            OUTPUT_STRIP_TRAILING_WHITESPACE
+        )
+        if(_dora_version_result EQUAL 0)
+            string(REGEX MATCH "([0-9]+\\.[0-9]+\\.[0-9]+)" _dora_version_match "${_dora_version_out}")
+            if(_dora_version_match)
+                set(DORA_GIT_TAG "v${CMAKE_MATCH_1}")
+            endif()
+        endif()
+    endif()
+
+    ExternalProject_Add(Dora
+        PREFIX ${CMAKE_CURRENT_BINARY_DIR}/dora
+        GIT_REPOSITORY https://github.com/dora-rs/dora.git
+        GIT_TAG ${DORA_GIT_TAG}
+        BUILD_IN_SOURCE True
+        CONFIGURE_COMMAND ""
+        BUILD_COMMAND
+            cargo build
+            --package dora-node-api-c
+            --target-dir ${CMAKE_CURRENT_BINARY_DIR}/dora/src/Dora/target
+            &&
+            cargo build
+            --package dora-node-api-cxx
+            --target-dir ${CMAKE_CURRENT_BINARY_DIR}/dora/src/Dora/target
+        INSTALL_COMMAND ""
+    )
+
+    add_custom_command(OUTPUT ${node_bridge} ${dora_cxx_include_dir} ${dora_c_include_dir}
+        WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/dora/src/Dora/target
+        DEPENDS Dora
+        COMMAND
+            mkdir ${CMAKE_CURRENT_BINARY_DIR}/include/c -p
+            &&
+            mkdir ${dora_cxx_include_dir} -p
+            &&
+            cp cxxbridge/dora-node-api-cxx/src/lib.rs.cc ${node_bridge}
+            &&
+            cp cxxbridge/dora-node-api-cxx/src/lib.rs.h ${dora_cxx_include_dir}/dora-node-api.h
+            &&
+            cp ../apis/c/node ${CMAKE_CURRENT_BINARY_DIR}/include/c -r
+    )
+
+    set(dora_link_dirs ${CMAKE_CURRENT_BINARY_DIR}/dora/src/Dora/target/debug)
+
+    add_custom_target(Dora_c DEPENDS ${dora_c_include_dir})
+    add_custom_target(Dora_cxx DEPENDS ${node_bridge} ${dora_cxx_include_dir})
+endif()

README.md

@@ -1,2 +1,74 @@
 # dora-zed
 
+Dora C++ node for ZED stereo cameras. Captures frames and publishes BGR images, XYZRGBA point clouds, and camera intrinsics.
+
+## Usage via `git:` in a dataflow
+
+```yaml
+nodes:
+  - id: zed-camera
+    git: ssh://git@gitea.liria.cl:5001/liria/dora-zed.git
+    build: mkdir -p build && cd build && cmake .. && make install
+    path: bin/dora_zed
+    inputs:
+      tick: dora/timer/millis/100
+    outputs:
+      - image_bgr
+      - point_cloud
+      - camera_info
+```
+
+Then run:
+
+```bash
+dora build dataflow.yml
+dora up
+dora start dataflow.yml
+```
+
+## Local build
+
+```bash
+git clone ssh://git@gitea.liria.cl:5001/liria/dora-zed.git
+cd dora-zed
+mkdir -p build && cd build
+cmake ..
+make install
+```
+
+The binary is installed to `bin/dora_zed` relative to the repository root.
+
+## Dependencies
+
+| Dependency | Minimum version | Notes |
+|---|---|---|
+| CMake | 3.21 | |
+| ZED SDK | 4.x | Default path: `/usr/local/zed` |
+| CUDA Toolkit | Required by ZED SDK | |
+| OpenCV | 4.x | |
+| Rust toolchain | stable | Used to build dora-rs C++ bindings |
+
+## Environment variables
+
+| Variable | Default | Description |
+|---|---|---|
+| `ZED_RESOLUTION` | `720` | Camera resolution: `720`, `1080`, or `2000` |
+| `ZED_FPS` | `15` | Camera frame rate |
+| `ZED_DEPTH_MODE` | `NEURAL` | Depth mode: `PERFORMANCE`, `QUALITY`, `NEURAL`, or `NONE` |
+| `ZED_DEPTH_MIN_MM` | `10` | Minimum depth distance in millimeters |
+| `ZED_DEPTH_MAX_MM` | `500` | Maximum depth distance in millimeters |
+| `ZED_DEPTH_FILL` | `false` | Enable depth fill mode |
+| `ZED_FLIP` | `ON` | Image flip: `ON` or `OFF` |
+| `ZED_WARMUP_FRAMES` | `30` | Number of warmup frames to discard |
+| `ZED_RETRY_INTERVAL_SECONDS` | `5` | Seconds between reconnection attempts |
+
+## Outputs
+
+### `image_bgr`
+BGR image as raw bytes. Metadata includes `shape` (HxWx3), `encoding` (`bgr8`), camera intrinsics (`fx`, `fy`, `cx`, `cy`), `distortion`, `frame_id`, and `timestamp_ns`.
+
+### `point_cloud`
+XYZRGBA point cloud as float32 bytes. Metadata includes `shape` (HxWx4), `units` (`mm`), and camera intrinsics.
+
+### `camera_info`
+Four float32 values: `[fx, fy, cx, cy]`. Metadata includes `distortion` coefficients.

dataflow.yml

@@ -0,0 +1,11 @@
+nodes:
+  - id: zed-camera
+    git: ssh://git@gitea.liria.cl:5001/liria/dora-zed.git
+    build: mkdir -p build && cd build && cmake .. && make install
+    path: bin/dora_zed
+    inputs:
+      tick: dora/timer/millis/100
+    outputs:
+      - image_bgr
+      - point_cloud
+      - camera_info

main.cc

@@ -0,0 +1,297 @@
+#include <dora-node-api.h>
+#include <sl/Camera.hpp>
+#include <opencv2/opencv.hpp>
+
+#include <array>
+#include <cctype>
+#include <chrono>
+#include <cstdlib>
+#include <initializer_list>
+#include <iostream>
+#include <iterator>
+#include <string>
+#include <thread>
+#include <tuple>
+#include <vector>
+
+namespace {
+
+int get_env_int(const char* name, int default_value) {
+    const char* value = std::getenv(name);
+    if (!value || std::string(value).empty()) {
+        return default_value;
+    }
+    return std::stoi(value);
+}
+
+bool get_env_bool(const char* name, bool default_value) {
+    const char* value = std::getenv(name);
+    if (!value || std::string(value).empty()) {
+        return default_value;
+    }
+    std::string v(value);
+    for (auto& c : v) {
+        c = static_cast<char>(std::tolower(c));
+    }
+    return v == "1" || v == "true" || v == "yes" || v == "on";
+}
+
+std::string get_env_str(const char* name, const std::string& default_value) {
+    const char* value = std::getenv(name);
+    if (!value || std::string(value).empty()) {
+        return default_value;
+    }
+    return std::string(value);
+}
+
+sl::RESOLUTION parse_resolution(int resolution) {
+    switch (resolution) {
+        case 1080:
+            return sl::RESOLUTION::HD1080;
+        case 2000:
+            return sl::RESOLUTION::HD2K;
+        case 720:
+        default:
+            return sl::RESOLUTION::HD720;
+    }
+}
+
+sl::DEPTH_MODE parse_depth_mode(const std::string& mode) {
+    if (mode == "PERFORMANCE") {
+        return sl::DEPTH_MODE::PERFORMANCE;
+    }
+    if (mode == "QUALITY") {
+        return sl::DEPTH_MODE::QUALITY;
+    }
+    if (mode == "NONE") {
+        return sl::DEPTH_MODE::NONE;
+    }
+    return sl::DEPTH_MODE::NEURAL;
+}
+
+sl::FLIP_MODE parse_flip(const std::string& mode) {
+    if (mode == "OFF") {
+        return sl::FLIP_MODE::OFF;
+    }
+    return sl::FLIP_MODE::ON;
+}
+
+rust::Vec<int64_t> make_shape(int height, int width, int channels) {
+    rust::Vec<int64_t> shape;
+    shape.push_back(height);
+    shape.push_back(width);
+    shape.push_back(channels);
+    return shape;
+}
+
+rust::Vec<double> make_distortion(const std::vector<double>& distortion) {
+    rust::Vec<double> values;
+    for (double v : distortion) {
+        values.push_back(v);
+    }
+    return values;
+}
+
+rust::Vec<int64_t> make_vector_int(std::initializer_list<int64_t> values) {
+    rust::Vec<int64_t> out;
+    for (int64_t v : values) {
+        out.push_back(v);
+    }
+    return out;
+}
+
+void list_available_cameras() {
+    auto devices = sl::Camera::getDeviceList();
+    std::cout << "Detected ZED cameras: " << devices.size() << std::endl;
+    for (std::size_t i = 0; i < devices.size(); i++) {
+        const auto& dev = devices[i];
+        std::cout << " - [" << i << "] model=" << sl::toString(dev.camera_model)
+                  << ", serial=" << dev.serial_number << std::endl;
+    }
+}
+
+bool open_camera(sl::Camera& zed, const sl::InitParameters& init_params) {
+    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;
+        return false;
+    }
+    return true;
+}
+
+bool ensure_camera_ready(
+    sl::Camera& zed,
+    const sl::InitParameters& init_params,
+    int sleep_ms
+) {
+    while (true) {
+        if (open_camera(zed, init_params)) {
+            return true;
+        }
+        std::this_thread::sleep_for(std::chrono::milliseconds(sleep_ms));
+    }
+}
+
+} // namespace
+
+int main() {
+    auto dora_node = init_dora_node();
+
+    const int resolution = get_env_int("ZED_RESOLUTION", 720);
+    const int fps = get_env_int("ZED_FPS", 15);
+    const std::string depth_mode = get_env_str("ZED_DEPTH_MODE", "NEURAL");
+    const int depth_min_mm = get_env_int("ZED_DEPTH_MIN_MM", 10);
+    const int depth_max_mm = get_env_int("ZED_DEPTH_MAX_MM", 500);
+    const bool depth_fill = get_env_bool("ZED_DEPTH_FILL", false);
+    const std::string flip = get_env_str("ZED_FLIP", "ON");
+    const int warmup_frames = get_env_int("ZED_WARMUP_FRAMES", 30);
+    const int retry_interval_seconds = get_env_int("ZED_RETRY_INTERVAL_SECONDS", 5);
+    const int retry_interval_ms = retry_interval_seconds * 1000;
+
+    sl::InitParameters init_params;
+    init_params.camera_resolution = parse_resolution(resolution);
+    init_params.camera_fps = fps;
+    init_params.depth_mode = parse_depth_mode(depth_mode);
+    init_params.coordinate_units = sl::UNIT::MILLIMETER;
+    init_params.camera_image_flip = parse_flip(flip);
+    init_params.depth_minimum_distance = depth_min_mm;
+    init_params.depth_maximum_distance = depth_max_mm;
+
+    list_available_cameras();
+
+    sl::Camera zed;
+    ensure_camera_ready(zed, init_params, retry_interval_ms);
+
+    sl::RuntimeParameters runtime_params;
+    runtime_params.enable_depth = true;
+    runtime_params.enable_fill_mode = depth_fill;
+
+    sl::Mat image_mat;
+    sl::Mat point_cloud_mat;
+
+    for (int i = 0; i < warmup_frames; i++) {
+        zed.grab(runtime_params);
+    }
+
+    auto reload_calibration = [&]() {
+        sl::CameraInformation info = zed.getCameraInformation();
+        auto calib = info.camera_configuration.calibration_parameters;
+        auto left = calib.left_cam;
+
+        std::vector<double> dist;
+        dist.reserve(std::size(left.disto));
+        for (double v : left.disto) {
+            dist.push_back(v);
+        }
+        return std::tuple<float, float, float, float, std::vector<double>>(
+            left.fx, left.fy, left.cx, left.cy, std::move(dist));
+    };
+
+    auto [fx, fy, cx, cy, distortion] = reload_calibration();
+
+    int64_t frame_id = 0;
+    while (true) {
+        auto event = dora_node.events->next();
+        auto type = event_type(event);
+
+        if (type == DoraEventType::Stop || type == DoraEventType::AllInputsClosed) {
+            break;
+        }
+
+        if (type != DoraEventType::Input) {
+            continue;
+        }
+
+        auto input = event_as_input(std::move(event));
+        (void)input;
+
+        if (zed.grab(runtime_params) != sl::ERROR_CODE::SUCCESS) {
+            std::cerr << "Grab failed; attempting to reconnect..." << std::endl;
+            zed.close();
+            list_available_cameras();
+            ensure_camera_ready(zed, init_params, retry_interval_ms);
+            std::tie(fx, fy, cx, cy, distortion) = reload_calibration();
+            continue;
+        }
+
+        zed.retrieveImage(image_mat, sl::VIEW::LEFT);
+        zed.retrieveMeasure(point_cloud_mat, sl::MEASURE::XYZRGBA);
+
+        int width = image_mat.getWidth();
+        int height = image_mat.getHeight();
+
+        cv::Mat image_rgba(height, width, CV_8UC4, image_mat.getPtr<sl::uchar1>());
+        cv::Mat image_bgr;
+        cv::cvtColor(image_rgba, image_bgr, cv::COLOR_BGRA2BGR);
+
+        const auto image_size = static_cast<size_t>(image_bgr.total() * image_bgr.elemSize());
+        rust::Slice<const uint8_t> image_slice{
+            reinterpret_cast<const uint8_t*>(image_bgr.data), image_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));
+        image_metadata->set_float("fx", fx);
+        image_metadata->set_float("fy", fy);
+        image_metadata->set_float("cx", cx);
+        image_metadata->set_float("cy", cy);
+        image_metadata->set_list_float("distortion", make_distortion(distortion));
+        image_metadata->set_int("frame_id", frame_id++);
+        image_metadata->set_int("timestamp_ns", static_cast<int64_t>(
+            std::chrono::duration_cast<std::chrono::nanoseconds>(
+                std::chrono::steady_clock::now().time_since_epoch()
+            ).count()
+        ));
+
+        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;
+        }
+
+        int pc_width = point_cloud_mat.getWidth();
+        int pc_height = point_cloud_mat.getHeight();
+        const size_t pc_size = static_cast<size_t>(pc_width * pc_height * sizeof(sl::float4));
+        const auto* pc_ptr = reinterpret_cast<const uint8_t*>(point_cloud_mat.getPtr<sl::float4>());
+        rust::Slice<const uint8_t> pc_slice{pc_ptr, pc_size};
+
+        auto pc_metadata = new_metadata();
+        pc_metadata->set_string("dtype", "float32");
+        pc_metadata->set_string("layout", "HWC");
+        pc_metadata->set_string("channels", "XYZRGBA");
+        pc_metadata->set_string("units", "mm");
+        pc_metadata->set_list_int("shape", make_shape(pc_height, pc_width, 4));
+        pc_metadata->set_float("fx", fx);
+        pc_metadata->set_float("fy", fy);
+        pc_metadata->set_float("cx", cx);
+        pc_metadata->set_float("cy", cy);
+        pc_metadata->set_list_float("distortion", make_distortion(distortion));
+
+        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;
+        }
+
+        float camera_data[4] = {fx, fy, cx, cy};
+        rust::Slice<const uint8_t> cam_slice{
+            reinterpret_cast<const uint8_t*>(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(distortion));
+
+        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;
+        }
+    }
+
+    zed.close();
+    return 0;
+}