LabeledPointCloudMerger.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * 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::ArmarXObjects::LabeledPointCloudMerger
 * @author     Rainer Kartmann ( rainer dot kartmann at kit dot edu )
 * @date       2019
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "LabeledPointCloudMerger.h"
 
#include <RobotAPI/libraries/core/Pose.h>
 
 
namespace visionx
{
 
    LabeledPointCloudMergerPropertyDefinitions::LabeledPointCloudMergerPropertyDefinitions(std::string prefix) :
        visionx::PointCloudProcessorPropertyDefinitions(prefix)
    {
        defineOptionalProperty<std::string>("ice.DebugObserverName", "DebugObserver",
                                            "Name of the topic the DebugObserver listens to.");
 
        defineOptionalProperty<int>("ProviderWaitMs", 100,
                                    "Time to wait for each point cloud provider [ms].");
    }
 
 
    std::string LabeledPointCloudMerger::getDefaultName() const
    {
        return "LabeledPointCloudMerger";
    }
 
 
    void LabeledPointCloudMerger::onInitPointCloudProcessor()
    {
        offeringTopicFromProperty("ice.DebugObserverName");
        // debugDrawer.offeringTopic(*this);  // Calls this->offeringTopic().
 
        providerWaitTime = IceUtil::Time::milliSeconds(getProperty<int>("ProviderWaitMs").getValue());
    }
 
 
    void LabeledPointCloudMerger::onConnectPointCloudProcessor()
    {
        getTopicFromProperty(debugObserver, "ice.DebugObserverName");
        // debugDrawer.getTopic(*this);  // Calls this->getTopic().
 
        const std::vector<std::string> providerNames = getPointCloudProviderNames();
        if (!providerNames.empty())
        {
            enableResultPointCloudForInputProvider<PointT>(providerNames.front());
        }
    }
 
 
    void LabeledPointCloudMerger::onDisconnectPointCloudProcessor()
    {
    }
 
    void LabeledPointCloudMerger::onExitPointCloudProcessor()
    {
    }
 
 
    void LabeledPointCloudMerger::process()
    {
        // Fetch input clouds.
        for (const auto& providerName : getPointCloudProviderNames())
        {
            if (waitForPointClouds(providerName, static_cast<int>(providerWaitTime.toMilliSeconds())))
            {
                const PointContentType type = getPointCloudFormat(providerName)->type;
                if (visionx::tools::isLabeled(type))
                {
                    pcl::PointCloud<MergedLabeledPointCloud::PointL>::Ptr inputCloud(
                        new pcl::PointCloud<MergedLabeledPointCloud::PointL>);
                    getPointClouds(providerName, inputCloud);
                    merged.setInputPointCloud(providerName, inputCloud, true);
                }
                else
                {
                    pcl::PointCloud<MergedLabeledPointCloud::PointT>::Ptr inputCloud(
                        new pcl::PointCloud<MergedLabeledPointCloud::PointT>);
                    getPointClouds(providerName, inputCloud);
                    merged.setInputPointCloud(providerName, inputCloud);
                }
            }
            else
            {
                ARMARX_INFO << "Timeout waiting for point cloud provider " << providerName << ".";
            }
        }
 
        // Publish result cloud.
        provideResultPointClouds(merged.getResultPointCloud());
    }
 
 
    armarx::PropertyDefinitionsPtr LabeledPointCloudMerger::createPropertyDefinitions()
    {
        return armarx::PropertyDefinitionsPtr(new LabeledPointCloudMergerPropertyDefinitions(
                getConfigIdentifier()));
    }
 
}