cristhian/dora_littlehand
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

First commit

Browse Source
This commit is contained in:
Cristhian Aguilera
2026-01-28 16:21:37 -03:00
parent 2da5baf508
commit 610c43e16d
17 changed files with 2113 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
dora_zed_cpp/CMakeLists.txt Normal file
View File

@@ -0,0 +1,34 @@
cmake_minimum_required(VERSION 3.21)
project(dora_zed_cpp 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_cpp main.cc ${node_bridge})
add_dependencies(dora_zed_cpp Dora_cxx)
target_include_directories(
dora_zed_cpp
PRIVATE
${dora_cxx_include_dir}
${dora_c_include_dir}
${ZED_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
${CUDAToolkit_INCLUDE_DIRS}
)
target_link_libraries(dora_zed_cpp dora_node_api_cxx ${ZED_LIBRARIES} ${OpenCV_LIBS})
install(TARGETS dora_zed_cpp DESTINATION ${CMAKE_CURRENT_SOURCE_DIR}/bin)

79
dora_zed_cpp/DoraTargets.cmake Normal file
View File

@@ -0,0 +1,79 @@
set(DORA_ROOT_DIR "/home/cristhian/workspace/garbage/dora" 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)
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)
ExternalProject_Add(Dora
PREFIX ${CMAKE_CURRENT_BINARY_DIR}/dora
GIT_REPOSITORY https://github.com/dora-rs/dora.git
GIT_TAG main
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()

31
dora_zed_cpp/README.md Normal file
View File

@@ -0,0 +1,31 @@
# dora_zed_cpp
C++ Dora node for ZED camera streaming.
## Build & run
From the repo root:
```bash
cmake -S dora_zed_cpp -B dora_zed_cpp/build
cmake --build dora_zed_cpp/build
dora run dataflow_zed_cpp.yml
```
## Configuration (env)
- `ZED_RESOLUTION` (720, 1080, 2000)
- `ZED_FPS` (default 15)
- `ZED_DEPTH_MODE` (NEURAL, PERFORMANCE, QUALITY, NONE)
- `ZED_DEPTH_MIN_MM` (default 10)
- `ZED_DEPTH_MAX_MM` (default 500)
- `ZED_DEPTH_FILL` (true/false, default false)
- `ZED_FLIP` (ON/OFF, default ON)
- `ZED_WARMUP_FRAMES` (default 30)
## Outputs
- `image_bgr` raw bytes, metadata: `shape`, `dtype`, `encoding`, `layout`, and intrinsics (`fx`, `fy`, `cx`, `cy`, `distortion`)
- `point_cloud` raw bytes, metadata: `shape`, `dtype`, `channels`, `units`, `layout`, and intrinsics
- `camera_info` raw bytes (4 floats: fx, fy, cx, cy), metadata includes `distortion`

290
dora_zed_cpp/main.cc Normal file
View File

@@ -0,0 +1,290 @@
#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();
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));
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;
}