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/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/services/sharedmemory/IceSharedMemoryProvider.h>
#include <ArmarXCore/core/time/TimeUtil.h>
 
#include <VisionX/interface/core/DataTypes.h>
#include <VisionX/interface/core/PointCloudProcessorInterface.h>
#include <VisionX/interface/core/PointCloudProviderInterface.h>
#include <VisionX/tools/FPSCounter.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 ============================== //
    // ====================================================================== //
    /**
     * PointCloudProvider abstract class defines a component which provide point clouds via ice or shared memory.
     */
    class PointCloudProvider :
        virtual public armarx::Component,
        virtual public PointCloudProviderInterface
    {
 
    public:
        // ================================================================== //
        // == PointCloudProvider ice interface ============================== //
        // ================================================================== //
 
        /**
         * Retrieve point clouds via Ice. Bypasses the IcesharedMemoryProvider
         */
        armarx::Blob getPointCloud(MetaPointCloudFormatPtr& info,
                                   const Ice::Current& c = Ice::emptyCurrent) override;
 
        /**
         * Returns the point cloud format info struct via Ice
         */
        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 =============================== //
        // ================================================================== //
        /**
         * This is called when the Component::onInitComponent() is called.
         *
         * Implement this method in the derived PointCloudProvider in order to setup its parameters.
         * Use this to set the point cloud format.
         */
        virtual void onInitPointCloudProvider() = 0;
 
        /**
         * This is called when the Component::onConnectComponent() setup is called
         */
        virtual void
        onConnectPointCloudProvider()
        {
        }
 
        /**
         * This is called when the Component::onExitComponent() setup is called
         *
         * Implement this method in the derived PointCloudProvider in order clean up right before terminating.
         */
        virtual void onExitPointCloudProvider() = 0;
 
        /**
         * offer the new point cloud.
         */
        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 ====================================== //
        // ================================================================== //
        /**
        * @see Component::onInitComponent()
        */
        void onInitComponent() override;
 
        /**
         * @see Component::onConnectComponent()
         */
        void onConnectComponent() override;
 
        void onDisconnectComponent() override;
 
        /**
         * @see Component::onExitComponent()
         */
        void onExitComponent() override;
 
    protected:
        /**
         * Retrieve whether provider is exiting
         *
         * @return exiting status
         */
        bool
        isExiting() const
        {
            return exiting;
        }
 
        void updateComponentMetaInfo(const MetaPointCloudFormatPtr& info);
 
        /**
         * Ice proxy of the point cloud processor interface
         */
        PointCloudProcessorInterfacePrx pointCloudProcessorProxy;
 
        /**
         * default point cloud format used to initialize shared memory
         */
        virtual MetaPointCloudFormatPtr getDefaultPointCloudFormat();
 
    private:
        /**
         * shared memory provider
        */
        armarx::IceSharedMemoryProvider<unsigned char, MetaPointCloudFormat>::pointer_type
            sharedMemoryProvider;
 
        /**
         * Point cloud information
         */
        MetaPointCloudFormatPtr pointCloudFormat;
 
        /**
         * Indicates that exit is in process and thread needs to be stopped
         */
        bool exiting;
 
        int seq;
        FPSCounter fps;
 
        std::vector<unsigned char> intermediateBuffer;
    };
} // namespace visionx