DebugDrawerComponent.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2012-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    RobotAPI::ArmarXObjects::DebugDrawerComponent
 * @author     Nikolaus Vahrenkamp ( vahrenkamp at kit dot edu )
 * @author     Peter Kaiser ( peter dot kaiser at kit dot edu )
 * @date       2014
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "DebugDrawerComponent.h"
 
#include <RobotAPI/libraries/core/math/ColorUtils.h>
 
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
#include <Inventor/nodes/SoUnits.h>
#include <Inventor/nodes/SoCube.h>
#include <Inventor/nodes/SoMaterial.h>
#include <Inventor/nodes/SoAnnotation.h>
#include <Inventor/nodes/SoTransform.h>
#include <Inventor/nodes/SoFont.h>
#include <Inventor/nodes/SoText2.h>
#include <Inventor/nodes/SoSphere.h>
#include <Inventor/nodes/SoCylinder.h>
#include <Inventor/nodes/SoComplexity.h>
#include <Inventor/nodes/SoCoordinate3.h>
#include <Inventor/nodes/SoPointSet.h>
#include <Inventor/actions/SoWriteAction.h>
#include <Inventor/nodes/SoTranslation.h>
#include <Inventor/nodes/SoMaterialBinding.h>
#include <Inventor/nodes/SoDrawStyle.h>
#include <Inventor/nodes/SoLineSet.h>
#include <Inventor/SbVec3f.h>
#include <Inventor/fields/SoMFVec3f.h>
#include <Inventor/fields/SoMFColor.h>
#include <Inventor/nodes/SoShapeHints.h>
 
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
 
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h>
#include <VirtualRobot/Visualization/TriMeshModel.h>
 
using namespace VirtualRobot;
 
#define SELECTION_NAME_PREFIX   ("selection_" + std::to_string(reinterpret_cast<std::uintptr_t>(this)))
#define SELECTION_NAME_SPLITTER "____"
#define SELECTION_NAME(layerName, elementName)   simox::alg::replace_all(std::string(SELECTION_NAME_PREFIX + "_" + layerName + SELECTION_NAME_SPLITTER + elementName), " ", "_").c_str()
 
namespace armarx
{
    std::recursive_mutex DebugDrawerComponent::selectionMutex;
 
    void selection_callback(void* userdata, SoPath* path)
    {
        if (userdata)
        {
            ((DebugDrawerComponent*) userdata)->selectionCallback();
        }
    }
 
    void deselection_callback(void* userdata, SoPath* path)
    {
        if (userdata)
        {
            ((DebugDrawerComponent*) userdata)->deselectionCallback();
        }
    }
 
    static const std::string DEBUG_LAYER_NAME
    {
        "debug"
    };
 
    DebugDrawerComponent::DebugDrawerComponent():
        mutex(new RecursiveMutex()),
        dataUpdateMutex(new RecursiveMutex()),
        topicMutex(new RecursiveMutex()),
        defaultLayerVisibility(true)
    {
        cycleTimeMS = 1000.0f / 30.0f;
        timerSensor = NULL;
        verbose = true;
 
        coinVisu = new SoSeparator;
        coinVisu->ref();
 
        selectionNode = new SoSelection;
        selectionNode->policy = SoSelection::SHIFT;
        selectionNode->ref();
        coinVisu->addChild(selectionNode);
 
        layerMainNode = new SoSeparator;
        layerMainNode->ref();
 
        SoUnits* u = new SoUnits();
        u->units = SoUnits::MILLIMETERS;
        selectionNode->addChild(u);
        selectionNode->addChild(layerMainNode);
    }
 
    void DebugDrawerComponent::enableSelections(const std::string& layerName, const ::Ice::Current&)
    {
        auto l = getScopedVisuLock();
 
        ARMARX_INFO << "Enabling selections for layer " << layerName;
        selectableLayers.insert(layerName);
    }
 
    void DebugDrawerComponent::disableSelections(const std::string& layerName, const ::Ice::Current&)
    {
        auto l = getScopedVisuLock();
 
        ARMARX_INFO << "Disabling selections for layer " << layerName;
        if (layerName == "")
        {
            selectableLayers.clear();
        }
        else if (layers.find(layerName) != layers.end())
        {
            selectableLayers.erase(layerName);
        }
    }
 
    void DebugDrawerComponent::clearSelections(const std::string& layerName, const Ice::Current&)
    {
        auto l = getScopedVisuLock();
        std::unique_lock l2(selectionMutex);
 
        removeSelectionCallbacks();
 
        ARMARX_INFO << "Clearing selections on layer " << layerName;
        if (layerName == "")
        {
            selectionNode->deselectAll();
        }
        else if (layers.find(layerName) != layers.end())
        {
            for (int i = 0; i < selectionNode->getNumSelected(); i++)
            {
                SoPath* path = selectionNode->getPath(i);
                if (path->containsNode(layers.at(layerName).mainNode))
                {
                    selectionNode->deselect(path);
                }
            }
        }
 
        installSelectionCallbacks();
    }
 
    void DebugDrawerComponent::select(const std::string& layerName, const std::string& elementName, const Ice::Current&)
    {
        auto l = getScopedVisuLock();
        std::unique_lock l2(selectionMutex);
 
        removeSelectionCallbacks();
 
        ARMARX_DEBUG << "Selecting element '" << elementName << "' on layer '" << layerName << "' (internal name: " << SELECTION_NAME(layerName, elementName) << ")";
 
        SoNode* n = SoSelection::getByName(SELECTION_NAME(layerName, elementName));
        if (n)
        {
            selectionNode->select(n);
        }
        else
        {
            ARMARX_WARNING << "Element to select not found";
        }
 
        // Force visualization update
        selectionNode->touch();
 
        installSelectionCallbacks();
    }
 
    void DebugDrawerComponent::deselect(const std::string& layerName, const std::string& elementName, const Ice::Current&)
    {
        auto l = getScopedVisuLock();
        std::unique_lock l2(selectionMutex);
 
        removeSelectionCallbacks();
 
        ARMARX_DEBUG << "Deselecting element '" << elementName << "' on layer '" << layerName << "' (internal name: " << SELECTION_NAME(layerName, elementName) << ")";
 
        SoNode* n = SoSelection::getByName(SELECTION_NAME(layerName, elementName));
        if (n)
        {
            selectionNode->deselect(n);
        }
        else
        {
            ARMARX_WARNING << "Element to deselect not found";
        }
 
        // Force visualization update
        selectionNode->touch();
 
        installSelectionCallbacks();
    }
 
    void DebugDrawerComponent::selectionCallback()
    {
        std::unique_lock l(*topicMutex);
        listenerPrx->reportSelectionChanged(getSelections());
    }
 
    void DebugDrawerComponent::deselectionCallback()
    {
        std::unique_lock l(*topicMutex);
        listenerPrx->reportSelectionChanged(getSelections());
    }
 
    void DebugDrawerComponent::installSelectionCallbacks()
    {
        auto l = getScopedVisuLock();
        selectionNode->addSelectionCallback(selection_callback, this);
        selectionNode->addDeselectionCallback(deselection_callback, this);
    }
 
    void DebugDrawerComponent::removeSelectionCallbacks()
    {
        auto l = getScopedVisuLock();
        selectionNode->removeSelectionCallback(selection_callback, this);
        selectionNode->removeDeselectionCallback(deselection_callback, this);
    }
 
    void DebugDrawerComponent::reportSelectionChanged(const DebugDrawerSelectionList& selectedElements, const Ice::Current&)
    {
        auto l = getScopedVisuLock();
        std::unique_lock l2(selectionMutex);
 
        removeSelectionCallbacks();
        selectionNode->deselectAll();
 
        for (auto& e : selectedElements)
        {
            SoNode* n = SoSelection::getByName(SELECTION_NAME(e.layerName, e.elementName));
            if (n)
            {
                selectionNode->select(n);
            }
        }
 
        // Force visualization update
        selectionNode->touch();
 
        installSelectionCallbacks();
    }
 
    DebugDrawerSelectionList DebugDrawerComponent::getSelections(const ::Ice::Current&)
    {
        auto l = getScopedVisuLock();
 
        DebugDrawerSelectionList selectedElements;
 
        for (int i = 0; i < selectionNode->getNumSelected(); i++)
        {
            SoPath* path = selectionNode->getPath(i);
 
            for (int j = 0; j < path->getLength(); j++)
            {
                SoNode* node = path->getNodeFromTail(j);
                if (!node)
                {
                    continue;
                }
 
                std::string name = node->getName().getString();
                if (name.length() > 0 && name.find(SELECTION_NAME_PREFIX) == 0)
                {
                    int index = name.rfind(SELECTION_NAME_SPLITTER);
                    name = name.substr(index + strlen(SELECTION_NAME_SPLITTER));
 
                    // Check if selected element is 'selectable'
                    for (auto& l : layers)
                    {
                        std::string layer = l.first;
                        if (layers[layer].addedBoxVisualizations.find(name) != layers[layer].addedBoxVisualizations.end()
                            || layers[layer].addedTextVisualizations.find(name) != layers[layer].addedTextVisualizations.end()
                            || layers[layer].addedSphereVisualizations.find(name) != layers[layer].addedSphereVisualizations.end()
                            || layers[layer].addedCylinderVisualizations.find(name) != layers[layer].addedCylinderVisualizations.end()
                            || layers[layer].addedPolygonVisualizations.find(name) != layers[layer].addedPolygonVisualizations.end())
                        {
                            DebugDrawerSelectionElement e;
                            e.layerName = layer;
                            e.elementName = name;
 
                            selectedElements.push_back(e);
                            break;
                        }
                    }
 
                    // We only process the first node that matches the naming scheme
                    break;
                }
            }
        }
 
        return selectedElements;
    }
 
    void DebugDrawerComponent::setVisuUpdateTime(float visuUpdatesPerSec)
    {
        if (timerSensor)
        {
            ARMARX_ERROR << "Cannot change the cycle time, once the thread has been started...";
            return;
        }
 
        if (visuUpdatesPerSec <= 0)
        {
            cycleTimeMS = 0;
        }
        else
        {
            cycleTimeMS = 1000.0f / (float)visuUpdatesPerSec;
        }
    }
 
    DebugDrawerComponent::~DebugDrawerComponent()
    {
        /*{
                auto l = getScopedLock();
                layerMainNode->unref();
                coinVisu->unref();
            }*/
    }
 
    void DebugDrawerComponent::onInitComponent()
    {
        usingTopic(getProperty<std::string>("DebugDrawerTopic").getValue());
        bool enabled = getProperty<bool>("ShowDebugDrawing").getValue();
 
        if (!enabled)
        {
            disableAllLayers();
        }
 
        if (cycleTimeMS > 0)
        {
            SoSensorManager* sensor_mgr = SoDB::getSensorManager();
            timerSensor = new SoTimerSensor(updateVisualizationCB, this);
            timerSensor->setInterval(SbTime(cycleTimeMS / 1000.0f));
            sensor_mgr->insertTimerSensor(timerSensor);
 
            /*execTaskVisuUpdates = new PeriodicTask<DebugDrawerComponent>(this, &DebugDrawerComponent::updateVisualization, cycleTimeMS, false, "DebugDrawerComponentVisuUpdates");
                execTaskVisuUpdates->start();
                execTaskVisuUpdates->setDelayWarningTolerance(100);*/
        }
 
        offeringTopic(getProperty<std::string>("DebugDrawerSelectionTopic").getValue());
        usingTopic(getProperty<std::string>("DebugDrawerSelectionTopic").getValue());
 
        installSelectionCallbacks();
    }
 
    void DebugDrawerComponent::updateVisualizationCB(void* data, SoSensor* sensor)
    {
        DebugDrawerComponent* drawer = static_cast<DebugDrawerComponent*>(data);
 
        if (!drawer)
        {
            return;
        }
 
        std::unique_lock lock(drawer->timerSensorMutex);
        drawer->updateVisualization();
    }
 
 
    void DebugDrawerComponent::onConnectComponent()
    {
        verbose = true;
 
        listenerPrx = getTopic<DebugDrawerListenerPrx>(getProperty<std::string>("DebugDrawerSelectionTopic").getValue());
    }
 
    void DebugDrawerComponent::onDisconnectComponent()
    {
        std::cout << "onDisconnectComponent debug drawer" << std::endl;
        /*verbose = false;
            size_t c = layers.size();
            size_t counter = 0;
            while (layers.size()>0 && counter < 2*c)
            {
                std::cout << "removing layer " << layers.begin()->first;
                removeLayer(layers.begin()->first);
                counter++; // sec counter
            }
            {
                auto l = getScopedLock();
                coinVisu->removeAllChildren();
            }*/
    }
 
    void DebugDrawerComponent::onExitComponent()
    {
        ARMARX_INFO << "onExitComponent debug drawer";
        verbose = false;
 
        //ARMARX_DEBUG << "onExitComponent";
        if (timerSensor)
        {
            std::unique_lock lock(timerSensorMutex);
            SoSensorManager* sensor_mgr = SoDB::getSensorManager();
            sensor_mgr->removeTimerSensor(timerSensor);
        }
 
        size_t c = layers.size();
        size_t counter = 0;
 
        while (layers.size() > 0 && counter < 2 * c)
        {
            ARMARX_INFO << "removing layer " << layers.begin()->first;
            removeLayer(layers.begin()->first);
            counter++; // sec counter
        }
 
        {
            auto l = getScopedVisuLock();
            coinVisu->removeAllChildren();
            layerMainNode->unref();
            selectionNode->unref();
            coinVisu->unref();
        }
    }
 
    void DebugDrawerComponent::exportScene(const std::string& filename, const Ice::Current&)
    {
        auto l = getScopedVisuLock();
 
        ARMARX_INFO << "Exporting scene to '" << filename << "'";
 
        SoSeparator* sep = new SoSeparator;
        SoUnits* u = new SoUnits;
        u->units = SoUnits::MILLIMETERS;
        sep->addChild(u);
        sep->addChild(layerMainNode);
 
        SoWriteAction writeAction;
        writeAction.getOutput()->openFile(filename.c_str());
        writeAction.getOutput()->setBinary(false);
        writeAction.apply(sep);
        writeAction.getOutput()->closeFile();
 
        sep->unref();
    }
 
    void DebugDrawerComponent::exportLayer(const std::string& filename, const std::string& layerName, const Ice::Current&)
    {
        auto l = getScopedVisuLock();
 
        ARMARX_INFO << "Exporting layer '" << layerName << "' to '" << filename << "'";
 
        if (!hasLayer(layerName))
        {
            ARMARX_WARNING << "Unknown layer to export";
            return;
        }
 
        SoSeparator* sep = new SoSeparator;
        SoUnits* u = new SoUnits;
        u->units = SoUnits::MILLIMETERS;
        sep->addChild(u);
        sep->addChild(layers[layerName].mainNode);
 
        SoWriteAction writeAction;
        writeAction.getOutput()->openFile(filename.c_str());
        writeAction.getOutput()->setBinary(false);
        writeAction.apply(sep);
        writeAction.getOutput()->closeFile();
 
        sep->unref();
    }
 
    void DebugDrawerComponent::drawCoordSystem(const CoordData& d)
    {
        auto l = getScopedVisuLock();
        ARMARX_DEBUG << "drawing coord system";
 
        auto& layer = requestLayer(d.layerName);
 
        removeCoordSystem(d.layerName, d.name);
 
        if (d.scale <= 0 || !d.active)
        {
            return;
        }
 
        //if (layer->addedCoordVisualizations.find(name) == layer->addedCoordVisualizations.end())
        //{
        SoSeparator* newS = new SoSeparator;
        SoMatrixTransform* newM = new SoMatrixTransform;
        newS->addChild(newM);
        std::string n = d.name;
        newS->addChild(CoinVisualizationFactory::CreateCoordSystemVisualization(d.scale, &n));
        layer.addedCoordVisualizations[d.name] = newS;
        layer.mainNode->addChild(newS);
        //}
        SoSeparator* s = layer.addedCoordVisualizations[d.name];
        SoMatrixTransform* m = (SoMatrixTransform*)(s->getChild(0));
        SbMatrix mNew = CoinVisualizationFactory::getSbMatrix(d.globalPose);
        m->matrix.setValue(mNew);
 
        ARMARX_DEBUG << "end";
    }
 
    void DebugDrawerComponent::drawLine(const LineData& d)
    {
        auto l = getScopedVisuLock();
        ARMARX_DEBUG << "drawLine1" << flush;
 
        auto& layer = requestLayer(d.layerName);
 
        removeLine(d.layerName, d.name);
 
        if (!d.active)
        {
            return;
        }
 
        SoSeparator* newS = new SoSeparator;
        newS->addChild(CoinVisualizationFactory::createCoinLine(d.p1, d.p2, d.scale, d.color));
        layer.addedLineVisualizations[d.name] = newS;
        layer.mainNode->addChild(newS);
        ARMARX_DEBUG << "drawLine2" << flush;
    }
 
    void DebugDrawerComponent::drawLineSet(const DebugDrawerComponent::LineSetData& d)
    {
        auto l = getScopedVisuLock();
        ARMARX_DEBUG << "drawLineSet";
 
        auto& layer = requestLayer(d.layerName);
 
        removeLineSet(d.layerName, d.name);
 
        if (!d.active)
        {
            return;
        }
 
        if (d.lineSet.points.size() % 2 != 0)
        {
            ARMARX_WARNING << "A line set requires an even amount of points";
            return;
        }
 
        if (d.lineSet.points.size() != 2 * d.lineSet.intensities.size())
        {
            ARMARX_WARNING << "Amount of intensities has to be half the amount of points for a line set";
            return;
        }
 
        for (const DebugDrawerPointCloudElement& e : d.lineSet.points)
        {
            if (!std::isfinite(e.x) || !std::isfinite(e.y) || !std::isfinite(e.z))
            {
                ARMARX_WARNING << deactivateSpam(10, d.layerName + d.name) << "A point of lineset " << d.name << " on layer " << d.layerName << " is not finite! this set will not be drawn!";
                return;
            }
        }
 
        // Initialize color map for affordance visualization
        std::vector<VirtualRobot::VisualizationFactory::Color> colors;
        colors.push_back(VirtualRobot::VisualizationFactory::Color(d.lineSet.colorNoIntensity.r, d.lineSet.colorNoIntensity.g, d.lineSet.colorNoIntensity.b));
        colors.push_back(VirtualRobot::VisualizationFactory::Color(d.lineSet.colorFullIntensity.r, d.lineSet.colorFullIntensity.g, d.lineSet.colorFullIntensity.b));
        VirtualRobot::ColorMap visualizationColorMap = VirtualRobot::ColorMap::customColorMap(colors);
 
        // Initialize visualization nodes
        SoSeparator* sep = new SoSeparator;
 
        SoMaterialBinding* binding = new SoMaterialBinding;
        binding->value = SoMaterialBinding::PER_PART;
        sep->addChild(binding);
 
        SoDrawStyle* lineStyle = new SoDrawStyle;
        lineStyle->lineWidth.setValue(d.lineSet.lineWidth);
        sep->addChild(lineStyle);
 
        SoCoordinate3* coordinateNode = new SoCoordinate3;
        sep->addChild(coordinateNode);
 
        SoMaterial* materialNode = new SoMaterial;
        sep->addChild(materialNode);
 
        SoLineSet* lineSetNode = new SoLineSet;
        lineSetNode->startIndex.setValue(0);
        sep->addChild(lineSetNode);
 
 
        // Allocate index and material arrays
        SbVec3f* coordinateValues = new SbVec3f[d.lineSet.points.size()];
        int32_t* lineSetValues = new int32_t[d.lineSet.intensities.size()];
        SbColor* colorValues = new SbColor[d.lineSet.intensities.size()];
 
        for (unsigned int i = 0; i < d.lineSet.intensities.size(); i++)
        {
            lineSetValues[i] = 2;
 
            coordinateValues[2 * i].setValue(d.lineSet.points[2 * i].x, d.lineSet.points[2 * i].y, d.lineSet.points[2 * i].z);
            coordinateValues[2 * i + 1].setValue(d.lineSet.points[2 * i + 1].x, d.lineSet.points[2 * i + 1].y, d.lineSet.points[2 * i + 1].z);
 
            if (d.lineSet.useHeatMap)
            {
                auto c = colorutils::HeatMapRGBAColor(d.lineSet.intensities[i]);
                colorValues[i].setValue(c.r, c.g, c.b);
            }
            else
            {
                VirtualRobot::VisualizationFactory::Color c = visualizationColorMap.getColor(d.lineSet.intensities[i]);
                colorValues[i].setValue(c.r, c.g, c.b);
            }
        }
 
        coordinateNode->point.setValuesPointer(d.lineSet.points.size(), coordinateValues);
        lineSetNode->numVertices.setValuesPointer(d.lineSet.intensities.size(), lineSetValues);
        materialNode->ambientColor.setValuesPointer(d.lineSet.intensities.size(), colorValues);
        materialNode->diffuseColor.setValuesPointer(d.lineSet.intensities.size(), colorValues);
 
        layer.addedLineSetVisualizations[d.name] = sep;
        layer.mainNode->addChild(sep);
    }
 
    void DebugDrawerComponent::drawBox(const BoxData& d)
    {
        auto l = getScopedVisuLock();
        ARMARX_DEBUG << "drawBox";
 
        auto& layer = requestLayer(d.layerName);
 
        removeBox(d.layerName, d.name);
 
        if (!d.active)
        {
            return;
        }
 
        SoSeparator* newS = new SoSeparator;
        Eigen::Matrix4f m = d.globalPose;
        newS->addChild(CoinVisualizationFactory::getMatrixTransform(m));
 
        SoMaterial* material = new SoMaterial;
        material->ambientColor.setValue(d.color.r, d.color.g, d.color.b);
        material->diffuseColor.setValue(d.color.r, d.color.g, d.color.b);
        material->transparency.setValue(d.color.transparency);
        newS->addChild(material);
 
        SoCube* cube = new SoCube;
        cube->setName(SELECTION_NAME(d.layerName, d.name));
        cube->width = d.width;
        cube->height = d.height;
        cube->depth = d.depth;
        newS->addChild(cube);
 
        layer.addedBoxVisualizations[d.name] = newS;
        layer.mainNode->addChild(newS);
    }
 
    void DebugDrawerComponent::drawText(const TextData& d)
    {
        auto l = getScopedVisuLock();
        ARMARX_DEBUG << "drawText1";
 
        auto& layer = requestLayer(d.layerName);
 
        removeText(d.layerName, d.name);
 
        if (!d.active)
        {
            return;
        }
 
        SoSeparator* sep = new SoSeparator;
        SoAnnotation* ann = new SoAnnotation;
        sep->addChild(ann);
 
        SoMaterial* mat = new SoMaterial;
        mat->ambientColor.setValue(d.color.r, d.color.g, d.color.b);
        mat->diffuseColor.setValue(d.color.r, d.color.g, d.color.b);
        mat->transparency.setValue(d.color.transparency);
        ann->addChild(mat);
 
        SoTransform* tr = new SoTransform;
        tr->translation.setValue(d.position.x(), d.position.y(), d.position.z());
        ann->addChild(tr);
 
        SoFont* font = new SoFont;
        font->name.setValue("TGS_Triplex_Roman");
        font->size = d.size;
 
        ann->addChild(font);
 
        SoText2* te = new SoText2;
        te->setName(SELECTION_NAME(d.layerName, d.name));
        te->string = d.text.c_str();
        te->justification = SoText2::CENTER;
        ann->addChild(te);
 
        layer.addedTextVisualizations[d.name] = sep;
        layer.mainNode->addChild(sep);
        ARMARX_DEBUG << "drawText2";
    }
 
 
    void DebugDrawerComponent::drawSphere(const SphereData& d)
    {
        auto l = getScopedVisuLock();
        ARMARX_DEBUG << "drawSphere";
 
        auto& layer = requestLayer(d.layerName);
 
        removeSphere(d.layerName, d.name);
 
        if (!d.active)
        {
            return;
        }
 
        if (!std::isfinite(d.position(0)) || !std::isfinite(d.position(1)) || !std::isfinite(d.position(2)))
        {
            ARMARX_WARNING << deactivateSpam(10, d.layerName + d.name) << "A coordinate of sphere " << d.name << " on layer " << d.layerName << " is not finite! this sphere will not be drawn!";
            return;
        }
 
        SoSeparator* sep = new SoSeparator;
        SoMaterial* mat = new SoMaterial;
        mat->ambientColor.setValue(d.color.r, d.color.g, d.color.b);
        mat->diffuseColor.setValue(d.color.r, d.color.g, d.color.b);
        mat->transparency.setValue(d.color.transparency);
        sep->addChild(mat);
 
        SoTransform* tr = new SoTransform;
        tr->translation.setValue(d.position.x(), d.position.y(), d.position.z());
        sep->addChild(tr);
 
        SoSphere* sphere = new SoSphere;
        sphere->setName(SELECTION_NAME(d.layerName, d.name));
        sphere->radius = d.radius;
        sep->addChild(sphere);
 
        layer.addedSphereVisualizations[d.name] = sep;
        layer.mainNode->addChild(sep);
    }
 
    void DebugDrawerComponent::drawCylinder(const DebugDrawerComponent::CylinderData& d)
    {
        auto l = getScopedVisuLock();
        ARMARX_DEBUG << "drawCylinder";
 
        auto& layer = requestLayer(d.layerName);
 
        removeCylinder(d.layerName, d.name);
 
        if (!d.active)
        {
            return;
        }
 
        SoSeparator* sep = new SoSeparator;
        SoMaterial* mat = new SoMaterial;
        mat->ambientColor.setValue(d.color.r, d.color.g, d.color.b);
        mat->diffuseColor.setValue(d.color.r, d.color.g, d.color.b);
        mat->transparency.setValue(d.color.transparency);
        sep->addChild(mat);
 
        // The cylinder extends in y-direction, hence choose the transformation
        SoTransform* tr = new SoTransform;
        tr->translation.setValue(d.position.x(), d.position.y(), d.position.z());
        tr->rotation.setValue(SbRotation(SbVec3f(0, 1, 0), SbVec3f(d.direction.x(), d.direction.y(), d.direction.z())));
        sep->addChild(tr);
 
        SoCylinder* cylinder = new SoCylinder;
        cylinder->setName(SELECTION_NAME(d.layerName, d.name));
        cylinder->height = d.length;
        cylinder->radius = d.radius;
        sep->addChild(cylinder);
 
        layer.addedCylinderVisualizations[d.name] = sep;
        layer.mainNode->addChild(sep);
    }
 
    Eigen::Vector3f GetOrthonormalVectors(Eigen::Vector3f vec, Eigen::Vector3f& dir1, Eigen::Vector3f& dir2)
    {
 
        vec = vec.normalized();
 
        dir1 = vec.cross(Eigen::Vector3f(0, 0, 1));
 
        if (dir1.norm() < 0.1f)
 
        {
 
            dir1 = vec.cross(Eigen::Vector3f(0, 1, 0));
 
        }
 
        dir1 = -dir1.normalized();
 
        dir2 = vec.cross(dir1).normalized();
 
        return vec;
 
 
    }
 
    void DebugDrawerComponent::drawCircle(const DebugDrawerComponent::CircleData& d)
    {
        auto l = getScopedVisuLock();
 
        auto& layer = requestLayer(d.layerName);
 
        removeCircle(d.layerName, d.name);
 
        if (!d.active)
        {
            return;
        }
 
 
 
        SoSeparator* sep = new SoSeparator;
        SoMaterial* mat = new SoMaterial;
        mat->ambientColor.setValue(d.color.r, d.color.g, d.color.b);
        mat->diffuseColor.setValue(d.color.r, d.color.g, d.color.b);
        mat->transparency.setValue(d.color.transparency);
        sep->addChild(mat);
 
        // The circle extends in x-y-plane, hence choose the transformation
        SoTransform* tr = new SoTransform;
        tr->translation.setValue(d.position.x(), d.position.y(), d.position.z());
        Eigen::Vector3f xDir, yDir;
        Eigen::Vector3f zDir = GetOrthonormalVectors(d.direction, xDir, yDir);
        Eigen::Matrix3f rotGoal;
        rotGoal << xDir, yDir, zDir;
        //        auto quat = VirtualRobot::MathTools::getRotation(Eigen::Vector3f::UnitZ(), d.direction);
        //        Eigen::AngleAxisf rotAA(Eigen::Quaternionf(quat.w, quat.x, quat.y, quat.z));
        Eigen::AngleAxisf rotAA(rotGoal);
        tr->rotation.setValue(SbVec3f(rotAA.axis().x(), rotAA.axis().y(), rotAA.axis().z()), rotAA.angle());
        sep->addChild(tr);
        auto node = CoinVisualizationFactory().createCircleArrow(d.radius,
                    d.width,
                    d.circleCompletion,
                    d.color,
                    16, 30);
        SoNode* circle = dynamic_cast<CoinVisualizationNode&>(*node).getCoinVisualization();
        circle->setName(SELECTION_NAME(d.layerName, d.name));
        sep->addChild(circle);
 
        layer.addedCircleVisualizations[d.name] = sep;
        layer.mainNode->addChild(sep);
    }
 
    void DebugDrawerComponent::drawPointCloud(const PointCloudData& d)
    {
        auto l = getScopedVisuLock();
 
        auto& layer = requestLayer(d.layerName);
 
        removePointCloud(d.layerName, d.name);
 
        if (!d.active)
        {
            return;
        }
 
        const auto& pcl = d.pointCloud.points;
 
        SoSeparator* pclSep = new SoSeparator;
 
        SoMaterial* pclMat = new SoMaterial;
        pclMat->diffuseColor.setValue(0.2, 0.2, 0.2);
        pclMat->diffuseColor.setValue(0.2, 0.2, 0.2);
        pclSep->addChild(pclMat);
 
        SoMaterialBinding* pclMatBind = new SoMaterialBinding;
        pclMatBind->value = SoMaterialBinding::OVERALL;
        pclSep->addChild(pclMatBind);
 
        SoCoordinate3* pclCoords = new SoCoordinate3;
        std::vector<SbVec3f> coords;
        coords.reserve(pcl.size());
        std::transform(
            pcl.begin(), pcl.end(), std::back_inserter(coords),
            [](const DebugDrawerPointCloudElement & elem)
        {
            return SbVec3f {elem.x, elem.y, elem.z};
        }
        );
        pclCoords->point.setValues(0, coords.size(), coords.data());
        pclSep->addChild(pclCoords);
 
        SoDrawStyle* pclStye = new SoDrawStyle;
        pclStye->pointSize = d.pointCloud.pointSize;
        pclSep->addChild(pclStye);
 
        pclSep->addChild(new SoPointSet);
 
        layer.addedPointCloudVisualizations[d.name] = pclSep;
        layer.mainNode->addChild(pclSep);
    }
 
    void DebugDrawerComponent::drawPolygon(const DebugDrawerComponent::PolygonData& d)
    {
        auto l = getScopedVisuLock();
 
        auto& layer = requestLayer(d.layerName);
 
        removePolygon(d.layerName, d.name);
 
        if (!d.active)
        {
            return;
        }
 
        SoSeparator* sep = new SoSeparator;
 
        SoShapeHints* hints = new SoShapeHints;
        hints->vertexOrdering = SoShapeHints::COUNTERCLOCKWISE;
        hints->shapeType = SoShapeHints::UNKNOWN_SHAPE_TYPE;
        hints->faceType = SoShapeHints::UNKNOWN_FACE_TYPE;
        sep->addChild(hints);
 
        sep->addChild(VirtualRobot::CoinVisualizationFactory::CreatePolygonVisualization(d.points, d.colorInner, d.colorBorder, d.lineWidth));
        sep->setName(SELECTION_NAME(d.layerName, d.name));
        layer.addedPolygonVisualizations[d.name] = sep;
        layer.mainNode->addChild(sep);
    }
 
    void DebugDrawerComponent::drawTriMesh(const DebugDrawerComponent::TriMeshData& d)
    {
        auto l = getScopedVisuLock();
 
        auto& layer = requestLayer(d.layerName);
 
        removeTriMesh(d.layerName, d.name);
 
        if (!d.active)
        {
            return;
        }
 
        TriMeshModelPtr triMesh = TriMeshModelPtr(new TriMeshModel());
 
        for (DrawColor color : d.triMesh.colors)
        {
            triMesh->addColor(VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, color.a));
        }
 
        for (DebugDrawerVertex v : d.triMesh.vertices)
        {
            triMesh->addVertex(Eigen::Vector3f(v.x, v.y, v.z));
        }
 
        for (DebugDrawerFace f : d.triMesh.faces)
        {
            MathTools::TriangleFace face;
 
            face.id1 = f.vertex1.vertexID;
            face.idNormal1 = f.vertex1.normalID;
            face.idColor1 = f.vertex1.colorID;
 
            face.id2 = f.vertex2.vertexID;
            face.idNormal2 = f.vertex2.normalID;
            face.idColor2 = f.vertex2.colorID;
 
            face.id3 = f.vertex3.vertexID;
            face.idNormal3 = f.vertex3.normalID;
            face.idColor3 = f.vertex3.colorID;
 
            face.normal = Eigen::Vector3f(f.normal.x, f.normal.y, f.normal.z);
 
            triMesh->addFace(face);
        }
 
        SoSeparator* sep = new SoSeparator();
        SoNode* c = CoinVisualizationFactory::getCoinVisualization(triMesh);
        sep->addChild(c);
        layer.addedTriMeshVisualizations[d.name] = sep;
        layer.mainNode->addChild(sep);
 
    }
 
    void DebugDrawerComponent::drawArrow(const DebugDrawerComponent::ArrowData& d)
    {
        auto l = getScopedVisuLock();
 
        auto& layer = requestLayer(d.layerName);
 
        removeArrow(d.layerName, d.name);
 
        if (!d.active)
        {
            return;
        }
 
        SoSeparator* sep = new SoSeparator;
 
        SoTransform* tr = new SoTransform;
        tr->translation.setValue(d.position.x(), d.position.y(), d.position.z());
        sep->addChild(tr);
 
        SoSeparator* sepArrow = VirtualRobot::CoinVisualizationFactory::CreateArrow(d.direction, d.length, d.width, d.color);
        sep->addChild(sepArrow);
        layer.addedArrowVisualizations[d.name] = sep;
        layer.mainNode->addChild(sep);
    }
 
    void DebugDrawerComponent::ensureExistingRobotNodes(DebugDrawerComponent::RobotData& d)
    {
        auto l = getScopedVisuLock();
 
        if (!d.active)
        {
            return;
        }
 
        // load or get robot
        RobotPtr rob = requestRobot(d);
 
        if (!rob)
        {
            ARMARX_ERROR << deactivateSpam() << "Could not determine robot " << d.name << " at layer " << d.layerName;
            return;
        }
 
        std::map < std::string, float >::iterator i = d.configuration.begin();
        while (i != d.configuration.end())
        {
            if (!rob->hasRobotNode(i->first))
            {
                ARMARX_WARNING << deactivateSpam() << "Robot " << rob->getName() << " does not know RobotNode " << i->first;
                i = d.configuration.erase(i);
            }
            else
            {
                i++;
            }
        }
    }
 
    void DebugDrawerComponent::drawRobot(const DebugDrawerComponent::RobotData& d)
    {
        auto l = getScopedVisuLock();
 
        auto& layer = requestLayer(d.layerName);
 
        if (!d.active)
        {
            removeRobot(d.layerName, d.name);
            return;
        }
 
        // load or get robot
        RobotPtr rob = requestRobot(d);
 
        if (!rob)
        {
            ARMARX_ERROR << deactivateSpam() << "Could not determine robot " << d.name << " at layer " << d.layerName;
            return;
        }
 
        if (d.updatePose)
        {
            rob->setGlobalPose(d.globalPose);
        }
 
        if (d.updateConfig)
        {
            rob->setJointValues(d.configuration);
        }
 
        if (d.updateColor)
        {
            if (layer.addedRobotVisualizations.find(d.name) == layer.addedRobotVisualizations.end())
            {
                ARMARX_WARNING << deactivateSpam() << "Internal robot visu error";
            }
            else
            {
                SoSeparator* sep = layer.addedRobotVisualizations[d.name];
 
                for (int i = 0; i < sep->getNumChildren(); i++)
                {
                    SoSeparator* nodeSep = dynamic_cast<SoSeparator*>(sep->getChild(i));
                    SoMaterial* m = dynamic_cast<SoMaterial*>(nodeSep->getChild(0));
 
                    if (!m)
                    {
                        ARMARX_ERROR << "Internal robot layer error2";
                        return;
                    }
 
                    if (d.color.isNone())
                    {
                        m->setOverride(false);
                    }
                    else
                    {
                        if (d.color.r < 0 || d.color.g < 0 || d.color.b < 0)
                        {
 
                        }
 
                        m->diffuseColor = SbColor(d.color.r, d.color.g, d.color.b);
                        m->ambientColor = SbColor(0, 0, 0);
                        m->transparency = std::max(0.0f, d.color.transparency);
                        m->setOverride(true);
                    }
                }
            }
        }
 
        if (d.updateNodeColor)
        {
            if (layer.addedRobotVisualizations.find(d.name) == layer.addedRobotVisualizations.end())
            {
                ARMARX_WARNING << deactivateSpam() << "Internal robot visu error";
            }
            else
            {
                SoSeparator* sep = layer.addedRobotVisualizations[d.name];
 
                if (!sep || sep->getNumChildren() < 2)
                {
                    ARMARX_ERROR << "Internal robot layer error1";
                    return;
                }
 
                std::string entryName = simox::alg::replace_all(std::string("__" + d.layerName + "__" + d.name + "__"), " ", "_");
                VirtualRobot::RobotPtr robot = activeRobots[entryName];
 
                std::vector<std::string> nodeNameList;
 
                for (size_t i = 0; i < robot->getRobotNodes().size(); ++i)
                {
                    nodeNameList.push_back(robot->getRobotNodes()[i]->getName());
                }
 
                for (std::map < std::string, VirtualRobot::VisualizationFactory::Color >::const_iterator it = d.nodeColors.begin();
                     it != d.nodeColors.end();
                     it++)
                {
 
                    std::string nodeName = it->first;
                    VirtualRobot::VisualizationFactory::Color nodeColor = it->second;
 
                    std::vector<std::string>::iterator strit;
                    strit = std::find(nodeNameList.begin(), nodeNameList.end(), nodeName);
 
                    if (strit == nodeNameList.end())
                    {
                        ARMARX_WARNING << "Cannot find correct robot node name " + nodeName + " for DebugDrawer node color update !!!";
                        continue;
                    }
 
                    size_t ind = strit - nodeNameList.begin();
                    SoSeparator* nodeSep = dynamic_cast<SoSeparator*>(sep->getChild(ind));
 
                    if (!nodeSep)
                    {
                        ARMARX_ERROR << "Internal robot layer error3";
                        return;
                    }
 
                    SoMaterial* nodeMat = dynamic_cast<SoMaterial*>(nodeSep->getChild(0));
 
                    if (!nodeMat)
                    {
                        ARMARX_ERROR << "Internal robot layer error4";
                        return;
                    }
 
                    if (nodeColor.isNone())
                    {
                        nodeMat->setOverride(false);
                    }
                    else
                    {
                        if (nodeColor.r < 0 || nodeColor.g < 0 || nodeColor.b < 0)
                        {
                            ARMARX_ERROR << "Internal robot layer node color error";
                            return;
                        }
 
                        nodeMat->diffuseColor = SbColor(nodeColor.r, nodeColor.g, nodeColor.b);
                        nodeMat->ambientColor = SbColor(0, 0, 0);
                        nodeMat->transparency = std::max(0.0f, nodeColor.transparency);
                        nodeMat->setOverride(true);
                    }
 
                }
            }
        }
    }
 
    VirtualRobot::RobotPtr DebugDrawerComponent::requestRobot(const DebugDrawerComponent::RobotData& d)
    {
        auto l = getScopedVisuLock();
 
        auto& layer = requestLayer(d.layerName);
        std::string entryName = simox::alg::replace_all(std::string("__" + d.layerName + "__" + d.name + "__"), " ", "_");
 
        ARMARX_DEBUG << "Requesting robot " << entryName;
 
        if (activeRobots.find(entryName) != activeRobots.end())
        {
            ARMARX_DEBUG << "Found robot " << entryName << ":" << activeRobots[entryName]->getName();
            return activeRobots[entryName];
        }
 
        VirtualRobot::RobotPtr result;
 
        if (d.robotFile.empty())
        {
            ARMARX_INFO << deactivateSpam() << "No robot defined for layer " << d.layerName << ", with name " << d.name;
            return result;
        }
 
        if (!d.armarxProject.empty())
        {
            ARMARX_INFO << "Adding to datapaths of " << d.armarxProject;
            armarx::CMakePackageFinder finder(d.armarxProject);
 
            if (!finder.packageFound())
            {
                ARMARX_WARNING << "ArmarX Package " << d.armarxProject << " has not been found!";
            }
            else
            {
                ARMARX_INFO << "Adding to datapaths: " << finder.getDataDir();
                armarx::ArmarXDataPath::addDataPaths(finder.getDataDir());
            }
        }
 
        // load robot
        std::string filename = d.robotFile;
        ArmarXDataPath::getAbsolutePath(filename, filename);
 
        VirtualRobot::SceneObject::VisualizationType visuType = VirtualRobot::SceneObject::Full;
        VirtualRobot::RobotIO::RobotDescription loadMode = VirtualRobot::RobotIO::eFull;
 
        if (d.drawStyle == DrawStyle::CollisionModel)
        {
            visuType = VirtualRobot::SceneObject::Collision;
            loadMode = VirtualRobot::RobotIO::eCollisionModel;
        }
 
        ARMARX_INFO << "Loading robot from " << filename;
 
        try
        {
            if (loadedRobots.find(filename) == loadedRobots.end())
            {
                loadedRobots[filename] = RobotIO::loadRobot(filename, loadMode);
            }
 
            result = loadedRobots[filename]->clone(d.name);
        }
        catch (...)
        {
 
        }
 
        if (!result)
        {
            ARMARX_IMPORTANT << "Robot loading failed, file:" << filename;
            return result;
        }
 
        SoSeparator* sep = new SoSeparator;
        layer.mainNode->addChild(sep);
 
        for (size_t i = 0; i < result->getRobotNodes().size(); ++i)
        {
            VirtualRobot::RobotNodePtr robNode = result->getRobotNodes()[i];
            SoSeparator* nodeSep = new SoSeparator;
 
            SoMaterial* nodeMat = new SoMaterial;
            nodeMat->setOverride(false);
            nodeSep->addChild(nodeMat);
 
            CoinVisualizationPtr robNodeVisu = robNode->getVisualization<CoinVisualization>(visuType);
 
            if (robNodeVisu)
            {
                SoNode* sepRobNode = robNodeVisu->getCoinVisualization();
 
                if (sepRobNode)
                {
                    nodeSep->addChild(sepRobNode);
                }
            }
 
            sep->addChild(nodeSep);
        }
 
        activeRobots[entryName] = result;
        ARMARX_DEBUG << "setting robot to activeRobots, entryName:" << entryName << ", activeRobots.size():" << activeRobots.size();
        layer.addedRobotVisualizations[d.name] = sep;
        ARMARX_DEBUG << "adding sep to layer.addedRobotVisualizations, d.name:" << d.name << ", layer:" << d.layerName << ", layer.addedRobotVisualizations.size():" << layer.addedRobotVisualizations.size();
 
        return result;
    }
 
    void DebugDrawerComponent::removeLine(const std::string& layerName, const std::string& name)
    {
        auto l = getScopedVisuLock();
 
        if (!hasLayer(layerName))
        {
            return;
        }
 
        auto& layer = layers.at(layerName);
 
        if (layer.addedLineVisualizations.find(name) == layer.addedLineVisualizations.end())
        {
            return;
        }
 
        layer.mainNode->removeChild(layer.addedLineVisualizations[name]);
        layer.addedLineVisualizations.erase(name);
    }
 
    void DebugDrawerComponent::removeLineSet(const std::string& layerName, const std::string& name)
    {
        auto l = getScopedVisuLock();
 
        if (!hasLayer(layerName))
        {
            return;
        }
 
        auto& layer = layers.at(layerName);
 
        if (layer.addedLineSetVisualizations.find(name) == layer.addedLineSetVisualizations.end())
        {
            return;
        }
 
        layer.mainNode->removeChild(layer.addedLineSetVisualizations[name]);
        layer.addedLineSetVisualizations.erase(name);
    }
 
    void DebugDrawerComponent::removeBox(const std::string& layerName, const std::string& name)
    {
        auto l = getScopedVisuLock();
 
        if (!hasLayer(layerName))
        {
            return;
        }
 
        auto& layer = layers.at(layerName);
 
        if (layer.addedBoxVisualizations.find(name) == layer.addedBoxVisualizations.end())
        {
            return;
        }
 
        layer.mainNode->removeChild(layer.addedBoxVisualizations[name]);
        layer.addedBoxVisualizations.erase(name);
    }
 
    void DebugDrawerComponent::removeText(const std::string& layerName, const std::string& name)
    {
        auto l = getScopedVisuLock();
 
        if (!hasLayer(layerName))
        {
            return;
        }
 
        auto& layer = layers.at(layerName);
 
        if (layer.addedTextVisualizations.find(name) == layer.addedTextVisualizations.end())
        {
            return;
        }
 
        layer.mainNode->removeChild(layer.addedTextVisualizations[name]);
        layer.addedTextVisualizations.erase(name);
    }
 
    void DebugDrawerComponent::removeSphere(const std::string& layerName, const std::string& name)
    {
        auto l = getScopedVisuLock();
 
        if (!hasLayer(layerName))
        {
            return;
        }
 
        auto& layer = layers.at(layerName);
 
        if (layer.addedSphereVisualizations.find(name) == layer.addedSphereVisualizations.end())
        {
            return;
        }
 
        layer.mainNode->removeChild(layer.addedSphereVisualizations[name]);
        layer.addedSphereVisualizations.erase(name);
    }
 
    void DebugDrawerComponent::removeCylinder(const std::string& layerName, const std::string& name)
    {
        auto l = getScopedVisuLock();
 
        if (!hasLayer(layerName))
        {
            return;
        }
 
        auto& layer = layers.at(layerName);
 
        if (layer.addedCylinderVisualizations.find(name) == layer.addedCylinderVisualizations.end())
        {
            return;
        }
 
        layer.mainNode->removeChild(layer.addedCylinderVisualizations[name]);
        layer.addedCylinderVisualizations.erase(name);
    }
 
    void DebugDrawerComponent::removeCircle(const std::string& layerName, const std::string& name)
    {
        auto l = getScopedVisuLock();
 
        if (!hasLayer(layerName))
        {
            return;
        }
 
        auto& layer = layers.at(layerName);
 
        if (layer.addedCircleVisualizations.find(name) == layer.addedCircleVisualizations.end())
        {
            return;
        }
 
        layer.mainNode->removeChild(layer.addedCircleVisualizations[name]);
        layer.addedCircleVisualizations.erase(name);
    }
 
    void DebugDrawerComponent::removePointCloud(const std::string& layerName, const std::string& name)
    {
        auto l = getScopedVisuLock();
 
        if (!hasLayer(layerName))
        {
            return;
        }
 
        auto& layer = layers.at(layerName);
 
        if (layer.addedPointCloudVisualizations.find(name) == layer.addedPointCloudVisualizations.end())
        {
            return;
        }
 
        layer.mainNode->removeChild(layer.addedPointCloudVisualizations[name]);
        layer.addedPointCloudVisualizations.erase(name);
    }
 
    void DebugDrawerComponent::removePolygon(const std::string& layerName, const std::string& name)
    {
        auto l = getScopedVisuLock();
 
        if (!hasLayer(layerName))
        {
            return;
        }
 
        auto& layer = layers.at(layerName);
 
        if (layer.addedPolygonVisualizations.find(name) == layer.addedPolygonVisualizations.end())
        {
            return;
        }
 
        layer.mainNode->removeChild(layer.addedPolygonVisualizations[name]);
        layer.addedPolygonVisualizations.erase(name);
    }
 
    void DebugDrawerComponent::removeTriMesh(const std::string& layerName, const std::string& name)
    {
        auto l = getScopedVisuLock();
 
        if (!hasLayer(layerName))
        {
            return;
        }
 
        auto& layer = layers.at(layerName);
 
        if (layer.addedTriMeshVisualizations.find(name) == layer.addedTriMeshVisualizations.end())
        {
            return;
        }
 
        layer.mainNode->removeChild(layer.addedTriMeshVisualizations[name]);
        layer.addedTriMeshVisualizations.erase(name);
    }
 
    void DebugDrawerComponent::removeArrow(const std::string& layerName, const std::string& name)
    {
        auto l = getScopedVisuLock();
 
        if (!hasLayer(layerName))
        {
            return;
        }
 
        auto& layer = layers.at(layerName);
 
        if (layer.addedArrowVisualizations.find(name) == layer.addedArrowVisualizations.end())
        {
            return;
        }
 
        layer.mainNode->removeChild(layer.addedArrowVisualizations[name]);
        layer.addedArrowVisualizations.erase(name);
    }
 
    void DebugDrawerComponent::removeRobot(const std::string& layerName, const std::string& name)
    {
        auto l = getScopedVisuLock();
 
        // process active robots
        std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + name + "__"), " ", "_");
        ARMARX_DEBUG << "Removing robot " << entryName;
 
        if (activeRobots.find(entryName) != activeRobots.end())
        {
            ARMARX_DEBUG << "Found robot to remove " << entryName;
            activeRobots.erase(entryName);
            ARMARX_DEBUG << "after Found robot to remove, activeRobots.size() = " << activeRobots.size();
        }
 
        // process visualizations
        if (!hasLayer(layerName))
        {
            ARMARX_DEBUG << "Layer not found " << layerName;
            return;
        }
 
        auto& layer = layers.at(layerName);
 
        if (layer.addedRobotVisualizations.find(name) == layer.addedRobotVisualizations.end())
        {
            ARMARX_INFO << "Could not find robot with name " << name;
            return;
        }
 
        ARMARX_DEBUG << "Removing visualization for " << entryName;
 
        ARMARX_DEBUG << "removing sep from layer.addedRobotVisualizations, d.name:" << name << ", layer:" << layerName << ", layer.addedRobotVisualizations.size():" << layer.addedRobotVisualizations.size();
 
        if (layer.addedRobotVisualizations.find(name) == layer.addedRobotVisualizations.end())
        {
            ARMARX_WARNING << "separator not found...";
            return;
        }
 
        ARMARX_DEBUG << "removing from layer.mainNode, layer.mainNode->getNumChildren()=" << layer.mainNode->getNumChildren();
 
        if (layer.mainNode->findChild(layer.addedRobotVisualizations[name]) < 0)
        {
            ARMARX_WARNING << "separator with wrong index...";
            return;
        }
 
        layer.mainNode->removeChild(layer.addedRobotVisualizations[name]);
        layer.addedRobotVisualizations.erase(name);
        ARMARX_DEBUG << "after removing from layer.mainNode, layer.mainNode->getNumChildren()=" << layer.mainNode->getNumChildren();
        ARMARX_DEBUG << "after removing sep from layer.addedRobotVisualizations, d.name:" << name << ", layer:" << layerName << ", layer.addedRobotVisualizations.size():" << layer.addedRobotVisualizations.size();
    }
 
    void DebugDrawerComponent::removeCoordSystem(const std::string& layerName, const std::string& name)
    {
        auto l = getScopedVisuLock();
 
        if (!hasLayer(layerName))
        {
            return;
        }
 
        auto& layer = layers.at(layerName);
 
        if (layer.addedCoordVisualizations.find(name) == layer.addedCoordVisualizations.end())
        {
            return;
        }
 
        layer.mainNode->removeChild(layer.addedCoordVisualizations[name]);
        layer.addedCoordVisualizations.erase(name);
    }
 
    void DebugDrawerComponent::setLayerVisibility(const std::string& layerName, bool visible)
    {
        auto l = getScopedVisuLock();
 
        if (!hasLayer(layerName))
        {
            return;
        }
 
        auto& layer = layers.at(layerName);
        layer.visible = visible;
 
        if (visible)
        {
            if (layerMainNode->findChild(layer.mainNode) < 0)
            {
                layerMainNode->addChild(layer.mainNode);
            }
        }
        else
        {
            if (layerMainNode->findChild(layer.mainNode) >= 0)
            {
                layerMainNode->removeChild(layer.mainNode);
            }
        }
    }
 
    void DebugDrawerComponent::disableAllLayers(const ::Ice::Current&)
    {
        auto l = getScopedVisuLock();
 
        if (selectionNode->findChild(layerMainNode) >= 0)
        {
            selectionNode->removeChild(layerMainNode);
        }
    }
 
    void DebugDrawerComponent::enableAllLayers(const ::Ice::Current&)
    {
        auto l = getScopedVisuLock();
 
        if (selectionNode->findChild(layerMainNode) < 0)
        {
            selectionNode->addChild(layerMainNode);
        }
    }
 
    void DebugDrawerComponent::setScaledPoseVisu(const std::string& layerName, const std::string& poseName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Float scale, const ::Ice::Current&)
    {
        ARMARX_DEBUG << VAROUT(layerName) << VAROUT(poseName);
        Eigen::Matrix4f gp = PosePtr::dynamicCast(globalPose)->toEigen();
        {
            auto l = getScopedAccumulatedDataLock();
            std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + poseName + "__"), " ", "_");
            CoordData& d = accumulatedUpdateData.coord[entryName];
            d.globalPose = gp;
            d.layerName = layerName;
            d.name = poseName;
            d.scale = scale;
            d.active = true;
        }
    }
 
    void DebugDrawerComponent::setScaledPoseDebugLayerVisu(const std::string& poseName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Float scale, const ::Ice::Current&)
    {
        setScaledPoseVisu(DEBUG_LAYER_NAME, poseName, globalPose, scale);
    }
 
    void DebugDrawerComponent::setPoseVisu(const std::string& layerName, const std::string& poseName, const PoseBasePtr& globalPose, const Ice::Current&)
    {
        setScaledPoseVisu(layerName, poseName, globalPose, 1.f);
    }
 
    void DebugDrawerComponent::setPoseDebugLayerVisu(const std::string& poseName, const PoseBasePtr& globalPose, const Ice::Current&)
    {
        setScaledPoseVisu(DEBUG_LAYER_NAME, poseName, globalPose, 1.f);
    }
 
    void DebugDrawerComponent::removePoseVisu(const std::string& layerName, const std::string& poseName, const Ice::Current&)
    {
        {
            auto l = getScopedAccumulatedDataLock();
            std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + poseName + "__"), " ", "_");
            CoordData& d = accumulatedUpdateData.coord[entryName];
            d.layerName = layerName;
            d.name = poseName;
            d.active = false;
        }
    }
 
    void DebugDrawerComponent::removePoseDebugLayerVisu(const std::string& poseName, const Ice::Current&)
    {
        removePoseVisu(DEBUG_LAYER_NAME, poseName);
    }
 
    void DebugDrawerComponent::setLineVisu(const std::string& layerName, const std::string& lineName, const Vector3BasePtr& globalPosition1, const Vector3BasePtr& globalPosition2, float lineWidth, const DrawColor& color, const Ice::Current&)
    {
        Eigen::Vector3f p1 = Vector3Ptr::dynamicCast(globalPosition1)->toEigen();
        Eigen::Vector3f p2 = Vector3Ptr::dynamicCast(globalPosition2)->toEigen();
        VirtualRobot::VisualizationFactory::Color c(color.r, color.g, color.b, 1 - color.a);
        {
            auto l = getScopedAccumulatedDataLock();
            std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + lineName + "__"), " ", "_");
            LineData& d = accumulatedUpdateData.line[entryName];
            d.p1 = p1;
            d.p2 = p2;
            d.layerName = layerName;
            d.name = lineName;
            d.color = c;
            d.scale = lineWidth;
            d.active = true;
        }
    }
 
    void DebugDrawerComponent::setLineDebugLayerVisu(const std::string& lineName, const Vector3BasePtr& globalPosition1, const Vector3BasePtr& globalPosition2, float lineWidth, const DrawColor& color, const Ice::Current&)
    {
        setLineVisu(DEBUG_LAYER_NAME, lineName, globalPosition1, globalPosition2, lineWidth, color);
    }
 
    void DebugDrawerComponent::removeLineVisu(const std::string& layerName, const std::string& lineName, const Ice::Current&)
    {
        {
            auto l = getScopedAccumulatedDataLock();
            std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + lineName + "__"), " ", "_");
            LineData& d = accumulatedUpdateData.line[entryName];
            d.layerName = layerName;
            d.name = lineName;
            d.active = false;
        }
    }
 
    void DebugDrawerComponent::removeLineDebugLayerVisu(const std::string& lineName, const Ice::Current&)
    {
        removeLineVisu(DEBUG_LAYER_NAME, lineName);
    }
 
    void DebugDrawerComponent::setLineSetVisu(const std::string& layerName, const std::string& lineSetName, const DebugDrawerLineSet& lineSet, const Ice::Current&)
    {
        ARMARX_DEBUG << VAROUT(layerName) << VAROUT(lineSetName);
        {
            auto l = getScopedAccumulatedDataLock();
            std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + lineSetName + "__"), " ", "_");
            LineSetData& d = accumulatedUpdateData.lineSet[entryName];
            d.lineSet = lineSet;
            d.layerName = layerName;
            d.name = lineSetName;
            d.active = true;
        }
    }
 
    void DebugDrawerComponent::setLineSetDebugLayerVisu(const std::string& lineSetName, const DebugDrawerLineSet& lineSet, const Ice::Current&)
    {
        setLineSetVisu(DEBUG_LAYER_NAME, lineSetName, lineSet);
    }
 
    void DebugDrawerComponent::removeLineSetVisu(const std::string& layerName, const std::string& lineSetName, const Ice::Current&)
    {
        {
            auto l = getScopedAccumulatedDataLock();
            std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + lineSetName + "__"), " ", "_");
            LineSetData& d = accumulatedUpdateData.lineSet[entryName];
            d.layerName = layerName;
            d.name = lineSetName;
            d.active = false;
        }
    }
 
    void DebugDrawerComponent::removeLineSetDebugLayerVisu(const std::string& lineSetName, const Ice::Current&)
    {
        removeLineSetVisu(DEBUG_LAYER_NAME, lineSetName);
    }
 
    void DebugDrawerComponent::setBoxVisu(const std::string& layerName, const std::string& boxName, const PoseBasePtr& globalPose, const Vector3BasePtr& dimensions, const DrawColor& color, const Ice::Current&)
    {
        Eigen::Matrix4f gp = PosePtr::dynamicCast(globalPose)->toEigen();
        VirtualRobot::VisualizationFactory::Color c(color.r, color.g, color.b, 1 - color.a);
        {
            auto l = getScopedAccumulatedDataLock();
            std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + boxName + "__"), " ", "_");
            BoxData& d = accumulatedUpdateData.box[entryName];
            d.width = dimensions->x;
            d.height = dimensions->y;
            d.depth = dimensions->z;
            d.layerName = layerName;
            d.name = boxName;
            d.color = c;
            d.globalPose = gp;
            d.active = true;
        }
    }
 
    void DebugDrawerComponent::setBoxDebugLayerVisu(const std::string& boxName, const PoseBasePtr& globalPose, const Vector3BasePtr& dimensions, const DrawColor& color, const Ice::Current&)
    {
        setBoxVisu(DEBUG_LAYER_NAME, boxName, globalPose, dimensions, color);
    }
 
    void DebugDrawerComponent::removeBoxVisu(const std::string& layerName, const std::string& boxName, const Ice::Current&)
    {
        {
            auto l = getScopedAccumulatedDataLock();
            std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + boxName + "__"), " ", "_");
            BoxData& d = accumulatedUpdateData.box[entryName];
            d.layerName = layerName;
            d.name = boxName;
            d.active = false;
        }
    }
 
    void DebugDrawerComponent::removeBoxDebugLayerVisu(const std::string& boxName, const Ice::Current&)
    {
        removeBoxVisu(DEBUG_LAYER_NAME, boxName);
    }
 
    void DebugDrawerComponent::setTextVisu(const std::string& layerName, const std::string& textName, const std::string& text, const Vector3BasePtr& globalPosition, const DrawColor& color, int size, const Ice::Current&)
    {
        ARMARX_DEBUG << VAROUT(layerName) << VAROUT(textName);
        {
            auto l = getScopedAccumulatedDataLock();
            std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + textName + "__"), " ", "_");
            TextData& d = accumulatedUpdateData.text[entryName];
            d.text = text;
            d.position = Vector3Ptr::dynamicCast(globalPosition)->toEigen();
            d.layerName = layerName;
            d.name = textName;
            d.color = VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, 1 - color.a);
            d.size = size;
            d.active = true;
        }
    }
 
    void DebugDrawerComponent::setTextDebugLayerVisu(const std::string& textName, const std::string& text, const Vector3BasePtr& globalPosition, const DrawColor& color, int size, const Ice::Current&)
    {
        setTextVisu(DEBUG_LAYER_NAME, textName, text, globalPosition, color, size);
    }
 
    void DebugDrawerComponent::removeTextVisu(const std::string& layerName, const std::string& textName, const Ice::Current&)
    {
        {
            auto l = getScopedAccumulatedDataLock();
            std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + textName + "__"), " ", "_");
            TextData& d = accumulatedUpdateData.text[entryName];
            d.layerName = layerName;
            d.name = textName;
            d.active = false;
        }
    }
 
    void DebugDrawerComponent::removeTextDebugLayerVisu(const std::string& textName, const Ice::Current&)
    {
        removeTextVisu(DEBUG_LAYER_NAME, textName);
    }
 
    void DebugDrawerComponent::setSphereVisu(const std::string& layerName, const std::string& sphereName, const Vector3BasePtr& globalPosition, const DrawColor& color, float radius, const Ice::Current&)
    {
        ARMARX_DEBUG << VAROUT(layerName) << VAROUT(sphereName);
        {
            auto l = getScopedAccumulatedDataLock();
            std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + sphereName + "__"), " ", "_");
            SphereData& d = accumulatedUpdateData.sphere[entryName];
            d.radius = radius;
            d.position = Vector3Ptr::dynamicCast(globalPosition)->toEigen();
            d.layerName = layerName;
            d.name = sphereName;
            d.color = VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, 1 - color.a);
            d.active = true;
        }
    }
 
    void DebugDrawerComponent::setSphereDebugLayerVisu(const std::string& sphereName, const Vector3BasePtr& globalPosition, const DrawColor& color, float radius, const Ice::Current&)
    {
        setSphereVisu(DEBUG_LAYER_NAME, sphereName, globalPosition, color, radius);
    }
 
    void DebugDrawerComponent::removeSphereVisu(const std::string& layerName, const std::string& sphereName, const Ice::Current&)
    {
        {
            auto l = getScopedAccumulatedDataLock();
            std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + sphereName + "__"), " ", "_");
            SphereData& d = accumulatedUpdateData.sphere[entryName];
            d.layerName = layerName;
            d.name = sphereName;
            d.active = false;
        }
    }
 
    void DebugDrawerComponent::removeSphereDebugLayerVisu(const std::string& sphereName, const Ice::Current&)
    {
        removeSphereVisu(DEBUG_LAYER_NAME, sphereName);
    }
 
    void DebugDrawerComponent::setCylinderVisu(const std::string& layerName, const std::string& cylinderName, const Vector3BasePtr& globalPosition, const Vector3BasePtr& direction, float length, float radius, const DrawColor& color, const Ice::Current&)
    {
        ARMARX_DEBUG << VAROUT(layerName) << VAROUT(cylinderName);
        {
            auto l = getScopedAccumulatedDataLock();
            std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + cylinderName + "__"), " ", "_");
            CylinderData& d = accumulatedUpdateData.cylinder[entryName];
            d.position = Vector3Ptr::dynamicCast(globalPosition)->toEigen();
            d.direction = Vector3Ptr::dynamicCast(direction)->toEigen();
            d.length = length;
            d.radius = radius;
            d.layerName = layerName;
            d.name = cylinderName;
            d.color = VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, 1 - color.a);
            d.active = true;
        }
    }
 
    void DebugDrawerComponent::setCylinderDebugLayerVisu(const std::string& cylinderName, const Vector3BasePtr& globalPosition, const Vector3BasePtr& direction, float length, float radius, const DrawColor& color, const Ice::Current&)
    {
        setCylinderVisu(DEBUG_LAYER_NAME, cylinderName, globalPosition, direction, length, radius, color);
    }
 
    void DebugDrawerComponent::removeCylinderVisu(const std::string& layerName, const std::string& cylinderName, const Ice::Current&)
    {
        {
            auto l = getScopedAccumulatedDataLock();
            std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + cylinderName + "__"), " ", "_");
            CylinderData& d = accumulatedUpdateData.cylinder[entryName];
            d.layerName = layerName;
            d.name = cylinderName;
            d.active = false;
        }
    }
 
    void DebugDrawerComponent::removeCylinderDebugLayerVisu(const std::string& cylinderName, const Ice::Current&)
    {
        removeCylinderVisu(DEBUG_LAYER_NAME, cylinderName);
    }
 
    void DebugDrawerComponent::setPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const DebugDrawerPointCloud& pointCloud, const Ice::Current&)
    {
        ARMARX_DEBUG << VAROUT(layerName) << VAROUT(pointCloudName);
        {
            auto l = getScopedAccumulatedDataLock();
            std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
            PointCloudData& d = accumulatedUpdateData.pointcloud[entryName];
            d.pointCloud = pointCloud;
            d.layerName = layerName;
            d.name = pointCloudName;
            d.active = true;
        }
    }
 
    void DebugDrawerComponent::setPointCloudDebugLayerVisu(const std::string& pointCloudName, const DebugDrawerPointCloud& pointCloud, const Ice::Current&)
    {
        setPointCloudVisu(DEBUG_LAYER_NAME, pointCloudName, pointCloud);
    }
 
    void DebugDrawerComponent::removePointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const Ice::Current&)
    {
        {
            auto l = getScopedAccumulatedDataLock();
            std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
            PointCloudData& d = accumulatedUpdateData.pointcloud[entryName];
            d.layerName = layerName;
            d.name = pointCloudName;
            d.active = false;
        }
    }
 
    void DebugDrawerComponent::removePointCloudDebugLayerVisu(const std::string& pointCloudName, const Ice::Current&)
    {
        removePointCloudVisu(DEBUG_LAYER_NAME, pointCloudName);
    }
 
    void DebugDrawerComponent::setPolygonVisu(const std::string& layerName, const std::string& polygonName, const std::vector<Vector3BasePtr>& polygonPoints, const DrawColor& colorInner, const DrawColor& colorBorder, float lineWidth, const Ice::Current&)
    {
        {
            auto l = getScopedAccumulatedDataLock();
            std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + polygonName + "__"), " ", "_");
            PolygonData& d = accumulatedUpdateData.polygons[entryName];
 
            std::vector< Eigen::Vector3f > points;
 
            for (size_t i = 0; i < polygonPoints.size(); i++)
            {
                Eigen::Vector3f p = Vector3Ptr::dynamicCast(polygonPoints.at(i))->toEigen();;
                points.push_back(p);
            }
 
            d.points = points;
            d.colorInner = VirtualRobot::VisualizationFactory::Color(colorInner.r, colorInner.g, colorInner.b, 1 - colorInner.a);;
            d.colorBorder = VirtualRobot::VisualizationFactory::Color(colorBorder.r, colorBorder.g, colorBorder.b, 1 - colorBorder.a);;
 
            d.lineWidth = lineWidth;
            d.layerName = layerName;
            d.name = polygonName;
            d.active = true;
        }
    }
 
    void DebugDrawerComponent::setPolygonDebugLayerVisu(const std::string& polygonName, const std::vector<Vector3BasePtr>& polygonPoints, const DrawColor& colorInner, const DrawColor& colorBorder, float lineWidth, const Ice::Current&)
    {
        setPolygonVisu(DEBUG_LAYER_NAME, polygonName, polygonPoints, colorInner, colorBorder, lineWidth);
    }
 
    void DebugDrawerComponent::removePolygonVisu(const std::string& layerName, const std::string& polygonName, const Ice::Current&)
    {
        {
            auto l = getScopedAccumulatedDataLock();
            std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + polygonName + "__"), " ", "_");
            PolygonData& d = accumulatedUpdateData.polygons[entryName];
            d.layerName = layerName;
            d.name = polygonName;
            d.active = false;
        }
    }
 
    void DebugDrawerComponent::removePolygonDebugLayerVisu(const std::string& polygonName, const Ice::Current&)
    {
        removePolygonVisu(DEBUG_LAYER_NAME, polygonName);
    }
 
    void DebugDrawerComponent::setTriMeshVisu(const std::string& layerName, const std::string& triMeshName, const DebugDrawerTriMesh& triMesh, const Ice::Current&)
    {
        {
            auto l = getScopedAccumulatedDataLock();
            std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + triMeshName + "__"), " ", "_");
            TriMeshData& d = accumulatedUpdateData.triMeshes[entryName];
            d.triMesh = triMesh;
            d.layerName = layerName;
            d.name = triMeshName;
            d.active = true;
        }
    }
 
    void DebugDrawerComponent::setTriMeshDebugLayerVisu(const std::string& triMeshName, const DebugDrawerTriMesh& triMesh, const Ice::Current&)
    {
        setTriMeshVisu(DEBUG_LAYER_NAME, triMeshName, triMesh);
    }
 
    void DebugDrawerComponent::removeTriMeshVisu(const std::string& layerName, const std::string& triMeshName, const Ice::Current&)
    {
        {
            auto l = getScopedAccumulatedDataLock();
            std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + triMeshName + "__"), " ", "_");
            TriMeshData& d = accumulatedUpdateData.triMeshes[entryName];
            d.layerName = layerName;
            d.name = triMeshName;
            d.active = false;
        }
    }
 
    void DebugDrawerComponent::removeTriMeshDebugLayerVisu(const std::string& triMeshName, const Ice::Current&)
    {
        removeTriMeshVisu(DEBUG_LAYER_NAME, triMeshName);
    }
 
    void DebugDrawerComponent::setArrowVisu(const std::string& layerName, const std::string& arrowName, const Vector3BasePtr& position, const Vector3BasePtr& direction, const DrawColor& color, float length, float width, const Ice::Current&)
    {
        {
            auto l = getScopedAccumulatedDataLock();
            std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + arrowName + "__"), " ", "_");
            ArrowData& d = accumulatedUpdateData.arrows[entryName];
            d.position = Vector3Ptr::dynamicCast(position)->toEigen();
            d.direction = Vector3Ptr::dynamicCast(direction)->toEigen();
            d.color = VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, 1 - color.a);;
            d.length = length;
            d.width = width;
            d.layerName = layerName;
            d.name = arrowName;
            d.active = true;
        }
    }
 
    void DebugDrawerComponent::setArrowDebugLayerVisu(const std::string& arrowName, const Vector3BasePtr& position, const Vector3BasePtr& direction, const DrawColor& color, float length, float width, const Ice::Current&)
    {
        setArrowVisu(DEBUG_LAYER_NAME, arrowName, position, direction, color, length, width);
    }
 
    void DebugDrawerComponent::removeArrowVisu(const std::string& layerName, const std::string& arrowName, const Ice::Current&)
    {
        {
            auto l = getScopedAccumulatedDataLock();
            std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + arrowName + "__"), " ", "_");
            ArrowData& d = accumulatedUpdateData.arrows[entryName];
            d.layerName = layerName;
            d.name = arrowName;
            d.active = false;
        }
    }
 
    void DebugDrawerComponent::removeArrowDebugLayerVisu(const std::string& arrowName, const Ice::Current&)
    {
        removeArrowVisu(DEBUG_LAYER_NAME, arrowName);
    }
 
    void DebugDrawerComponent::setRobotVisu(const std::string& layerName, const std::string& robotName, const std::string& robotFile, const std::string& armarxProject, DrawStyle drawStyle, const Ice::Current&)
    {
        auto l = getScopedAccumulatedDataLock();
        std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
        ARMARX_DEBUG << "setting robot visualization for " << entryName;
        RobotData& d = accumulatedUpdateData.robots[entryName];
 
        d.robotFile = robotFile;
        d.armarxProject = armarxProject;
        d.drawStyle = drawStyle;
 
        d.layerName = layerName;
        d.name = robotName;
        d.active = true;
    }
 
    void DebugDrawerComponent::updateRobotPose(const std::string& layerName, const std::string& robotName, const PoseBasePtr& globalPose, const Ice::Current&)
    {
        auto l = getScopedAccumulatedDataLock();
        std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
        ARMARX_DEBUG << "updating robot pose for " << entryName;
        RobotData& d = accumulatedUpdateData.robots[entryName];
 
        // update data
        d.update = true;
        d.updatePose = true;
        d.globalPose = PosePtr::dynamicCast(globalPose)->toEigen();
 
        d.layerName = layerName;
        d.name = robotName;
        d.active = true;
    }
 
    void DebugDrawerComponent::updateRobotConfig(const std::string& layerName, const std::string& robotName, const std::map<std::string, float>& configuration, const Ice::Current&)
    {
        auto l = getScopedAccumulatedDataLock();
        std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
        ARMARX_DEBUG << "updating robot config for " << entryName;
        RobotData& d = accumulatedUpdateData.robots[entryName];
 
        // update data
        d.update = true;
        d.updateConfig = true;
        d.layerName = layerName;
        d.name = robotName;
 
        for (auto& it : configuration)
        {
            d.configuration[it.first] = it.second;
        }
 
        d.active = true;
    }
 
    void DebugDrawerComponent::updateRobotColor(const std::string& layerName, const std::string& robotName, const DrawColor& c, const Ice::Current&)
    {
        auto l = getScopedAccumulatedDataLock();
        std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
        ARMARX_DEBUG << "updating robot color for " << entryName;
        RobotData& d = accumulatedUpdateData.robots[entryName];
 
        // update data
        d.update = true;
        d.updateColor = true;
        d.layerName = layerName;
        d.name = robotName;
 
        if (c.a == 0 && c.b == 0 && c.r == 0 && c.g == 0)
        {
            d.color = VirtualRobot::VisualizationFactory::Color::None();
        }
        else
        {
            d.color = VirtualRobot::VisualizationFactory::Color(c.r, c.g, c.b, 1 - c.a);
        }
 
        d.active = true;
    }
 
    void DebugDrawerComponent::updateRobotNodeColor(const std::string& layerName, const std::string& robotName, const std::string& robotNodeName, const DrawColor& c, const Ice::Current&)
    {
        auto l = getScopedAccumulatedDataLock();
        std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
        ARMARX_DEBUG << "updating robot color for " << entryName;
        RobotData& d = accumulatedUpdateData.robots[entryName];
 
        d.update = true;
        d.updateNodeColor = true;
        d.layerName = layerName;
        d.name = robotName;
 
        if (c.a == 0 && c.b == 0 && c.r == 0 && c.g == 0)
        {
            d.nodeColors[robotNodeName] = VirtualRobot::VisualizationFactory::Color::None();
        }
        else
        {
            d.nodeColors[robotNodeName] = VirtualRobot::VisualizationFactory::Color(c.r, c.g, c.b, 1 - c.a);
        }
 
        d.active = true;
 
 
    }
 
    void DebugDrawerComponent::removeRobotVisu(const std::string& layerName, const std::string& robotName, const Ice::Current&)
    {
        auto l = getScopedAccumulatedDataLock();
        std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
        ARMARX_DEBUG << "removing robot visu for " << entryName;
        RobotData& d = accumulatedUpdateData.robots[entryName];
 
        d.layerName = layerName;
        d.name = robotName;
        d.active = false;
    }
 
    void DebugDrawerComponent::clearAll(const Ice::Current&)
    {
        for (auto& i : layers)
        {
            clearLayer(i.first);
        }
    }
 
    void DebugDrawerComponent::clearLayer(const std::string& layerName, const Ice::Current&)
    {
        auto lockData = getScopedAccumulatedDataLock();
        //updates should only contain elements to add for each layer after the last clear for this layer was executed
        //this prevents a sequence add,clear,add to result in an empty layer
        //=>remove all objects pending for this layer
        removeAccumulatedData(layerName);
        accumulatedUpdateData.clearLayers.emplace(layerName);
 
    }
 
    void DebugDrawerComponent::clearLayerQt(const std::string& layerName)
    {
        auto lockData = getScopedAccumulatedDataLock();
        auto lockVisu = getScopedVisuLock();
 
        ARMARX_DEBUG << "clearing layer " << layerName;
 
        if (!hasLayer(layerName))
        {
            if (verbose)
            {
                ARMARX_VERBOSE << "Layer " << layerName << " can't be cleared, because it does not exist.";
            }
 
            return;
        }
 
        if (verbose)
        {
            ARMARX_VERBOSE << "Clearing layer " << layerName;
        }
 
        selectionNode->deselectAll();
 
        Layer& layer = layers.at(layerName);
 
        for (const auto& i : layer.addedCoordVisualizations)
        {
            removePoseVisu(layerName, i.first);
        }
 
        for (const auto& i : layer.addedLineVisualizations)
        {
            removeLineVisu(layerName, i.first);
        }
 
        for (const auto& i : layer.addedLineSetVisualizations)
        {
            removeLineSetVisu(layerName, i.first);
        }
 
        for (const auto& i : layer.addedBoxVisualizations)
        {
            removeBoxVisu(layerName, i.first);
        }
 
        for (const auto& i : layer.addedTextVisualizations)
        {
            removeTextVisu(layerName, i.first);
        }
 
        for (const auto& i : layer.addedSphereVisualizations)
        {
            removeSphereVisu(layerName, i.first);
        }
 
        for (const auto& i : layer.addedCylinderVisualizations)
        {
            removeCylinderVisu(layerName, i.first);
        }
 
        for (const auto& i : layer.addedCircleVisualizations)
        {
            removeCircleVisu(layerName, i.first);
        }
 
        for (const auto& i : layer.addedPointCloudVisualizations)
        {
            removePointCloudVisu(layerName, i.first);
        }
 
        for (const auto& i : layer.addedColoredPointCloudVisualizations)
        {
            removeColoredPointCloudVisu(layerName, i.first);
        }
 
        for (const auto& i : layer.addedPolygonVisualizations)
        {
            removePolygonVisu(layerName, i.first);
        }
 
        for (const auto& i : layer.addedTriMeshVisualizations)
        {
            removeTriMeshVisu(layerName, i.first);
        }
 
        for (const auto& i : layer.added24BitColoredPointCloudVisualizations)
        {
            remove24BitColoredPointCloudVisu(layerName, i.first);
        }
 
        for (const auto& i : layer.addedArrowVisualizations)
        {
            removeArrowVisu(layerName, i.first);
        }
 
        for (const auto& i : layer.addedRobotVisualizations)
        {
            removeRobotVisu(layerName, i.first);
        }
 
        for (const auto& i : layer.addedCustomVisualizations)
        {
            removeCustomVisu(layerName, i.first);
        }
    }
 
    void DebugDrawerComponent::clearDebugLayer(const Ice::Current&)
    {
        clearLayer(DEBUG_LAYER_NAME);
    }
 
    void DebugDrawerComponent::setMutex(RecursiveMutexPtr const& m)
    {
        mutex = m;
    }
 
    auto DebugDrawerComponent::getScopedVisuLock() -> RecursiveMutexLockPtr
    {
        RecursiveMutexLockPtr l;
 
        if (mutex)
        {
            //ARMARX_IMPORTANT << mutex.get();
            l.reset(new RecursiveMutexLock(*mutex));
        }
        else
        {
            ARMARX_IMPORTANT << deactivateSpam(5) << "NO 3D MUTEX!!!";
        }
 
        return l;
    }
 
    auto DebugDrawerComponent::getScopedAccumulatedDataLock() -> RecursiveMutexLockPtr
    {
        RecursiveMutexLockPtr l(new RecursiveMutexLock(*dataUpdateMutex));
        return l;
    }
 
    SoSeparator* DebugDrawerComponent::getVisualization()
    {
        return coinVisu;
    }
 
    DebugDrawerComponent::Layer& DebugDrawerComponent::requestLayer(const std::string& layerName)
    {
        auto l = getScopedVisuLock();
 
        if (hasLayer(layerName))
        {
            return layers.at(layerName);
        }
 
        ARMARX_VERBOSE << "Created layer " << layerName;
 
        SoSeparator* mainNode = new SoSeparator {};
        mainNode->ref();
        layers[layerName] = Layer();
        layers.at(layerName).mainNode = mainNode;
        layers.at(layerName).visible = getDefaultLayerVisibility(layerName);
        if (layers.at(layerName).visible)
        {
            layerMainNode->addChild(mainNode);
        }
        return layers.at(layerName);
    }
 
    void DebugDrawerComponent::updateVisualization()
    {
        auto lockData = getScopedAccumulatedDataLock();
        auto lockVisu = getScopedVisuLock();
        // check for clear&remove
        //(updates only contain elements to add for each layer after the last clear for this layer was executed)
        //this prevents a sequence add,clear,add to result in an empty layer
        for (const auto& layer : accumulatedUpdateData.clearLayers)
        {
            clearLayerQt(layer);
        }
        accumulatedUpdateData.clearLayers.clear();
 
        for (const auto& layer : accumulatedUpdateData.removeLayers)
        {
            removeLayerQt(layer);
        }
        accumulatedUpdateData.removeLayers.clear();
 
        //add elements
        for (auto& e : accumulatedUpdateData.coord)
        {
            drawCoordSystem(e.second);
        }
        accumulatedUpdateData.coord.clear();
 
        for (auto& e : accumulatedUpdateData.box)
        {
            drawBox(e.second);
        }
        accumulatedUpdateData.box.clear();
 
        for (auto& e : accumulatedUpdateData.line)
        {
            drawLine(e.second);
        }
        accumulatedUpdateData.line.clear();
 
        for (auto& e : accumulatedUpdateData.lineSet)
        {
            drawLineSet(e.second);
        }
        accumulatedUpdateData.lineSet.clear();
 
        for (auto& e : accumulatedUpdateData.sphere)
        {
            drawSphere(e.second);
        }
        accumulatedUpdateData.sphere.clear();
 
        for (auto& e : accumulatedUpdateData.cylinder)
        {
            drawCylinder(e.second);
        }
        accumulatedUpdateData.cylinder.clear();
 
        for (auto& e : accumulatedUpdateData.circle)
        {
            drawCircle(e.second);
        }
        accumulatedUpdateData.circle.clear();
 
        for (auto& e : accumulatedUpdateData.text)
        {
            drawText(e.second);
        }
        accumulatedUpdateData.text.clear();
 
        for (auto& e : accumulatedUpdateData.pointcloud)
        {
            drawPointCloud(e.second);
        }
        accumulatedUpdateData.pointcloud.clear();
 
        for (auto& e : accumulatedUpdateData.polygons)
        {
            drawPolygon(e.second);
        }
        accumulatedUpdateData.polygons.clear();
 
        for (auto& e : accumulatedUpdateData.arrows)
        {
            drawArrow(e.second);
        }
        accumulatedUpdateData.arrows.clear();
 
        for (auto& e : accumulatedUpdateData.triMeshes)
        {
            drawTriMesh(e.second);
        }
        accumulatedUpdateData.triMeshes.clear();
 
        for (auto& e : accumulatedUpdateData.robots)
        {
            ARMARX_DEBUG << "update visu / drawRobot for robot " << e.first;
            ensureExistingRobotNodes(e.second);
 
            drawRobot(e.second);
            e.second.nodeColors.clear();
        }
        accumulatedUpdateData.robots.clear();
 
        for (auto& e : accumulatedUpdateData.coloredpointcloud)
        {
            drawColoredPointCloud(e.second);
        }
        accumulatedUpdateData.coloredpointcloud.clear();
 
        for (auto& e : accumulatedUpdateData.colored24Bitpointcloud)
        {
            draw24BitColoredPointCloud(e.second);
        }
        accumulatedUpdateData.colored24Bitpointcloud.clear();
 
        onUpdateVisualization();
    }
 
    bool DebugDrawerComponent::hasLayer(const std::string& layerName, const ::Ice::Current&)
    {
        auto l = getScopedVisuLock();
        return layers.find(layerName) != layers.end();
    }
 
    void DebugDrawerComponent::removeLayer(const std::string& layerName, const ::Ice::Current&)
    {
        auto lockData = getScopedAccumulatedDataLock();
        //updates should only contain elements to add for each layer after the last clear/remove for this layer was executed
        //this prevents a sequence add,clear,add to result in an empty layer
        //=>remove all objects pending for this layer
        removeAccumulatedData(layerName);
        accumulatedUpdateData.removeLayers.emplace(layerName);
    }
 
    void DebugDrawerComponent::removeLayerQt(const std::string& layerName)
    {
        auto lockData = getScopedAccumulatedDataLock();
        auto lockVisu = getScopedVisuLock();
        if (!hasLayer(layerName))
        {
            if (verbose)
            {
                ARMARX_VERBOSE << "Layer " << layerName << " can't be removed, because it does not exist.";
            }
 
            return;
        }
 
        if (verbose)
        {
            ARMARX_VERBOSE << "Removing layer " << layerName;
        }
 
        clearLayerQt(layerName);
        {
            auto& layer = layers.at(layerName);
            layerMainNode->removeChild(layer.mainNode);
            layer.mainNode->unref();
            layers.erase(layerName);
        }
    }
 
    template<class UpdateDataType>
    void removeUpdateDataFromMap(const std::string& layerName, std::map<std::string, UpdateDataType>& map)
    {
        auto it = map.begin();
        while (it != map.end())
        {
            auto curr = it++;
            if (curr->second.layerName == layerName)
            {
                map.erase(curr);
            }
        }
    }
 
    void DebugDrawerComponent::removeAccumulatedData(const std::string& layerName)
    {
        auto lockData = getScopedAccumulatedDataLock();
 
        removeUpdateDataFromMap(layerName, accumulatedUpdateData.coord);
        removeUpdateDataFromMap(layerName, accumulatedUpdateData.line);
        removeUpdateDataFromMap(layerName, accumulatedUpdateData.lineSet);
        removeUpdateDataFromMap(layerName, accumulatedUpdateData.box);
        removeUpdateDataFromMap(layerName, accumulatedUpdateData.text);
        removeUpdateDataFromMap(layerName, accumulatedUpdateData.sphere);
        removeUpdateDataFromMap(layerName, accumulatedUpdateData.cylinder);
        removeUpdateDataFromMap(layerName, accumulatedUpdateData.pointcloud);
        removeUpdateDataFromMap(layerName, accumulatedUpdateData.coloredpointcloud);
        removeUpdateDataFromMap(layerName, accumulatedUpdateData.colored24Bitpointcloud);
        removeUpdateDataFromMap(layerName, accumulatedUpdateData.polygons);
        removeUpdateDataFromMap(layerName, accumulatedUpdateData.arrows);
        removeUpdateDataFromMap(layerName, accumulatedUpdateData.robots);
        removeUpdateDataFromMap(layerName, accumulatedUpdateData.triMeshes);
 
        onRemoveAccumulatedData(layerName);
 
    }
 
    void DebugDrawerComponent::enableLayerVisu(const std::string& layerName, bool visible, const ::Ice::Current&)
    {
        setLayerVisibility(layerName, visible);
    }
 
    void DebugDrawerComponent::enableDebugLayerVisu(bool visible, const ::Ice::Current&)
    {
        enableLayerVisu(DEBUG_LAYER_NAME, visible);
    }
 
    Ice::StringSeq DebugDrawerComponent::layerNames(const ::Ice::Current&)
    {
        auto l = getScopedVisuLock();
        Ice::StringSeq seq {};
 
        for (const auto& layer : layers)
        {
            seq.push_back(layer.first);
        }
 
        return seq;
    }
 
    ::armarx::LayerInformationSequence DebugDrawerComponent::layerInformation(const ::Ice::Current&)
    {
        ::armarx::LayerInformationSequence seq {};
        auto l = getScopedVisuLock();
 
        for (const auto& layer : layers)
        {
            int count = layer.second.addedCoordVisualizations.size() +
                        layer.second.addedLineVisualizations.size() +
                        layer.second.addedLineSetVisualizations.size() +
                        layer.second.addedBoxVisualizations.size() +
                        layer.second.addedTextVisualizations.size() +
                        layer.second.addedSphereVisualizations.size() +
                        layer.second.addedCylinderVisualizations.size() +
                        layer.second.addedCircleVisualizations.size() +
                        layer.second.addedPointCloudVisualizations.size() +
                        layer.second.addedColoredPointCloudVisualizations.size() +
                        layer.second.added24BitColoredPointCloudVisualizations.size() +
                        layer.second.addedPolygonVisualizations.size() +
                        layer.second.addedTriMeshVisualizations.size() +
                        layer.second.addedArrowVisualizations.size() +
                        layer.second.addedRobotVisualizations.size() +
                        layer.second.addedCustomVisualizations.size();
            ::armarx::LayerInformation info = {layer.first, layer.second.visible, count};
            seq.push_back(info);
        }
 
        return seq;
    }
 
    void DebugDrawerComponent::drawColoredPointCloud(const ColoredPointCloudData& d)
    {
        auto l = getScopedVisuLock();
 
        auto& layer = requestLayer(d.layerName);
 
        removeColoredPointCloud(d.layerName, d.name);
 
        if (!d.active)
        {
            return;
        }
 
        const auto& pcl = d.pointCloud.points;
 
        SoSeparator* pclSep = new SoSeparator;
 
        SoMaterial* pclMat = new SoMaterial;
        std::vector<SbColor> colors;
        colors.reserve(pcl.size());
        std::transform(
            pcl.begin(), pcl.end(), std::back_inserter(colors),
            [](const DebugDrawerColoredPointCloudElement & elem)
        {
            return SbColor {elem.color.r, elem.color.g, elem.color.b};
        }
        );
        pclMat->diffuseColor.setValues(0, colors.size(), colors.data());
        pclMat->ambientColor.setValues(0, colors.size(), colors.data());
        pclSep->addChild(pclMat);
 
        SoMaterialBinding* pclMatBind = new SoMaterialBinding;
        pclMatBind->value = SoMaterialBinding::PER_PART;
        pclSep->addChild(pclMatBind);
 
        SoCoordinate3* pclCoords = new SoCoordinate3;
        std::vector<SbVec3f> coords;
        coords.reserve(pcl.size());
        std::transform(
            pcl.begin(), pcl.end(), std::back_inserter(coords),
            [](const DebugDrawerColoredPointCloudElement & elem)
        {
            return SbVec3f {elem.x, elem.y, elem.z};
        }
        );
        pclCoords->point.setValues(0, coords.size(), coords.data());
        pclSep->addChild(pclCoords);
 
        SoDrawStyle* pclStye = new SoDrawStyle;
        pclStye->pointSize = d.pointCloud.pointSize;
        pclSep->addChild(pclStye);
 
        pclSep->addChild(new SoPointSet);
 
        ARMARX_DEBUG << d.name << "PointCloud Update " << pcl.size();
 
        layer.addedColoredPointCloudVisualizations[d.name] = pclSep;
        layer.mainNode->addChild(pclSep);
    }
 
    void DebugDrawerComponent::removeColoredPointCloud(const std::string& layerName, const std::string& name)
    {
        auto l = getScopedVisuLock();
 
        if (!hasLayer(layerName))
        {
            return;
        }
 
        auto& layer = layers.at(layerName);
 
        if (layer.addedColoredPointCloudVisualizations.find(name) == layer.addedColoredPointCloudVisualizations.end())
        {
            return;
        }
 
        layer.mainNode->removeChild(layer.addedColoredPointCloudVisualizations[name]);
        layer.addedColoredPointCloudVisualizations.erase(name);
    }
 
    void DebugDrawerComponent::setColoredPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const DebugDrawerColoredPointCloud& pointCloud, const Ice::Current&)
    {
        {
            auto l = getScopedAccumulatedDataLock();
            std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
            ColoredPointCloudData& d = accumulatedUpdateData.coloredpointcloud[entryName];
            d.pointCloud = pointCloud;
            d.layerName = layerName;
            d.name = pointCloudName;
            d.active = true;
        }
    }
 
    void DebugDrawerComponent::setColoredPointCloudDebugLayerVisu(const std::string& pointCloudName, const DebugDrawerColoredPointCloud& pointCloud, const Ice::Current&)
    {
        setColoredPointCloudVisu(DEBUG_LAYER_NAME, pointCloudName, pointCloud);
    }
 
    void DebugDrawerComponent::removeColoredPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const Ice::Current&)
    {
        {
            auto l = getScopedAccumulatedDataLock();
            std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
            ColoredPointCloudData& d = accumulatedUpdateData.coloredpointcloud[entryName];
            d.layerName = layerName;
            d.name = pointCloudName;
            d.active = false;
        }
    }
 
    void DebugDrawerComponent::removeColoredPointCloudDebugLayerVisu(const std::string& pointCloudName, const Ice::Current&)
    {
        removeColoredPointCloudVisu(DEBUG_LAYER_NAME, pointCloudName);
    }
 
    void DebugDrawerComponent::draw24BitColoredPointCloud(const Colored24BitPointCloudData& d)
    {
        auto l = getScopedVisuLock();
 
        auto& layer = requestLayer(d.layerName);
 
        remove24BitColoredPointCloud(d.layerName, d.name);
 
        if (!d.active)
        {
            return;
        }
 
        const auto& pcl = d.pointCloud.points;
 
        SoSeparator* pclSep = new SoSeparator;
 
        SoMaterial* pclMat = new SoMaterial;
        std::vector<SbColor> colors;
        colors.reserve(pcl.size());
        std::transform(
            pcl.begin(), pcl.end(), std::back_inserter(colors),
            [](const DebugDrawer24BitColoredPointCloudElement & elem)
        {
            return SbColor {static_cast<float>(elem.color.r) / 255.f, static_cast<float>(elem.color.g) / 255.f, static_cast<float>(elem.color.b) / 255.f};
        }
        );
        pclMat->diffuseColor.setValues(0, colors.size(), colors.data());
        pclMat->ambientColor.setValues(0, colors.size(), colors.data());
        pclMat->transparency = d.pointCloud.transparency;
        pclSep->addChild(pclMat);
 
        SoMaterialBinding* pclMatBind = new SoMaterialBinding;
        pclMatBind->value = SoMaterialBinding::PER_PART;
        pclSep->addChild(pclMatBind);
 
        SoCoordinate3* pclCoords = new SoCoordinate3;
        std::vector<SbVec3f> coords;
        coords.reserve(pcl.size());
        std::transform(
            pcl.begin(), pcl.end(), std::back_inserter(coords),
            [](const DebugDrawer24BitColoredPointCloudElement & elem)
        {
            return SbVec3f {elem.x, elem.y, elem.z};
        }
        );
        pclCoords->point.setValues(0, coords.size(), coords.data());
        pclSep->addChild(pclCoords);
 
        SoDrawStyle* pclStye = new SoDrawStyle;
        pclStye->pointSize = d.pointCloud.pointSize;
        pclSep->addChild(pclStye);
 
        pclSep->addChild(new SoPointSet);
 
        ARMARX_DEBUG << d.name << "PointCloud Update " << pcl.size();
 
        layer.added24BitColoredPointCloudVisualizations[d.name] = pclSep;
        layer.mainNode->addChild(pclSep);
    }
 
    void DebugDrawerComponent::remove24BitColoredPointCloud(const std::string& layerName, const std::string& name)
    {
        auto l = getScopedVisuLock();
 
        if (!hasLayer(layerName))
        {
            return;
        }
 
        auto& layer = layers.at(layerName);
 
        if (layer.added24BitColoredPointCloudVisualizations.find(name) == layer.added24BitColoredPointCloudVisualizations.end())
        {
            return;
        }
 
        layer.mainNode->removeChild(layer.added24BitColoredPointCloudVisualizations[name]);
        layer.added24BitColoredPointCloudVisualizations.erase(name);
    }
 
    void DebugDrawerComponent::set24BitColoredPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const DebugDrawer24BitColoredPointCloud& pointCloud, const Ice::Current&)
    {
        {
            auto l = getScopedAccumulatedDataLock();
            std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
            Colored24BitPointCloudData& d = accumulatedUpdateData.colored24Bitpointcloud[entryName];
            d.pointCloud = pointCloud;
            d.layerName = layerName;
            d.name = pointCloudName;
            d.active = true;
        }
    }
 
    void DebugDrawerComponent::set24BitColoredPointCloudDebugLayerVisu(const std::string& pointCloudName, const DebugDrawer24BitColoredPointCloud& pointCloud, const Ice::Current&)
    {
        set24BitColoredPointCloudVisu(DEBUG_LAYER_NAME, pointCloudName, pointCloud);
    }
 
    void DebugDrawerComponent::remove24BitColoredPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const Ice::Current&)
    {
        {
            auto l = getScopedAccumulatedDataLock();
            std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
            Colored24BitPointCloudData& d = accumulatedUpdateData.colored24Bitpointcloud[entryName];
            d.layerName = layerName;
            d.name = pointCloudName;
            d.active = false;
        }
    }
 
    void DebugDrawerComponent::remove24BitColoredPointCloudDebugLayerVisu(const std::string& pointCloudName, const Ice::Current&)
    {
        remove24BitColoredPointCloudVisu(DEBUG_LAYER_NAME, pointCloudName);
    }
 
    void DebugDrawerComponent::setCircleArrowVisu(const std::string& layerName, const std::string& circleName, const Vector3BasePtr& globalPosition, const Vector3BasePtr& directionVec, Ice::Float radius, Ice::Float circleCompletion, Ice::Float width, const DrawColor& color, const Ice::Current&)
    {
 
        auto l = getScopedAccumulatedDataLock();
        std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + circleName + "__"), " ", "_");
        CircleData& d = accumulatedUpdateData.circle[entryName];
        d.position = Vector3Ptr::dynamicCast(globalPosition)->toEigen();
        d.direction = Vector3Ptr::dynamicCast(directionVec)->toEigen();
        d.circleCompletion = circleCompletion;
        d.radius = radius;
        d.width = width;
        d.layerName = layerName;
        d.name = circleName;
        d.color = VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, 1 - color.a);
        d.active = true;
 
    }
 
    void DebugDrawerComponent::setCircleDebugLayerVisu(const std::string& circleName, const Vector3BasePtr& globalPosition, const Vector3BasePtr& directionVec, Ice::Float radius, Ice::Float circleCompletion, Ice::Float width, const DrawColor& color, const Ice::Current& c)
    {
        setCircleArrowVisu(DEBUG_LAYER_NAME, circleName, globalPosition, directionVec, radius, circleCompletion, width, color, c);
    }
 
    void DebugDrawerComponent::removeCircleVisu(const std::string& layerName, const std::string& circleName, const Ice::Current&)
    {
 
        auto l = getScopedAccumulatedDataLock();
        std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + circleName + "__"), " ", "_");
        CircleData& d = accumulatedUpdateData.circle[entryName];
        d.layerName = layerName;
        d.name = circleName;
        d.active = false;
 
    }
 
    void DebugDrawerComponent::removeCircleDebugLayerVisu(const std::string& circleName, const Ice::Current& c)
    {
        removeCircleVisu(DEBUG_LAYER_NAME, circleName, c);
    }
}