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