PointCloudProcessor.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 "PointCloudProcessor.h"
 
#include <chrono>
#include <thread>
 
#include <pcl/console/print.h>
 
#include <ArmarXCore/util/CPPUtility/trace.h>
#include <ArmarXCore/core/ArmarXManager.h>
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/exceptions/Exception.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include <VisionX/interface/components/Calibration.h>  // For ReferenceFrameInterface
 
 
using namespace armarx;
 
 
namespace visionx
{
 
    ResultPointCloudProviderPropertyDefinitions::ResultPointCloudProviderPropertyDefinitions(std::string prefix) :
        PointCloudProviderPropertyDefinitions(prefix)
    {
    }
 
    ResultPointCloudProvider::ResultPointCloudProvider() :
        resultPointCloudProviderName("ResultPointCloudProvider"),
        pointContentType(PointContentType::eColoredPoints)
    {
    }
 
    void ResultPointCloudProvider::setResultPointCloudProviderName(const std::string& name)
    {
        resultPointCloudProviderName = name;
    }
 
    std::string ResultPointCloudProvider::getDefaultName() const
    {
        return resultPointCloudProviderName;
    }
 
    void ResultPointCloudProvider::setShmCapacity(size_t shmCapacity)
    {
        this->shmCapacity = shmCapacity;
    }
 
    size_t ResultPointCloudProvider::getShmCapacity()
    {
        return shmCapacity;
    }
 
    void ResultPointCloudProvider::setPointContentType(PointContentType type)
    {
        pointContentType = type;
    }
 
    PointContentType ResultPointCloudProvider::getPointContentType() const
    {
        return pointContentType;
    }
 
    void ResultPointCloudProvider::onInitPointCloudProvider()
    {
    }
 
    void ResultPointCloudProvider::onExitPointCloudProvider()
    {
    }
 
    MetaPointCloudFormatPtr ResultPointCloudProvider::getDefaultPointCloudFormat()
    {
        MetaPointCloudFormatPtr info = new MetaPointCloudFormat();
        info->capacity = static_cast<Ice::Int>(shmCapacity);
        info->size = static_cast<Ice::Int>(shmCapacity);
        info->type = pointContentType;
        return info;
    }
 
    PropertyDefinitionsPtr ResultPointCloudProvider::createPropertyDefinitions()
    {
        return armarx::PropertyDefinitionsPtr(new ResultPointCloudProviderPropertyDefinitions(getConfigIdentifier()));
    }
 
 
    PointCloudProcessorPropertyDefinitions::PointCloudProcessorPropertyDefinitions(std::string prefix) :
        ComponentPropertyDefinitions(prefix)
    {
        defineOptionalProperty<float>("FrameRate", 0.0,
                                      "Number of frames per second. If zero then this property is ignored.");
        defineOptionalProperty<bool>("AutomaticTypeConversion", false,
                                     "Automatically convert different point cloud types.");
        defineOptionalProperty<std::string>("ProviderName", "",
                                            "Name(s) of the point cloud provider(s).");
    }
 
 
    void PointCloudProcessor::onInitComponent()
    {
 
        switch (getProperty<armarx::MessageTypeT>("MinimumLoggingLevel"))
        {
            case armarx::MessageTypeT::VERBOSE:
                pcl::console::setVerbosityLevel(pcl::console::L_VERBOSE);
                break;
            case armarx::MessageTypeT::DEBUG:
                pcl::console::setVerbosityLevel(pcl::console::L_DEBUG);
                break;
            case armarx::MessageTypeT::IMPORTANT:
            case armarx::MessageTypeT::INFO:
                pcl::console::setVerbosityLevel(pcl::console::L_INFO);
                break;
            case armarx::MessageTypeT::WARN:
                pcl::console::setVerbosityLevel(pcl::console::L_WARN);
                break;
            case armarx::MessageTypeT::ERROR:
            case armarx::MessageTypeT::FATAL:
                pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
                break;
            default:
                pcl::console::setVerbosityLevel(pcl::console::L_ALWAYS);
                break;
        }
 
 
 
        if (hasProperty("FrameRate"))
        {
            getProperty(frameRate, "FrameRate");
        }
        else
        {
            frameRate = 0.0;
        }
 
        if (hasProperty("AutomaticTypeConversion"))
        {
            getProperty(automaticConversion, "AutomaticTypeConversion");
        }
        else
        {
            automaticConversion = false;
        }
 
        if (hasProperty("ProviderName") && !getProperty<std::string>("ProviderName").getValue().empty())
        {
            std::vector<std::string> providerNames =
                armarx::Split(getProperty<std::string>("ProviderName"), ",", true);
 
            for (const std::string& providerName : providerNames)
            {
                if(providerName.empty())
                {
                    ARMARX_ERROR << "Property  " << VAROUT(providerNames) << " contains an invalid value";
                }
                else
                {
                    usingPointCloudProvider(providerName);
                }
            }
        }
        else
        {
            ARMARX_INFO << "The PointCloudProcessor " << getDefaultName() << "'s properties seem to"
                        << "\nnot derive from PointCloudProcessorPropertyDefinitions. Consider"
                        << "\nderiving " << getDefaultName() << "'s properties from this class to get"
                        << "\ndefault point cloud processor properties, such as 'ProviderName', and"
                        << "\nthe respective features.";
        }
 
 
 
        onInitPointCloudProcessor();
    }
 
    void PointCloudProcessor::onConnectComponent()
    {
        processorTask = new RunningTask<PointCloudProcessor>(this, &PointCloudProcessor::runProcessor);
 
        for (const auto& [providerName, _] : usedPointCloudProviders)
        {
            if (pointCloudProviderInfoMap.count(providerName) == 0)
            {
                getPointCloudProvider(providerName);
            }
        }
 
        onConnectPointCloudProcessor();
        processorTask->start();
    }
 
    void PointCloudProcessor::onDisconnectComponent()
    {
        onDisconnectPointCloudProcessor();
        processorTask->stop();
    }
 
    void PointCloudProcessor::onExitComponent()
    {
        ARMARX_VERBOSE << "PointCloudProcessor::onExitComponent()";
        onExitPointCloudProcessor();
 
        std::shared_lock lock(pointCloudProviderInfoMutex);
 
        for (auto& resultProvider : resultPointCloudProviders)
        {
            getArmarXManager()->removeObjectBlocking(resultProvider.first);
        }
    }
 
    void PointCloudProcessor::runProcessor()
    {
        ARMARX_INFO << "Starting PointCloud Processor";
 
        // main loop of component
        while (!processorTask->isStopped())
        {
            // call process method of sub class
            process();
 
            if (frameRate > 0)
            {
                fpsCounter.assureFPS(frameRate);
            }
            else
            {
                std::this_thread::sleep_for(std::chrono::milliseconds(1));
            }
 
        }
 
        ARMARX_INFO << "Stopping PointCloud Processor";
    }
 
 
    void PointCloudProcessor::usingPointCloudProvider(std::string providerName)
    {
        std::unique_lock lock(pointCloudProviderInfoMutex);
 
        // use PointCloud event topic
        usingTopic(providerName + ".PointCloudListener");
 
        // create shared memory consumer
        IceSharedMemoryConsumer<unsigned char, MetaPointCloudFormat>::pointer_type consumer =
            new IceSharedMemoryConsumer<unsigned char, MetaPointCloudFormat>(this, providerName, "PointCloudProvider");
 
        usedPointCloudProviders.emplace(providerName, consumer);
    }
 
    void PointCloudProcessor::releasePointCloudProvider(std::string providerName)
    {
        std::unique_lock lock(pointCloudProviderInfoMutex);
 
        if (usedPointCloudProviders.count(providerName))
        {
            usedPointCloudProviders.erase(providerName);
        }
 
        {
            if (pointCloudProviderInfoMap.count(providerName))
            {
                pointCloudProviderInfoMap.erase(providerName);
            }
        }
 
        // Following calls take care of existence of argument themself, no need to check.
        // If they return false, we can still be sure the dependency is removed and the
        // topic unsubscribed.
 
        // Unsubscribe topic
        this->unsubscribeFromTopic(providerName + ".PointCloudListener");
 
        // Remove proxy dependencies
        this->removeProxyDependency(providerName);
        std::string memoryName = providerName + "Memory" + "PointCloudProvider";
        this->removeProxyDependency(memoryName);
    }
 
    PointCloudProviderInfo PointCloudProcessor::getPointCloudProvider(std::string providerName, bool waitForProxy)
    {
        std::unique_lock lock(pointCloudProviderInfoMutex);
 
        if (pointCloudProviderInfoMap.count(providerName))
        {
            // return pointCloudProviderInfoMap[providerName];
            ARMARX_INFO << "Point cloud provider already started:  " << providerName;
        }
 
        PointCloudProviderInfo providerInfo;
 
        // get proxy for PointCloud polling
        ARMARX_DEBUG << "getProxy '" << providerName << "' waitForProxy = " << waitForProxy
                     << "\nproviders: " << getMapKeys(pointCloudProviderInfoMap);
        providerInfo.proxy = getProxy<PointCloudProviderInterfacePrx>(providerName, waitForProxy);
        ARMARX_DEBUG << "getProxy '" << providerName << "'...done!";
        providerInfo.pointCloudFormat = providerInfo.proxy->getPointCloudFormat();
 
        providerInfo.pointCloudAvailableEvent.reset(new std::condition_variable);
        providerInfo.pointCloudAvailable = false;
 
        size_t pointCloudBufferSize = static_cast<size_t>(providerInfo.pointCloudFormat->size);
        providerInfo.buffer.resize(pointCloudBufferSize);
 
 
        {
            pointCloudProviderInfoMap.emplace(providerName, providerInfo);
        }
 
 
        std::unique_lock lock2(statisticsMutex);
 
        statistics[providerName].pollingFPS.reset();
        statistics[providerName].pointCloudProviderFPS.reset();
 
        if (!providerInfo.proxy->hasSharedMemorySupport())
        {
            ARMARX_WARNING << "shared memory not available for provider " << providerName;
            usedPointCloudProviders[providerName]->setTransferMode(eIce);
            pointCloudProviderInfoMap[providerName].pointCloudTransferMode = eIceTransfer;
            //            pointCloudProviderInfoMap[providerName].info = new MetaInfoSizeBase(0, 0, TimeUtil::GetTime().toMicroSeconds());
            removeProxyDependency(usedPointCloudProviders[providerName]->getMemoryName());
        }
        else
        {
            //////////////////////////
            // start communication
            //////////////////////////
            usedPointCloudProviders[providerName]->start();
        }
 
 
        //        usedPointCloudProviders[providerName]->start();
 
        return providerInfo;
    }
 
    PointCloudProviderInfo PointCloudProcessor::getPointCloudProvider(bool waitForProxy)
    {
        std::string providerName;
        {
            std::shared_lock lock(pointCloudProviderInfoMutex);
            ARMARX_CHECK_NOT_EQUAL(0, pointCloudProviderInfoMap.size());
            ARMARX_TRACE;
            if (pointCloudProviderInfoMap.size() > 1)
            {
                ARMARX_ERROR << "Calling getPointCloudProvider without "
                             << "PointCloudProvider name but using multiple "
                             << "PointCloudProviders";
            }
            ARMARX_TRACE;
            providerName = pointCloudProviderInfoMap.begin()->first;
        }
        return getPointCloudProvider(providerName, waitForProxy);
    }
 
    std::vector<std::string> PointCloudProcessor::getPointCloudProviderNames() const
    {
        std::vector<std::string> names;
        names.reserve(usedPointCloudProviders.size());
        for (const auto& [name, _] : usedPointCloudProviders)
        {
            names.push_back(name);
        }
        return names;
    }
 
    bool PointCloudProcessor::isPointCloudProviderKnown(const std::string& providerName) const
    {
        return usedPointCloudProviders.count(providerName) > 0;
    }
 
 
    void PointCloudProcessor::enableResultPointClouds(std::string resultProviderName, size_t shmCapacity, PointContentType pointContentType)
    {
        if (resultProviderName == "")
        {
            resultProviderName = getName() + "Result";
        }
 
        std::unique_lock lock(resultProviderMutex);
 
        if (resultPointCloudProviders.count(resultProviderName))
        {
            ARMARX_WARNING << "result point cloud provider already exists: " << resultProviderName;
        }
        else
        {
            IceInternal::Handle<ResultPointCloudProvider> resultProvider = Component::create<ResultPointCloudProvider>();
            resultProvider->setName(resultProviderName);
            resultProvider->setShmCapacity(shmCapacity);
            resultProvider->setPointContentType(pointContentType);
 
            getArmarXManager()->addObject(resultProvider);
 
            {
                resultPointCloudProviders.emplace(resultProvider->getName(), resultProvider);
            }
 
            lock.unlock();
 
            resultProvider->getObjectScheduler()->waitForObjectState(eManagedIceObjectStarted);
 
        }
    }
 
 
 
    bool PointCloudProcessor::waitForPointClouds(int milliseconds)
    {
        std::shared_lock lock(pointCloudProviderInfoMutex);
 
        ARMARX_TRACE;
        if (pointCloudProviderInfoMap.size() > 1)
        {
            ARMARX_ERROR << "Calling waitForPointClouds without PointCloudProvider name but using multiple PointCloudProviders";
            return false;
        }
 
        ARMARX_TRACE;
        const std::string& providerName = pointCloudProviderInfoMap.begin()->first;
 
        ARMARX_TRACE;
        return waitForPointClouds(providerName, milliseconds);
    }
 
    bool PointCloudProcessor::waitForPointClouds(const std::string& providerName, int milliseconds)
    {
        std::shared_lock lock(pointCloudProviderInfoMutex);
 
        ARMARX_TRACE;
        // find PointCloud provider by name
        auto iter = pointCloudProviderInfoMap.find(providerName);
        if (iter == pointCloudProviderInfoMap.end())
        {
            ARMARX_ERROR << deactivateSpam(5)
                         << "Trying to wait for PointClouds from unknown PointCloud provider '"
                         << providerName << "'. Call usingPointCloudProvider before."
                         << "\nKnown providers: " << getPointCloudProviderNames();
            return false;
        }
 
        if (iter->second.pointCloudAvailable)
        {
            return true;
        }
        ARMARX_TRACE;
 
        auto cond = iter->second.pointCloudAvailableEvent;
 
        lock.unlock();
 
        // wait for conditionale
        std::mutex mut;
        std::unique_lock lock2(mut);
        auto td = std::chrono::milliseconds(milliseconds);
 
 
        return cond->wait_for(lock2, td) != std::cv_status::timeout;
    }
 
    bool PointCloudProcessor::pointCloudHasNewData(std::string providerName)
    {
        std::shared_lock lock(pointCloudProviderInfoMutex);
 
        // find PointCloud provider by name
        auto iter = pointCloudProviderInfoMap.find(providerName);
        if (iter == pointCloudProviderInfoMap.end())
        {
            ARMARX_ERROR << deactivateSpam(5)
                         << "Asked for new point cloud data for unknown PointCloud provider '"
                         << providerName << "'. Call usingPointCloudProvider() beforehand."
                         << "\nKnown providers: " << getPointCloudProviderNames();
 
            return false;
        }
 
        return iter->second.pointCloudAvailable;
    }
 
 
    MetaPointCloudFormatPtr PointCloudProcessor::getPointCloudFormat(std::string providerName)
    {
        std::shared_lock lock(pointCloudProviderInfoMutex);
 
        MetaPointCloudFormatPtr format;
 
        // find PointCloud provider
        auto iter = pointCloudProviderInfoMap.find(providerName);
        if (iter == pointCloudProviderInfoMap.end())
        {
            ARMARX_ERROR << deactivateSpam(5)
                         << "Trying to wait for PointClouds from unknown PointCloud provider '"
                         << providerName << "'. Call usingPointCloudProvider before."
                         << "\nKnown providers: " << getPointCloudProviderNames();
        }
        else
        {
            format = iter->second.proxy->getPointCloudFormat();
        }
 
        return format;
    }
 
 
    PointCloudTransferStats PointCloudProcessor::getPointCloudTransferStats(std::string providerName, bool resetStats)
    {
        std::unique_lock lock(statisticsMutex);
 
        auto iter = statistics.find(providerName);
        if (iter == statistics.end())
        {
            ARMARX_ERROR << "Requesting statistics for unknown PointCloud provider '" << providerName << "'"
                         << "\nKnown providers: " << getPointCloudProviderNames();
            return PointCloudTransferStats();
        }
 
        PointCloudTransferStats stats = iter->second;
 
        if (resetStats)
        {
            iter->second.pointCloudProviderFPS.reset();
            iter->second.pollingFPS.reset();
        }
        else
        {
            iter->second.pointCloudProviderFPS.recalculate();
            iter->second.pollingFPS.recalculate();
        }
 
        return stats;
    }
 
 
    std::string PointCloudProcessor::getPointCloudFrame(const std::string& providerName, const std::string& defaultFrame)
    {
        std::shared_lock lock(pointCloudProviderInfoMutex);
 
        auto find = pointCloudProviderInfoMap.find(providerName);
        ARMARX_CHECK_EXPRESSION(find != pointCloudProviderInfoMap.end())
                << "Requesting information about unknown point cloud provider ''" << providerName << "'."
                << "\nKnown providers: " << getPointCloudProviderNames();
 
        const PointCloudProviderInfo& providerInfo = find->second;
        auto frameProv = ReferenceFrameInterfacePrx::checkedCast(providerInfo.proxy);
 
        return frameProv ? frameProv->getReferenceFrame() : defaultFrame;
    }
 
 
    void PointCloudProcessor::reportPointCloudAvailable(const std::string& providerName, const Ice::Current&)
    {
        std::shared_lock lock(pointCloudProviderInfoMutex);
 
        // find provider
        std::map<std::string, PointCloudProviderInfo>::iterator iter = pointCloudProviderInfoMap.find(providerName);
 
        if (iter == pointCloudProviderInfoMap.end())
        {
            ARMARX_ERROR << "Received notification from unknown point cloud provider '" << providerName << "'."
                         << "\nKnown providers: " << getPointCloudProviderNames();
            return;
        }
 
        iter->second.pointCloudAvailable = true;
        iter->second.pointCloudAvailableEvent->notify_all();
 
        // update statistics
        std::unique_lock lock2(statisticsMutex);
        statistics[providerName].pointCloudProviderFPS.update();
    }
 
}