GGS
/
fwt_software
8
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
fwt_software/src/camera/VimbaCameraSource.cpp

418 lines
17 KiB
C++
Raw Blame History

#include "fgc/VimbaCameraSource.h"
#include "fgc/Logger.h"
#include <algorithm>
#include <atomic>
#include <chrono>
#include <memory>
#include <mutex>
#include <stdexcept>
#include <thread>
#include <VmbCPP/VmbCPP.h>
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 a paced 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/white-balance time to converge to the
// current light. This avoids the software-trigger path, which stalls on this host.
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;
};
// Feature setters: log (but tolerate) failures - cameras vary in which features they
// expose, so a missing/locked feature is a warning, not fatal.
void setEnum(const CameraPtr& cam, const char* name, const char* value) {
FeaturePtr f;
if (SP_ACCESS(cam)->GetFeatureByName(name, f) == VmbErrorSuccess &&
f->SetValue(value) == VmbErrorSuccess)
return;
LOG_WARN << "camera: could not set " << name << '=' << value;
}
void setInt(const CameraPtr& cam, const char* name, VmbInt64_t value) {
FeaturePtr f;
if (SP_ACCESS(cam)->GetFeatureByName(name, f) == VmbErrorSuccess &&
f->SetValue(value) == VmbErrorSuccess)
return;
LOG_WARN << "camera: could not set " << name << '=' << value;
}
void setFloat(const CameraPtr& cam, const char* name, double value) {
FeaturePtr f;
if (SP_ACCESS(cam)->GetFeatureByName(name, f) == VmbErrorSuccess &&
f->SetValue(value) == VmbErrorSuccess)
return;
LOG_WARN << "camera: could not set " << name << '=' << value;
}
void setBool(const CameraPtr& cam, const char* name, bool value) {
FeaturePtr f;
if (SP_ACCESS(cam)->GetFeatureByName(name, f) == VmbErrorSuccess &&
f->SetValue(value) == VmbErrorSuccess)
return;
LOG_WARN << "camera: could not set " << name << '=' << (value ? "true" : "false");
}
VmbInt64_t featureValue(const CameraPtr& cam, const char* name, VmbInt64_t fallback) {
FeaturePtr f;
VmbInt64_t v = fallback;
if (SP_ACCESS(cam)->GetFeatureByName(name, f) == VmbErrorSuccess && f->GetValue(v) == VmbErrorSuccess)
return v;
return fallback;
}
// 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 {};
}
// White-balance ratio for a colour channel ("Red"/"Blue"): select then read.
double wbRatio(const CameraPtr& cam, const char* channel) {
FeaturePtr sel;
if (SP_ACCESS(cam)->GetFeatureByName("BalanceRatioSelector", sel) == VmbErrorSuccess)
sel->SetValue(channel);
return getFloat(cam, "BalanceRatio", 0.0);
}
// Apply the config-driven imaging settings, in-session. Order per the Alvium user
// guide: ROI/binning -> throughput -> exposure. We drive a PACED FREE-RUN stream
// (TriggerMode=Off + a fixed AcquisitionFrameRate), NOT software trigger: large
// frames streamed fast stall this USB3 host, but a low paced rate sustains and keeps
// the on-camera auto adjustments converged. (TriggerMode for FrameStart is read-only
// while AcquisitionFrameRateEnable is true, so trigger must be turned off first.)
void configureCamera(const CameraPtr& cam, const CameraConfig& c) {
setInt(cam, "BinningHorizontal", c.binning);
setInt(cam, "BinningVertical", c.binning);
setInt(cam, "OffsetX", 0);
setInt(cam, "OffsetY", 0);
setInt(cam, "Width", c.width > 0 ? c.width : featureValue(cam, "WidthMax", 0));
setInt(cam, "Height", c.height > 0 ? c.height : featureValue(cam, "HeightMax", 0));
if (c.offset_x > 0) setInt(cam, "OffsetX", c.offset_x);
if (c.offset_y > 0) setInt(cam, "OffsetY", c.offset_y);
setEnum(cam, "PixelFormat", c.pixel_format.c_str());
setEnum(cam, "DeviceLinkThroughputLimitMode", "On");
setInt(cam, "DeviceLinkThroughputLimit", static_cast<VmbInt64_t>(c.throughput_mbytes) * 1000000);
setEnum(cam, "TriggerSelector", "FrameStart");
// AcquisitionFrameRateEnable=On makes TriggerMode (FrameStart) read-only. If the camera was
// left with the rate enabled (or in software-trigger mode from a previous run), TriggerMode=Off
// would be rejected and we'd get NO free-run frames. So drop the rate control first to unlock
// TriggerMode, force free-run, then re-enable the paced rate.
setBool(cam, "AcquisitionFrameRateEnable", false);
setEnum(cam, "TriggerMode", "Off"); // free-run
setEnum(cam, "AcquisitionMode", "Continuous");
setBool(cam, "AcquisitionFrameRateEnable", true);
setFloat(cam, "AcquisitionFrameRate", c.stream_fps);
setEnum(cam, "ExposureAuto", c.exposure_auto ? "Continuous" : "Off");
if (c.exposure_max_us > 0.0) setFloat(cam, "ExposureAutoMax", c.exposure_max_us);
setEnum(cam, "GainAuto", c.gain_auto ? "Continuous" : "Off");
if (c.gain_max_db > 0.0) setFloat(cam, "GainAutoMax", c.gain_max_db);
setEnum(cam, "BalanceWhiteAuto", c.white_balance_auto ? "Continuous" : "Off");
LOG_INFO << "camera configured: binning=" << c.binning << " fmt=" << c.pixel_format
<< " " << c.stream_fps << "fps throughput=" << c.throughput_mbytes << "MB/s"
<< " autoExp=" << c.exposure_auto << " autoWB=" << c.white_balance_auto;
}
} // 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, wb_red = 0, wb_blue = 0, temperature_c = 0, actual_fps = 0;
};
CameraConfig config;
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::start() {
for (auto& cam : impl_->cameras) configureCamera(cam, impl_->config);
impl_->startAcquisition();
impl_->started = true;
}
void VimbaCameraSource::stop() {
impl_->stopAcquisition();
impl_->started = false;
}
bool VimbaCameraSource::trigger() {
// Paced free-run: 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;
}
bool VimbaCameraSource::setFrameRate(double fps) {
VmbErrorType result = VmbErrorSuccess;
for (auto& cam : impl_->cameras) {
FeaturePtr feature;
if (SP_ACCESS(cam)->GetFeatureByName("AcquisitionFrameRate", feature) == VmbErrorSuccess)
result = feature->SetValue(fps);
}
if (result == VmbErrorSuccess) {
impl_->config.stream_fps = fps;
LOG_INFO << "camera fps set to " << fps;
}
return result == VmbErrorSuccess;
}
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");
t.wb_red = wbRatio(cam, "Red");
t.wb_blue = wbRatio(cam, "Blue");
}
}
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.wb_red = t.wb_red;
d.wb_blue = t.wb_blue;
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
Reference in New Issue View Git Blame Copy Permalink