849 lines
38 KiB
C++
849 lines
38 KiB
C++
#include "fgc/VimbaCameraSource.h"
|
|
|
|
// The SDK comes first on purpose. <VmbCPP/VmbCPP.h> pulls in
|
|
// VmbCPP/LoggerDefines.h, which on some SDK versions (the 2024-era /opt/VimbaX on
|
|
// the LattePanda, though not 2025-3) defines a FUNCTION-like LOG_ERROR(txt,
|
|
// errCode). Ours is object-like - `LOG_ERROR << "..."` - so whichever header is
|
|
// included last wins the redefinition, and when theirs wins the stream form never
|
|
// expands and fails to compile. Take the SDK's macros, drop them, then define
|
|
// ours over the top. The #undefs are harmless no-ops on SDKs that lack them.
|
|
#include <VmbCPP/VmbCPP.h>
|
|
#undef LOG_ERROR
|
|
#undef LOG_FREE_TEXT
|
|
|
|
#include "fgc/CameraStall.h"
|
|
#include "fgc/Logger.h"
|
|
|
|
#include <algorithm>
|
|
#include <atomic>
|
|
#include <chrono>
|
|
#include <deque>
|
|
#include <memory>
|
|
#include <mutex>
|
|
#include <stdexcept>
|
|
#include <string>
|
|
#include <thread>
|
|
#include <utility>
|
|
#include <vector>
|
|
|
|
using namespace VmbCPP;
|
|
|
|
namespace fgc {
|
|
|
|
namespace {
|
|
|
|
long long nowMs() {
|
|
return std::chrono::duration_cast<std::chrono::milliseconds>(
|
|
std::chrono::system_clock::now().time_since_epoch())
|
|
.count();
|
|
}
|
|
|
|
// Cumulative per-camera acquisition stats. Lives in Impl (NOT the observer) so it
|
|
// survives the restart-on-stall, which recreates the observer each session.
|
|
struct CamStats {
|
|
// The last frames' brightness, exposure and gain, for the keep rule.
|
|
std::mutex recent_mutex;
|
|
std::deque<FrameStats> recent;
|
|
uint64_t seq = 0;
|
|
void pushRecent(const Frame& f) {
|
|
std::lock_guard<std::mutex> lock(recent_mutex);
|
|
recent.push_back(FrameStats{++seq, f.brightness.valid() ? f.brightness.mean : -1.0,
|
|
f.exposure_us, f.gain_db});
|
|
while (recent.size() > 64) recent.pop_front();
|
|
}
|
|
|
|
std::atomic<long long> delivered{0};
|
|
std::atomic<long long> dropped{0};
|
|
std::atomic<long long> restarts{0};
|
|
std::atomic<unsigned> last_w{0};
|
|
std::atomic<unsigned> last_h{0};
|
|
std::atomic<long long> last_bytes{0};
|
|
};
|
|
|
|
// The program's name for a delivered pixel format; Unknown for anything else.
|
|
PixelLayout layoutOf(VmbPixelFormatType f) {
|
|
switch (f) {
|
|
case VmbPixelFormatMono8: return PixelLayout::Mono8;
|
|
case VmbPixelFormatRgb8: return PixelLayout::RGB8;
|
|
case VmbPixelFormatBayerRG8: return PixelLayout::BayerRG8;
|
|
case VmbPixelFormatBayerRG12p: return PixelLayout::BayerRG12p;
|
|
// The older GigE cameras' 12 bits (Manta): packed differently, see RawImage.h.
|
|
case VmbPixelFormatBayerRG12Packed: return PixelLayout::BayerRG12Packed;
|
|
default: return PixelLayout::Unknown;
|
|
}
|
|
}
|
|
|
|
// Frame observer for the free-running stream: keeps the most recent COMPLETE
|
|
// frame. trigger() (once per scan waypoint) reads it out; the intermediate frames
|
|
// just give the on-camera auto-exposure/gain time to converge to the current
|
|
// light.
|
|
class FrameObserver : public IFrameObserver {
|
|
public:
|
|
FrameObserver(CameraPtr camera, int cam_id, CamStats* stats)
|
|
: IFrameObserver(camera), cam_id_(cam_id), stats_(stats) {}
|
|
|
|
// Copy out the latest complete frame; false if none has arrived yet.
|
|
bool takeLatest(Frame& out) {
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
if (!has_) return false;
|
|
out = latest_;
|
|
return true;
|
|
}
|
|
|
|
// Timestamp of the latest complete frame (0 if none) - lets the source detect a
|
|
// stalled stream (timestamp stops advancing) without copying the whole frame.
|
|
long long latestTimestamp() {
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
return has_ ? latest_.timestamp_ms : 0;
|
|
}
|
|
|
|
void FrameReceived(const FramePtr pFrame) override {
|
|
VmbFrameStatusType status;
|
|
VmbErrorType rc = pFrame->GetReceiveStatus(status);
|
|
if (rc == VmbErrorSuccess && status == VmbFrameStatusComplete) {
|
|
VmbUint32_t w = 0, h = 0, sz = 0;
|
|
const VmbUchar_t* buf = nullptr;
|
|
if (pFrame->GetWidth(w) == VmbErrorSuccess &&
|
|
pFrame->GetHeight(h) == VmbErrorSuccess &&
|
|
pFrame->GetBufferSize(sz) == VmbErrorSuccess &&
|
|
pFrame->GetBuffer(buf) == VmbErrorSuccess && buf && w && h) {
|
|
Frame f;
|
|
f.width = w;
|
|
f.height = h;
|
|
f.cam_id = cam_id_;
|
|
f.timestamp_ms = nowMs();
|
|
VmbPixelFormatType pf = VmbPixelFormatLast;
|
|
if (pFrame->GetPixelFormat(pf) == VmbErrorSuccess) f.layout = layoutOf(pf);
|
|
size_t bytes = sz;
|
|
if (f.layout != PixelLayout::Unknown) {
|
|
// The buffer can carry chunk data after the image; keep the image.
|
|
bytes = std::min<size_t>(sz, layoutFrameBytes(f.layout, w, h));
|
|
f.channels = f.layout == PixelLayout::RGB8 ? 3 : 1;
|
|
} else {
|
|
// A format the program does not handle: say what it holds, as
|
|
// before layouts existed, and let the pipeline refuse it.
|
|
f.channels = static_cast<int>(sz / (static_cast<size_t>(w) * h));
|
|
if (!warned_layout_) {
|
|
warned_layout_ = true;
|
|
LOG_WARN << "camera cam" << cam_id_ << ": pixel format 0x" << std::hex
|
|
<< pf << std::dec << " is not one the program handles "
|
|
<< "(Mono8, RGB8, BayerRG8, BayerRG12p, BayerRG12Packed)";
|
|
}
|
|
}
|
|
f.data.assign(buf, buf + bytes);
|
|
readChunks(pFrame, f);
|
|
f.brightness = sampleBrightness(f.data.data(), f.data.size(), f.layout, w, h);
|
|
if (stats_) stats_->pushRecent(f);
|
|
{
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
latest_ = std::move(f);
|
|
has_ = true;
|
|
}
|
|
if (stats_) {
|
|
stats_->delivered.fetch_add(1);
|
|
stats_->last_w.store(w);
|
|
stats_->last_h.store(h);
|
|
stats_->last_bytes.store(sz);
|
|
}
|
|
LOG_TRACE_CAT(LogCat::Camera)
|
|
<< "RX frame cam" << cam_id_ << ' ' << w << 'x' << h << ' ' << sz << 'B';
|
|
}
|
|
} else {
|
|
// Incomplete/invalid delivery - usually USB bandwidth starvation on large
|
|
// frames. Surface it instead of silently dropping.
|
|
if (stats_) stats_->dropped.fetch_add(1);
|
|
LOG_WARN << "camera cam" << cam_id_ << " dropped frame (status="
|
|
<< (rc == VmbErrorSuccess ? static_cast<int>(status) : -1) << ')';
|
|
}
|
|
m_pCamera->QueueFrame(pFrame);
|
|
}
|
|
|
|
private:
|
|
// Exposure, gain and white balance as the camera reports them for this
|
|
// frame. Only when the profile turns chunk mode on; after the first frame
|
|
// without chunk data, stop asking for the rest of the session.
|
|
void readChunks(const FramePtr& frame, Frame& f) {
|
|
if (!chunks_available_) return;
|
|
const VmbErrorType rc = frame->AccessChunkData([&f](ChunkFeatureContainerPtr& c) {
|
|
auto num = [&c](const char* name, double& out) {
|
|
FeaturePtr feat;
|
|
double v = 0;
|
|
if (c->GetFeatureByName(name, feat) == VmbErrorSuccess &&
|
|
feat->GetValue(v) == VmbErrorSuccess)
|
|
out = v;
|
|
};
|
|
num("ChunkExposureTime", f.exposure_us);
|
|
num("ChunkGain", f.gain_db);
|
|
num("ChunkBalanceRatioRed", f.wb_red);
|
|
num("ChunkBalanceRatioBlue", f.wb_blue);
|
|
return VmbErrorSuccess;
|
|
});
|
|
if (rc != VmbErrorSuccess) chunks_available_ = false;
|
|
}
|
|
|
|
int cam_id_;
|
|
CamStats* stats_;
|
|
bool warned_layout_ = false;
|
|
bool chunks_available_ = true;
|
|
std::mutex mutex_;
|
|
Frame latest_;
|
|
bool has_ = false;
|
|
};
|
|
|
|
// The older GigE cameras (Manta G-1236 on rietschen, firmware 00.01.54) predate
|
|
// the SFNC names the program uses and the Alvium answers to. Where a camera has
|
|
// no feature by the program's name, it is looked up by the older one. The
|
|
// IntensityController pair is the Alvium's auto-exposure target and tolerance;
|
|
// the Manta's ExposureAuto* pair means the same, in percent too.
|
|
const char* legacyFeatureName(const std::string& name) {
|
|
static const std::pair<const char*, const char*> kAliases[] = {
|
|
{"ExposureTime", "ExposureTimeAbs"},
|
|
{"AcquisitionFrameRate", "AcquisitionFrameRateAbs"},
|
|
{"IntensityControllerTarget", "ExposureAutoTarget"},
|
|
{"IntensityControllerTolerance", "ExposureAutoAdjustTol"},
|
|
};
|
|
for (const auto& [sfnc, legacy] : kAliases)
|
|
if (name == sfnc) return legacy;
|
|
return nullptr;
|
|
}
|
|
|
|
// A feature by the program's name, or by its older name on a camera that has only
|
|
// that one.
|
|
bool findFeature(const CameraPtr& cam, const std::string& name, FeaturePtr& f) {
|
|
if (SP_ACCESS(cam)->GetFeatureByName(name.c_str(), f) == VmbErrorSuccess) return true;
|
|
const char* legacy = legacyFeatureName(name);
|
|
return legacy && SP_ACCESS(cam)->GetFeatureByName(legacy, f) == VmbErrorSuccess;
|
|
}
|
|
|
|
// Read a numeric/string feature, returning `fallback`/"" if absent (tolerant, for
|
|
// telemetry). Numeric takes an integer feature too: some are Int on one model and
|
|
// Float on another (IntensityControllerTolerance is Int on the Alvium 1800 U-1240c).
|
|
double getFloat(const CameraPtr& cam, const char* name, double fallback = 0.0) {
|
|
FeaturePtr f;
|
|
if (!findFeature(cam, name, f)) return fallback;
|
|
double v = fallback;
|
|
if (f->GetValue(v) == VmbErrorSuccess) return v;
|
|
VmbInt64_t i = 0;
|
|
if (f->GetValue(i) == VmbErrorSuccess) return static_cast<double>(i);
|
|
return fallback;
|
|
}
|
|
std::string getString(const CameraPtr& cam, const char* name) {
|
|
FeaturePtr f;
|
|
std::string v;
|
|
if (findFeature(cam, name, f) && f->GetValue(v) == VmbErrorSuccess)
|
|
return v;
|
|
return {};
|
|
}
|
|
|
|
// Apply a profile by handing its settings XML to the SDK.
|
|
//
|
|
// This replaces a hand-rolled feature applier, and the reason is validity rather
|
|
// than brevity: GenICam writability is stateful (AcquisitionFrameRate is only
|
|
// writable once AcquisitionFrameRateEnable is set, Width's maximum depends on
|
|
// binning, and so on), so a text list applied line by line can be individually
|
|
// plausible and collectively wrong. The XML comes out of the VimbaX Viewer,
|
|
// which greys out invalid combinations as the operator configures, and
|
|
// LoadSettings replays exactly that. Nothing else may write settings.
|
|
bool applyProfile(const CameraPtr& cam, const CameraProfile& profile) {
|
|
VmbFeaturePersistSettings settings{};
|
|
settings.persistType = VmbFeaturePersistNoLUT;
|
|
// RemoteDevice only: the transport-layer and stream sections of the export
|
|
// describe the host that produced it, not the sensor.
|
|
settings.modulePersistFlags = VmbModulePersistFlagsRemoteDevice;
|
|
settings.maxIterations = 10; // features can need several passes to settle
|
|
settings.loggingLevel = VmbLogLevelWarn;
|
|
|
|
const VmbErrorType rc = SP_ACCESS(cam)->LoadSettings(profile.xml_path.c_str(), &settings);
|
|
if (rc != VmbErrorSuccess) {
|
|
LOG_ERROR << "camera: applying profile '" << profile.name << "' from "
|
|
<< profile.xml_path << " failed (error " << rc << ')';
|
|
return false;
|
|
}
|
|
LOG_INFO << "camera profile '" << profile.name << "' applied from " << profile.xml_path
|
|
<< " (" << profile.settings.entries.size() << " settings)";
|
|
return true;
|
|
}
|
|
|
|
} // namespace
|
|
|
|
struct VimbaCameraSource::Impl {
|
|
Impl(std::vector<CameraSpec> s, std::vector<CameraProfile> p)
|
|
: specs(std::move(s)), profiles(std::move(p)), sys(VmbSystem::GetInstance()) {}
|
|
|
|
struct Identity { std::string id, model, serial; };
|
|
struct Telemetry {
|
|
double exposure_us = 0, gain_db = 0, temperature_c = 0, actual_fps = 0;
|
|
};
|
|
|
|
// One spec and one profile per camera, index-aligned and in open order, so a
|
|
// camera's settings are never those of the camera next to it.
|
|
std::vector<CameraSpec> specs;
|
|
std::vector<CameraProfile> profiles;
|
|
VmbSystem& sys;
|
|
std::vector<CameraPtr> cameras;
|
|
std::vector<IFrameObserverPtr> observers;
|
|
std::vector<std::unique_ptr<CamStats>> stats; // one per camera; survives restart
|
|
std::vector<Identity> identity; // read once at open()
|
|
std::vector<Telemetry> telemetry; // live values, cached ~1 Hz
|
|
long long telemetry_ms = 0; // last telemetry refresh
|
|
ICameraSource::FrameCallback callback;
|
|
bool started = false;
|
|
long long last_captured_ts = 0; // freshness watermark
|
|
long long last_no_frame_warn_ms = 0;
|
|
long long session_start_ms = 0; // this acquisition session
|
|
int failed_restarts = 0; // since the last frame arrived
|
|
// Per camera: the value each feature held before `test camera` first changed
|
|
// it, in the order changed - so it can be put back live, without stopping
|
|
// the stream (which is what a profile reload has to do).
|
|
std::vector<std::vector<std::pair<std::string, std::string>>> touched;
|
|
|
|
// Begin/end a free-run acquisition session (fresh observers each time). Split out so
|
|
// trigger() can restart the stream when it stalls. Stats persist across restarts.
|
|
void startAcquisition() {
|
|
// Stop first, unconditionally. Clearing `observers` drops the last
|
|
// references to the FrameObserver objects, and until
|
|
// StopContinuousImageAcquisition() has returned the SDK's acquisition
|
|
// thread may still be calling FrameReceived on them - destroying them
|
|
// under it is a use-after-free, not a leak.
|
|
stopAcquisition();
|
|
// A fresh session has delivered nothing, so a watermark from the previous
|
|
// one would make every genuinely-new frame look stale to trigger().
|
|
last_captured_ts = 0;
|
|
session_start_ms = nowMs();
|
|
int cam_id = 0;
|
|
for (auto& cam : cameras) {
|
|
IFrameObserverPtr observer(new FrameObserver(cam, cam_id, stats[cam_id].get()));
|
|
if (cam->StartContinuousImageAcquisition(5, observer) != VmbErrorSuccess)
|
|
throw std::runtime_error("Could not start acquisition");
|
|
observers.push_back(observer);
|
|
++cam_id;
|
|
}
|
|
}
|
|
void stopAcquisition() {
|
|
for (auto& cam : cameras) cam->StopContinuousImageAcquisition();
|
|
observers.clear();
|
|
}
|
|
// Longest exposure any camera is set to, for how long a frame may take.
|
|
double longestExposureUs() {
|
|
double e = 0;
|
|
for (auto& cam : cameras) e = std::max(e, getFloat(cam, "ExposureTime"));
|
|
return e;
|
|
}
|
|
// Longest frame period any camera runs at (the rate it reports), 0 if unknown.
|
|
double slowestFramePeriodMs() {
|
|
double p = 0;
|
|
for (auto& cam : cameras) {
|
|
const double fps = getFloat(cam, "AcquisitionFrameRate");
|
|
if (fps > 0) p = std::max(p, 1000.0 / fps);
|
|
}
|
|
return p;
|
|
}
|
|
|
|
// Reboot one camera, reopen it by id and re-apply its profile. See
|
|
// CameraStall.h for why this exists.
|
|
bool resetOne(size_t idx) {
|
|
const std::string& id = specs[idx].id;
|
|
LOG_WARN << "camera '" << specs[idx].name << "': resetting it (DeviceReset)";
|
|
const bool was_started = started;
|
|
stopAcquisition();
|
|
FeaturePtr f;
|
|
if (SP_ACCESS(cameras[idx])->GetFeatureByName("DeviceReset", f) != VmbErrorSuccess ||
|
|
f->RunCommand() != VmbErrorSuccess) {
|
|
LOG_ERROR << "camera '" << specs[idx].name << "': DeviceReset is not available";
|
|
if (was_started) startAcquisition();
|
|
return false;
|
|
}
|
|
cameras[idx]->Close();
|
|
// It drops off the bus and re-enumerates: ~10 s for an Alvium 1800 U on
|
|
// tower 111. Reopen it by id as soon as it answers.
|
|
std::this_thread::sleep_for(std::chrono::seconds(3));
|
|
CameraPtr fresh;
|
|
const long long deadline = nowMs() + 45000;
|
|
bool opened = false;
|
|
while (!opened && nowMs() < deadline) {
|
|
CameraPtr c;
|
|
if (sys.GetCameraByID(id.c_str(), c) == VmbErrorSuccess &&
|
|
c->Open(VmbAccessModeFull) == VmbErrorSuccess) {
|
|
fresh = c;
|
|
opened = true;
|
|
} else {
|
|
std::this_thread::sleep_for(std::chrono::seconds(1));
|
|
}
|
|
}
|
|
if (!opened) {
|
|
LOG_ERROR << "camera '" << specs[idx].name << "' did not come back within 45 s of "
|
|
"a DeviceReset - it needs a power cycle";
|
|
return false;
|
|
}
|
|
cameras[idx] = fresh;
|
|
if (idx < touched.size()) touched[idx].clear();
|
|
applyProfile(fresh, profiles[idx]);
|
|
if (was_started) startAcquisition();
|
|
LOG_INFO << "camera '" << specs[idx].name << "' is back after a reset";
|
|
return true;
|
|
}
|
|
|
|
// Newest frame timestamp across all cameras (0 if none delivered yet).
|
|
long long newestTs() {
|
|
long long t = 0;
|
|
for (auto& obs : observers)
|
|
t = std::max(t, SP_DYN_CAST<FrameObserver>(obs)->latestTimestamp());
|
|
return t;
|
|
}
|
|
|
|
};
|
|
|
|
VimbaCameraSource::VimbaCameraSource(std::vector<CameraSpec> specs,
|
|
std::vector<CameraProfile> profiles)
|
|
: impl_(std::make_unique<Impl>(std::move(specs), std::move(profiles))) {}
|
|
|
|
VimbaCameraSource::~VimbaCameraSource() {
|
|
try {
|
|
stop();
|
|
close();
|
|
} catch (const std::exception& e) {
|
|
LOG_WARN << "VimbaCameraSource shutdown: " << e.what();
|
|
}
|
|
}
|
|
|
|
void VimbaCameraSource::open() {
|
|
if (impl_->sys.Startup() != VmbErrorSuccess)
|
|
throw std::runtime_error("Could not start Vimba X API");
|
|
for (const auto& spec : impl_->specs) {
|
|
const std::string& id = spec.id;
|
|
CameraPtr cam;
|
|
if (impl_->sys.GetCameraByID(id.c_str(), cam) != VmbErrorSuccess)
|
|
throw std::runtime_error("Camera '" + spec.name + "' not found: " + id);
|
|
if (const VmbErrorType rc = cam->Open(VmbAccessModeFull); rc != VmbErrorSuccess) {
|
|
std::string why;
|
|
if (rc == VmbErrorInvalidAccess || rc == VmbErrorBusy)
|
|
why = " - another process has it open (is the VimbaX Viewer still running?)";
|
|
else if (rc == VmbErrorNotFound)
|
|
why = " - it disappeared between discovery and open (USB reset?)";
|
|
throw std::runtime_error("Could not open camera '" + spec.name + "': " + id +
|
|
" (Vimba error " + std::to_string(rc) + ")" + why);
|
|
}
|
|
impl_->cameras.push_back(cam);
|
|
// Per-camera stats (persist across restart) + identity (read once here).
|
|
impl_->stats.push_back(std::make_unique<CamStats>());
|
|
impl_->telemetry.emplace_back();
|
|
impl_->touched.emplace_back();
|
|
impl_->identity.push_back({id, getString(cam, "DeviceModelName"),
|
|
getString(cam, "DeviceSerialNumber")});
|
|
LOG_INFO << "Opened camera '" << spec.name << "' " << id << " ("
|
|
<< impl_->identity.back().model << ')';
|
|
}
|
|
// Configure each camera ONCE, here. Settings are GenICam features that live on
|
|
// the device and survive a stop/start of acquisition, so re-applying them
|
|
// whenever the stream starts is both wasted work (thousands of features) and
|
|
// wrong in principle: it lets a request to *stream* alter the *configuration*.
|
|
// A scan frame and a manual snapshot must come off an identically configured
|
|
// camera, and the only way to guarantee that is to never touch the
|
|
// configuration in between.
|
|
for (size_t i = 0; i < impl_->cameras.size() && i < impl_->profiles.size(); ++i)
|
|
applyProfile(impl_->cameras[i], impl_->profiles[i]);
|
|
}
|
|
|
|
void VimbaCameraSource::close() {
|
|
for (auto& cam : impl_->cameras) cam->Close();
|
|
impl_->cameras.clear();
|
|
impl_->sys.Shutdown();
|
|
}
|
|
|
|
// Start/stop the free-running stream. Configuration is NOT touched here - that
|
|
// happened at open(), and changes only through setProfile().
|
|
void VimbaCameraSource::start() {
|
|
// Idempotent. Several callers reach here without knowing whether another
|
|
// already has - `start` (scan), `snapshot`, and a [Startup] mode = auto -
|
|
// and re-entering would restart acquisition under the SDK's feet.
|
|
if (impl_->started) return;
|
|
impl_->startAcquisition();
|
|
impl_->started = true;
|
|
}
|
|
|
|
bool VimbaCameraSource::setProfile(int camera_index, const CameraProfile& profile) {
|
|
const size_t idx = static_cast<size_t>(camera_index);
|
|
if (camera_index < 0 || idx >= impl_->cameras.size() || idx >= impl_->profiles.size()) {
|
|
LOG_WARN << "camera: setProfile for unknown camera index " << camera_index;
|
|
return false;
|
|
}
|
|
|
|
impl_->profiles[idx] = profile;
|
|
// A profile sets every feature, so nothing is left to put back by hand.
|
|
if (idx < impl_->touched.size()) impl_->touched[idx].clear();
|
|
if (!impl_->started) {
|
|
// Idle: every feature is writable, so apply directly. start() no longer
|
|
// applies profiles, so skipping this would leave the camera on the old one.
|
|
return applyProfile(impl_->cameras[idx], profile);
|
|
}
|
|
|
|
// Binning, ROI and pixel format cannot be written while the stream runs, and a
|
|
// profile may change any of them, so always cycle the stream. Profile switches
|
|
// are rare; a per-feature "is this writable live" allow-list would be fragile.
|
|
try {
|
|
impl_->stopAcquisition();
|
|
const bool ok = applyProfile(impl_->cameras[idx], profile);
|
|
impl_->startAcquisition();
|
|
if (!ok) return false;
|
|
} catch (const std::exception& e) {
|
|
LOG_ERROR << "camera: applying profile '" << profile.name << "' failed: " << e.what();
|
|
return false;
|
|
}
|
|
// The next trigger() must not hand back a frame exposed under the old profile.
|
|
impl_->last_captured_ts = nowMs();
|
|
return true;
|
|
}
|
|
|
|
void VimbaCameraSource::stop() {
|
|
if (!impl_->started) return;
|
|
impl_->stopAcquisition();
|
|
impl_->started = false;
|
|
}
|
|
|
|
bool VimbaCameraSource::trigger() {
|
|
// Deliver the freshest complete frame per waypoint. This host's USB3 stream
|
|
// reliably delivers the first frames of a session but stalls after a burst; if
|
|
// no frame newer than the last capture is available, the stream has stalled -
|
|
// restart acquisition to unstick it and wait briefly for a fresh frame. Without
|
|
// this, keep-latest would hand the same stale frame to every remaining waypoint
|
|
// (all overwriting one file).
|
|
const long long newest = impl_->newestTs();
|
|
|
|
// A session that has delivered NOTHING is warming up, not stalled. These are
|
|
// different states that both fail the "no newer frame" test, and conflating
|
|
// them is expensive: restarting throws away the warm-up and blocks the control
|
|
// thread for 3 s, so a cold stream would be repeatedly reset just as it was
|
|
// about to produce its first frame. Return empty-handed instead; the caller
|
|
// retries every tick.
|
|
//
|
|
// But not forever. A session that never delivers is not warming up, and it
|
|
// used never to be restarted at all: tower 111's camera sat like that for 9
|
|
// minutes on 2026-09-24, capturing nothing and saying only "warming up".
|
|
// CameraStall.h decides when waiting has become a stall, allowing for long
|
|
// exposures, and when restarting has stopped helping.
|
|
if (newest == 0) {
|
|
const long long now = nowMs();
|
|
const long long waited = now - impl_->session_start_ms;
|
|
const double exposure = impl_->longestExposureUs();
|
|
switch (onNoFirstFrame(waited, exposure, impl_->failed_restarts)) {
|
|
case StallAction::Wait:
|
|
if (now - impl_->last_no_frame_warn_ms > 5000) {
|
|
LOG_WARN << "camera: no frame yet (stream warming up)";
|
|
impl_->last_no_frame_warn_ms = now;
|
|
}
|
|
break;
|
|
case StallAction::Restart:
|
|
LOG_WARN << "camera: no frame at all " << waited / 1000
|
|
<< " s into the stream; restarting it";
|
|
++impl_->failed_restarts;
|
|
for (auto& st : impl_->stats) st->restarts.fetch_add(1);
|
|
try {
|
|
impl_->stopAcquisition();
|
|
impl_->startAcquisition();
|
|
} catch (const std::exception& e) {
|
|
LOG_WARN << "camera: acquisition restart failed: " << e.what();
|
|
}
|
|
break;
|
|
case StallAction::Reset:
|
|
LOG_ERROR << "camera: still no frame after " << impl_->failed_restarts
|
|
<< " stream restarts; resetting the camera";
|
|
for (size_t i = 0; i < impl_->cameras.size(); ++i) impl_->resetOne(i);
|
|
impl_->failed_restarts = 0;
|
|
break;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
if (newest <= impl_->last_captured_ts) {
|
|
// Nothing newer than the last frame kept - but with long exposures the
|
|
// next one may simply not be due yet. Restarting now would throw away the
|
|
// exposure in progress, so wait while that is still plausible.
|
|
const long long since_newest = nowMs() - newest;
|
|
if (onStaleFrame(since_newest, impl_->longestExposureUs(),
|
|
impl_->slowestFramePeriodMs()) == StallAction::Wait)
|
|
return false;
|
|
LOG_TRACE_CAT(LogCat::Camera) << "stream stale (" << since_newest
|
|
<< " ms since the newest frame); restarting acquisition";
|
|
for (auto& s : impl_->stats) s->restarts.fetch_add(1);
|
|
try {
|
|
impl_->stopAcquisition();
|
|
impl_->startAcquisition();
|
|
} catch (const std::exception& e) {
|
|
LOG_WARN << "camera: acquisition restart failed: " << e.what();
|
|
}
|
|
const long long deadline = nowMs() + 3000;
|
|
while (impl_->newestTs() <= impl_->last_captured_ts && nowMs() < deadline)
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
|
if (impl_->newestTs() <= impl_->last_captured_ts &&
|
|
onRestartFailed(++impl_->failed_restarts) == StallAction::Reset) {
|
|
LOG_ERROR << "camera: the stream stays stale after " << impl_->failed_restarts
|
|
<< " restarts; resetting the camera";
|
|
for (size_t i = 0; i < impl_->cameras.size(); ++i) impl_->resetOne(i);
|
|
impl_->failed_restarts = 0;
|
|
}
|
|
}
|
|
|
|
bool any = false;
|
|
for (auto& obs : impl_->observers) {
|
|
Frame f;
|
|
if (SP_DYN_CAST<FrameObserver>(obs)->takeLatest(f)) {
|
|
LOG_TRACE_CAT(LogCat::Camera) << "capture cam" << f.cam_id << " ts=" << f.timestamp_ms;
|
|
impl_->last_captured_ts = std::max(impl_->last_captured_ts, f.timestamp_ms);
|
|
if (impl_->callback) impl_->callback(f);
|
|
any = true;
|
|
impl_->failed_restarts = 0;
|
|
}
|
|
}
|
|
if (!any) {
|
|
// Frames HAVE arrived this session (the newest == 0 case returned above), so
|
|
// this is the restart-did-not-help path. The scheduler retries every tick,
|
|
// so rate-limit.
|
|
long long now = nowMs();
|
|
if (now - impl_->last_no_frame_warn_ms > 5000) {
|
|
LOG_WARN << "camera: stream stalled and did not recover after a restart";
|
|
impl_->last_no_frame_warn_ms = now;
|
|
}
|
|
}
|
|
return any;
|
|
}
|
|
|
|
void VimbaCameraSource::setFrameCallback(FrameCallback cb) { impl_->callback = std::move(cb); }
|
|
|
|
int VimbaCameraSource::cameraCount() const { return static_cast<int>(impl_->cameras.size()); }
|
|
|
|
std::vector<CameraDeviceInfo> VimbaCameraSource::deviceInfo() {
|
|
// Refresh the live sensor telemetry at most ~1 Hz - deviceInfo() is called every UI
|
|
// tick (~10 Hz) but runs on the control thread (serialized with trigger()), and reading
|
|
// camera features is not free. Identity + stats come from cached/atomic state.
|
|
const long long now = nowMs();
|
|
if (now - impl_->telemetry_ms > 1000) {
|
|
impl_->telemetry_ms = now;
|
|
for (size_t i = 0; i < impl_->cameras.size(); ++i) {
|
|
auto& cam = impl_->cameras[i];
|
|
auto& t = impl_->telemetry[i];
|
|
t.exposure_us = getFloat(cam, "ExposureTime");
|
|
t.gain_db = getFloat(cam, "Gain");
|
|
t.temperature_c = getFloat(cam, "DeviceTemperature");
|
|
t.actual_fps = getFloat(cam, "AcquisitionFrameRate");
|
|
}
|
|
}
|
|
|
|
std::vector<CameraDeviceInfo> out;
|
|
out.reserve(impl_->cameras.size());
|
|
for (size_t i = 0; i < impl_->cameras.size(); ++i) {
|
|
const CamStats& s = *impl_->stats[i];
|
|
const Impl::Telemetry& t = impl_->telemetry[i];
|
|
CameraDeviceInfo d;
|
|
d.id = impl_->identity[i].id;
|
|
d.model = impl_->identity[i].model;
|
|
d.serial = impl_->identity[i].serial;
|
|
d.streaming = impl_->started && !impl_->observers.empty();
|
|
d.width = s.last_w.load();
|
|
d.height = s.last_h.load();
|
|
d.payload_bytes = s.last_bytes.load();
|
|
d.exposure_us = t.exposure_us;
|
|
d.gain_db = t.gain_db;
|
|
d.temperature_c = t.temperature_c;
|
|
d.actual_fps = t.actual_fps;
|
|
d.frames_delivered = s.delivered.load();
|
|
d.frames_dropped = s.dropped.load();
|
|
d.restarts = s.restarts.load();
|
|
out.push_back(std::move(d));
|
|
}
|
|
return out;
|
|
}
|
|
|
|
// ---- `test camera` -------------------------------------------------------------
|
|
|
|
bool VimbaCameraSource::grabFrame(int camera_index, Frame& out, int timeout_ms) {
|
|
const size_t idx = static_cast<size_t>(camera_index);
|
|
if (camera_index < 0 || idx >= impl_->observers.size()) return false; // not streaming
|
|
auto* obs = SP_DYN_CAST<FrameObserver>(impl_->observers[idx]).get();
|
|
// Strictly newer than now: a frame that was already waiting was exposed
|
|
// before the caller asked, possibly under settings it has since changed.
|
|
const long long since = nowMs();
|
|
const long long deadline = since + timeout_ms;
|
|
while (nowMs() < deadline) {
|
|
if (obs->latestTimestamp() > since) return obs->takeLatest(out);
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
|
}
|
|
return false;
|
|
}
|
|
|
|
bool VimbaCameraSource::setFeature(int camera_index, const std::string& name,
|
|
const std::string& value) {
|
|
const size_t idx = static_cast<size_t>(camera_index);
|
|
if (camera_index < 0 || idx >= impl_->cameras.size()) return false;
|
|
FeaturePtr f;
|
|
VmbFeatureDataType type = VmbFeatureDataUnknown;
|
|
if (!findFeature(impl_->cameras[idx], name, f) || f->GetDataType(type) != VmbErrorSuccess)
|
|
return false;
|
|
// The value before the first change, so restoreProfile() can put it back
|
|
// live. Only the first: a second change must not record the test's own value.
|
|
auto& mine = impl_->touched[idx];
|
|
const bool seen = std::any_of(mine.begin(), mine.end(),
|
|
[&](const std::pair<std::string, std::string>& t) {
|
|
return t.first == name;
|
|
});
|
|
if (!seen) {
|
|
std::string before;
|
|
switch (type) {
|
|
case VmbFeatureDataFloat: {
|
|
double v = 0;
|
|
if (f->GetValue(v) == VmbErrorSuccess) {
|
|
char b[40]; std::snprintf(b, sizeof(b), "%.17g", v); before = b;
|
|
}
|
|
break;
|
|
}
|
|
case VmbFeatureDataInt: {
|
|
VmbInt64_t v = 0;
|
|
if (f->GetValue(v) == VmbErrorSuccess) before = std::to_string(v);
|
|
break;
|
|
}
|
|
case VmbFeatureDataBool: {
|
|
bool v = false;
|
|
if (f->GetValue(v) == VmbErrorSuccess) before = v ? "true" : "false";
|
|
break;
|
|
}
|
|
case VmbFeatureDataEnum:
|
|
case VmbFeatureDataString: f->GetValue(before); break;
|
|
default: break;
|
|
}
|
|
if (!before.empty()) mine.emplace_back(name, before);
|
|
}
|
|
auto write = [&]() -> VmbErrorType {
|
|
try {
|
|
switch (type) {
|
|
case VmbFeatureDataFloat: return f->SetValue(std::stod(value));
|
|
case VmbFeatureDataInt: return f->SetValue(static_cast<VmbInt64_t>(std::stoll(value)));
|
|
case VmbFeatureDataBool: return f->SetValue(value == "true" || value == "1");
|
|
case VmbFeatureDataEnum:
|
|
case VmbFeatureDataString: return f->SetValue(value.c_str());
|
|
default: return VmbErrorWrongType;
|
|
}
|
|
} catch (const std::exception&) {
|
|
return VmbErrorBadParameter; // text that is not a number for a numeric feature
|
|
}
|
|
};
|
|
VmbErrorType rc = write();
|
|
// Some features cannot change while the camera streams - BlackLevel on the
|
|
// Alvium 1800 answers InvalidAccess (measured on tower 121) where ExposureTime
|
|
// and Gain are fine. Stop, write, and start again, as setProfile() does.
|
|
if (rc == VmbErrorInvalidAccess && impl_->started) {
|
|
try {
|
|
impl_->stopAcquisition();
|
|
rc = write();
|
|
impl_->startAcquisition();
|
|
} catch (const std::exception& e) {
|
|
LOG_WARN << "camera: restarting the stream around " << name << " failed: " << e.what();
|
|
rc = VmbErrorInternalFault;
|
|
}
|
|
}
|
|
if (rc != VmbErrorSuccess) {
|
|
LOG_WARN << "camera: setting " << name << " = " << value << " failed (error " << rc << ')';
|
|
return false;
|
|
}
|
|
LOG_DEBUG << "camera cam" << camera_index << ": " << name << " = " << value;
|
|
return true;
|
|
}
|
|
|
|
uint64_t VimbaCameraSource::frameSeq() {
|
|
if (impl_->stats.empty()) return 0;
|
|
std::lock_guard<std::mutex> lock(impl_->stats[0]->recent_mutex);
|
|
return impl_->stats[0]->seq;
|
|
}
|
|
|
|
std::vector<FrameStats> VimbaCameraSource::framesSince(uint64_t after) {
|
|
std::vector<FrameStats> out;
|
|
if (impl_->stats.empty()) return out;
|
|
std::lock_guard<std::mutex> lock(impl_->stats[0]->recent_mutex);
|
|
for (const auto& f : impl_->stats[0]->recent)
|
|
if (f.seq > after) out.push_back(f);
|
|
return out;
|
|
}
|
|
|
|
bool VimbaCameraSource::readFeature(int camera_index, const std::string& name, double& value) {
|
|
const size_t idx = static_cast<size_t>(camera_index);
|
|
if (camera_index < 0 || idx >= impl_->cameras.size()) return false;
|
|
FeaturePtr f;
|
|
VmbFeatureDataType type = VmbFeatureDataUnknown;
|
|
if (!findFeature(impl_->cameras[idx], name, f) || f->GetDataType(type) != VmbErrorSuccess)
|
|
return false;
|
|
if (type == VmbFeatureDataFloat) return f->GetValue(value) == VmbErrorSuccess;
|
|
if (type == VmbFeatureDataInt) {
|
|
VmbInt64_t v = 0;
|
|
if (f->GetValue(v) != VmbErrorSuccess) return false;
|
|
value = static_cast<double>(v);
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
bool VimbaCameraSource::featureRange(int camera_index, const std::string& name, double& min,
|
|
double& max) {
|
|
const size_t idx = static_cast<size_t>(camera_index);
|
|
if (camera_index < 0 || idx >= impl_->cameras.size()) return false;
|
|
FeaturePtr f;
|
|
VmbFeatureDataType type = VmbFeatureDataUnknown;
|
|
if (!findFeature(impl_->cameras[idx], name, f) || f->GetDataType(type) != VmbErrorSuccess)
|
|
return false;
|
|
if (type == VmbFeatureDataFloat) return f->GetRange(min, max) == VmbErrorSuccess;
|
|
if (type == VmbFeatureDataInt) {
|
|
VmbInt64_t a = 0, b = 0;
|
|
if (f->GetRange(a, b) != VmbErrorSuccess) return false;
|
|
min = static_cast<double>(a);
|
|
max = static_cast<double>(b);
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
bool VimbaCameraSource::restoreProfile(int camera_index) {
|
|
const size_t idx = static_cast<size_t>(camera_index);
|
|
if (camera_index < 0 || idx >= impl_->profiles.size()) return false;
|
|
|
|
// Put back what setFeature() changed, live and in reverse order - values
|
|
// first, then the auto modes that take over from them. No stream restart:
|
|
// stopping and starting it repeatedly is exactly what preceded tower 111's
|
|
// camera getting stuck (known-issues #15).
|
|
auto undo = std::move(impl_->touched[idx]);
|
|
impl_->touched[idx].clear();
|
|
bool ok = !undo.empty();
|
|
for (auto it = undo.rbegin(); ok && it != undo.rend(); ++it) {
|
|
FeaturePtr f;
|
|
VmbFeatureDataType type = VmbFeatureDataUnknown;
|
|
if (!findFeature(impl_->cameras[idx], it->first, f) ||
|
|
f->GetDataType(type) != VmbErrorSuccess) { ok = false; break; }
|
|
VmbErrorType rc = VmbErrorWrongType;
|
|
try {
|
|
switch (type) {
|
|
case VmbFeatureDataFloat: rc = f->SetValue(std::stod(it->second)); break;
|
|
case VmbFeatureDataInt: rc = f->SetValue(static_cast<VmbInt64_t>(std::stoll(it->second))); break;
|
|
case VmbFeatureDataBool: rc = f->SetValue(it->second == "true"); break;
|
|
case VmbFeatureDataEnum:
|
|
case VmbFeatureDataString: rc = f->SetValue(it->second.c_str()); break;
|
|
default: break;
|
|
}
|
|
} catch (const std::exception&) { rc = VmbErrorBadParameter; }
|
|
ok = rc == VmbErrorSuccess;
|
|
}
|
|
if (ok) {
|
|
LOG_INFO << "camera: " << undo.size() << " setting(s) put back live";
|
|
return true;
|
|
}
|
|
// Nothing recorded, or a write refused: the whole profile, the slow way.
|
|
// setProfile() cycles the stream when one is running, since a profile may
|
|
// carry features that are not writable live.
|
|
return setProfile(camera_index, impl_->profiles[idx]);
|
|
}
|
|
|
|
bool VimbaCameraSource::resetCamera(int camera_index) {
|
|
const size_t idx = static_cast<size_t>(camera_index);
|
|
if (camera_index < 0 || idx >= impl_->cameras.size()) return false;
|
|
const bool ok = impl_->resetOne(idx);
|
|
impl_->failed_restarts = 0;
|
|
return ok;
|
|
}
|
|
|
|
} // namespace fgc
|