diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..d5697ec --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +build/ +bin/ diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..499a48e --- /dev/null +++ b/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) diff --git a/DoraTargets.cmake b/DoraTargets.cmake new file mode 100644 index 0000000..836f09c --- /dev/null +++ b/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() diff --git a/README.md b/README.md index 1e1e623..671baa0 100644 --- a/README.md +++ b/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. diff --git a/dataflow.yml b/dataflow.yml new file mode 100644 index 0000000..3d8faaf --- /dev/null +++ b/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 diff --git a/main.cc b/main.cc new file mode 100644 index 0000000..89927fa --- /dev/null +++ b/main.cc @@ -0,0 +1,297 @@ +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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(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 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_distortion(const std::vector& distortion) { + rust::Vec values; + for (double v : distortion) { + values.push_back(v); + } + 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 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 dist; + dist.reserve(std::size(left.disto)); + for (double v : left.disto) { + dist.push_back(v); + } + return std::tuple>( + 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()); + 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}; + + 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( + std::chrono::duration_cast( + 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(pc_width * pc_height * sizeof(sl::float4)); + const auto* pc_ptr = reinterpret_cast(point_cloud_mat.getPtr()); + rust::Slice 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 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(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; +}