UserAssistedSegmenter.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    SceneUnderstanding::ArmarXObjects::UserAssistedSegmenter
 * @author     Rainer Kartmann ( rainer dot kartmann at student dot kit dot edu )
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "UserAssistedSegmenter.h"
 
#include <VisionX/components/pointcloud_core/PointCloudConversions.h>
 
namespace visionx
{
 
    UserAssistedSegmenterPropertyDefinitions::UserAssistedSegmenterPropertyDefinitions(
        std::string prefix) :
        visionx::PointCloudProcessorPropertyDefinitions(prefix)
    {
        // POINT CLOUDS
 
        defineOptionalProperty<std::string>("ResultPointCloudName",
                                            "UserAssistedSegmenterResult",
                                            "Name of the output segmented point cloud.");
 
 
        // TOPICS
 
        defineOptionalProperty<std::string>("UserAssistedSegmenterTopicName",
                                            "UserAssistedSegmenterUpdates",
                                            "Topic where received point clouds are reported.");
 
 
        // PARAMETERS
 
        defineOptionalProperty<float>("PublishFrequency",
                                      1.0f,
                                      "Rate [1/s] the result point cloud will be published at."
                                      "If zero, the point cloud will be published exactly once.");
    }
 
    std::string
    UserAssistedSegmenter::getDefaultName() const
    {
        return "UserAssistedSegmenter";
    }
 
    void
    UserAssistedSegmenter::onInitPointCloudProcessor()
    {
        resultPointCloudName = getProperty<std::string>("ResultPointCloudName");
 
        offeringTopic(getProperty<std::string>("UserAssistedSegmenterTopicName"));
 
 
        publishFrequency = getProperty<float>("PublishFrequency");
    }
 
    void
    UserAssistedSegmenter::onConnectPointCloudProcessor()
    {
        enableResultPointClouds<PointL>(resultPointCloudName);
 
        updatesListener = getTopic<visionx::UserAssistedSegmenterListenerPrx>(
            getProperty<std::string>("UserAssistedSegmenterTopicName"));
 
        if (publishFrequency > 0)
        {
            provideTask = new armarx::PeriodicTask<UserAssistedSegmenter>(
                this,
                &UserAssistedSegmenter::provideResultPointCloud,
                static_cast<int>(std::round(1000.f / publishFrequency)));
        }
    }
 
    void
    UserAssistedSegmenter::onDisconnectPointCloudProcessor()
    {
        if (provideTask)
        {
            provideTask->stop();
        }
    }
 
    void
    UserAssistedSegmenter::onExitPointCloudProcessor()
    {
    }
 
    void
    UserAssistedSegmenter::process()
    {
        if (!waitForPointClouds(50000))
        {
            ARMARX_WARNING << "Timeout or error in wait for pointclouds";
            return;
        }
 
        PointCloudL::Ptr pointCloudPtr(new PointCloudL());
        if (!getPointClouds(pointCloudPtr))
        {
            ARMARX_WARNING << "Unable to get point cloud data.";
            return;
        }
 
        ARMARX_VERBOSE << "Received point cloud with " << pointCloudPtr->size() << " points.";
 
        const visionx::ColoredLabeledPointCloud visionxPointCloud =
            visionx::tools::convertFromPCL<visionx::ColoredLabeledPoint3D>(*pointCloudPtr);
        updatesListener->reportSegmentation(visionxPointCloud);
    }
 
    void
    UserAssistedSegmenter::publishSegmentation(const visionx::ColoredLabeledPointCloud& pointCloud,
                                               const Ice::Current&)
    {
        ARMARX_INFO << "Publishing user segmentation";
 
        resultPointCloud.reset(new PointCloudL());
        visionx::tools::convertToPCL(pointCloud, *resultPointCloud);
 
        if (!provideTask)
        {
            provideResultPointCloud(); // one shot
        }
        else if (!provideTask->isRunning())
        {
            provideTask->start();
        }
    }
 
    void
    UserAssistedSegmenter::provideResultPointCloud()
    {
        if (resultPointCloud)
        {
            provideResultPointClouds(resultPointCloud, resultPointCloudName);
        }
    }
 
    armarx::PropertyDefinitionsPtr
    UserAssistedSegmenter::createPropertyDefinitions()
    {
        return armarx::PropertyDefinitionsPtr(
            new UserAssistedSegmenterPropertyDefinitions(getConfigIdentifier()));
    }
 
} // namespace visionx