ImaqGrabber.cpp

Go to the documentation of this file.

#include <ImaqGrabber.h>

#include <utils.h>
#include <stringtricks.h>
#include <RocLogger.h>
#include <GrabberStatsPublisher.h>

/* c++11 std lib additions */
#include <unordered_set>
#include <functional>
#include <chrono>

/* for development */
#include <iostream>

//#define DEBUG

using namespace std;

using std::chrono::high_resolution_clock;
using std::chrono::milliseconds;

ImaqException::ImaqException(const std::string & _message) {
    message = _message;
}
const char* ImaqException::what() const throw() {
    return message.c_str();
}

ImaqException::~ImaqException() throw() {

}

static Int32 handleImaqError(Int32 err) {
    if(err != 0) {
        char temp[256];
        imgShowError(err, temp);
        throw ImaqException(temp);
    }
    return err;
}

_ImaqGrabberImpl::_ImaqGrabberImpl(const std::string & _deviceName, const std::string & _deviceSource, zmq::context_t & _ctx):
            deviceName(_deviceName),
            deviceSource(_deviceSource),
            ifid(0),
            sid(0),
            interfaceOpen(false),
            sessionOpen(false),
            openBuffers(0),
            totalBufferUsers(0),
            ctx(_ctx) {
    handleImaqError(imgInterfaceOpen(deviceSource.c_str(), &ifid));
    interfaceOpen = true;
    handleImaqError(imgInterfaceReset(ifid));
    handleImaqError(imgSessionOpen(ifid, &sid));
    sessionOpen = true;
    (void) getWidth(); /* dummy request to ensure that communication is working */
}
_ImaqGrabberImpl::~_ImaqGrabberImpl() {
    if(sessionOpen) {
        imgClose(sid, 1);
        sessionOpen = false;
    }
    if(interfaceOpen) {
        imgClose(ifid, 1);
        interfaceOpen = false;
    }
}
int _ImaqGrabberImpl::getInteger(uInt32 attr) {
    Int32 temp;
    handleImaqError(imgGetAttribute(sid, attr, &temp));
    return temp;
}
int _ImaqGrabberImpl::getWidth() {
    return getInteger(IMG_ATTR_ROI_WIDTH);
}
int _ImaqGrabberImpl::getHeight() {
    return getInteger(IMG_ATTR_ROI_HEIGHT);
}
int _ImaqGrabberImpl::getBytesPerPixel() {
    return getInteger(IMG_ATTR_BYTESPERPIXEL);
}
    
void _ImaqGrabberImpl::startServing() {
    enableAcquisition = true;
    acquisitionThread = thread(&_ImaqGrabberImpl::acquisition, this);
    setThreadPriority(acquisitionThread, REALTIME);
}
    
void _ImaqGrabberImpl::stopServing() {
    enableAcquisition  = false;
    if(acquisitionThread.joinable()) {
        cout << "wait for grabber thread to join" << endl;
        acquisitionThread.join();
    }
}
    
void _ImaqGrabberImpl::addEndpoint(const string & _endpoint) {
    endpoints.push_back(_endpoint);
}
    
        
/***** INTERNAL *****/

#define IMAQ_BUFFER_COUNT (256)

void _ImaqGrabberImpl::acquisition() {
    RocLogger logger(ctx, "inproc://log", "camserver", SSTR("grabber.", deviceName));
    
    zmq::socket_t pubSocket(ctx, ZMQ_PUB);
    for(const auto & ep: endpoints) {
        pubSocket.bind(ep.c_str());
    }
    int width = getWidth();
    int height = getHeight();
    int bytesPerPixel = getBytesPerPixel();
    char * bufferList[IMAQ_BUFFER_COUNT];
    bufferUsers.resize(IMAQ_BUFFER_COUNT);

    GrabberStatsPublisher statsPublisher(ctx, deviceName);

    for(int i = 0; i < IMAQ_BUFFER_COUNT; ++i) {
        bufferList[i] = new char[width * height * bytesPerPixel];
        assert(bufferList[i] != nullptr);
        bufferUsers[i] = new atomic<int>(0);
        assert(bufferUsers[i] != nullptr);
    }
    try {            
        handleImaqError(imgGrabSetup(sid, 0 /*startNow*/));
#ifdef DEBUG
        cout << "grab initialized" << endl;
#endif
        logger.debug(SSTR("grab initialized"));
        handleImaqError(imgSessionStartAcquisition(sid));
        
        int currentBuffer = 0;
        auto t1 = chrono::high_resolution_clock::now();
        auto t2 = chrono::high_resolution_clock::now();
        /* do work here */
        while(enableAcquisition) {
            statsPublisher.publishBufferLoad(openBuffers, IMAQ_BUFFER_COUNT);
            int currentBufferIndex = 0;
            /* find a free buffer */
            while(currentBufferIndex < IMAQ_BUFFER_COUNT && *bufferUsers[currentBufferIndex] > 0) {
                ++currentBufferIndex;
            }
            if(currentBufferIndex >= IMAQ_BUFFER_COUNT) {
                throw ImaqException("out of buffers!");
            }
#ifdef DEBUG
            cout << "use buffer: " << currentBufferIndex << endl;
#endif
            Int32 res;
            res = imgGrab(sid, (void**)&bufferList[currentBufferIndex], 1/* waitForNext */);
            t2 = chrono::high_resolution_clock::now();
            auto grabTime = chrono::system_clock::now(); /* wall time */
#ifdef DEBUG
            milliseconds dt = chrono::duration_cast<milliseconds>(t2 - t1);
            cout << "dt: " << dt.count() << " ms" << endl;
#endif
            t1 = t2;
            if(res == 0) {
                int lostFrames;
                
                /* times are serialized as microseconds since the epoch */
                uint64_t grabHighResTime = chrono::duration_cast<chrono::microseconds>(t1.time_since_epoch()).count();
                uint64_t grabWallTime = chrono::duration_cast<chrono::microseconds>(grabTime.time_since_epoch()).count();
#ifdef DEBUG
                cout << "current buffer: " << currentBuffer << " idx: " << currentBufferIndex << endl;
#endif
                lostFrames = getInteger(IMG_ATTR_LOST_FRAMES);
                if(lostFrames > totalLostFrames) {
                    cout << "buffer overrun! " << (lostFrames - totalLostFrames) << " lost frames!! (" << lostFrames << " in session)" << endl;
                    totalLostFrames = lostFrames;
                }
                acquireBuffer(currentBufferIndex); /* will be released by zmq-callback */
                /* compose multipart message to extractor workers */
                zmq::message_t header(deviceName.length());
                memcpy(header.data(), deviceName.c_str(), deviceName.length());
                pubSocket.send(header, ZMQ_SNDMORE);
                zmq::message_t dimensions(3 * 4 + 2 * 8);
                insertNetworkUInt32(((char*)dimensions.data()) + 0, width);
                insertNetworkUInt32(((char*)dimensions.data()) + 4, height);
                insertNetworkUInt32(((char*)dimensions.data()) + 8, bytesPerPixel);
                insertNetworkUInt64(((char*)dimensions.data()) + 12, grabHighResTime);
                insertNetworkUInt64(((char*)dimensions.data()) + 20, grabWallTime);
                pubSocket.send(dimensions, ZMQ_SNDMORE);
                zmq::message_t data(bufferList[currentBufferIndex],
                                    width * height * bytesPerPixel,
                                    cbWrapper,
                                    new std::function<void()>(
                                        bind(&_ImaqGrabberImpl::releaseBuffer,
                                             this,
                                             currentBufferIndex)
                                    ));
                pubSocket.send(data);
                /* message is complete */
            } else if(res == (Int32)(IMG_ERR_TIMEOUT)) {
                continue;
            } else {
                handleImaqError(res);
            }
            ++currentBuffer;
        }
        cout << "aquisition stopped" << endl;
        
    } catch(ImaqException &e) {
        logger.error(SSTR("grabbing service canceled! (", e.what(), ")"));
    }
    /* cleanup */
    while(totalBufferUsers > 0) {
        mutex m;
        unique_lock<mutex> lock(m);
        bufferFreeNotifier.wait(lock);
    }
    handleImaqError(imgSessionStopAcquisition(sid));
    for(int i = 0; i < IMAQ_BUFFER_COUNT; ++i) {
        delete[] bufferList[i];
        delete bufferUsers[i];
    }
    bufferUsers.clear();
    logger.debug(SSTR("grabber worker is done"));
}
    
void _ImaqGrabberImpl::acquireBuffer(int bufferId) {
    ++totalBufferUsers;
    if((*bufferUsers[bufferId])++ == 0) {
#ifdef DEBUG
        cout << deviceName << " acquired buffer " << bufferId << endl;
#endif
        ++openBuffers;
    }
}
    
void _ImaqGrabberImpl::releaseBuffer(int bufferId) {
    if(--(*bufferUsers[bufferId]) == 0) {
#ifdef DEBUG
        cout << deviceName << " released buffer " << bufferId << endl;
#endif
        --openBuffers;
        --totalBufferUsers;
        bufferFreeNotifier.notify_all();
    } else {
        --totalBufferUsers;
    }
}