PointCloudProcessor.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
 
// ArmarXCore
#include <ArmarXCore/core/IceManager.h>
#include <ArmarXCore/core/services/sharedmemory/IceSharedMemoryConsumer.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <ArmarXCore/core/util/algorithm.h>
 
// VisionXInterfaces
#include <VisionX/interface/core/DataTypes.h>
#include <VisionX/interface/core/PointCloudProcessorInterface.h>
 
// VisionXTools
#include <condition_variable>
#include <mutex>
#include <shared_mutex>
 
#include <IceUtil/IceUtil.h>
 
#include <VisionX/tools/FPSCounter.h>
 
#include "PointCloudConversions.h"
#include "PointCloudProvider.h"
 
namespace visionx
{
    // ====================================================================== //
    // == class PointCloudTransferStats declaration ============================== //
    // ====================================================================== //
 
    /**
     * The PointCloudTransferStats class provides information on the connection
     * between PointCloudProvider and PointCloudProcessor. Use
     * PointCloudProcessorBase::getPointCloudTransferStats() in order to retrieve the
     * statistics
     */
    class PointCloudTransferStats
    {
 
    public:
        PointCloudTransferStats()
        {
            pointCloudProviderFPS.reset();
            pollingFPS.reset();
        }
 
        /// Statistics for the PointClouds announced by the PointCloudProvider.
        FPSCounter pointCloudProviderFPS;
 
        /// Statistics for the PointClouds polled by the PointCloudProcessor.
        FPSCounter pollingFPS;
    };
 
    // ====================================================================== //
    // == class PointCloudProviderInfo declaration ========================== //
    // ====================================================================== //
 
    class PointCloudProviderInfo
    {
    public:
        /// Proxy to PointCloud provider.
        PointCloudProviderInterfacePrx proxy;
 
        /// Memory block.
        std::vector<unsigned char> buffer;
 
        /// PointCloud format struct that contains all necessary PointCloud information.
        MetaPointCloudFormatPtr pointCloudFormat;
 
        /// Indicates whether an PointCloud is available.
        bool pointCloudAvailable;
 
        /// Transfer mode of images
        ImageTransferMode pointCloudTransferMode;
 
        /// Conditional variable used internally for synchronization purposes
        std::shared_ptr<std::condition_variable> pointCloudAvailableEvent;
    };
 
    class ResultPointCloudProviderPropertyDefinitions : public PointCloudProviderPropertyDefinitions
    {
    public:
        ResultPointCloudProviderPropertyDefinitions(std::string prefix);
    };
 
    // ====================================================================== //
    // == class ResultPointCloudProvider declaration ======================== //
    // ====================================================================== //
    /**
     * The ResultPointCloudProvider is used by the PointCloudProcessor to stream
     * result PointClouds to any other processor (e.g. PointCloudMonitor)
     * Use PointCloudProcessor::enableVisualization() and PointCloudProcessor::provideResultPointClouds()
     * in order to offer result PointClouds in an PointCloud processor.
     */
    class ResultPointCloudProvider : virtual public PointCloudProvider
    {
        friend class PointCloudProcessor;
 
    public:
        ResultPointCloudProvider();
 
        template <typename PointCloudPtrT>
        void
        provideResultPointClouds(PointCloudPtrT pointCloudPtr)
        {
            providePointCloud(pointCloudPtr);
        }
 
    protected:
        void setResultPointCloudProviderName(const std::string& name);
 
        virtual std::string getDefaultName() const override;
 
        void setShmCapacity(size_t shmCapacity);
        size_t getShmCapacity();
 
        void setPointContentType(PointContentType type);
        PointContentType getPointContentType() const;
 
 
        virtual void onInitPointCloudProvider() override;
        virtual void onExitPointCloudProvider() override;
 
        virtual MetaPointCloudFormatPtr getDefaultPointCloudFormat() override;
        virtual armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
 
    private:
        std::string resultPointCloudProviderName;
 
        size_t shmCapacity;
 
        PointContentType pointContentType;
    };
 
    /**
     * @brief Properties of `PointCloudProcessor`.
     *
     * Let your own point cloud processor's properties derive from this class
     * to get the default properties of `PointCloudProcessor`. These include
     * `ProviderName`, `FrameRate` and `AutomaticTypeConversion`.
     */
    class PointCloudProcessorPropertyDefinitions : public armarx::ComponentPropertyDefinitions
    {
    public:
        PointCloudProcessorPropertyDefinitions(std::string prefix);
    };
 
    /**
     * The PointCloudProcessor class provides an interface for access to
     * PointCloudProviders via Ice and shared memory. The interface defines a set of
     * convenience methods which simplify the PointCloud access.
     */
    class PointCloudProcessor :
        virtual public armarx::Component,
        virtual public PointCloudProcessorInterface
    {
    public:
        /**
         * Registers a delayed topic subscription and a delayed provider proxy
         * retrieval which will be available on the start of the component.
         *
         * @param name                  Provider name
         */
        void usingPointCloudProvider(std::string providerName);
 
        void
        usingPointCloudProviderFromProperty(const std::string& prop)
        {
            usingPointCloudProvider(getProperty<std::string>(prop).getValue());
        }
 
        /**
         * Removes topic subscription and provider proxy dependency to release
         * a point cloud provider. After this, provider is not available in
         * waitForPointClouds() or getPointClouds() calls.
         */
        void releasePointCloudProvider(std::string providerName);
 
        /**
         * Select an PointCloudProvider.
         *
         * This method subscribes to an an PointCloudProvider and makes the provider
         * available in the waitForPointClouds() and getPointClouds() methods.
         *
         * @param name                  Ice adapter name of the PointCloudProvider
         * @param waitForProxy          If true, this function blocks until the proxy for the pointCloudProvider becomes available.
         *
         * @return Information of the PointCloud provider
         */
        PointCloudProviderInfo getPointCloudProvider(std::string name, bool waitForProxy = false);
        PointCloudProviderInfo getPointCloudProvider(bool waitForProxy = false);
 
        /// Get the names of providers for which `usingPointCloudProvider()` has been called.
        std::vector<std::string> getPointCloudProviderNames() const;
        /// Indicate whether the given name identifies a known point cloud provider.
        bool isPointCloudProviderKnown(const std::string& providerName) const;
 
        /**
         * Enables visualization
         *
         * @param numberPointClouds number of PointClouds provided by the visualization
         * @param PointCloud::Grid2DDimensions size of PointClouds
         * @param PointCloudFormatInfo type of PointClouds
         *
         * @return Information of the PointCloud provider
         */
        template <typename PointT>
        void
        enableResultPointClouds(std::string resultProviderName = "")
        {
            std::shared_lock lock(pointCloudProviderInfoMutex);
 
            if (pointCloudProviderInfoMap.size() == 1)
            {
                enableResultPointCloudForInputProvider<PointT>(
                    pointCloudProviderInfoMap.begin()->first, resultProviderName);
            }
            else
            {
                ARMARX_ERROR << "unable to determine shared memory capacity for result provider. "
                                "(result provider is not enabled!)";
            }
        }
 
        template <typename PointT>
        void
        enableResultPointCloudForInputProvider(const std::string& inputProviderName,
                                               const std::string& resultProviderName = "")
        {
            std::shared_lock lock(pointCloudProviderInfoMutex);
 
            if (pointCloudProviderInfoMap.count(inputProviderName))
            {
                MetaPointCloudFormatPtr info =
                    pointCloudProviderInfoMap.at(inputProviderName).pointCloudFormat;
 
                PointContentType pointContentType = tools::getPointContentType<PointT>();
 
                size_t capacity = info->capacity * tools::getBytesPerPoint(pointContentType) /
                                  tools::getBytesPerPoint(info->type);
 
                enableResultPointClouds(resultProviderName, capacity, pointContentType);
            }
            else
            {
                ARMARX_ERROR << "unable to determine shared memory capacity for result provider. "
                                "(result provider is not enabled!)"
                             << "\nThe according input provider is " << inputProviderName;
            }
        }
 
        void enableResultPointClouds(std::string resultProviderName,
                                     size_t shmCapacity,
                                     PointContentType pointContentType);
 
        /**
         * sends result PointClouds for visualization
         * @see enableVisualization
         *
         * @param PointClouds array of PointClouds to send
         */
        template <typename PointCloudPtrT>
        void
        provideResultPointClouds(const PointCloudPtrT& pointClouds, std::string providerName = "")
        {
            std::shared_lock lock(resultProviderMutex);
 
            if (providerName == "" && resultPointCloudProviders.size() > 0)
            {
                providerName = resultPointCloudProviders.begin()->first;
            }
 
            if (resultPointCloudProviders.count(providerName))
            {
                resultPointCloudProviders[providerName]->template provideResultPointClouds(
                    pointClouds);
            }
            else
            {
                ARMARX_WARNING << "unable to find provider name: " << providerName
                               << "\nknown names :\n"
                               << armarx::getMapKeys(resultPointCloudProviders);
            }
        }
 
        template <typename PointCloudPtrT>
        void
        provideResultPointClouds(const std::string& providerName, const PointCloudPtrT& pointClouds)
        {
            provideResultPointClouds(pointClouds, providerName);
        }
 
        /**
         * Wait for new PointClouds.
         *
         * Wait for new PointCloud of an PointCloud provider. Use if only one
         * PointCloudProvider is used (see usingPointCloudProvider).
         *
         * @param milliseconds          Timeout for waiting
         *
         * @return True if new PointClouds are available. False in case of error or
         *         timeout
         */
        bool waitForPointClouds(int milliseconds = 1000);
 
        /**
         * Wait for new PointClouds.
         *
         * Wait for new PointCloud of an PointCloud provider. Use if multiple
         * PointCloudProviders are used (see usingPointCloudProvider).
         *
         * @param providerName          Name of provider to wait for PointClouds
         * @param milliseconds          Timeout for waiting
         *
         * @return True if new PointClouds are available. False in case of error or
         *         timeout
         */
        bool waitForPointClouds(const std::string& providerName, int milliseconds = 1000);
 
        /**
         * Returns current status for the given point cloud. True if new data is available, false otherwise.
         *
         * This operation does not block. It just returns the current status.
         * Use this if you want to get status of a point cloud without polling
         * it for a given time like the other methods do.
         *
         * @param providerName          Name of provider to wait for PointClouds
         *
         * @return True if new data from the provider is available. False otherwise.
         */
        bool pointCloudHasNewData(std::string providerName);
 
        /**
         * Poll PointClouds from provider.
         *
         * Polls PointClouds from a used PointCloudProvider either via shared memory or
         * via Ice. If both components run on the same machine, shared memory
         * transfer is used. Otherwise Ice is used for PointCloud transmission. The
         * transfer type is decided in the usingPointCloudProvider method and is set
         * in the corresponding PointCloudFormatInfo.
         *
         * Use this method if only one PointCloudProvider is used.
         *
         * @param ppPointClouds              PointCloud buffers where the PointClouds are
         *                              copied to. The buffers have to be
         *                              initialized by the component. All
         *                              required information for the allocation
         *                              of the buffers can be found in the
         *                              corresponding PointCloudFormatInfo.
         *
         * @return Number of PointClouds copied. Zero if no new PointClouds have been
         *         available.
         */
        template <typename PointCloudPtrT>
        int
        getPointClouds(const PointCloudPtrT& pointCloudPtr)
        {
            if (pointCloudProviderInfoMap.size() != 1)
            {
                ARMARX_ERROR << "Calling getPointClouds without PointCloudProvider name but using "
                                "multiple PointCloudProviders or without usingPointCloudProvider";
                return false;
            }
 
            std::shared_lock lock(pointCloudProviderInfoMutex);
 
            std::string providerName = pointCloudProviderInfoMap.begin()->first;
 
            lock.unlock();
 
            return getPointClouds(providerName, pointCloudPtr);
        }
 
        /**
         * Poll PointClouds from provider.
         *
         * Polls PointClouds from a used PointCloudProvider either via shared memory or
         * via Ice. If both components run on the same machine, shared memory
         * transfer is used. Otherwise Ice is used for PointCloud transmission. The
         * transfer type is decided in the usingPointCloudProvider method and is set
         * in the corresponding PointCloudFormatInfo.
         *
         * Use this method if multiple PointCloudProviders are used.
         *
         * @param providerName          Name of provider to poll from
         *
         * @param ppPointClouds              PointCloud buffers where the PointClouds are
         *                              copied to. The buffers have to be
         *                              initialized by the component. All
         *                              required information for the allocation
         *                 PointCloudProcessor.h             of the buffers can be found in the
         *                              corresponding PointCloudFormatInfo.
         *
         * @return Number of PointClouds copied. Zero if no new PointClouds have been
         *         available.
         */
        template <typename PointCloudPtrT>
        bool
        getPointClouds(std::string providerName, const PointCloudPtrT& pointClouds)
        {
            using PointCloudT = typename PointCloudPtrT::element_type;
            using PointT = typename PointCloudT::PointType;
 
            if (pointClouds == NULL)
            {
                ARMARX_ERROR << "pointClouds is NULL";
                return false;
            }
 
            std::shared_lock lock(pointCloudProviderInfoMutex);
 
            // find PointCloud provider
            std::map<std::string, PointCloudProviderInfo>::iterator iter =
                pointCloudProviderInfoMap.find(providerName);
 
            if (iter == pointCloudProviderInfoMap.end())
            {
                ARMARX_ERROR << "Trying to retrieve PointClouds from unknown PointCloud provider. "
                                "Call usingPointCloudProvider before";
                return false;
            }
 
            // check if new PointClouds are available
            if (!iter->second.pointCloudAvailable)
            {
                ARMARX_WARNING << "no point cloud available. use waitForPointCloud()";
                return false;
            }
 
            PointCloudProviderInfo& providerInfo = iter->second;
 
            if (providerInfo.pointCloudFormat->type !=
                    visionx::tools::getPointContentType<PointT>() &&
                !automaticConversion)
            {
                auto requested = visionx::tools::getPointContentType<PointT>();
                ARMARX_WARNING << deactivateSpam(5) << "Requested point cloud format ("
                               << tools::toString(requested) << ")"
                               << " differs from provided format ("
                               << tools::toString(providerInfo.pointCloudFormat->type) << "),"
                               << "\nbut automatic type conversion is disabled."
                               << "\nTo enable automatic type conversion, set the property "
                                  "'AutomaticTypeConversion' to true"
                               << "\n (make sure your point cloud processor's properties derive "
                                  "from 'PointCloudProcessorPropertyDefinitions')."
                               << "\nThe received point cloud will appear empty.";
                return false;
            }
 
            try
            {
                if (providerInfo.pointCloudTransferMode == eIceTransfer)
                {
                    //                    providerInfo.info->timeProvided = TimeUtil::GetTime().toMicroSeconds();
                    std::vector<Ice::Byte> blob =
                        providerInfo.proxy->getPointCloud(providerInfo.pointCloudFormat);
                    blob.swap(providerInfo.buffer);
                }
                else
                {
                    usedPointCloudProviders[providerName]->getData(providerInfo.buffer,
                                                                   providerInfo.pointCloudFormat);
                }
                //                usedPointCloudProviders[providerName]->getData(providerInfo.buffer, providerInfo.pointCloudFormat);
 
                void* bufferPtr = providerInfo.buffer.data();
                void** bufferPtrPtr = &bufferPtr; // Why though?
                visionx::tools::convertToPCL(
                    bufferPtrPtr, providerInfo.pointCloudFormat, pointClouds);
            }
            catch (...)
            {
                ARMARX_ERROR << "unable to get point cloud: "
                             << armarx::GetHandledExceptionString();
                return false;
            }
 
 
            // todo lock for writing...
            iter->second.pointCloudAvailable = false;
 
            IceUtil::Time timeReceived = IceUtil::Time::now();
            IceUtil::Time timeProvided =
                IceUtil::Time::microSeconds(providerInfo.pointCloudFormat->timeProvided);
 
            pointClouds->header.stamp = providerInfo.pointCloudFormat->timeProvided;
            pointClouds->header.seq = providerInfo.pointCloudFormat->seq;
            //pointClouds->header.frame_id = providerInfo.pointCloudFormat->frameId;
 
            long transferTime = (timeReceived - timeProvided).toMilliSeconds();
 
            ARMARX_DEBUG << "received point cloud size: " << pointClouds->width << "x"
                         << pointClouds->height << ". took " << transferTime << " ms.";
 
            std::unique_lock lock2(statisticsMutex);
 
            statistics[providerName].pollingFPS.update();
 
            return true;
        }
 
        MetaPointCloudFormatPtr getPointCloudFormat(std::string providerName);
 
 
        /**
         * Retrieve statistics for a connection to an PointCloudProvider.
         *
         * @param providerName          Name of the provider
         * @param resetStats            Reset statistics
         *
         * @return Reference to statistics for the connection to the provder
         */
        PointCloudTransferStats getPointCloudTransferStats(std::string providerName,
                                                           bool resetStats = false);
 
        /**
         * @brief Get the reference frame of the point cloud by given provider.
         * @param providerName The point cloud provider's name.
         * @return The point cloud's reference frame, or empty string if provider
         *  is unknown or has not specified a reference frame.
         */
        std::string getPointCloudFrame(const std::string& providerName,
                                       const std::string& defaultFrame = "");
 
    protected:
        // ================================================================== //
        // == Interface of an PointCloudProcessor =========================== //
        // ================================================================== //
        /**
         * Setup the vision component.
         *
         * Implement this method in the PointCloudProcessor in order to setup its
         * parameters. Use this for the registration of adaptars and
         * subscription to topics
         *
         * @param argc number of filtered command line arguments
         * @param argv filtered command line arguments
         */
        virtual void onInitPointCloudProcessor() = 0;
 
        /**
         * Implement this method in the PointCloudProcessor in order execute parts
         * when the component is fully initialized and about to run.
         */
        virtual void onConnectPointCloudProcessor() = 0;
 
        /**
         * Implement this method in the PointCloudProcessor in order execute parts
         * when the component looses network connectivity.
         */
        virtual void
        onDisconnectPointCloudProcessor()
        {
        }
 
        /**
         * Exit the ImapeProcessor component.
         *
         * Implement this method in order to clean up the PointCloudProcessor
         */
        virtual void onExitPointCloudProcessor() = 0;
 
        /**
         * Process the vision component.
         *
         * The main loop of the PointCloudProcessor to be implemented in the
         * subclass. Do not block this method. One process should execute
         * exactly one PointCloud processing step.
         */
        virtual void process() = 0;
 
        // ================================================================== //
        // == RunningComponent implementation =============================== //
        // ================================================================== //
 
        /// @see Component::onInitComponent()
        virtual void onInitComponent() override;
 
        /// @see Component::onConnectComponent()
        virtual void onConnectComponent() override;
 
        /// @see Component::onDisconnectComponent()
        virtual void onDisconnectComponent() override;
 
        /// @see Component::onExitComponent()
        virtual void onExitComponent() override;
 
        /// @see RunningTask
        virtual void runProcessor();
 
 
    protected:
        // ================================================================== //
        // == PointCloudListener Ice interface ============================== //
        // ================================================================== //
        /**
         * Listener callback function. This is called by the used PointCloud
         * providers to report the availability of a newly captured
         * PointCloud.
         *
         * @param providerName          The reporting PointCloud provider name
         */
        void reportPointCloudAvailable(const std::string& providerName,
                                       const Ice::Current& c = Ice::emptyCurrent) override;
 
        // PointCloud provider.
 
        using PointCloudProviderMap = std::map<
            std::string,
            armarx::IceSharedMemoryConsumer<unsigned char, MetaPointCloudFormat>::pointer_type>;
        PointCloudProviderMap usedPointCloudProviders;
 
        std::map<std::string, PointCloudProviderInfo> pointCloudProviderInfoMap;
        std::shared_mutex pointCloudProviderInfoMutex;
 
        // Result PointCloud visualization.
        std::map<std::string, IceInternal::Handle<ResultPointCloudProvider>>
            resultPointCloudProviders;
        std::shared_mutex resultProviderMutex;
 
        // Statistics.
        std::mutex statisticsMutex;
        std::map<std::string, PointCloudTransferStats> statistics;
 
        armarx::RunningTask<PointCloudProcessor>::pointer_type processorTask;
 
        FPSCounter fpsCounter;
        float frameRate;
 
        bool automaticConversion;
    };
 
    /// Shared pointer for convenience.
    using PointCloudProcessorPtr = IceInternal::Handle<PointCloudProcessor>;
 
} // namespace visionx