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.

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)

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

src/main.cc

@@ -1,50 +1,22 @@
-#include "util/env.h"
-#include "zed/camera.h"
+#include "zed/capture.h"
 
 #include <dora-node-api.h>
-#include <opencv2/opencv.hpp>
 
-#include <algorithm>
 #include <chrono>
-#include <cmath>
+#include <cstdlib>
 #include <initializer_list>
 #include <iostream>
-#include <iterator>
 #include <string>
 #include <vector>
 
 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<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<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;
 }
 
 rust::Vec<double> make_distortion(const std::vector<double>& distortion) {
@@ -55,14 +27,6 @@ rust::Vec<double> make_distortion(const std::vector<double>& distortion) {
     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 set_intrinsics(Metadata& metadata, const zed::Calibration& calib) {
     metadata.set_float("fx", calib.fx);
     metadata.set_float("fy", calib.fy);
@@ -74,66 +38,20 @@ 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<float> 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<sl::float1>();
-
-    std::vector<float> out(static_cast<size_t>(height) * static_cast<size_t>(width));
-    for (int y = 0; y < height; y++) {
-        const float* row = src + static_cast<size_t>(y) * step;
-        float* dst = out.data() + static_cast<size_t>(y) * static_cast<size_t>(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<size_t>(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<double>(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() {
     return std::chrono::duration_cast<std::chrono::nanoseconds>(
@@ -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<const uint8_t> state_slice{
-            reinterpret_cast<const uint8_t*>(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<const uint8_t> state_slice{
+                reinterpret_cast<const uint8_t*>(state.data()), state.size()};
 
-        if (!camera.is_open()) {
-            publish_camera_state(down_state);
-            if (Clock::now() < next_open_attempt_at) {
+            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;
         }
-            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.
-            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<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};
+        rust::Slice<const uint8_t> 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<float> depth_buf = pack_depth(depth_mat);
         rust::Slice<const uint8_t> depth_slice{
-            reinterpret_cast<const uint8_t*>(depth_buf.data()), depth_buf.size() * sizeof(float)};
+            reinterpret_cast<const uint8_t*>(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();
+        if (frame.has_point_cloud) {
+            rust::Slice<const uint8_t> pc_slice{
+                reinterpret_cast<const uint8_t*>(frame.point_cloud.data()),
+                frame.point_cloud.size() * sizeof(float)};
 
-            std::vector<float> 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);
-
-            if (pc_mode == PcMode::HALF) {
-                int hw = pc_width / 2, hh = pc_height / 2;
-                pc_buf.resize(static_cast<size_t>(hh * hw * 4));
-                const sl::float4* src = point_cloud_mat.getPtr<sl::float4>();
-                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<const uint8_t*>(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<size_t>(pc_width * pc_height * sizeof(sl::float4));
-                pc_data_ptr  = reinterpret_cast<const uint8_t*>(point_cloud_mat.getPtr<sl::float4>());
-                pc_metadata->set_string("layout", "HWC");
-                pc_metadata->set_list_int("shape", make_shape(pc_height, pc_width, 4));
-            }
+            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);
 
-            rust::Slice<const uint8_t> 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<const uint8_t> cam_slice{
                 reinterpret_cast<const uint8_t*>(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;
 }

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);

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";

src/zed/capture.cc

@@ -0,0 +1,302 @@
+#include "zed/capture.h"
+
+#include <opencv2/opencv.hpp>
+
+#include <algorithm>
+#include <chrono>
+#include <cmath>
+#include <iostream>
+
+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<size_t>(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<double>(Clock::now() - since).count();
+}
+
+int64_t host_timestamp_ns() {
+    return std::chrono::duration_cast<std::chrono::nanoseconds>(
+               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<float>& 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<sl::float1>();
+
+    out.resize(static_cast<size_t>(height) * static_cast<size_t>(width));
+    for (int y = 0; y < height; y++) {
+        const float* row = src + static_cast<size_t>(y) * step;
+        float* dst = out.data() + static_cast<size_t>(y) * static_cast<size_t>(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<std::mutex> lock(mutex_);
+        running_ = true;
+    }
+    thread_ = std::thread(&Capture::run, this);
+}
+
+void Capture::stop() {
+    std::lock_guard<std::mutex> lock(mutex_);
+    running_ = false;
+    wake_.notify_all();
+}
+
+bool Capture::wait_exit(std::chrono::milliseconds timeout) {
+    std::unique_lock<std::mutex> lock(mutex_);
+    return exited_.wait_for(lock, timeout, [this] { return finished_; });
+}
+
+bool Capture::take_frame(Frame& out) {
+    std::lock_guard<std::mutex> 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<std::mutex> lock(mutex_);
+    if (!state_changed_) {
+        return false;
+    }
+    out = state_;
+    state_changed_ = false;
+    return true;
+}
+
+void Capture::set_state(const char* state) {
+    std::lock_guard<std::mutex> 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<std::mutex> 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<std::chrono::milliseconds>(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<sl::uchar1>(),
+                       image_mat.getStep(sl::MEM::CPU) * sizeof(sl::uchar4));
+    frame.image_bgr.resize(static_cast<size_t>(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<sl::float4>();
+
+    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<size_t>(frame.pc_height) * frame.pc_width * 4);
+
+    for (int y = 0; y < frame.pc_height; y++) {
+        const sl::float4* row = src + static_cast<size_t>(y * stride) * pc_step;
+        float* dst = frame.point_cloud.data() + static_cast<size_t>(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<std::mutex> 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<std::mutex> 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<std::mutex> lock(mutex_);
+        finished_ = true;
+        exited_.notify_all();
+    }
+}
+
+} // namespace zed

src/zed/capture.h

@@ -0,0 +1,94 @@
+#pragma once
+
+#include "zed/camera.h"
+
+#include <condition_variable>
+#include <cstdint>
+#include <mutex>
+#include <string>
+#include <thread>
+#include <vector>
+
+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<uint8_t> image_bgr; // height * width * 3
+    std::vector<float> depth;       // height * width, millimetres
+    bool has_point_cloud = false;
+    int pc_width = 0;
+    int pc_height = 0;
+    std::vector<float> 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