diff --git a/AGENTS.md b/AGENTS.md index ff129ca..c4f26b4 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -36,9 +36,9 @@ Camera behavior is configured entirely through environment variables (see README ## Architecture - **`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 (with retry loop), warmup, grab (image + depth + point cloud), calibration, device timestamp, and close. Exposes `handle()` for engine access. +- **`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/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. Handles reconnection on grab failure. +- **`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. - **`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. - **`dataflow.yml`** — Example dora dataflow definition using the `git:` source format, pinned with `tag:` — dora 1.0 pins a source with the mutually exclusive `tag:`, `rev:` or `branch:`. diff --git a/README.md b/README.md index fa7bfbc..b634abe 100644 --- a/README.md +++ b/README.md @@ -20,6 +20,7 @@ nodes: - depth - point_cloud - camera_info + - camera_state ``` See [`dataflow.yml`](dataflow.yml) for a complete runnable example with a camera display node. @@ -103,7 +104,6 @@ All behavior is controlled via environment variables. | `ZED_DEPTH_FILL` | `false` | Interpolate depth over holes. Overrides both confidence thresholds. | | `ZED_FLIP` | `ON` | `ON` or `OFF` | | `ZED_WARMUP_FRAMES` | `30` | Frames to discard on startup | -| `ZED_RETRY_INTERVAL_SECONDS` | `5` | Reconnection interval (s) | The resolved depth mode, both confidence thresholds and the depth range are logged at startup. The defaults match the reference camera driver the perception stack was tuned @@ -140,6 +140,36 @@ Consumers that need 3D back-project `depth` with the published intrinsics instea | `depth` | `float32 [H, W]` | Per-pixel depth in millimetres, published on every frame `image_bgr` is. Metadata: `dtype`, `layout=HW`, `shape`, `units=mm`, intrinsics, timestamps. `H` and `W` match `image_bgr` for the same `frame_id`. Missing depth is `0` — the payload contains no `inf` and no `nan`. Holes are not interpolated unless `ZED_DEPTH_FILL` is set. | | `point_cloud` | `float32 [*, 4]` | XYZRGBA point cloud, only when `ZED_POINT_CLOUD` is on. Shape depends on `ZED_PC_MODE`. Metadata: `units=mm`, intrinsics, timestamps. | | `camera_info` | `float32 [4]` | `[fx, fy, cx, cy]`. Metadata: `distortion` coefficients, timestamps. Published **once per camera session** — with the first frame after the camera opens, and again after a reconnection — not on every frame, since the intrinsics are fixed for a given camera and resolution. | +| `camera_state` | `string` | UTF-8 state name, published on each change. Metadata: `dtype=string`, `encoding=utf8`, `host_timestamp_ns`. | + +### Camera state and recovery + +`camera_state` lets a downstream node tell a camera that has not delivered its first +frame yet from one that has been lost: + +| State | Meaning | +|---|---| +| `opening` | The camera has not delivered a usable frame yet in this run | +| `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 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 +uniform frame. A camera that delivers no usable frame for 5 seconds is recovered — 30 +seconds if the SDK reports it is reconnecting the link itself, since taking the handle +away mid-reconnect aborts a recovery that was already under way. + +The first recovery attempt closes the handle. Past that the device itself is wedged, and +the node escalates to `sl::Camera::reboot()`, which resets it over USB without unplugging +it. That call reaches the ZED 2 and newer only; on the first-generation ZED and the ZED +Mini the node closes the handle and lets the backoff give the endpoint time to clear. +Every attempt and every escalation is logged. + Consumers that need 3D back-project `depth` with the published intrinsics rather than consuming `point_cloud`; the point cloud is not a prerequisite for 3D. diff --git a/dataflow.yml b/dataflow.yml index df9c3e9..f03c9db 100644 --- a/dataflow.yml +++ b/dataflow.yml @@ -11,6 +11,7 @@ nodes: - depth - point_cloud - camera_info + - camera_state - id: plot git: https://github.com/dora-rs/dora-hub.git diff --git a/src/main.cc b/src/main.cc index 00880da..94dbfee 100644 --- a/src/main.cc +++ b/src/main.cc @@ -4,10 +4,12 @@ #include #include +#include #include #include #include #include +#include #include #include @@ -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 make_shape(int height, int width, int channels) { rust::Vec shape; shape.push_back(height); @@ -82,6 +106,35 @@ std::vector pack_depth(sl::Mat& depth_mat) { return out; } +std::chrono::seconds reopen_backoff(int attempt) { + const size_t index = + std::min(static_cast(attempt), std::size(REOPEN_BACKOFF_S)) - 1; + return std::chrono::seconds(REOPEN_BACKOFF_S[index]); +} + +double seconds_since(Clock::time_point since) { + return std::chrono::duration(Clock::now() - since).count(); +} + +// Release the camera and schedule the next open attempt. The first attempt only +// closes the handle, which is enough when the driver rather than the device lost +// its way; past that the device itself is wedged and nothing short of resetting +// it will bring the frames back. +// +// The wait is a deadline rather than a sleep: the loop returns to dora between +// attempts, so Stop is still honoured while the camera is down. +void begin_recovery(zed::Camera& camera, int attempt, Clock::time_point& next_attempt_at) { + if (attempt <= 1) { + camera.close(); + } else { + camera.reset(); + } + const auto wait = reopen_backoff(attempt); + std::cerr << "recovering camera (attempt " << attempt << "); next open in " << wait.count() + << "s" << std::endl; + next_attempt_at = Clock::now() + wait; +} + int64_t host_timestamp_ns() { return std::chrono::duration_cast( 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 state_slice{ + reinterpret_cast(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(); diff --git a/src/zed/camera.cc b/src/zed/camera.cc index ed52202..1e3b31d 100644 --- a/src/zed/camera.cc +++ b/src/zed/camera.cc @@ -1,12 +1,11 @@ #include "zed/camera.h" #include "util/env.h" -#include +#include #include #include #include #include -#include #include 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(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(); + + 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(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( 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 diff --git a/src/zed/camera.h b/src/zed/camera.h index 598dc48..f13be90 100644 --- a/src/zed/camera.h +++ b/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 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