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 <thread>
 
#include <VisionX/components/pointcloud_core/PointCloudConversions.h>
 
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;
    }
} // namespace visionx