fix camera recovery blocking the dora event loop

Camera::open() retried forever inside a fixed sleep, so a camera that did not
come back left the node stuck there: it stopped reading dora events and no
longer honoured Stop or AllInputsClosed.

Recovery is now a state machine over ticks. open() makes one attempt and
returns; the loop decides when to try again from a 1/2/5/10 s backoff deadline,
and nothing sleeps, so events keep being read while the camera is down. The
first attempt closes the handle; past that the device is wedged and the node
escalates to sl::Camera::reboot(), guarded to the ZED 2 and newer since it does
nothing on the ZED and ZED Mini. Reopening pins the serial so recovery gets the
same physical camera back.

Set async_grab_camera_recovery so a link hiccup returns CAMERA_REBOOTING
immediately instead of blocking the grab for ~17 s, and enable_image_validity_
check so the SDK flags frames a stalled camera hands back looking valid. Those
transient errors no longer drop the handle, since the SDK is already
reconnecting it.

An all-black frame now counts as a failed grab rather than being published, and
a camera delivering nothing usable for 5 s (30 s while the SDK reports it is
reconnecting) triggers recovery. A new camera_state output publishes
opening/streaming/recovering so a downstream node can tell "no frame yet" from
"camera lost".

ZED_RETRY_INTERVAL_SECONDS is replaced by the backoff.

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

src/main.cc

@@ -4,10 +4,12 @@
 #include <dora-node-api.h>
 #include <opencv2/opencv.hpp>
 
+#include <algorithm>
 #include <chrono>
 #include <cmath>
 #include <initializer_list>
 #include <iostream>
+#include <iterator>
 #include <string>
 #include <vector>
 
@@ -15,6 +17,28 @@ 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);
@@ -82,6 +106,35 @@ std::vector<float> pack_depth(sl::Mat& depth_mat) {
     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>(
                std::chrono::system_clock::now().time_since_epoch())
@@ -97,9 +150,29 @@ int main() {
 
     auto dora_node = init_dora_node();
     zed::Camera camera(config);
-    camera.open();
-    camera.warmup();
-    auto calib = camera.calibration();
+    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()};
+
+        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;
@@ -110,6 +183,16 @@ int main() {
     // 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;
     while (true) {
         auto event = dora_node.events->next();
@@ -123,15 +206,64 @@ int main() {
             continue;
         }
 
-        if (!camera.grab()) {
-            std::cerr << "Grab failed; attempting to reconnect..." << std::endl;
-            camera.close();
-            camera.open();
+        const char* down_state = streamed_once ? STATE_RECOVERING : STATE_OPENING;
+
+        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.
             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();
 

src/zed/camera.cc

@@ -1,12 +1,11 @@
 #include "zed/camera.h"
 #include "util/env.h"
 
-#include <chrono>
+#include <algorithm>
 #include <cstdlib>
 #include <iostream>
 #include <iterator>
 #include <string>
-#include <thread>
 #include <utility>
 
 namespace {
@@ -56,6 +55,24 @@ sl::FLIP_MODE parse_flip(const std::string& mode) {
     return sl::FLIP_MODE::ON;
 }
 
+// A blank frame is checked on a strided sample rather than the full image, which
+// is just as conclusive and costs nothing at frame rate.
+constexpr int BLANK_FRAME_STRIDE = 16;
+
+// sl::Camera::reboot() reaches the ZED 2 and newer. On the first-generation ZED
+// and the ZED Mini it returns without touching the device, so calling it there
+// only costs time and fills the log.
+bool model_supports_reboot(sl::MODEL model) {
+    return model != sl::MODEL::ZED && model != sl::MODEL::ZED_M;
+}
+
+// The SDK reconnects a camera whose link hiccuped by itself. Dropping the handle
+// over one of these takes the recovery away from the SDK and hands it to us,
+// which is slower and, on a device the kernel is about to suspend, often fatal.
+bool is_transient_grab_error(sl::ERROR_CODE err) {
+    return err == sl::ERROR_CODE::CAMERA_REBOOTING || err == sl::ERROR_CODE::CORRUPTED_FRAME;
+}
+
 void list_available_cameras() {
     auto devices = sl::Camera::getDeviceList();
     std::cout << "Detected ZED cameras: " << devices.size() << std::endl;
@@ -84,8 +101,6 @@ Config Config::from_env() {
         util::get_env_int("ZED_TEXTURE_CONFIDENCE_THRESHOLD", c.texture_confidence_threshold);
     c.flip = util::get_env_str("ZED_FLIP", c.flip);
     c.warmup_frames = util::get_env_int("ZED_WARMUP_FRAMES", c.warmup_frames);
-    int retry_seconds = util::get_env_int("ZED_RETRY_INTERVAL_SECONDS", 5);
-    c.retry_interval_ms = retry_seconds * 1000;
     return c;
 }
 
@@ -102,6 +117,13 @@ Camera::Camera(const Config& config) : config_(config) {
     init_params_.camera_image_flip = parse_flip(config_.flip);
     init_params_.depth_minimum_distance = config_.depth_min_mm;
     init_params_.depth_maximum_distance = config_.depth_max_mm;
+    // Recover a hiccuping link inside the SDK rather than out here. Without this
+    // a grab blocks for the roughly 17 s the SDK spends reconnecting, freezing the
+    // dora event loop; with it the grab returns CAMERA_REBOOTING straight away and
+    // the SDK reconnects in the background, holding the device open meanwhile.
+    init_params_.async_grab_camera_recovery = true;
+    // Let the SDK flag the frames a stalled camera hands back looking valid.
+    init_params_.enable_image_validity_check = 1;
 
     runtime_params_.enable_depth = true;
     runtime_params_.enable_fill_mode = config_.depth_fill;
@@ -116,28 +138,64 @@ Camera::Camera(const Config& config) : config_(config) {
               << ", confidence=" << config_.confidence_threshold
               << ", texture_confidence=" << config_.texture_confidence_threshold
               << ", fill=" << (config_.depth_fill ? "on" : "off") << std::endl;
+
+    list_available_cameras();
 }
 
 Camera::~Camera() {
     close();
 }
 
-void Camera::open() {
-    list_available_cameras();
-    while (true) {
-        sl::ERROR_CODE err = zed_.open(init_params_);
-        if (err == sl::ERROR_CODE::SUCCESS) {
-            return;
-        }
+bool Camera::open() {
+    sl::ERROR_CODE err = zed_.open(init_params_);
+    if (err != sl::ERROR_CODE::SUCCESS) {
         std::cerr << "Failed to open ZED camera: " << sl::toString(err) << std::endl;
-        std::this_thread::sleep_for(std::chrono::milliseconds(config_.retry_interval_ms));
+        return false;
     }
+    recovering_ = false;
+
+    sl::CameraInformation info = zed_.getCameraInformation();
+    serial_number_ = info.serial_number;
+    model_ = info.camera_model;
+    // After the first open, reopen the same physical camera. Recovery runs while
+    // the device may be mid-re-enumeration, and picking whichever ZED answers
+    // first is not the same thing as picking ours.
+    init_params_.input.setFromSerialNumber(serial_number_);
+
+    std::cout << "ZED camera opened (" << sl::toString(model_) << ", SN " << serial_number_ << ")"
+              << std::endl;
+    return true;
 }
 
 void Camera::close() {
+    recovering_ = false;
     zed_.close();
 }
 
+void Camera::reset() {
+    close();
+    if (serial_number_ == 0 || !model_supports_reboot(model_)) {
+        return;
+    }
+    // The ZED can stop delivering frames while staying enumerated: the video
+    // endpoint halts, grabs fail, and reopening returns CAMERA FAILED TO SETUP
+    // because the endpoint is still halted. A device reset clears that, and
+    // reboot() is the SDK's way to ask for one without unplugging the camera.
+    // The device re-enumerates afterwards, so the caller's backoff before the
+    // next open is what lets it settle.
+    sl::ERROR_CODE err = sl::Camera::reboot(static_cast<int>(serial_number_), true);
+    std::cout << "reset ZED camera (SN " << serial_number_ << "): " << sl::toString(err)
+              << std::endl;
+}
+
+bool Camera::is_open() const {
+    return zed_.isOpened();
+}
+
+bool Camera::recovering() const {
+    return recovering_;
+}
+
 void Camera::warmup() {
     for (int i = 0; i < config_.warmup_frames; i++) {
         zed_.grab(runtime_params_);
@@ -145,9 +203,27 @@ void Camera::warmup() {
 }
 
 bool Camera::grab() {
-    if (zed_.grab(runtime_params_) != sl::ERROR_CODE::SUCCESS) {
+    sl::ERROR_CODE err = zed_.grab(runtime_params_);
+    if (err != sl::ERROR_CODE::SUCCESS) {
+        if (is_transient_grab_error(err)) {
+            // Every grab repeats this for as long as the reconnect takes, so only
+            // the first one is worth a line in the log.
+            if (!recovering_) {
+                recovering_ = true;
+                std::cerr << "ZED camera link dropped (" << sl::toString(err)
+                          << "); the SDK is reconnecting it" << std::endl;
+            }
+        } else {
+            recovering_ = false;
+            std::cerr << "Failed to grab frame: " << sl::toString(err) << std::endl;
+        }
         return false;
     }
+    if (recovering_) {
+        recovering_ = false;
+        std::cout << "ZED camera link restored" << std::endl;
+    }
+
     zed_.retrieveImage(image_mat_, sl::VIEW::LEFT);
     zed_.retrieveMeasure(depth_mat_, sl::MEASURE::DEPTH);
     // At 1080p a full XYZRGBA cloud is ~14.7 MB per frame, and retrieving it
@@ -175,6 +251,33 @@ Calibration Camera::calibration() const {
     return result;
 }
 
+bool Camera::image_is_blank() {
+    const int height = image_mat_.getHeight();
+    const int width = image_mat_.getWidth();
+    if (height == 0 || width == 0) {
+        return true;
+    }
+
+    const size_t step = image_mat_.getStep(sl::MEM::CPU);
+    const sl::uchar4* src = image_mat_.getPtr<sl::uchar4>();
+
+    unsigned char lowest[3] = {255, 255, 255};
+    unsigned char highest[3] = {0, 0, 0};
+    for (int y = 0; y < height; y += BLANK_FRAME_STRIDE) {
+        const sl::uchar4* row = src + static_cast<size_t>(y) * step;
+        for (int x = 0; x < width; x += BLANK_FRAME_STRIDE) {
+            const unsigned char channels[3] = {row[x].x, row[x].y, row[x].z};
+            for (int c = 0; c < 3; c++) {
+                lowest[c] = std::min(lowest[c], channels[c]);
+                highest[c] = std::max(highest[c], channels[c]);
+            }
+        }
+    }
+    // Sensor noise makes a single-colour frame impossible for a live camera, so
+    // this tells a dead device apart from any real scene.
+    return lowest[0] == highest[0] && lowest[1] == highest[1] && lowest[2] == highest[2];
+}
+
 int64_t Camera::device_timestamp_ns() {
     return static_cast<int64_t>(
         zed_.getTimestamp(sl::TIME_REFERENCE::IMAGE).getNanoseconds());
@@ -192,8 +295,4 @@ sl::Mat& Camera::point_cloud() {
     return point_cloud_mat_;
 }
 
-sl::Camera& Camera::handle() {
-    return zed_;
-}
-
 } // namespace zed

src/zed/camera.h

@@ -19,13 +19,12 @@ struct Config {
     int texture_confidence_threshold = 100;
     std::string flip = "ON";
     int warmup_frames = 30;
-    int retry_interval_ms = 5000;
 
     static Config from_env();
 };
 
 struct Calibration {
-    float fx, fy, cx, cy;
+    float fx = 0, fy = 0, cx = 0, cy = 0;
     std::vector<double> distortion;
 };
 
@@ -34,16 +33,26 @@ public:
     explicit Camera(const Config& config);
     ~Camera();
 
-    void open();
+    // One attempt, returning whether it succeeded. The caller decides when to try
+    // again, so a camera that is down never blocks the dora event loop.
+    bool open();
     void close();
+    // Close the handle and reset the device over USB, where the model allows it.
+    void reset();
     void warmup();
     bool grab();
+    bool is_open() const;
+    // True while the SDK is reconnecting the camera after a link hiccup.
+    bool recovering() const;
+    // True when the last grabbed frame carries no picture at all. A camera whose
+    // link has dropped does not necessarily fail its grabs: it can keep reporting
+    // success and hand back a uniform frame.
+    bool image_is_blank();
     Calibration calibration() const;
     int64_t device_timestamp_ns();
     sl::Mat& image();
     sl::Mat& depth();
     sl::Mat& point_cloud();
-    sl::Camera& handle();
 
 private:
     Config config_;
@@ -53,6 +62,11 @@ private:
     sl::Mat image_mat_;
     sl::Mat depth_mat_;
     sl::Mat point_cloud_mat_;
+    // Read off the device once it is open and kept afterwards: resetting a wedged
+    // camera needs the serial and the model, and by then the handle is gone.
+    unsigned int serial_number_ = 0;
+    sl::MODEL model_ = sl::MODEL::LAST;
+    bool recovering_ = false;
 };
 
 } // namespace zed