PointCloudAndImageProcessor.h

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
 */
 
#pragma once
 
 
// VisionX
#include <VisionX/interface/core/DataTypes.h>
#include <VisionX/interface/core/PointCloudProcessorInterface.h>
 
#include <VisionX/core/ImageProcessor.h>
#include <VisionX/components/pointcloud_core/PointCloudProcessor.h>
 
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
 
namespace visionx
{
 
    class ResultImageAndPointCloudProvider
        : public virtual ResultImageProvider,
          public virtual ResultPointCloudProvider,
          public virtual ImageAndPointCloudProviderInterface
    {
        friend class PointCloudAndImageProcessor;
 
    public:
        std::string getDefaultName() const
        {
            return ResultPointCloudProvider::getDefaultName();
        }
 
        // ManagedIceObject interface
    protected:
        void onInitComponent()
        {
            ARMARX_IMPORTANT << "ResultImageProvider::onInitComponent();";
            ResultImageProvider::onInitComponent();
            ARMARX_IMPORTANT << "ResultPointCloudProvider::onInitComponent();";
            ResultPointCloudProvider::onInitComponent();
        }
 
        void onConnectComponent()
        {
            ARMARX_IMPORTANT << "ResultImageProvider::onConnectComponent();";
            ResultImageProvider::onConnectComponent();
            ARMARX_IMPORTANT << "ResultPointCloudProvider::onConnectComponent();";
            ResultPointCloudProvider::onConnectComponent();
        }
        void onDisconnectComponent()
        {
            ARMARX_IMPORTANT << "ResultImageProvider::onDisconnectComponent();";
            ResultImageProvider::onDisconnectComponent();
            ARMARX_IMPORTANT << "ResultPointCloudProvider::onDisconnectComponent();";
            ResultPointCloudProvider::onDisconnectComponent();
        }
        void onExitComponent()
        {
            ARMARX_IMPORTANT << "ResultImageProvider::onExitComponent();";
            ResultImageProvider::onExitComponent();
            ARMARX_IMPORTANT << "ResultPointCloudProvider::onExitComponent();";
            ResultPointCloudProvider::onExitComponent();
        }
 
    public:
        bool hasSharedMemorySupport(const Ice::Current& c)
        {
            return ResultImageProvider::hasSharedMemorySupport(c) && ResultPointCloudProvider::hasSharedMemorySupport(c);
        }
    };
 
    /**
     * The PointCloudAndImageProcessor class provides an interface for access to
     * PointCloudProviders and ImageProviders via Ice and shared memory. The interface
     * defines a set of convenience methods which simplify the pointcloud and image access.
     */
    class PointCloudAndImageProcessor :
        virtual public ImageProcessor,
        virtual public PointCloudProcessor,
        virtual public PointCloudAndImageProcessorInterface
    {
    protected:
        // ================================================================== //
        // == Interface of PointCloudAndImageProcessor ====================== //
        // ================================================================== //
        /**
         * Setup the vision component.
         *
         * Implement this method in your PointCloudAndImageProcessor in order to setup
         * its parameters. Use this for the registration of adapters and
         * subscription to topics
         */
        virtual void onInitPointCloudAndImageProcessor() = 0;
 
        /**
         * Implement this method in your PointCloudAndImageProcessor in order execute parts
         * when the component is fully initialized and about to run.
         */
        virtual void onConnectPointCloudAndImageProcessor() = 0;
 
        /**
         * Implement this method in the PointCloudAndImageProcessor in order to execute parts
         * when the component looses network connectivity.
         */
        virtual void onDisconnectPointCloudAndImageProcessor() { }
 
        /**
         * Exit the ImapeProcessor component.
         *
         * Implement this method in order to clean up the PointCloudAndImageProcessor
         */
        virtual void onExitPointCloudAndImageProcessor() = 0;
 
        /**
         * Process the vision component.
         *
         * The main loop of the PointCloudAndImageProcessor to be implemented in the
         * subclass. Do not block this method. One process should execute
         * exactly one processing step.
         */
        void process() override { }
 
        // ================================================================== //
        // == RunningComponent implementation =============================== //
        // ================================================================== //
        /**
         * @see Component::onInitComponent()
         */
        void onInitComponent() override
        {
            ImageProcessor::onInitComponent();
            PointCloudProcessor::onInitComponent();
        }
 
        /**
         * @see Component::onConnectComponent()
         */
        void onConnectComponent() override
        {
            // Prevent race condition by not starting the process task in the image processor
            ImageProcessor::startProcessorTask = false;
            ImageProcessor::onConnectComponent();
            PointCloudProcessor::onConnectComponent();
        }
 
        /**
         * @see Component::onDisconnectComponent()
         */
        void onDisconnectComponent() override
        {
            ImageProcessor::onDisconnectComponent();
            PointCloudProcessor::onDisconnectComponent();
        }
 
        /**
         * @see Component::onExitComponent()
         */
        void onExitComponent() override
        {
            ImageProcessor::onExitComponent();
            PointCloudProcessor::onExitComponent();
        }
 
 
        // those are replaced by the on*PointCloudAndImageProcessor()
        void onInitImageProcessor() override { }
        void onConnectImageProcessor() override { }
        void onExitImageProcessor() override { }
 
        void onInitPointCloudProcessor() override
        {
            onInitPointCloudAndImageProcessor();
        }
        void onConnectPointCloudProcessor() override
        {
            onConnectPointCloudAndImageProcessor();
        }
        void onDisconnectPointCloudProcessor() override
        {
            onDisconnectPointCloudAndImageProcessor();
        }
        void onExitPointCloudProcessor() override
        {
            onExitPointCloudAndImageProcessor();
        }
 
 
        template<typename PointT>
        void enableResultImagesAndPointClouds(std::string resultProviderName,
                                              int numberImages, ImageDimension imageDimension, ImageType imageType)
        {
            if (!resultImageProvider)
            {
                ARMARX_VERBOSE << "Enabling ResultImageProvider with " << numberImages << " result images.";
                IceInternal::Handle<ResultImageAndPointCloudProvider> myProvider = Component::create<ResultImageAndPointCloudProvider>();
                myProvider->setName(resultProviderName.empty() ? getName() + "Result" : resultProviderName);
 
 
                myProvider->setNumberResultImages(numberImages);
                myProvider->setResultImageFormat(imageDimension, imageType);
 
 
                resultImageProvider = myProvider;
 
                if (resultProviderName == "")
                {
                    resultProviderName = getName() + "Result";
                }
 
                std::unique_lock lock(resultProviderMutex);
 
                if (resultPointCloudProviders.count(resultProviderName))
                {
                    ARMARX_WARNING << "result point cloud provider already exists: " << resultProviderName;
                }
                else
                {
                    MetaPointCloudFormatPtr info = pointCloudProviderInfoMap.begin()->second.pointCloudFormat;
 
                    PointContentType pointContentType = tools::getPointContentType<PointT>();
 
                    size_t capacity = info->capacity * tools::getBytesPerPoint(pointContentType) / tools::getBytesPerPoint(info->type);
 
                    //enableResultPointClouds(resultProviderName, capacity, pointContentType);
 
                    //IceInternal::Handle<ResultPointCloudProvider> resultProvider = Component::create<ResultPointCloudProvider>();
                    myProvider->setName(resultProviderName);
                    myProvider->setShmCapacity(capacity);
                    myProvider->setPointContentType(pointContentType);
 
                    //getArmarXManager()->addObject(myProvider);
 
                    {
                        resultPointCloudProviders.emplace(myProvider->getName(), myProvider);
                    }
                    lock.unlock();
 
                    //myProvider->getObjectScheduler()->waitForObjectState(eManagedIceObjectStarted);
 
                }
 
                ARMARX_IMPORTANT << 7;
                getArmarXManager()->addObject(myProvider);
                // wait for resultImageProvider
                ARMARX_IMPORTANT << "before waitForObjectState(armarx::eManagedIceObjectStarted);";
                myProvider->getObjectScheduler()->waitForObjectState(armarx::eManagedIceObjectStarted);
                ARMARX_IMPORTANT << "after waitForObjectState(armarx::eManagedIceObjectStarted);";
 
            }
        }
    };
}