liria/dora-zed
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 3 Wiki Activity

fix the camera blocking the dora event loop by moving it to its own thread
validate / validate (push) Successful in 59m1s
Details
validate / validate (pull_request) Successful in 59m2s
Details

Browse Source 
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
This commit is contained in:
cesar
2026-09-08 12:04:35 -03:00
co-authored by Claude Opus 5
parent 2f55ca3f74
commit 5822a870bb
8 changed files with 498 additions and 287 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main.cc
+81 -282
View File
@@ -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;
}