409 lines
16 KiB
C++
409 lines
16 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/Logger.h"
|
|
|
|
#include <algorithm>
|
|
#include <atomic>
|
|
#include <chrono>
|
|
#include <memory>
|
|
#include <mutex>
|
|
#include <stdexcept>
|
|
#include <string>
|
|
#include <thread>
|
|
|
|
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 {
|
|
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};
|
|
};
|
|
|
|
// 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.channels = static_cast<int>(sz / (static_cast<size_t>(w) * h));
|
|
f.cam_id = cam_id_;
|
|
f.timestamp_ms = nowMs();
|
|
f.data.assign(buf, buf + sz);
|
|
{
|
|
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:
|
|
int cam_id_;
|
|
CamStats* stats_;
|
|
std::mutex mutex_;
|
|
Frame latest_;
|
|
bool has_ = false;
|
|
};
|
|
|
|
// Read a float/string feature, returning `fallback`/"" if absent (tolerant, for telemetry).
|
|
double getFloat(const CameraPtr& cam, const char* name, double fallback = 0.0) {
|
|
FeaturePtr f;
|
|
double v = fallback;
|
|
if (SP_ACCESS(cam)->GetFeatureByName(name, f) == VmbErrorSuccess && f->GetValue(v) == VmbErrorSuccess)
|
|
return v;
|
|
return fallback;
|
|
}
|
|
std::string getString(const CameraPtr& cam, const char* name) {
|
|
FeaturePtr f;
|
|
std::string v;
|
|
if (SP_ACCESS(cam)->GetFeatureByName(name, f) == VmbErrorSuccess && f->GetValue(v) == VmbErrorSuccess)
|
|
return v;
|
|
return {};
|
|
}
|
|
|
|
bool parseBool(const std::string& v) {
|
|
return v == "1" || v == "true" || v == "True" || v == "yes" || v == "on";
|
|
}
|
|
|
|
// Write one feature from a mode file, converting the string to whatever type the
|
|
// camera says the feature is. Mode files are hand-edited and cameras differ in
|
|
// which features they expose (and which are locked at a given moment), so a
|
|
// failure is a warning, never fatal - the warning IS the feedback that a mode
|
|
// file has a typo in it.
|
|
void applyFeature(const CameraPtr& cam, const std::string& name, const std::string& value) {
|
|
FeaturePtr f;
|
|
if (SP_ACCESS(cam)->GetFeatureByName(name.c_str(), f) != VmbErrorSuccess) {
|
|
LOG_WARN << "camera: no such feature '" << name << "' (skipped)";
|
|
return;
|
|
}
|
|
|
|
VmbFeatureDataType type = VmbFeatureDataUnknown;
|
|
if (f->GetDataType(type) != VmbErrorSuccess) {
|
|
LOG_WARN << "camera: cannot read type of feature '" << name << "' (skipped)";
|
|
return;
|
|
}
|
|
|
|
VmbErrorType rc = VmbErrorSuccess;
|
|
try {
|
|
switch (type) {
|
|
case VmbFeatureDataInt:
|
|
rc = f->SetValue(static_cast<VmbInt64_t>(std::stoll(value)));
|
|
break;
|
|
case VmbFeatureDataFloat:
|
|
rc = f->SetValue(std::stod(value));
|
|
break;
|
|
case VmbFeatureDataBool:
|
|
rc = f->SetValue(parseBool(value));
|
|
break;
|
|
case VmbFeatureDataEnum:
|
|
case VmbFeatureDataString:
|
|
rc = f->SetValue(value.c_str());
|
|
break;
|
|
default:
|
|
LOG_WARN << "camera: feature '" << name << "' is not settable from a mode file";
|
|
return;
|
|
}
|
|
} catch (const std::exception&) {
|
|
LOG_WARN << "camera: feature '" << name << "' rejected value '" << value
|
|
<< "' (wrong number format)";
|
|
return;
|
|
}
|
|
|
|
if (rc != VmbErrorSuccess)
|
|
LOG_WARN << "camera: could not set " << name << '=' << value << " (error " << rc << ')';
|
|
else
|
|
LOG_TRACE_CAT(LogCat::Camera) << "SET " << name << '=' << value;
|
|
}
|
|
|
|
// Apply a whole mode, IN FILE ORDER - GenICam selectors are stateful, so order is
|
|
// part of the meaning (see CameraMode.h).
|
|
void applyMode(const CameraPtr& cam, const CameraMode& mode) {
|
|
for (const auto& [name, value] : mode.features) applyFeature(cam, name, value);
|
|
LOG_INFO << "camera mode '" << mode.name << "' applied (" << mode.features.size()
|
|
<< " features)";
|
|
}
|
|
|
|
} // namespace
|
|
|
|
struct VimbaCameraSource::Impl {
|
|
explicit Impl(CameraConfig cfg) : config(std::move(cfg)), 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;
|
|
};
|
|
|
|
CameraConfig config;
|
|
CameraMode mode; // applied at start() and by setMode()
|
|
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;
|
|
|
|
// 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() {
|
|
observers.clear();
|
|
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();
|
|
}
|
|
// 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(CameraConfig config)
|
|
: impl_(std::make_unique<Impl>(std::move(config))) {}
|
|
|
|
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& id : impl_->config.ids) {
|
|
CameraPtr cam;
|
|
if (impl_->sys.GetCameraByID(id.c_str(), cam) != VmbErrorSuccess)
|
|
throw std::runtime_error("Camera not found: " + id);
|
|
if (cam->Open(VmbAccessModeFull) != VmbErrorSuccess)
|
|
throw std::runtime_error("Could not open camera: " + id);
|
|
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_->identity.push_back({id, getString(cam, "DeviceModelName"),
|
|
getString(cam, "DeviceSerialNumber")});
|
|
LOG_INFO << "Opened camera " << id << " (" << impl_->identity.back().model << ')';
|
|
}
|
|
}
|
|
|
|
void VimbaCameraSource::close() {
|
|
for (auto& cam : impl_->cameras) cam->Close();
|
|
impl_->cameras.clear();
|
|
impl_->sys.Shutdown();
|
|
}
|
|
|
|
void VimbaCameraSource::setInitialMode(CameraMode mode) { impl_->mode = std::move(mode); }
|
|
|
|
void VimbaCameraSource::start() {
|
|
for (auto& cam : impl_->cameras) applyMode(cam, impl_->mode);
|
|
impl_->startAcquisition();
|
|
impl_->started = true;
|
|
}
|
|
|
|
bool VimbaCameraSource::setMode(const CameraMode& mode) {
|
|
impl_->mode = mode;
|
|
if (!impl_->started) return true; // start() will apply it
|
|
|
|
// Binning, ROI and pixel format cannot be written while the stream runs, and a
|
|
// mode may change any of them, so always cycle the stream. Mode switches are
|
|
// rare; a per-feature "is this writable live" allow-list would be fragile.
|
|
try {
|
|
impl_->stopAcquisition();
|
|
for (auto& cam : impl_->cameras) applyMode(cam, impl_->mode);
|
|
impl_->startAcquisition();
|
|
} catch (const std::exception& e) {
|
|
LOG_ERROR << "camera: applying mode '" << mode.name << "' failed: " << e.what();
|
|
return false;
|
|
}
|
|
// The next trigger() must not hand back a frame exposed under the old mode.
|
|
impl_->last_captured_ts = nowMs();
|
|
return true;
|
|
}
|
|
|
|
void VimbaCameraSource::stop() {
|
|
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).
|
|
if (impl_->newestTs() <= impl_->last_captured_ts) {
|
|
LOG_TRACE_CAT(LogCat::Camera) << "stream stale; 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));
|
|
}
|
|
|
|
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;
|
|
}
|
|
}
|
|
if (!any) {
|
|
// The scheduler retries trigger() every tick until a frame is ready, so a brief
|
|
// warm-up (or a fully stalled stream) would otherwise flood the log. Rate-limit.
|
|
long long now = nowMs();
|
|
if (now - impl_->last_no_frame_warn_ms > 5000) {
|
|
LOG_WARN << "camera: no frame available to capture yet (stream warming up or stalled)";
|
|
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;
|
|
}
|
|
|
|
} // namespace fgc
|