CapturingImageProvider.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       2012
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "CapturingImageProvider.h"
 
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/observers/variant/Variant.h>
 
#include <string>
#include <thread>
 
using namespace armarx;
namespace visionx
{
    // ================================================================== //
    // == ImageProvider ice interface =================================== //
    // ================================================================== //
    void CapturingImageProvider::startCapture(float framesPerSecond,
            const Ice::Current& ctx)
    {
        std::unique_lock lock(captureMutex);
        onStartCapture(framesPerSecond);
 
        captureEnabled = true;
        frameRate = framesPerSecond;
 
        ARMARX_INFO << "Starting image capture with " << frameRate << "fps";
    }
 
 
    void CapturingImageProvider::stopCapture(const Ice::Current& ctx)
    {
        std::unique_lock lock(captureMutex);
        captureEnabled = false;
        onStopCapture();
    }
 
    // ================================================================== //
    // == Component implementation =============================== //
    // ================================================================== //
    void CapturingImageProvider::onInitImageProvider()
    {
        // init members
        frameRate = hasProperty("FPS") ? getProperty<float>("FPS").getValue() : 30;
        captureEnabled = false;
 
        // default sync mode
        setImageSyncMode(eFpsSynchronization);
 
        // call setup of image provider implementation to setup image size
        onInitCapturingImageProvider();
 
        // capature task
        captureTask = new RunningTask<CapturingImageProvider>(this, &CapturingImageProvider::capture);
        ARMARX_INFO << "started capturing thread";
    }
 
 
    void CapturingImageProvider::onConnectImageProvider()
    {
        onStartCapturingImageProvider();
 
        captureTask->start();
 
        // TODO: hack start capturing
        startCapture(frameRate);
    }
 
 
    void CapturingImageProvider::onExitImageProvider()
    {
        // TODO: hack stop capturing
        stopCapture();
 
        if (captureTask)
        {
            captureTask->stop();
        }
 
        onExitCapturingImageProvider();
    }
 
 
    void CapturingImageProvider::capture()
    {
        ARMARX_INFO << "Starting Image Provider: " << getName();
 
        // main loop of component
        std::chrono::milliseconds td(1);
 
        while (!captureTask->isStopped() && !isExiting() && sharedMemoryProvider)
        {
            auto tmpSharedMemoryProvider = sharedMemoryProvider;
            if (captureEnabled && tmpSharedMemoryProvider)
            {
 
                MetaInfoSizeBasePtr info = tmpSharedMemoryProvider->getMetaInfo();
                ARMARX_CHECK_EXPRESSION(info);
                long oldTimestamp = info ? info->timeProvided : 0;
                bool succeeded = capture(imageBuffers);
 
                if (succeeded)
                {
                    MetaInfoSizeBasePtr info = tmpSharedMemoryProvider->getMetaInfo();
                    ARMARX_CHECK_EXPRESSION(info);
                    IceUtil::Time image_timestamp = armarx::TimeUtil::GetTime();
                    if (!info || info->timeProvided == oldTimestamp)
                    {
                        ARMARX_WARNING << deactivateSpam(10000000) << "The image provider did not set a timestamp - measuring the timestamp now. The ImageProvider implementation should call updateTimestamp()!";
                        updateTimestamp(image_timestamp.toMicroSeconds());
                    }
                    else if (info)
                    {
                        image_timestamp = IceUtil::Time::microSeconds(info->timeProvided);
                    }
                    auto imageProcessorProxy = this->imageProcessorProxy;
                    recordImages(image_timestamp);
                    ARMARX_CHECK_EXPRESSION(imageProcessorProxy);
                    imageProcessorProxy->reportImageAvailable(getName());
                }
 
                if (imageSyncMode == eFpsSynchronization)
                {
                    fpsCounter.assureFPS(frameRate);
                    setMetaInfo("fps", new Variant(fpsCounter.getFPS()));
                    setMetaInfo("minCycleTimeMs", new Variant(fpsCounter.getMinCycleTimeMS()));
                    setMetaInfo("maxCycleTimeMs", new Variant(fpsCounter.getMaxCycleTimeMS()));
                    auto dimension = getImageFormat().dimension;
                    setMetaInfo("resolution", new Variant(std::to_string(dimension.width) + "x" + std::to_string(dimension.height)));
                }
            }
            else
            {
                std::this_thread::sleep_for(td);
            }
        }
 
        ARMARX_INFO << "Stopping ImageProvider";
    }
 
    // ================================================================== //
    // == Utility methods for CapturingImageProviders =================== //
    // ================================================================== //
    void CapturingImageProvider::setImageSyncMode(ImageSyncMode imageSyncMode)
    {
        this->imageSyncMode = imageSyncMode;
    }
 
    ImageSyncMode CapturingImageProvider::getImageSyncMode()
    {
        return imageSyncMode;
    }
}