fix the camera blocking the dora event loop by moving it to its own thread

Hardware testing showed the tick-driven recovery state machine did not work.
async_grab_camera_recovery does not make grab() prompt on a disconnected ZED-M
under SDK 5.3: grabs took ~3 s each while the link was down and then blocked
indefinitely. The event loop stopped turning, so Stop went unanswered, the
30 s patience timer and the backoff were never reached, and replugging the
camera did nothing. Measured: 122 s frozen, then dora stop needed 10 s and a
forced kill.

Move everything that can block onto a capture thread. zed::Capture owns the
camera and runs open, warmup, grab, blank and stall detection, backoff and
escalation; it packs each frame into owned buffers and hands the newest one to
the reader by swapping under a mutex. The dora event loop now only drains that
buffer and publishes, so it never calls into the SDK and stays responsive
however long the SDK blocks.

The newest frame wins and uncollected frames are dropped, which is what a
consumer wants from a camera source. Swapping rather than assigning returns the
previous frame's buffers to the capture thread, so steady state does not
reallocate. The point cloud is now copied out of the SDK buffer rather than
published in place, since the reader is another thread.

On exit the loop waits 2 s for the thread and then _Exit()s if it is still
inside the SDK, so Stop is always answered promptly.

This also removes the blocking warmup from the event loop: it stays a blocking
burst, which is harmless on the capture thread.

Verified on a ZED-M: unplug leaves the node responsive and logs the backoff
1, 2, 5 then 10 s; replug gives "camera recovered after 13 attempt(s)" and
publishing resumes without restarting the dataflow; dora stop with the camera
unplugged returns in 0.70 s with the thread joining gracefully.

Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01Y7g2cBJ5e3r15ZbJ9WZn8c

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,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<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() {
@@ -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) {
-                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<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();
+        // --- point_cloud ---
+        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);
+            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<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));
-            }
-
-            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