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
This commit is contained in:
cesar
2026-09-08 09:26:17 -03:00
co-authored by Claude Opus 5
parent 42c2b791fd
commit d8956f269d
6 changed files with 307 additions and 31 deletions
+18 -4
View File
@@ -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