CapturingPointCloudProvider.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     David Gonzalez Aguirre (david dot gonzalez at kit dot edu)
 * @date       2014
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include "CapturingPointCloudProvider.h"
 
#include <string>
 
using namespace armarx;
namespace visionx
{
    // ================================================================== //
    // == PointCloudProvider ice interface ============================== //
    // ================================================================== //
    void CapturingPointCloudProvider::startCapture(const Ice::Current& ctx)
    {
        ARMARX_TRACE;
        std::unique_lock lock(captureMutex);
 
        onStartCapture(frameRate);
        fpsCounter.reset();
        numFramesToCapture = -1;
        captureEnabled = true;
 
        ARMARX_INFO << "Starting point cloud capture with framerate " << frameRate;
    }
 
    void CapturingPointCloudProvider::startCaptureForNumFrames(int numFrames, const Ice::Current& c)
    {
        ARMARX_TRACE;
        std::unique_lock lock(captureMutex);
 
        onStartCapture(frameRate);
        fpsCounter.reset();
        numFramesToCapture = numFrames;
        captureEnabled = true;
 
        ARMARX_INFO << "Starting point cloud capture with framerate " << frameRate << " (capture " << numFramesToCapture << " frames)";
    }
 
    void CapturingPointCloudProvider::stopCapture(const Ice::Current& ctx)
    {
        ARMARX_TRACE;
        std::unique_lock lock(captureMutex);
 
        captureEnabled = false;
        ARMARX_INFO << "Stopping point cloud capture";
        onStopCapture();
    }
 
    void  CapturingPointCloudProvider::changeFrameRate(float framesPerSecond, const Ice::Current& ctx)
    {
        ARMARX_TRACE;
        std::unique_lock lock(captureMutex);
 
        frameRate = framesPerSecond;
    }
 
    bool CapturingPointCloudProvider::isCaptureEnabled(const Ice::Current& c)
    {
        return captureEnabled;
    }
 
 
    // ================================================================== //
    // == Component implementation ====================================== //
    // ====================================t============================== //
    void CapturingPointCloudProvider::onInitPointCloudProvider()
    {
        ARMARX_TRACE;
        captureEnabled = false;
        frameRate = getProperty<float>("framerate").getValue();
 
        if (frameRate > 0)
        {
            setPointCloudSyncMode(eFpsSynchronization);
        }
        else
        {
            ARMARX_INFO << "framerate is zero. ignoring";
            setPointCloudSyncMode(eCaptureSynchronization);
        }
 
 
        // call setup of point clouds provider implementation to setup point clouds size
        onInitCapturingPointCloudProvider();
 
        // capture task
        captureTask = new RunningTask<CapturingPointCloudProvider>(this, &CapturingPointCloudProvider::capture);
 
    }
 
 
    void CapturingPointCloudProvider::onConnectPointCloudProvider()
    {
        ARMARX_TRACE;
        onStartCapturingPointCloudProvider();
 
        captureTask->start();
 
        bool isEnabled = getProperty<bool>("isEnabled").getValue();
 
        if (isEnabled)
        {
            startCapture();
        }
    }
 
 
    void CapturingPointCloudProvider::onExitPointCloudProvider()
    {
        ARMARX_TRACE;
        // TODO: hack stop capturing
        stopCapture();
 
        if (captureTask)
        {
            captureTask->stop();
        }
 
        onExitCapturingPointCloudProvider();
    }
 
 
    void CapturingPointCloudProvider::capture()
    {
        ARMARX_TRACE;
        ARMARX_INFO << "Starting point clouds Provider: " << getName() << flush;
 
        // main loop of component
        std::chrono::milliseconds td(1);
 
        while (!captureTask->isStopped() && !isExiting())
        {
            if (captureEnabled)
            {
                std::unique_lock lock(captureMutex);
 
                doCapture();
 
                if (numFramesToCapture > 0)
                {
                    numFramesToCapture--;
                }
                if (numFramesToCapture == 0)
                {
                    captureEnabled = false;
                    continue;
                }
 
                if (pointCloudSyncMode == eFpsSynchronization)
                {
                    fpsCounter.assureFPS(frameRate);
                }
            }
            else
            {
                std::this_thread::sleep_for(td);
            }
        }
 
        ARMARX_INFO << "Stopping point clouds Provider" << flush;
    }
 
    // ================================================================== //
    // == Utility methods for CapturingPointCloudProviders =================== //
    // ================================================================== //
    void CapturingPointCloudProvider::setPointCloudSyncMode(ImageSyncMode pointCloudSyncMode)
    {
        std::unique_lock lock(captureMutex);
 
        this->pointCloudSyncMode = pointCloudSyncMode;
    }
 
    ImageSyncMode CapturingPointCloudProvider::getPointCloudSyncMode()
    {
        std::unique_lock lock(captureMutex);
 
        return pointCloudSyncMode;
    }
}