fwt_software/src/camera/VimbaCameraSource.cpp

#include "fgc/VimbaCameraSource.h"

#include "fgc/Logger.h"

#include <atomic>
#include <chrono>
#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();
}

// Frame observer: dumps the first few frames after each trigger (sensor
// settling), then delivers completed frames to the source callback.
class FrameObserver : public IFrameObserver {
public:
    FrameObserver(CameraPtr camera, int cam_id, ICameraSource::FrameCallback cb)
        : IFrameObserver(camera), cam_id_(cam_id), cb_(std::move(cb)) {}

    void resetSettle() { settle_.store(3); }

    void FrameReceived(const FramePtr pFrame) override {
        if (settle_.fetch_sub(1) > 0) {  // still settling -> discard
            m_pCamera->QueueFrame(pFrame);
            return;
        }
        VmbFrameStatusType status;
        if (pFrame->GetReceiveStatus(status) == 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);
                LOG_TRACE_CAT(LogCat::Camera)
                    << "RX frame cam" << cam_id_ << ' ' << w << 'x' << h << ' ' << sz << 'B';
                if (cb_) cb_(f);
            }
        }
        m_pCamera->QueueFrame(pFrame);
    }

private:
    int                          cam_id_;
    ICameraSource::FrameCallback cb_;
    std::atomic<int>             settle_{3};
};

}  // namespace

struct VimbaCameraSource::Impl {
    explicit Impl(std::vector<std::string> ids)
        : camera_ids(std::move(ids)), sys(VmbSystem::GetInstance()) {}

    std::vector<std::string>          camera_ids;
    VmbSystem&                        sys;
    std::vector<CameraPtr>            cameras;
    std::vector<IFrameObserverPtr>    observers;
    ICameraSource::FrameCallback      callback;
    bool                              started = false;
};

VimbaCameraSource::VimbaCameraSource(std::vector<std::string> camera_ids)
    : impl_(std::make_unique<Impl>(std::move(camera_ids))) {}

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_->camera_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);
        LOG_INFO << "Opened camera " << id;
    }
}

void VimbaCameraSource::close() {
    for (auto& cam : impl_->cameras) cam->Close();
    impl_->cameras.clear();
    impl_->sys.Shutdown();
}

void VimbaCameraSource::start() {
    int cam_id = 0;
    for (auto& cam : impl_->cameras) {
        IFrameObserverPtr observer(new FrameObserver(cam, cam_id, impl_->callback));
        if (cam->StartContinuousImageAcquisition(5, observer) != VmbErrorSuccess)
            throw std::runtime_error("Could not start acquisition");
        impl_->observers.push_back(observer);
        ++cam_id;
    }
    impl_->started = true;
}

void VimbaCameraSource::stop() {
    for (auto& cam : impl_->cameras) cam->StopContinuousImageAcquisition();
    impl_->observers.clear();
    impl_->started = false;
}

bool VimbaCameraSource::trigger() {
    if (impl_->cameras.empty()) return false;
    LOG_TRACE_CAT(LogCat::Camera) << "TX trigger cam0";
    for (auto& obs : impl_->observers)
        SP_DYN_CAST<FrameObserver>(obs)->resetSettle();

    VmbErrorType result = VmbErrorSuccess;
    for (int i = 0; i < 4; ++i) {  // 3 settle frames + 1 real
        FeaturePtr feature;
        if (SP_ACCESS(impl_->cameras[0])->GetFeatureByName("TriggerSoftware", feature) ==
            VmbErrorSuccess)
            result = feature->RunCommand();
        std::this_thread::sleep_for(std::chrono::milliseconds(400));
    }
    return result == VmbErrorSuccess;
}

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) 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()); }

}  // namespace fgc