asynctransfer.cpp

/*******************************************************************************
 * Copyright (c) 2018 Nerian Vision GmbH
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *******************************************************************************/

#if __GNUC__ == 4 && __GNUC_MINOR__ < 9
// This is a very ugly workaround for GCC bug 54562. If omitted,
// passing timeouts to collectReceivedImage() is broken.
#include <bits/c++config.h>
#undef _GLIBCXX_USE_CLOCK_MONOTONIC
#endif

#include <iostream>
#include <functional>
#include <stdexcept>
#include <thread>
#include <condition_variable>
#include <chrono>
#include <mutex>
#include <vector>
#include <cstring>
#include <algorithm>
#include "visiontransfer/asynctransfer.h"
#include "visiontransfer/alignedallocator.h"


using namespace std;

/*************** Pimpl class containing all private members ***********/

class AsyncTransfer::Pimpl {
public:
    Pimpl(const char* address, const char* service,
        ImageProtocol::ProtocolType protType, bool server,
        int bufferSize, int maxUdpPacketSize);
    ~Pimpl();

    // Redeclaration of public members
    void sendImagePairAsync(const ImagePair& imagePair, bool deleteData);
    bool collectReceivedImagePair(ImagePair& imagePair, double timeout);
    int getNumDroppedFrames() const;
    bool isConnected() const;
    void disconnect();
    std::string getRemoteAddress() const;
    bool tryAccept();

private:
    static constexpr int NUM_BUFFERS = 6;
    static constexpr int SEND_THREAD_SHORT_WAIT_MS = 1;
    static constexpr int SEND_THREAD_LONG_WAIT_MS = 10;

    // The encapsulated image transfer object
    ImageTransfer imgTrans;

    // Variable for controlling thread termination
    volatile bool terminate;

    // There are two threads, one for sending and one for receiving.
    // Each has a mutex and condition variable for synchronization.
    std::thread sendThread;
    std::mutex sendMutex;
    std::condition_variable sendCond;
    std::condition_variable sendWaitCond;

    std::thread receiveThread;
    std::timed_mutex receiveMutex;
    std::condition_variable_any receiveCond;
    std::condition_variable_any receiveWaitCond;

    // Objects for exchanging images with the send and receive threads
    ImagePair receivedPair;
    std::vector<unsigned char, AlignedAllocator<unsigned char> > receivedData[NUM_BUFFERS];
    bool newDataReceived;

    ImagePair sendImagePair;
    bool sendPairValid;
    bool deleteSendData;

    // Exception occurred in one of the threads
    std::exception_ptr receiveException;
    std::exception_ptr sendException;

    bool sendThreadCreated;
    bool receiveThreadCreated;

    // Main loop for sending thread
    void sendLoop();

    // Main loop for receiving;
    void receiveLoop();

    void createSendThread();
};

/******************** Stubs for all public members ********************/

AsyncTransfer::AsyncTransfer(const char* address, const char* service,
        ImageProtocol::ProtocolType protType, bool server,
        int bufferSize, int maxUdpPacketSize)
    : pimpl(new Pimpl(address, service, protType, server, bufferSize, maxUdpPacketSize)) {
}

AsyncTransfer::AsyncTransfer(const DeviceInfo& device, int bufferSize, int maxUdpPacketSize)
    : pimpl(new Pimpl(device.getIpAddress().c_str(), "7681", static_cast<ImageProtocol::ProtocolType>(device.getNetworkProtocol()),
    false, bufferSize, maxUdpPacketSize)) {
}

AsyncTransfer::~AsyncTransfer() {
    delete pimpl;
}

void AsyncTransfer::sendImagePairAsync(const ImagePair& imagePair, bool deleteData) {
    pimpl->sendImagePairAsync(imagePair, deleteData);
}

bool AsyncTransfer::collectReceivedImagePair(ImagePair& imagePair, double timeout) {
    return pimpl->collectReceivedImagePair(imagePair, timeout);
}

int AsyncTransfer::getNumDroppedFrames() const {
    return pimpl->getNumDroppedFrames();
}

bool AsyncTransfer::isConnected() const {
    return pimpl->isConnected();
}

void AsyncTransfer::disconnect() {
    return pimpl->disconnect();
}

std::string AsyncTransfer::getRemoteAddress() const {
    return pimpl->getRemoteAddress();
}

bool AsyncTransfer::tryAccept() {
    return pimpl->tryAccept();
}

/******************** Implementation in pimpl class *******************/

AsyncTransfer::Pimpl::Pimpl(const char* address, const char* service,
        ImageProtocol::ProtocolType protType, bool server,
        int bufferSize, int maxUdpPacketSize)
    : imgTrans(address, service, protType, server, bufferSize, maxUdpPacketSize),
    terminate(false), newDataReceived(false), sendPairValid(false),
    deleteSendData(false), sendThreadCreated(false),
    receiveThreadCreated(false) {

    if(server) {
        createSendThread();
    }
}

AsyncTransfer::Pimpl::~Pimpl() {
    terminate = true;

    sendCond.notify_all();
    receiveCond.notify_all();
    sendWaitCond.notify_all();
    receiveWaitCond.notify_all();

    if(sendThreadCreated && sendThread.joinable()) {
        sendThread.join();
    }

    if(receiveThreadCreated && receiveThread.joinable()) {
        receiveThread.join();
    }

    if(sendPairValid && deleteSendData) {
        delete[] sendImagePair.getPixelData(0);
        delete[] sendImagePair.getPixelData(1);
    }
}

void AsyncTransfer::Pimpl::createSendThread() {
    if(!sendThreadCreated) {
        // Lazy initialization of the send thread as it is not always needed
        unique_lock<mutex> lock(sendMutex);
        sendThread = thread(bind(&AsyncTransfer::Pimpl::sendLoop, this));
        sendThreadCreated = true;
    }
}

void AsyncTransfer::Pimpl::sendImagePairAsync(const ImagePair& imagePair, bool deleteData) {
    createSendThread();

    while(true) {
        unique_lock<mutex> lock(sendMutex);

        // Test for errors
        if(sendException) {
            std::rethrow_exception(sendException);
        }

        if(!sendPairValid) {
            sendImagePair = imagePair;
            sendPairValid = true;
            deleteSendData = deleteData;

            // Wake up the sender thread
            sendCond.notify_one();

            return;
        } else {
            // Wait for old data to be processed first
            sendWaitCond.wait(lock);
        }
    }
}

bool AsyncTransfer::Pimpl::collectReceivedImagePair(ImagePair& imagePair, double timeout) {
    if(!receiveThreadCreated) {
        // Lazy initialization of receive thread
        unique_lock<timed_mutex> lock(receiveMutex);
        receiveThreadCreated = true;
        receiveThread = thread(bind(&AsyncTransfer::Pimpl::receiveLoop, this));
    }

    // Acquire mutex
    unique_lock<timed_mutex> lock(receiveMutex, std::defer_lock);
    if(timeout < 0) {
        lock.lock();
    } else {
        std::chrono::steady_clock::time_point lockStart =
            std::chrono::steady_clock::now();
        if(!lock.try_lock_for(std::chrono::microseconds(static_cast<unsigned int>(timeout*1e6)))) {
            // Timed out
            return false;
        }

        // Update timeout
        unsigned int lockDuration = static_cast<unsigned int>(std::chrono::duration_cast<std::chrono::microseconds>(
            std::chrono::steady_clock::now() - lockStart).count());
        timeout = std::max(0.0, timeout - lockDuration*1e-6);
    }

    // Test for errors
    if(receiveException) {
        std::rethrow_exception(receiveException);
    }

    if(timeout == 0 && !newDataReceived) {
        // No image has been received and we are not blocking
        return false;
    }

    // If there is no data yet then keep on waiting
    if(!newDataReceived) {
        if(timeout < 0) {
            while(!terminate && !receiveException && !newDataReceived) {
                receiveCond.wait(lock);
            }
        } else {
            receiveCond.wait_for(lock, std::chrono::microseconds(static_cast<unsigned int>(timeout*1e6)));
        }
    }

    // Test for errors again
    if(receiveException) {
        std::rethrow_exception(receiveException);
    }

    if(newDataReceived) {
        // Get the received image
        imagePair = receivedPair;

        newDataReceived = false;
        receiveWaitCond.notify_one();

        return true;
    } else {
        return false;
    }
}

void AsyncTransfer::Pimpl::sendLoop() {
    {
        // Delay the thread start
        unique_lock<mutex> lock(sendMutex);
    }

    ImagePair pair;
    bool deletePair = false;

    try {
        while(!terminate) {
            // Wait for next image
            {
                unique_lock<mutex> lock(sendMutex);
                // Wait for next frame to be queued
                bool firstWait = true;
                while(!terminate && !sendPairValid) {
                    imgTrans.transferData();
                    sendCond.wait_for(lock, std::chrono::milliseconds(
                        firstWait ? SEND_THREAD_SHORT_WAIT_MS : SEND_THREAD_LONG_WAIT_MS));
                    firstWait = false;
                }
                if(!sendPairValid) {
                    continue;
                }

                pair = sendImagePair;
                deletePair = deleteSendData;
                sendPairValid = false;

                sendWaitCond.notify_one();
            }

            if(!terminate) {
                imgTrans.setTransferImagePair(pair);
                imgTrans.transferData();
            }

            if(deletePair) {
                delete[] pair.getPixelData(0);
                delete[] pair.getPixelData(1);
                deletePair = false;
            }
        }
    } catch(...) {
        // Store the exception for later
        if(!sendException) {
            sendException = std::current_exception();
        }
        sendWaitCond.notify_all();

        // Don't forget to free the memory
        if(deletePair) {
            delete[] pair.getPixelData(0);
            delete[] pair.getPixelData(1);
            deletePair = false;
        }
    }
}

void AsyncTransfer::Pimpl::receiveLoop() {
    {
        // Delay the thread start
        unique_lock<timed_mutex> lock(receiveMutex);
    }

    try {
        ImagePair currentPair;
        int bufferIndex = 0;

        while(!terminate) {
            // Receive new image
            if(!imgTrans.receiveImagePair(currentPair)) {
                // No image available
                continue;
            }

            // Copy the pixel data
            for(int i=0;i<2;i++) {
                int bytesPerPixel = currentPair.getBytesPerPixel(i);
                int newStride = currentPair.getWidth() * bytesPerPixel;
                int totalSize = currentPair.getHeight() * newStride;
                if(static_cast<int>(receivedData[i + bufferIndex].size()) < totalSize) {
                    receivedData[i + bufferIndex].resize(totalSize);
                }
                if(newStride == currentPair.getRowStride(i)) {
                    memcpy(&receivedData[i + bufferIndex][0], currentPair.getPixelData(i),
                        newStride*currentPair.getHeight());
                } else {
                    for(int y = 0; y<currentPair.getHeight(); y++) {
                        memcpy(&receivedData[i + bufferIndex][y*newStride],
                            &currentPair.getPixelData(i)[y*currentPair.getRowStride(i)],
                            newStride);
                    }
                    currentPair.setRowStride(i, newStride);
                }
                currentPair.setPixelData(i, &receivedData[i + bufferIndex][0]);
            }

            {
                unique_lock<timed_mutex> lock(receiveMutex);

                // Wait for previously received data to be processed
                while(newDataReceived) {
                    receiveWaitCond.wait_for(lock, std::chrono::milliseconds(100));
                    if(terminate) {
                        return;
                    }
                }

                // Notify that a new image pair has been received
                newDataReceived = true;
                receivedPair = currentPair;
                receiveCond.notify_one();
            }

            // Increment index for data buffers
            bufferIndex = (bufferIndex + 2) % NUM_BUFFERS;
        }
    } catch(...) {
        // Store the exception for later
        if(!receiveException) {
            receiveException = std::current_exception();
        }
        receiveCond.notify_all();
    }
}

bool AsyncTransfer::Pimpl::isConnected() const {
    return imgTrans.isConnected();
}

void AsyncTransfer::Pimpl::disconnect() {
    imgTrans.disconnect();
}

std::string AsyncTransfer::Pimpl::getRemoteAddress() const {
    return imgTrans.getRemoteAddress();
}

int AsyncTransfer::Pimpl::getNumDroppedFrames() const {
    return imgTrans.getNumDroppedFrames();
}

bool AsyncTransfer::Pimpl::tryAccept() {
    return imgTrans.tryAccept();
}