v0.2.0: pure camera source on dora 1.0 #18
@@ -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:`.
|
||||
|
||||
@@ -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.
|
||||
|
||||
@@ -11,6 +11,7 @@ nodes:
|
||||
- depth
|
||||
- point_cloud
|
||||
- camera_info
|
||||
- camera_state
|
||||
|
||||
- id: plot
|
||||
git: https://github.com/dora-rs/dora-hub.git
|
||||
|
||||
+141
-9
@@ -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,14 +206,63 @@ int main() {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (!camera.grab()) {
|
||||
std::cerr << "Grab failed; attempting to reconnect..." << std::endl;
|
||||
camera.close();
|
||||
camera.open();
|
||||
calib = camera.calibration();
|
||||
camera_info_pending = true;
|
||||
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();
|
||||
|
||||
+115
-16
@@ -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) {
|
||||
bool Camera::open() {
|
||||
sl::ERROR_CODE err = zed_.open(init_params_);
|
||||
if (err == sl::ERROR_CODE::SUCCESS) {
|
||||
return;
|
||||
}
|
||||
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
|
||||
|
||||
+18
-4
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user