PointCloudVisualization.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::PointCloudVisualization
 * @author     Peter Kaiser ( peter dot kaiser at kit dot edu )
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "PointCloudVisualization.h"
 
#include <VisionX/components/pointcloud_core/PointCloudConversions.h>
 
#include <thread>
 
namespace visionx
{
    PointCloudVisualizationHandler::PointCloudVisualizationHandler(PointCloudVisualization* component, const std::string& providerName) :
        providerName(providerName),
        component(component)
    {
        task = new armarx::RunningTask<PointCloudVisualizationHandler>(this, &PointCloudVisualizationHandler::processPointCloudProvider);
        task->start();
    }
 
    PointCloudVisualizationHandler::~PointCloudVisualizationHandler()
    {
        stop();
    }
 
    void PointCloudVisualizationHandler::processPointCloudProvider()
    {
        ARMARX_INFO << "Starting PointCloudProvider handler for '" << providerName << "'";
 
        while (!task->isStopped())
        {
            if (!component->waitForNewPointCloud(providerName))
            {
                continue;
            }
 
            ARMARX_INFO << "Receiving point cloud from provider '" << providerName << "'";
            component->processPointCloudProvider(providerName);
 
            std::this_thread::sleep_for(std::chrono::milliseconds(1));
        }
 
        ARMARX_INFO << "Stopping PointCloudProvider handler for '" << providerName << "'";
    }
 
    void PointCloudVisualizationHandler::stop()
    {
        task->stop();
    }
 
 
    template<typename PointT>
    void PointCloudVisualization::processPointCloud(const std::string& providerName)
    {
        typename pcl::PointCloud<PointT>::Ptr pc(new pcl::PointCloud<PointT>());
        if (!getPointClouds(providerName, pc))
        {
            ARMARX_WARNING << "Unable to get point cloud data";
            return;
        }
 
        ARMARX_INFO << "Visualizing point cloud from provider '" << providerName << "'";
 
        {
            std::unique_lock lock(pointCloudMutex);
 
            visualizations[providerName].clear();
 
            DebugDrawerPointCloudPtr dd_pc_plain(new armarx::DebugDrawer24BitColoredPointCloud);
            visionx::tools::convertFromPCLToDebugDrawer(pc, *dd_pc_plain, visionx::tools::eConvertAsPoints);
 
            visualizations[providerName][visionx::ePoints] = dd_pc_plain;
 
 
            if (pointCloudFormats[providerName]->type == visionx::eColoredPoints || pointCloudFormats[providerName]->type == visionx::eColoredLabeledPoints)
            {
                DebugDrawerPointCloudPtr dd_pc_colors(new armarx::DebugDrawer24BitColoredPointCloud);
                visionx::tools::convertFromPCLToDebugDrawer(pc, *dd_pc_colors, visionx::tools::eConvertAsColors);
                visualizations[providerName][visionx::eColoredPoints] = dd_pc_colors;
            }
 
            if (pointCloudFormats[providerName]->type == visionx::eLabeledPoints || pointCloudFormats[providerName]->type == visionx::eColoredLabeledPoints)
            {
                DebugDrawerPointCloudPtr dd_pc_labels(new armarx::DebugDrawer24BitColoredPointCloud);
                visionx::tools::convertFromPCLToDebugDrawer(pc, *dd_pc_labels, visionx::tools::eConvertAsLabels);
                visualizations[providerName][visionx::eLabeledPoints] = dd_pc_labels;
            }
        }
 
        updateVisualization(providerName);
    }
 
    void PointCloudVisualization::processPointCloudProvider(const std::string& providerName)
    {
        switch (pointCloudFormats.at(providerName)->type)
        {
            case visionx::ePoints:
                processPointCloud<pcl::PointXYZ>(providerName);
                break;
 
            case visionx::eColoredPoints:
                processPointCloud<pcl::PointXYZRGBA>(providerName);
                break;
 
            case visionx::eLabeledPoints:
                processPointCloud<pcl::PointXYZL>(providerName);
                break;
 
            case visionx::eColoredLabeledPoints:
                processPointCloud<pcl::PointXYZRGBL>(providerName);
                break;
 
            default:
                ARMARX_WARNING << "Could not visualize point cloud: Unknown format";
                break;
        }
    }
 
    bool PointCloudVisualization::waitForNewPointCloud(const std::string& providerName)
    {
        return waitForPointClouds(providerName, 10000);
    }
 
    PointCloudProviderVisualizationInfoList PointCloudVisualization::getAvailableProviders(const Ice::Current& c)
    {
        PointCloudProviderVisualizationInfoList result;
 
        for (auto& provider : providerNames)
        {
            PointCloudProviderVisualizationInfo p;
            p.name = provider;
            p.type = pointCloudFormats[provider]->type;
            result.push_back(p);
        }
 
        return result;
    }
 
    void PointCloudVisualization::enablePointCloudVisualization(const std::string& providerName, PointContentType type, bool enable, const Ice::Current& c)
    {
        {
            std::unique_lock lock(pointCloudMutex);
 
            enabledFlags[providerName] = enable;
            visualizationTypes[providerName] = type;
 
            if (enable && (visualizations.find(providerName) == visualizations.end() || visualizations.at(providerName).find(type) == visualizations.at(providerName).end()))
            {
                ARMARX_WARNING << "Enabling visualization for " << providerName << " with type " << pointCloudTypeToText(type) << ", which has not been created yet (no point cloud reported so far?)";
            }
        }
 
        updateVisualization(providerName);
    }
 
    PointCloudVisualizationInfo PointCloudVisualization::getCurrentPointCloudVisualizationInfo(const std::string& providerName, const Ice::Current& c)
    {
        return PointCloudVisualizationInfo{visualizationTypes[providerName], enabledFlags[providerName]};
    }
 
    void PointCloudVisualization::onInitPointCloudProcessor()
    {
        offeringTopic(getProperty<std::string>("DebugDrawerTopicName").getValue());
 
        providerNames = armarx::Split(getProperty<std::string>("PointCloudProviders").getValue(), ",", true);
 
        for (auto& providerName : providerNames)
        {
            usingPointCloudProvider(providerName);
        }
 
        std::vector<std::string> initiallyDisabled = armarx::Split(getProperty<std::string>("InitiallyDisabledVisualizations").getValue(), ",", true);
        for (auto& providerName : providerNames)
        {
            enabledFlags[providerName] = (std::find(initiallyDisabled.begin(), initiallyDisabled.end(), providerName) == initiallyDisabled.end());
        }
    }
 
    void PointCloudVisualization::onConnectPointCloudProcessor()
    {
        debugDrawerTopicPrx = getTopic<armarx::DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopicName").getValue());
        if (!debugDrawerTopicPrx)
        {
            ARMARX_ERROR << "Failed to obtain debug drawer proxy";
            return;
        }
 
        pointCloudFormats.clear();
        debugDrawerVisualizationIteration.clear();
        providerHandlers.clear();
        visualizationTypes.clear();
        visualizations.clear();
 
        for (auto& providerName : providerNames)
        {
            pointCloudFormats[providerName] = getPointCloudFormat(providerName);
            debugDrawerVisualizationIteration[providerName] = 0;
            providerHandlers[providerName] = PointCloudVisualizationHandlerPtr(new PointCloudVisualizationHandler(this, providerName));
        }
 
        clearAllLayers();
    }
 
    void PointCloudVisualization::onDisconnectPointCloudProcessor()
    {
        for (auto& handler : providerHandlers)
        {
            handler.second->stop();
        }
 
        providerHandlers.clear();
    }
 
    void PointCloudVisualization::onExitPointCloudProcessor()
    {
    }
 
    void PointCloudVisualization::process()
    {
        // Empty, as we need to process point clouds coming from multiple providers
    }
 
    armarx::PropertyDefinitionsPtr PointCloudVisualization::createPropertyDefinitions()
    {
        return armarx::PropertyDefinitionsPtr(new PointCloudVisualizationPropertyDefinitions(getConfigIdentifier()));
    }
 
    void PointCloudVisualization::updateVisualization(const std::string& providerName)
    {
        std::string oldLayerName, currentLayerName, entityName;
 
        try
        {
            std::unique_lock lock(pointCloudMutex);
 
            int visualizationIteration = debugDrawerVisualizationIteration.at(providerName);
            debugDrawerVisualizationIteration[providerName]++;
 
            oldLayerName = getName() + "_" + providerName + "_" + std::to_string((visualizationIteration + 1) % 2);
            currentLayerName = getName() + "_" + providerName + "_" + std::to_string(visualizationIteration % 2);
            entityName = "PointCloud_" + std::to_string(visualizationIteration % 2);
 
            if (enabledFlags.at(providerName))
            {
                auto it = visualizationTypes.find(providerName);
                PointContentType visualizationType = (it != visualizationTypes.end()) ? it->second : pointCloudTextToType(getProperty<std::string>("DefaultVisualizationType").getValue());
 
                if (visualizations.at(providerName).find(visualizationType) == visualizations.at(providerName).end())
                {
                    visualizationType = visionx::ePoints;
                }
 
                visualizationTypes[providerName] = visualizationType;
 
                ARMARX_INFO << "Visualizing " << providerName << " as " << pointCloudTypeToText(visualizationType);
                debugDrawerTopicPrx->begin_set24BitColoredPointCloudVisu(currentLayerName, entityName, *visualizations.at(providerName).at(visualizationType));
            }
        }
        catch (const std::exception& e)
        {
            ARMARX_WARNING << "Requesting visualization for " << providerName << ", but no point cloud arrived yet";
        }
 
        // Wait 200ms to let the point cloud be drawn before we clear the old layer
        usleep(200000);
 
        debugDrawerTopicPrx->clearLayer(oldLayerName);
    }
 
    void PointCloudVisualization::clearAllLayers()
    {
        for (auto& providerName : providerNames)
        {
            debugDrawerTopicPrx->clearLayer(getName() + "_" + providerName + "_0");
            debugDrawerTopicPrx->clearLayer(getName() + "_" + providerName + "_1");
        }
    }
 
    std::string PointCloudVisualization::pointCloudTypeToText(PointContentType type)
    {
        switch (type)
        {
            case visionx::ePoints:
                return "Plain";
 
            case visionx::eColoredPoints:
                return "Colors";
 
            case visionx::eLabeledPoints:
                return "Labels";
 
            default:
                return "Unknown";
        }
    }
 
    PointContentType PointCloudVisualization::pointCloudTextToType(std::string text)
    {
        std::transform(text.begin(), text.end(), text.begin(), ::tolower);
        if (text == "plain")
        {
            return visionx::ePoints;
        }
        else if (text == "colors")
        {
            return visionx::eColoredPoints;
        }
        else if (text == "labels")
        {
            return visionx::eLabeledPoints;
        }
        return visionx::ePoints;
    }
}