PointCloudProvider.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
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/services/sharedmemory/IceSharedMemoryProvider.h>
#include <VisionX/tools/FPSCounter.h>
 
 
#include <VisionX/interface/core/DataTypes.h>
#include <VisionX/interface/core/PointCloudProcessorInterface.h>
#include <VisionX/interface/core/PointCloudProviderInterface.h>
 
#include "PointCloudConversions.h"
 
namespace visionx
{
 
    class PointCloudProviderPropertyDefinitions:
        public armarx::ComponentPropertyDefinitions
    {
    public:
        PointCloudProviderPropertyDefinitions(std::string prefix):
            ComponentPropertyDefinitions(prefix)
        {
            defineOptionalProperty<std::string>("frameName", "", "name of the source");
        }
    };
 
    // ====================================================================== //
    // == class PointCloudProvider declaration ============================== //
    // ====================================================================== //
    class PointCloudProvider :
        virtual public armarx::Component,
        virtual public PointCloudProviderInterface
    {
 
    public:
        // ================================================================== //
        // == PointCloudProvider ice interface ============================== //
        // ================================================================== //
 
        armarx::Blob getPointCloud(MetaPointCloudFormatPtr& info, const Ice::Current& c = Ice::emptyCurrent) override;
 
        MetaPointCloudFormatPtr getPointCloudFormat(const Ice::Current& c = Ice::emptyCurrent) override;
 
        bool hasSharedMemorySupport(const Ice::Current& c = Ice::emptyCurrent) override
        {
            return true;
        }
 
        void enablePointCloudVisualization(bool enable, bool preferRGB, const Ice::Current& c = Ice::emptyCurrent);
 
    protected:
        // ================================================================== //
        // == Interface of PointCloudProvider =============================== //
        // ================================================================== //
        virtual void onInitPointCloudProvider() = 0;
 
        virtual void onConnectPointCloudProvider() {}
 
        virtual void onExitPointCloudProvider() = 0;
 
 
        template<typename PointCloudPtrT>
        void providePointCloud(PointCloudPtrT pointCloudPtr)
        {
            using PointCloudT = typename PointCloudPtrT::element_type;
            using PointT = typename PointCloudT::PointType;
 
            if (!sharedMemoryProvider)
            {
                ARMARX_WARNING << "Shared memory provider is null (possibly shutting down)";
                return;
            }
            MetaPointCloudFormatPtr info(MetaPointCloudFormatPtr::dynamicCast(sharedMemoryProvider->getMetaInfo()->ice_clone()));
            ARMARX_CHECK_EXPRESSION(info);
            info->type = visionx::tools::getPointContentType<PointT>();
            info->size = pointCloudPtr->width * pointCloudPtr->height * visionx::tools::getBytesPerPoint(info->type);
 
            if (info->size > info->capacity)
            {
                const size_t maxPoints = info->capacity / visionx::tools::getBytesPerPoint(info->type);
                ARMARX_ERROR << deactivateSpam(1) << "Exceeding the capacity limit of supported points ("
                             << (maxPoints)  << " points). The provided point cloud contains "
                             << pointCloudPtr->width* pointCloudPtr->height
                             << " points. Cropping the point cloud to fit into limit.";
 
                pointCloudPtr->width = maxPoints;
                pointCloudPtr->height = 1;
                pointCloudPtr->resize(maxPoints);
                info->size = maxPoints * visionx::tools::getBytesPerPoint(info->type);
            }
 
            info->width = pointCloudPtr->width;
            info->height = pointCloudPtr->height;
 
            if (pointCloudPtr->header.stamp)
            {
                info->timeProvided = pointCloudPtr->header.stamp;
            }
            else
            {
                info->timeProvided = armarx::TimeUtil::GetTime().toMicroSeconds();
            }
 
            if (pointCloudPtr->header.seq)
            {
                info->seq = pointCloudPtr->header.seq;
            }
            else
            {
                info->seq = ++seq;
            }
 
            //            if (!pointCloudPtr->header.frame_id.empty())
            //            {
            //                info->frameId = pointCloudPtr->header.frame_id;
            //            }
            ARMARX_CHECK_GREATER_EQUAL((int)intermediateBuffer.size(), info->capacity);
            ARMARX_CHECK_GREATER_EQUAL((int)intermediateBuffer.size(), info->size);
            ARMARX_CHECK_GREATER(intermediateBuffer.size(), 0);
            auto buf = intermediateBuffer.data();
            ARMARX_CHECK_NOT_NULL(buf);
            visionx::tools::convertFromPCL(pointCloudPtr, (void**) &buf);
            {
                armarx::SharedMemoryScopedWriteLockPtr lock(sharedMemoryProvider->getScopedWriteLock());
 
                sharedMemoryProvider->setMetaInfo(info, false);
 
                unsigned char* buffer = sharedMemoryProvider->getBuffer();
 
                memcpy(buffer, intermediateBuffer.data(), info->size);
            }
            updateComponentMetaInfo(info);
            // notify processors
            pointCloudProcessorProxy->reportPointCloudAvailable(getName());
        }
 
        // ================================================================== //
        // == Component implementation ====================================== //
        // ================================================================== //
        void onInitComponent() override;
 
        void onConnectComponent() override;
 
        void onDisconnectComponent() override;
 
        void onExitComponent() override;
 
    protected:
 
        bool isExiting() const
        {
            return exiting;
        }
 
        void updateComponentMetaInfo(const MetaPointCloudFormatPtr& info);
 
        PointCloudProcessorInterfacePrx pointCloudProcessorProxy;
 
        virtual MetaPointCloudFormatPtr getDefaultPointCloudFormat();
 
    private:
 
        armarx::IceSharedMemoryProvider<unsigned char, MetaPointCloudFormat>::pointer_type sharedMemoryProvider;
 
        MetaPointCloudFormatPtr pointCloudFormat;
 
        bool exiting;
 
        int seq;
        FPSCounter fps;
 
        std::vector<unsigned char> intermediateBuffer;
    };
}