DummyGrabber.cpp

Go to the documentation of this file.

#include <DummyGrabber.h>

#include <string>
#include <iostream>
#include <mutex>
#include <chrono>
#include <functional>

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

#include <chrono>
#include <climits>              // UINT_MAX

using namespace std;

class DummyGrabberSynchronizer {
public:
    static DummyGrabberSynchronizer& getInstance() {
        static DummyGrabberSynchronizer instance;
        return instance;
    }
    int getCurrentFrame(double originalFrameRate, int originalNumberOfFrames) {
        if(originalFrameRate == 0.) {
            std::cout << "WARNING: original FPS is 0! assuming FPS == 20!" << std::endl;
            originalFrameRate = 20.;
        }
        std::chrono::milliseconds sensorDuration((long long)(((long long)originalNumberOfFrames * 1000LL) / originalFrameRate));
        if(sensorDuration < imagePeriod) {
            imagePeriod = sensorDuration;
        }
        auto currentPosition = (std::chrono::system_clock::now() - refTime) % imagePeriod;
        //int frameNo = (originalNumberOfFrames * currentPosition) / sensorDuration;
        return (std::chrono::duration_cast<std::chrono::microseconds>(currentPosition).count() * originalFrameRate) / 1000000LL;
    }
private:
    DummyGrabberSynchronizer():
        refTime(std::chrono::system_clock::now()) {};
    DummyGrabberSynchronizer(DummyGrabberSynchronizer const &);
    void operator=(DummyGrabberSynchronizer const &);

    std::chrono::system_clock::time_point refTime;
    std::chrono::system_clock::duration imagePeriod = std::chrono::system_clock::duration::max();
};

template <typename T> void recalculateExposure(T * target,
                                               T * source,
                                               int width,
                                               int height,
                                               double factor,
                                               T dark,
                                               T max);

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

DummyException::~DummyException() throw() {

}

_DummyGrabberImpl::_DummyGrabberImpl(const std::string & _deviceName,
                                     const std::string & _deviceSource,
                                     zmq::context_t & _ctx,
                                     const std::string & _brokerEndpoint) :
                RocServer(_ctx, _brokerEndpoint, _deviceName),
                ctx(_ctx),
                deviceName(_deviceName),
                deviceSource(_deviceSource),
                bufferUsers(0),
                openBuffers(0),
                totalBufferUsers(0),
                shutterClosed(false),
                fps(20.),
                tint(10.),
                rocsHeader(SSTR("ROCS\x01", _deviceName, ".controls")),
                rocsSocket(_ctx, ZMQ_PUB),
                synchronizer(DummyGrabberSynchronizer::getInstance()){
    // load dummy file
    dummyFile.openFile(deviceSource);
        /* dummyFile will be closed and unmapped by its destructor */
        if(deviceName == "vnir") {
            maxValue = (2<<11)-1;
        } else if(deviceName == "swir") {
            maxValue = (2<<13)-1;
        } else if(deviceName == "lwir") {
            maxValue = (2<<13)-1;
        } else {
            maxValue = (2<<7)-1;
        }
}

_DummyGrabberImpl::~_DummyGrabberImpl() {

}

int _DummyGrabberImpl::getWidth() {
    return dummyFile.getWidth();
}

int _DummyGrabberImpl::getHeight() {
    return dummyFile.getHeight();
}

int _DummyGrabberImpl::getBytesPerPixel() {
    return dummyFile.getBytesPerPixel();
}

void _DummyGrabberImpl::startServing() {
    enableAcquisition = true;
    acquisitionThread = std::thread(&_DummyGrabberImpl::acquisition, this);
}

void _DummyGrabberImpl::stopServing() {
    enableAcquisition  = false;
    if(acquisitionThread.joinable()) {
        std::cout << "wait for grabber thread to join" << std::endl;
        acquisitionThread.join();
    }
}

void _DummyGrabberImpl::acquisition() {
    RocLogger logger(ctx, "inproc://log", "camserver", SSTR("grabber.", deviceName));
        rocsSocket.connect("inproc://log");
    
    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();
        std::cout << "width: " << width << " height: " << height << " bpp: " << bytesPerPixel << std::endl;
        int bufferCount = 256;
        char * bufferList[bufferCount];
        bufferUsers.resize(bufferCount);

        GrabberStatsPublisher statsPublisher(ctx, deviceName);

        for(int i = 0; i < bufferCount; ++i) {
            bufferList[i] = new char[width * height * bytesPerPixel];
            assert(bufferList[i] != nullptr);
            bufferUsers[i] = new std::atomic<int>(0);
            assert(bufferUsers[i] != nullptr);
        }
#ifdef DEBUG
    std::cout << "grab initialized" << std::endl;
#endif
    logger.debug(SSTR("grab initialized"));

    int numFrames = dummyFile.getNumFrames();
        double originalFps = dummyFile.getFps();
    auto t1 = std::chrono::high_resolution_clock::now();
    auto t2 = std::chrono::high_resolution_clock::now();
    /* do work here */
    while(enableAcquisition) {
                statsPublisher.publishBufferLoad(openBuffers, bufferCount);
                sendStatus();

                int currentFrame = synchronizer.getCurrentFrame(originalFps, numFrames);
                int currentBufferIndex = 0;
                /* find a free buffer */
                while(currentBufferIndex < bufferCount && *bufferUsers[currentBufferIndex] > 0) {
                    ++currentBufferIndex;
                }
                if(currentBufferIndex >= bufferCount) {
                    throw DummyException("out of buffers!");
                }
        t2 = std::chrono::high_resolution_clock::now();
        auto dt = std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1);
        // adjust framerate
        std::this_thread::sleep_for(std::chrono::microseconds(int(1e6/fps)) - dt);
#ifdef DEBUG
        std::cout << "dt: " << dt.count() << " ms" << std::endl;
#endif
        auto grabTime = std::chrono::system_clock::now(); /* wall time */
        t1 = std::chrono::high_resolution_clock::now();
        /* times are serialized as microseconds since the epoch */
        uint64_t grabHighResTime = std::chrono::duration_cast<std::chrono::microseconds>(t1.time_since_epoch()).count();
        uint64_t grabWallTime = std::chrono::duration_cast<std::chrono::microseconds>(grabTime.time_since_epoch()).count();

        acquireBuffer(currentBufferIndex); /* will be released by zmq-callback */

                double dfTint = dummyFile.getTint();
                double tintFactor = 1.0;
                if(dfTint != 0.) {
                    tintFactor = tint / dfTint;
                }
                if(shutterClosed) {
                    tintFactor = 0.;
                }

                switch(bytesPerPixel) {
                    case 1:
                        recalculateExposure<uint8_t>((uint8_t*) bufferList[currentBufferIndex],
                                                     (uint8_t*) dummyFile.getDataByFrame(currentFrame),
                                                     width,
                                                     height,
                                                     tintFactor,
                                                     20,
                                                     maxValue);
                        break;
                    case 2:
                        recalculateExposure<uint16_t>((uint16_t*) bufferList[currentBufferIndex],
                                                     (uint16_t*) dummyFile.getDataByFrame(currentFrame),
                                                     width,
                                                     height,
                                                     tintFactor,
                                                     270,
                                                     maxValue);
                        break;
                    case 4:
                        recalculateExposure<uint32_t>((uint32_t*) bufferList[currentBufferIndex],
                                                     (uint32_t*) dummyFile.getDataByFrame(currentFrame),
                                                     width,
                                                     height,
                                                     tintFactor,
                                                     270,
                                                     maxValue);
                        break;
                }
        /* 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((char*)(bufferList[currentBufferIndex]),
                                            width * height * bytesPerPixel,
                                            cbWrapper,
                                            new std::function<void()>(
                                                    std::bind(&_DummyGrabberImpl::releaseBuffer,
                                                                this,
                                                                currentBufferIndex)
                                            ));
        pubSocket.send(data);
    }
    std::cout << "aquisition stopped" << std::endl;
    /* cleanup */
    while(totalBufferUsers > 0) {
        std::mutex m;
        std::unique_lock<std::mutex> lock(m);
        bufferFreeNotifier.wait(lock);
    }
    for(int i = 0; i < bufferCount; ++i) {
            delete[] bufferList[i];
            delete bufferUsers[i];
        }
        bufferUsers.clear();
    logger.debug(SSTR("grabber worker is done"));
}

void _DummyGrabberImpl::acquireBuffer(int bufferId) {
    ++totalBufferUsers;
    if((*bufferUsers[bufferId])++ == 0) {
#ifdef DEBUG
        cout << deviceName << " acquired buffer " << bufferId << endl;
#endif
        ++openBuffers;
    }
}

void _DummyGrabberImpl::releaseBuffer(int bufferId) {
    if(--(*bufferUsers[bufferId]) == 0) {
#ifdef DEBUG
        cout << deviceName << " released buffer " << bufferId << endl;
#endif
        --openBuffers;
        --totalBufferUsers;
        bufferFreeNotifier.notify_all();
    } else {
        --totalBufferUsers;
    }
}

void _DummyGrabberImpl::addEndpoint(const std::string & _endpoint) {
    endpoints.push_back(_endpoint);
}

/* RocServer stuff */

Json::Value _DummyGrabberImpl::call(const std::string & name, const Json::Value & arguments) {
    if(name == "openShutter") {
        shutterClosed = false;
        return Json::Value::null;
    } else if(name == "closeShutter") {
        shutterClosed = true;
        return Json::Value::null;
    }
    throw RocNotImplementedError();
}

Json::Value _DummyGrabberImpl::getattr(const std::string & name) {
    if (name == "fps") {
        return fps;
    } else if (name == "exposureTime") {
        return tint;
    }
    throw RocIsCallable();
}

void _DummyGrabberImpl::setattr(const std::string & name, const Json::Value & value) {
    if (name == "fps") {
        fps = value.asFloat();
        return;
    } else if (name == "exposureTime") {
        tint = value.asFloat();
        return;
    }
    throw RocNotImplementedError();
}

/* ROCS stuff */

void _DummyGrabberImpl::sendStatus() {
    zmq::message_t msgHdr(rocsHeader.length());
    memcpy((char*)msgHdr.data(), rocsHeader.c_str(), rocsHeader.length());
    rocsSocket.send(msgHdr, ZMQ_SNDMORE);

    Json::FastWriter writer;
    Json::Value root;
    if(shutterClosed) {
        root["shutter"] = "close";
    } else {
        root["shutter"] = "open";
    }
    root["exposureTime"] = tint;
    root["fps"] = fps;
    std::string data = writer.write(root);
    zmq::message_t msgData(data.length());
    memcpy((char *)msgData.data(), data.c_str(), data.length());
    rocsSocket.send(msgData);
}

template <typename T> void recalculateExposure(T * target,
                                               T * source,
                                               int width,
                                               int height,
                                               double factor,
                                               T dark,
                                               T max) {
    int size = width * height;
    double temp;
    for(int i = 0; i < size; ++i) {
        temp = source[i] - dark;
        if(temp < 0.) {
            temp = 0.;
        }
        temp = (temp * factor) + dark;
        if(temp > max) {
            target[i] = max;
        } else {
            target[i] = temp;
        }
    }
}