ImageProvider.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    VisionX::Core
 * @author     Kai Welke (kai dot welke at kit dot edu)
 * @date       2011
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#include "ImageProvider.h"
 
 
// STD/STL
#include <iostream>
#include <string>
 
// OpenCV
#include <opencv2/opencv.hpp>
 
// Simox
#include <SimoxUtility/algorithm.h>
 
// ArmarX
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
#include <ArmarXCore/util/time.h>
#include <VisionX/libraries/imrec/helper.h>
#include <VisionX/tools/TypeMapping.h>
#include <VisionX/tools/ImageUtil.cpp>
 
 
namespace visionx
{
 
    // ================================================================== //
    // == ImageProvider ice interface =================================== //
    // ================================================================== //
    armarx::Blob ImageProvider::getImages(const Ice::Current&)
    {
        if (numberImages == 0)
        {
            return armarx::Blob();
        }
 
        return sharedMemoryProvider->getData();
    }
 
 
    armarx::Blob ImageProvider::getImagesAndMetaInfo(armarx::MetaInfoSizeBasePtr& info, const Ice::Current&)
    {
        if (numberImages == 0)
        {
            return armarx::Blob();
        }
        return sharedMemoryProvider->getData(info);
    }
 
 
    ImageFormatInfo ImageProvider::getImageFormat(const Ice::Current&)
    {
        return imageFormat;
    }
 
    int ImageProvider::getNumberImages(const Ice::Current&)
    {
        return numberImages;
    }
 
    // ================================================================== //
    // == Component implementation =============================== //
    // ================================================================== //
    void ImageProvider::onInitComponent()
    {
        ARMARX_TRACE;
        // init members
        exiting = false;
 
        // default image format (640x480, bayerpattern).
        // call from within init to change!
        setImageFormat(ImageDimension(640, 480), eBayerPattern, eBayerPatternRg);
        setNumberImages(0);
 
        // call setup of image provider implementation to setup image size
        onInitImageProvider();
    }
 
 
    void ImageProvider::onConnectComponent()
    {
        ARMARX_INFO << "onConnectComponent " << getName();
        ARMARX_TRACE;
        // init shared memory
        int imageSize = getImageFormat().dimension.width *  getImageFormat().dimension.height * getImageFormat().bytesPerPixel;
 
        if (numberImages != 0)
        {
            armarx::MetaInfoSizeBasePtr info(new armarx::MetaInfoSizeBase(getNumberImages() * imageSize, getNumberImages() * imageSize, 0));
            if (sharedMemoryProvider)
            {
                sharedMemoryProvider->stop();
            }
            sharedMemoryProvider = new armarx::IceSharedMemoryProvider<unsigned char>(this, info, "ImageProvider");
 
            // reference to shared memory
            imageBuffers = (void**) new unsigned char* [getNumberImages()];
 
            for (int i = 0 ; i < getNumberImages() ; i++)
            {
                imageBuffers[i] = sharedMemoryProvider->getBuffer() + i * imageSize;
            }
 
            // offer topic for image events
            offeringTopic(getName() + ".ImageListener");
 
            // retrieve storm topic proxy
            imageProcessorProxy = getTopic<ImageProcessorInterfacePrx>(getName() + ".ImageListener");
 
            // start icesharedmemory provider
            sharedMemoryProvider->start();
        }
 
        onConnectImageProvider();
    }
 
    void ImageProvider::onDisconnectComponent()
    {
        ARMARX_TRACE;
        onDisconnectImageProvider();
        if (sharedMemoryProvider)
        {
            sharedMemoryProvider->stop();
            sharedMemoryProvider = nullptr;
        }
    }
 
    void ImageProvider::onExitComponent()
    {
        ARMARX_TRACE;
        exiting = true;
 
        onExitImageProvider();
 
        if (numberImages != 0)
        {
            delete [] imageBuffers;
        }
    }
 
    void ImageProvider::updateTimestamp(Ice::Long timestamp, bool threadSafe)
    {
        ARMARX_TRACE;
        auto sharedMemoryProvider = this->sharedMemoryProvider; // preserve from deleting
        if (!sharedMemoryProvider)
        {
            ARMARX_INFO << "Shared memory provider is null!"
                        << " Did you forget to set call setNumberImages and setImageFormat in onInitImageProvider?";
            return;
        }
        armarx::MetaInfoSizeBasePtr info = this->sharedMemoryProvider->getMetaInfo(threadSafe);
        if (info)
        {
            info->timeProvided = timestamp;
        }
        else
        {
            info = new armarx::MetaInfoSizeBase(imageFormat.dimension.width * imageFormat.dimension.height * imageFormat.bytesPerPixel,
                                                imageFormat.dimension.width * imageFormat.dimension.height * imageFormat.bytesPerPixel,
                                                timestamp);
        }
 
        this->sharedMemoryProvider->setMetaInfo(info, threadSafe);
    }
 
    void ImageProvider::updateTimestamp(IceUtil::Time timestamp, bool threadSafe)
    {
        ARMARX_TRACE;
        updateTimestamp(timestamp.toMicroSeconds(), threadSafe);
    }
 
    void ImageProvider::updateTimestamp(const armarx::core::time::DateTime& timestamp, bool threadSafe)
    {
        ARMARX_TRACE;
        updateTimestamp(timestamp.toMicroSecondsSinceEpoch(), threadSafe);
    }
 
    armarx::Blob ImageProvider::getCompressedImagesAndMetaInfo(CompressionType compressionType, Ice::Int compressionQuality, armarx::MetaInfoSizeBasePtr& info, const Ice::Current&)
    {
        int type;
        switch (imageFormat.type)
        {
            case eRgb:
                type = CV_8UC3;
                break;
            case eGrayScale:
                type = CV_8UC1;
                break;
            default:
                throw armarx::LocalException() << "unsupported image type " << (int)(imageFormat.type);
        }
 
        if (!sharedMemoryProvider)
            return {};
        auto lock = sharedMemoryProvider->getScopedReadLock();
        if (!lock) // already shutdown
        {
            return {};
        }
        if (numberImages == 0)
        {
            return armarx::Blob();
        }
        info = new armarx::MetaInfoSizeBase(*sharedMemoryProvider->getMetaInfo());
 
        auto imageTimestamp = IceUtil::Time::microSeconds(info->timeProvided);
        auto key = std::make_pair(compressionType, compressionQuality);
 
        std::unique_lock lock2(compressionDataMapMutex);
        if (compressionDataMap.count(key) && compressionDataMap.at(key).imageTimestamp == imageTimestamp)
        {
            ARMARX_VERBOSE << deactivateSpam(1) << "using already compressed image";
            return compressionDataMap.at(key).compressedImage;
        }
        cv::Mat mat(imageFormat.dimension.height * numberImages, imageFormat.dimension.width, type, imageBuffers[0]);
 
 
        armarx::Blob encodedImg;
 
        std::vector<int> compression_params;
        std::string extension;
        switch (compressionType)
        {
            case ePNG:
                extension = ".png";
                compression_params =  {cv::IMWRITE_PNG_COMPRESSION, compressionQuality,
                                       cv::IMWRITE_PNG_STRATEGY, cv::IMWRITE_PNG_STRATEGY_RLE
                                  };
                break;
            case eJPEG:
                extension = ".jpg";
                compression_params.push_back(cv::IMWRITE_JPEG_QUALITY);
                compression_params.push_back(compressionQuality);
                break;
            default:
                throw armarx::LocalException() << "unsupported image type " << (int)(imageFormat.type);
        }
        cv::imencode("*" + extension, mat, encodedImg, compression_params);
 
        compressionDataMap[key] = {encodedImg, imageTimestamp};
 
        //    cv::imwrite("/tmp/compressed" + extension, mat, compression_params);
        ARMARX_DEBUG << deactivateSpam(1) << "size before compression: " << imageFormat.dimension.height* numberImages* imageFormat.dimension.width* imageFormat.bytesPerPixel << " size after: " << encodedImg.size();
        return encodedImg;
    }
 
 
    // ================================================================== //
    // == Utility methods for ImageProviders ============================ //
    // ================================================================== //
    void ImageProvider::setImageFormat(ImageDimension imageDimension,
                                       ImageType imageType,
                                       BayerPatternType bayerPatternType)
    {
        ARMARX_TRACE;
        imageFormat.dimension  = imageDimension;
        imageFormat.type = imageType;
        imageFormat.bpType = bayerPatternType;
 
        switch (imageType)
        {
            case eGrayScale:
            case eBayerPattern:
                imageFormat.bytesPerPixel = 1;
                break;
 
            case eRgb:
                imageFormat.bytesPerPixel = 3;
                break;
 
            case eFloat1Channel:
                imageFormat.bytesPerPixel = 4;
                break;
 
            case eFloat3Channels:
                imageFormat.bytesPerPixel = 12;
                break;
 
            case ePointsScan:
                imageFormat.bytesPerPixel = 12;
                break;
 
            case eColoredPointsScan:
                imageFormat.bytesPerPixel = 16;
                break;
        }
    }
 
    void ImageProvider::setNumberImages(int numberImages)
    {
        ARMARX_TRACE;
        this->numberImages = numberImages;
    }
 
    void ImageProvider::provideImages(void** inputBuffers, const IceUtil::Time& imageTimestamp)
    {
        ARMARX_TRACE;
        if (numberImages == 0)
        {
            ARMARX_INFO << "Number of images is 0 - thus none can be provided";
            return;
        }
 
        int imageSize = imageFormat.dimension.width * imageFormat.dimension.height * imageFormat.bytesPerPixel;
 
        // copy
        {
            // lock memory access
            armarx::SharedMemoryScopedWriteLockPtr lock = getScopedWriteLock();
            if (!lock) // already shutdown
            {
                return;
            }
            if (imageTimestamp > IceUtil::Time())
            {
                updateTimestamp(imageTimestamp, false);
            }
            for (int i = 0 ; i < numberImages ; i++)
            {
                memcpy(imageBuffers[i], inputBuffers[i], imageSize);
            }
        }
 
        recordImages(imageTimestamp);
 
        // notify processors
        if (imageProcessorProxy)
        {
            ARMARX_DEBUG << "Notifying ImageProcessorProxy";
            imageProcessorProxy->reportImageAvailable(getName());
        }
        else
        {
            ARMARX_ERROR << deactivateSpam(4) << "imageProcessorProxy is NULL - could not report Image available";
        }
    }
 
    void ImageProvider::provideImages(CByteImage** images, const IceUtil::Time& imageTimestamp)
    {
        ARMARX_TRACE;
        if (numberImages == 0)
        {
            ARMARX_INFO << "Number of images is 0 - thus none can be provided";
            return;
        }
 
        //ISO C++ forbids variable length array [-Werror=vla] => use a vector (the array will be on the heap anyways)
        std::vector<void*> imageBuffers(numberImages);
 
        for (int i = 0 ; i < numberImages ; i++)
        {
            //ARMARX_VERBOSE << i;
            imageBuffers[i] = images[i]->pixels;
        }
 
        provideImages(imageBuffers.data(), imageTimestamp);
    }
 
    void ImageProvider::provideImages(const std::vector<CByteImageUPtr >& images, const IceUtil::Time& imageTimestamp)
    {
        ARMARX_TRACE;
        if (numberImages == 0)
        {
            ARMARX_INFO << "Number of images is 0 - thus none can be provided";
            return;
        }
 
        //ISO C++ forbids variable length array [-Werror=vla] => use a vector (the array will be on the heap anyways)
        std::vector<void*> imageBuffers(numberImages);
 
        for (int i = 0 ; i < numberImages ; i++)
        {
            //ARMARX_VERBOSE << i;
            imageBuffers[i] = images[i]->pixels;
        }
 
        provideImages(imageBuffers.data(), imageTimestamp);
    }
 
    void ImageProvider::provideImages(CFloatImage** images, const IceUtil::Time& imageTimestamp)
    {
        ARMARX_TRACE;
        if (numberImages == 0)
        {
            ARMARX_INFO << "Number of images is 0 - thus none can be provided";
            return;
        }
 
        //ISO C++ forbids variable length array [-Werror=vla] => use a vector (the array will be on the heap anyways)
        std::vector<void*> imageBuffers(numberImages);
 
        for (int i = 0 ; i < numberImages ; i++)
        {
            imageBuffers[i] = images[i]->pixels;
        }
 
        provideImages(imageBuffers.data(), imageTimestamp);
    }
 
 
    void ImageProvider::recordImages(const IceUtil::Time& image_timestamp)
    {
        ARMARX_TRACE;
 
        int imageSize = imageFormat.dimension.width * imageFormat.dimension.height * imageFormat.bytesPerPixel;
 
        const bool is_recording = [&]
        {
            ARMARX_TRACE;
 
            std::scoped_lock l{rec.statusMutex};
 
            if (rec.status.type == imrec::State::stopping)
            {
                rec.status.type = imrec::State::writing;
                // Still record current frames to notify potentially waiting recording task.
                return true;
            }
 
            return rec.status.type == imrec::State::running;
        }();
 
        if (is_recording)
        {
            ARMARX_TRACE;
 
            std::chrono::microseconds timestamp{image_timestamp.toMicroSeconds()};
            std::unordered_map<int, CByteImage*> current_frames;
            for (int i = 0; i < numberImages; ++i)
            {
                if (not rec.config.channelConfigs[i].disabled)
                {
                    current_frames[i] = tools::createByteImage(imageFormat, imageFormat.type);
                    std::memcpy(current_frames[i]->pixels, imageBuffers[i], imageSize);
                }
            }
 
            ARMARX_DEBUG << "Pushing data to buffer.";
            {
                std::scoped_lock l{rec.bufferMutex};
                rec.buffer.push_back({timestamp, current_frames});
            }
            ARMARX_DEBUG << "Signalling that buffer was filled...";
            rec.cv.notify_all();
        }
    }
 
 
    bool ImageProvider::startImageRecording(const imrec::Config& cfg, const Ice::Current&)
    {
        ARMARX_TRACE;
 
        ARMARX_DEBUG << "Starting recording...";
 
        ARMARX_CHECK_EQUAL(int(cfg.channelConfigs.size()), numberImages)
                << "Must supply same number of channel configs as there are channels";
 
        std::scoped_lock l{rec.callMutex};
 
        {
            std::scoped_lock l{rec.statusMutex};
 
            auto are_disabled = [](const auto & c)
            {
                return c.disabled;
            };
 
            // A recording can only be started if the component is ready and if at least one channel
            // is not disabled.
            if (rec.status.type != imrec::State::ready
                or std::all_of(cfg.channelConfigs.begin(), cfg.channelConfigs.end(), are_disabled))
            {
                return false;
            }
 
            rec.status.framesWritten = 0;
            rec.status.framesBuffered = 0;
            rec.status.type = imrec::State::scheduled;
        }
 
        rec.config = cfg;
        rec.config.name =
            simox::alg::replace_all(
                rec.config.name,
                "%TIMESTAMP%",
                imrec::datetime_to_string(std::chrono::microseconds{rec.config.startTimestamp}));
 
        rec.runningTask =
            new armarx::RunningTask<ImageProvider>(this, &ImageProvider::imageRecordingRunningTask);
        rec.runningTask->start();
 
        ARMARX_DEBUG << "Scheduled recordings...";
 
        return true;
    }
 
 
    imrec::Status ImageProvider::getImageRecordingStatus(const Ice::Current&)
    {
        ARMARX_TRACE;
 
        std::scoped_lock l{rec.statusMutex};
        return rec.status;
    }
 
 
    bool ImageProvider::stopImageRecording(const Ice::Current&)
    {
        ARMARX_TRACE;
 
        std::scoped_lock l{rec.callMutex};
 
        {
            std::scoped_lock l{rec.statusMutex};
 
            // A recording can only be stopped if one is scheduled or one is running.  Otherwise
            // there is no recording which could be stopped, or a recording is still being written.
            if (rec.status.type != imrec::State::scheduled
                and rec.status.type != imrec::State::running)
            {
                return false;
            }
 
            rec.status.type = imrec::State::stopping;
        }
 
        ARMARX_CHECK(rec.runningTask);
 
        const bool join = true;
        rec.runningTask->stop(join);
 
        return true;
    }
 
 
    void ImageProvider::imageRecordingRunningTask()
    {
        ARMARX_TRACE;
 
        ARMARX_DEBUG << "Started recording task.";
 
        // Loop.
        while (true)
        {
            ARMARX_TRACE;
 
            const imrec::State state = [&]
            {
                std::scoped_lock l{rec.statusMutex};
                return rec.status.type;
            }();
 
            // Component finished a recording and is ready again.
            if (state == imrec::State::ready)
            {
                ARMARX_TRACE;
 
                ARMARX_DEBUG << "Recording image provider ready.";
 
                break;  // Done.  Exit loop, stop recordings now and terminate thread.
            }
            // Component is scheduled to start a recording.
            else if (state == imrec::State::scheduled)
            {
                ARMARX_TRACE;
 
                const IceUtil::Time start_at = IceUtil::Time::microSeconds(rec.config.startTimestamp);
                const IceUtil::Time now = IceUtil::Time::now();
                if (start_at < now)
                {
                    ARMARX_DEBUG << "Starting recordings...";
 
                    const std::filesystem::path path =
                        armarx::PackagePath::toSystemPath(rec.config.location);
                    rec.channelRecordings.clear();
                    for (int i = 0; i < numberImages; ++i)
                    {
                        const imrec::ChannelConfig& channel_cfg = rec.config.channelConfigs[i];
                        if (not channel_cfg.disabled)
                        {
                            visionx::imrec::Recording r;
                            const std::string name = getName() + "_" + channel_cfg.name;
                            const imrec::Format format = imrec::str2format(channel_cfg.format);
                            r = visionx::imrec::newRecording(path / rec.config.name, name, format, channel_cfg.fps);
                            r->startRecording();
                            r->writeMetadataDatetime("recording_manager_time", std::chrono::microseconds{rec.config.startTimestamp});
                            r->writeMetadataLine("image_provider_name", "string", getName());
                            r->writeMetadataLine("channel_name", "string", channel_cfg.name);
                            rec.channelRecordings[i] = r;
                        }
                    }
 
                    ARMARX_DEBUG << "Started recordings.";
 
                    std::scoped_lock l{rec.statusMutex};
                    rec.status.type = imrec::State::running;
                }
 
                ARMARX_TRACE;
            }
            // Component is running, stopping, or writing.  Either way, try writing frames.
            else
            {
                ARMARX_TRACE;
 
                bool write_frames = false;
                std::chrono::microseconds timestamp;
                std::unordered_map<int, CByteImage*> frames;
 
                {
                    ARMARX_DEBUG << "Fetching frames from buffer...";
 
                    std::scoped_lock l{rec.bufferMutex, rec.statusMutex};
                    if (not rec.buffer.empty())
                    {
                        std::tie(timestamp, frames) = rec.buffer.front();
                        rec.buffer.pop_front();
                        write_frames = true;
 
                        rec.status.framesBuffered = static_cast<long>(rec.buffer.size());
                        ++rec.status.framesWritten;
 
                        ARMARX_DEBUG << "Fetched frames from buffer.";
                    }
                    else
                    {
                        ARMARX_DEBUG << "Buffer empty.";
                        if (state == imrec::State::writing)
                        {
                            ARMARX_DEBUG << "Buffer fully written to disk, exiting.";
                            rec.status.type = imrec::State::ready;
                        }
                    }
                }
 
                if (write_frames)
                {
                    ARMARX_DEBUG << "Writing frames...";
 
                    for (auto& [i, frame] : frames)
                    {
                        imrec::Recording r = rec.channelRecordings.at(i);
                        r->recordFrame(*frame, timestamp);
                        delete frame;
                    }
                    frames.clear();
 
                    ARMARX_DEBUG << "Wrote frames.";
                }
                else if (state == imrec::State::running)
                {
                    std::unique_lock l{rec.bufferMutex};
                    ARMARX_DEBUG << "Waiting for new frames...";
                    rec.cv.wait(l, [&] { return not rec.buffer.empty(); });
                    ARMARX_DEBUG << "Received new frames, continuing.";
                }
 
                ARMARX_TRACE;
            }
        }
 
        ARMARX_TRACE;
 
        ARMARX_DEBUG << "Stopping recordings...";
 
        // Stop.
        for (auto& [i, r] : rec.channelRecordings)
        {
            r->stopRecording();
        }
        rec.channelRecordings.clear();
 
        ARMARX_DEBUG << "Done stopping recordings.";
        ARMARX_DEBUG << "Stopping recording task...";
 
        ARMARX_TRACE;
    }
 
 
    std::vector<imrec::ChannelPreferences>
    ImageProvider::getImageRecordingChannelPreferences(const Ice::Current&)
    {
        ARMARX_TRACE;
 
        std::vector<imrec::ChannelPreferences> default_names;
        imrec::ChannelPreferences cp;
        cp.requiresLossless = false;
        for (int i = 0; i < numberImages; ++i)
        {
            cp.name = "c" + std::to_string(i);
            default_names.push_back(cp);
        }
        return default_names;
    }
}