liria/dora-zed
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 3 Wiki Activity

add dora C++ node for ZED stereo camera capture

Browse Source 
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.
This commit is contained in:
cristhian aguilera
2026-03-25 08:42:26 -03:00
parent bd9210e9a6
commit 9ac14c07e2
6 changed files with 513 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+2
View File
@@ -0,0 +1,2 @@
build/
bin/
CMakeLists.txt
+34
View File
@@ -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
+97
View File
@@ -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
+72
View File
@@ -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
+11
View File
@@ -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
+297
View File
@@ -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;
}