DebugDrawerComponent.h

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
 */
 
#pragma once
 
 
// Coin3D & SoQt
#include <Inventor/nodes/SoNode.h>
#include <Inventor/nodes/SoSeparator.h>
#include <Inventor/nodes/SoSelection.h>
#include <Inventor/sensors/SoTimerSensor.h>
 
// ArmarX
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/libraries/core/Pose.h>
 
 
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/Visualization/VisualizationFactory.h>
 
//std
#include <memory>
#include <mutex>
 
namespace armarx
{
 
    /*!
     * \class DebugDrawerPropertyDefinitions
     * \brief
     */
    class DebugDrawerPropertyDefinitions:
        public ComponentPropertyDefinitions
    {
    public:
        DebugDrawerPropertyDefinitions(std::string prefix):
            ComponentPropertyDefinitions(prefix)
        {
            defineOptionalProperty<bool>("ShowDebugDrawing", true, "The simulator implements the DebugDrawerInterface. The debug visualizations (e.g. coordinate systems) can enabled/disbaled with this flag.");
            defineOptionalProperty<std::string>("DebugDrawerTopic", "DebugDrawerUpdates", "Name of the DebugDrawerTopic");
            defineOptionalProperty<std::string>("DebugDrawerSelectionTopic", "DebugDrawerSelections", "Name of the DebugDrawerSelectionTopic");
        }
    };
 
    /*!
     * \defgroup Component-DebugDrawerComponent DebugDrawerComponent
     * \ingroup RobotAPI-Components
     * \brief Visualizes debug information.
     *
     * The DebugDrawerComponent implements the DebugDrawerInterface and provides a convenient way for visualizing debug information.
     * It creates a scene graph representation of the debug content and offers it to visualization components.
     * Several GUI plugins visualize this debug scene graph, among others:
     * - RobotViewer (RobotAPI)
     * - WorkingMemoryGui (MemoryX)
     * - SimulatorViewer (ArmarXSimulation)
     *
     * The following example shows an exemplary debug drawing:
    \code
        DebugDrawerInterfacePrx prxDD = getTopic<DebugDrawerInterfacePrx>("DebugDrawerUpdates");
        if (prxDD)
        {
            // draw in global coordinate system
            Eigen::Matrix4f p;
            p.setIdentity();
            p(2,3) = 1000.0f;
            PosePtr gp(new Pose(p));
            prxDD->setPoseDebugLayerVisu("testPose",gp);
 
            armarx::Vector3Ptr p1(new armarx::Vector3(0, 0, 0));
            armarx::Vector3Ptr p2(new armarx::Vector3(1000, 1000, 1000));
 
            armarx::DrawColor c = {1, 0, 0, 1}; // RGBA
            prxDD->setLineDebugLayerVisu("testLine", p1, p2, 2.0f, c);
        }
    \endcode
     */
 
    /**
     * @brief The DebugDrawerComponent class
     * @ingroup Component-DebugDrawerComponent
     */
    class DebugDrawerComponent :
        virtual public armarx::DebugDrawerInterfaceAndListener,
        virtual public Component
    {
    public:
 
        /*!
         * \brief DebugDrawerComponent Constructs a debug drawer
         */
        DebugDrawerComponent();
 
        /*!
         * \brief setVisuUpdateTime Specifies how often the accumulated draw commands should be applied to the visualization (default 30). Has to be called before init.
         * \param visuUpdatesPerSec All topic requests are collected asynchronously. This parameter specifies how often the rendering should be updated according to the accumulated updates.
         */
        void setVisuUpdateTime(float visuUpdatesPerSec);
 
        // inherited from Component
        std::string getDefaultName() const override
        {
            return "DebugDrawer";
        }
        void onInitComponent() override;
        void onConnectComponent() override;
        void onDisconnectComponent() override;
        void onExitComponent() override;
 
 
 
        /*!
         * \see PropertyUser::createPropertyDefinitions()
         */
        PropertyDefinitionsPtr createPropertyDefinitions() override
        {
            return PropertyDefinitionsPtr(new DebugDrawerPropertyDefinitions(
                                              getConfigIdentifier()));
        }
 
        /* Inherited from DebugDrawerInterface. */
        void exportScene(const std::string& filename, const ::Ice::Current& = Ice::emptyCurrent) override;
        void exportLayer(const std::string& filename, const std::string& layerName, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        void setPoseVisu(const std::string& layerName, const std::string& poseName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Current& = Ice::emptyCurrent) override;
        void setScaledPoseVisu(const std::string& layerName, const std::string& poseName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Float scale, const ::Ice::Current& = Ice::emptyCurrent) override;
        void setPoseDebugLayerVisu(const std::string& poseName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Current& = Ice::emptyCurrent) override;
        void setScaledPoseDebugLayerVisu(const std::string& poseName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Float scale, const ::Ice::Current& = Ice::emptyCurrent) override;
        void removePoseVisu(const std::string& layerName, const std::string& poseName, const ::Ice::Current& = Ice::emptyCurrent) override;
        void removePoseDebugLayerVisu(const std::string& poseName, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        void setLineVisu(const std::string& layerName, const std::string& lineName, const ::armarx::Vector3BasePtr& globalPosition1, const ::armarx::Vector3BasePtr& globalPosition2, float lineWidth, const ::armarx::DrawColor& color, const ::Ice::Current& = Ice::emptyCurrent) override;
        void setLineDebugLayerVisu(const std::string& lineName, const ::armarx::Vector3BasePtr& globalPosition1, const ::armarx::Vector3BasePtr& globalPosition2, float lineWidth, const ::armarx::DrawColor& color, const ::Ice::Current& = Ice::emptyCurrent) override;
        void removeLineVisu(const std::string& layerName, const std::string& lineName, const ::Ice::Current& = Ice::emptyCurrent) override;
        void removeLineDebugLayerVisu(const std::string& lineName, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        void setLineSetVisu(const std::string& layerName, const std::string& lineSetName, const DebugDrawerLineSet& lineSet, const ::Ice::Current& = Ice::emptyCurrent) override;
        void setLineSetDebugLayerVisu(const std::string& lineSetName, const DebugDrawerLineSet& lineSet, const ::Ice::Current& = Ice::emptyCurrent) override;
        void removeLineSetVisu(const std::string& layerName, const std::string& lineSetName, const ::Ice::Current& = Ice::emptyCurrent) override;
        void removeLineSetDebugLayerVisu(const std::string& lineSetName, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        void setBoxVisu(const std::string& layerName, const std::string& boxName, const ::armarx::PoseBasePtr& globalPose, const ::armarx::Vector3BasePtr& dimensions, const DrawColor& color, const ::Ice::Current& = Ice::emptyCurrent) override;
        void setBoxDebugLayerVisu(const std::string& boxName, const ::armarx::PoseBasePtr& globalPose, const ::armarx::Vector3BasePtr& dimensions, const DrawColor& color, const ::Ice::Current& = Ice::emptyCurrent) override;
        void removeBoxVisu(const std::string& layerName, const std::string& boxName, const ::Ice::Current& = Ice::emptyCurrent) override;
        void removeBoxDebugLayerVisu(const std::string& boxName, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        void setTextVisu(const std::string& layerName, const std::string& textName, const std::string& text, const ::armarx::Vector3BasePtr& globalPosition, const DrawColor& color, int size, const ::Ice::Current& = Ice::emptyCurrent) override;
        void setTextDebugLayerVisu(const std::string& textName, const std::string& text, const ::armarx::Vector3BasePtr& globalPosition, const DrawColor& color, int size, const ::Ice::Current& = Ice::emptyCurrent) override;
        void removeTextVisu(const std::string& layerName, const std::string& textName, const ::Ice::Current& = Ice::emptyCurrent) override;
        void removeTextDebugLayerVisu(const std::string& textName, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        void setSphereVisu(const std::string& layerName, const std::string& sphereName, const ::armarx::Vector3BasePtr& globalPosition, const DrawColor& color, float radius, const ::Ice::Current& = Ice::emptyCurrent) override;
        void setSphereDebugLayerVisu(const std::string& sphereName, const ::armarx::Vector3BasePtr& globalPosition, const DrawColor& color, float radius, const ::Ice::Current& = Ice::emptyCurrent) override;
        void removeSphereVisu(const std::string& layerName, const std::string& sphereName, const ::Ice::Current& = Ice::emptyCurrent) override;
        void removeSphereDebugLayerVisu(const std::string& sphereName, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        void setCylinderVisu(const std::string& layerName, const std::string& cylinderName, const ::armarx::Vector3BasePtr& globalPosition, const ::armarx::Vector3BasePtr& direction, float length, float radius, const DrawColor& color, const ::Ice::Current& = Ice::emptyCurrent) override;
        void setCylinderDebugLayerVisu(const std::string& cylinderName, const ::armarx::Vector3BasePtr& globalPosition, const ::armarx::Vector3BasePtr& direction, float length, float radius, const DrawColor& color, const ::Ice::Current& = Ice::emptyCurrent) override;
        void removeCylinderVisu(const std::string& layerName, const std::string& cylinderName, const ::Ice::Current& = Ice::emptyCurrent) override;
        void removeCylinderDebugLayerVisu(const std::string& cylinderName, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        void setPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const DebugDrawerPointCloud& pointCloud, const ::Ice::Current& = Ice::emptyCurrent) override;
        void setPointCloudDebugLayerVisu(const std::string& pointCloudName, const DebugDrawerPointCloud& pointCloud, const ::Ice::Current& = Ice::emptyCurrent) override;
        void removePointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const ::Ice::Current& = Ice::emptyCurrent) override;
        void removePointCloudDebugLayerVisu(const std::string& pointCloudName, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        virtual void setColoredPointCloudDebugLayerVisu(const std::string& pointCloudName, const DebugDrawerColoredPointCloud& pointCloud, const ::Ice::Current& = Ice::emptyCurrent);
        void setColoredPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const DebugDrawerColoredPointCloud& pointCloud, const ::Ice::Current& = Ice::emptyCurrent) override;
        void removeColoredPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const ::Ice::Current& = Ice::emptyCurrent) override;
        void removeColoredPointCloudDebugLayerVisu(const std::string& pointCloudName, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        void set24BitColoredPointCloudDebugLayerVisu(const std::string& pointCloudName, const DebugDrawer24BitColoredPointCloud& pointCloud, const ::Ice::Current& = Ice::emptyCurrent) override;
        void set24BitColoredPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const DebugDrawer24BitColoredPointCloud& pointCloud, const ::Ice::Current& = Ice::emptyCurrent) override;
        void remove24BitColoredPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const ::Ice::Current& = Ice::emptyCurrent) override;
        void remove24BitColoredPointCloudDebugLayerVisu(const std::string& pointCloudName, const ::Ice::Current& = Ice::emptyCurrent) override;
 
 
        void setPolygonVisu(const std::string& layerName, const std::string& polygonName, const std::vector< ::armarx::Vector3BasePtr >& polygonPoints, const DrawColor& colorInner, const DrawColor& colorBorder, float lineWidth, const ::Ice::Current& = Ice::emptyCurrent) override;
        void setPolygonDebugLayerVisu(const std::string& polygonName, const std::vector< ::armarx::Vector3BasePtr >& polygonPoints, const DrawColor& colorInner, const DrawColor& colorBorder, float lineWidth, const ::Ice::Current& = Ice::emptyCurrent) override;
        void removePolygonVisu(const std::string& layerName, const std::string& polygonName, const ::Ice::Current& = Ice::emptyCurrent) override;
        void removePolygonDebugLayerVisu(const std::string& polygonName, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        void setTriMeshVisu(const std::string& layerName, const std::string& triMeshName, const DebugDrawerTriMesh& triMesh, const ::Ice::Current& = Ice::emptyCurrent) override;
        void setTriMeshDebugLayerVisu(const std::string& triMeshName, const DebugDrawerTriMesh& triMesh, const ::Ice::Current& = Ice::emptyCurrent) override;
        void removeTriMeshVisu(const std::string& layerName, const std::string& triMeshName, const ::Ice::Current& = Ice::emptyCurrent) override;
        void removeTriMeshDebugLayerVisu(const std::string& triMeshName, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        void setArrowVisu(const std::string& layerName, const std::string& arrowName, const ::armarx::Vector3BasePtr& position, const ::armarx::Vector3BasePtr& direction, const DrawColor& color, float length, float width, const ::Ice::Current& = Ice::emptyCurrent) override;
        void setArrowDebugLayerVisu(const std::string& arrowName, const ::armarx::Vector3BasePtr& position, const ::armarx::Vector3BasePtr& direction, const DrawColor& color, float length, float width, const ::Ice::Current& = Ice::emptyCurrent) override;
        void removeArrowVisu(const std::string& layerName, const std::string& arrowName, const ::Ice::Current& = Ice::emptyCurrent) override;
        void removeArrowDebugLayerVisu(const std::string& arrowName, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        void 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&  = Ice::emptyCurrent) override;
        void 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& = Ice::emptyCurrent) override;
        void removeCircleVisu(const std::string& layerName, const std::string& circleName, const Ice::Current& = Ice::emptyCurrent) override;
        void removeCircleDebugLayerVisu(const std::string& circleName, const Ice::Current& = Ice::emptyCurrent) override;
 
 
        /*!
         * \brief setRobotVisu Initializes a robot visualization
         * \param layerName The layer
         * \param robotName The identifier of the robot
         * \param robotFile The filename of the robot. The robot must be locally present in a project.
         * \param DrawStyle Either FullModel ior CollisionModel.
         * \param armarxProject Additional armarx project that should be used to search the robot. Must be locally available and accessible through the armarx cmake search procedure.
         */
        void setRobotVisu(const std::string& layerName, const std::string& robotName, const std::string& robotFile, const std::string& armarxProject, DrawStyle drawStyle, const ::Ice::Current& = Ice::emptyCurrent) override;
        void updateRobotPose(const std::string& layerName, const std::string& robotName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Current& = Ice::emptyCurrent) override;
        void updateRobotConfig(const std::string& layerName, const std::string& robotName, const std::map< std::string, float>& configuration, const ::Ice::Current& = Ice::emptyCurrent) override;
        /*!
         * \brief updateRobotColor Colorizes the robot visualization
         * \param layerName The layer
         * \param robotName The robot identifyer
         * \param c The draw color, if all is set to 0, the colorization is disabled (i.e. the original vizaulization shows up)
         */
        void updateRobotColor(const std::string& layerName, const std::string& robotName, const armarx::DrawColor& c, const ::Ice::Current& = Ice::emptyCurrent) override;
        void updateRobotNodeColor(const std::string& layerName, const std::string& robotName, const std::string& robotNodeName, const armarx::DrawColor& c, const ::Ice::Current& = Ice::emptyCurrent) override;
        void removeRobotVisu(const std::string& layerName, const std::string& robotName, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        void clearAll(const ::Ice::Current& = Ice::emptyCurrent) override;
        void clearLayer(const std::string& layerName, const ::Ice::Current& = Ice::emptyCurrent) override;
        void clearDebugLayer(const ::Ice::Current& = Ice::emptyCurrent) override;
 
        bool hasLayer(const std::string& layerName, const ::Ice::Current& = Ice::emptyCurrent) override;
        void removeLayer(const std::string& layerName, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        void enableLayerVisu(const std::string& layerName, bool visible, const ::Ice::Current& = Ice::emptyCurrent) override;
        void enableDebugLayerVisu(bool visible, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        ::Ice::StringSeq layerNames(const ::Ice::Current& = Ice::emptyCurrent) override;
        ::armarx::LayerInformationSequence layerInformation(const ::Ice::Current& = Ice::emptyCurrent) override;
 
        void disableAllLayers(const ::Ice::Current& = Ice::emptyCurrent) override;
        void enableAllLayers(const ::Ice::Current& = Ice::emptyCurrent) override;
 
        // Methods for selection management
        void enableSelections(const std::string& layerName, const ::Ice::Current& = Ice::emptyCurrent) override;
        void disableSelections(const std::string& layerName, const ::Ice::Current& = Ice::emptyCurrent) override;
        void clearSelections(const std::string& layerName, const ::Ice::Current& = Ice::emptyCurrent) override;
        void select(const std::string&  layerName, const std::string& elementName, const ::Ice::Current& = Ice::emptyCurrent) override;
        void deselect(const std::string&  layerName, const std::string&  elementName, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        DebugDrawerSelectionList getSelections(const ::Ice::Current& = Ice::emptyCurrent) override;
 
        void selectionCallback();
        void deselectionCallback();
        void installSelectionCallbacks();
        void removeSelectionCallbacks();
 
        void reportSelectionChanged(const DebugDrawerSelectionList& selectedElements, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        // This is all total madness. Why do we put mutexes and locks into shared pointers?
        using RecursiveMutex = std::recursive_mutex;
        using RecursiveMutexPtr = std::shared_ptr<RecursiveMutex>;
        using RecursiveMutexLock = std::unique_lock<RecursiveMutex>;
        using RecursiveMutexLockPtr = std::shared_ptr<RecursiveMutexLock>;
 
        /*!
         * \brief getScopedLock If using the coin visualization it must be ensured that all rendering calls are protected with this mutex
         * \return The lock that is automatically destructed when leaving the current scope.
         */
        RecursiveMutexLockPtr getScopedVisuLock();
 
        /*!
         * \brief getVisualization Ensure that all access to this node is protected with the scoped lock mutex.
         * \return  The visualization
         */
        SoSeparator* getVisualization();
 
        void setMutex(RecursiveMutexPtr const& m);
 
        ~DebugDrawerComponent() override;
 
        void setDefaultLayerVisibility(bool defaultLayerVisibility)
        {
            this->defaultLayerVisibility = defaultLayerVisibility;
        }
        void setDefaultLayerVisibility(const std::string& layerName, bool defaultLayerVisibility)
        {
            defaultLayerVisibilityPerLayer[layerName] = defaultLayerVisibility;
        }
        void removeDefaultLayerVisibility(const std::string& layerName)
        {
            defaultLayerVisibilityPerLayer.erase(layerName);
        }
        bool getDefaultLayerVisibility() const
        {
            return defaultLayerVisibility;
        }
        bool getDefaultLayerVisibility(const std::string& layerName) const
        {
            auto elem = defaultLayerVisibilityPerLayer.find(layerName);
            if (elem != defaultLayerVisibilityPerLayer.end())
            {
                return elem->second;
            }
            return defaultLayerVisibility;
        }
        std::map<std::string, bool> getAllDefaultLayerVisibilities() const
        {
            return defaultLayerVisibilityPerLayer;
        }
    protected:
 
        static void updateVisualizationCB(void* data, SoSensor* sensor);
 
        // It turned out, that heavy data traffic (resulting in heavy mutex locking) somehow interferes with coin rendering: the
        // SoQt timers are misaligned at some point, could be a bug in SoQt, that results in a situation where the GL window is never updated
        // while the rest of the qt widget works well.
        // The solution for now: collect all data updates and update the coin scene by qt-timed thread (qt events ensure that no drawing event is active)
 
        struct DrawData
        {
            DrawData()
            {
                active = false;
                update = false;
            }
            std::string layerName;
            std::string name;
            bool active;
            /**
             * @brief Whether an existing visu should be updated
             */
            bool update;
        };
 
        struct CoordData : public DrawData
        {
            Eigen::Matrix4f globalPose;
            float scale;
        };
        struct LineData : public DrawData
        {
            Eigen::Vector3f p1;
            Eigen::Vector3f p2;
            float scale;
            VirtualRobot::VisualizationFactory::Color color;
        };
        struct LineSetData : public DrawData
        {
            DebugDrawerLineSet lineSet;
        };
        struct BoxData : public DrawData
        {
            Eigen::Matrix4f globalPose;
            float width;
            float height;
            float depth;
            VirtualRobot::VisualizationFactory::Color color;
        };
        struct TextData : public DrawData
        {
            std::string text;
            Eigen::Vector3f position;
            VirtualRobot::VisualizationFactory::Color color;
            int size;
        };
        struct SphereData : public DrawData
        {
            Eigen::Vector3f position;
            VirtualRobot::VisualizationFactory::Color color;
            float radius;
        };
        struct CylinderData : public DrawData
        {
            Eigen::Vector3f position;
            Eigen::Vector3f direction;
            float length;
            float radius;
            VirtualRobot::VisualizationFactory::Color color;
        };
        struct CircleData : public DrawData
        {
            Eigen::Vector3f position;
            Eigen::Vector3f direction;
            float radius;
            float width;
            float circleCompletion;
            VirtualRobot::VisualizationFactory::Color color;
        };
        struct PointCloudData : public DrawData
        {
            DebugDrawerPointCloud pointCloud;
        };
        struct ColoredPointCloudData : public DrawData
        {
            DebugDrawerColoredPointCloud pointCloud;
        };
        struct Colored24BitPointCloudData : public DrawData
        {
            DebugDrawer24BitColoredPointCloud pointCloud;
        };
        struct PolygonData : public DrawData
        {
            std::vector< Eigen::Vector3f > points;
            float lineWidth;
            VirtualRobot::VisualizationFactory::Color colorInner;
            VirtualRobot::VisualizationFactory::Color colorBorder;
        };
        struct TriMeshData : public DrawData
        {
            DebugDrawerTriMesh triMesh;
        };
 
        struct ArrowData : public DrawData
        {
            Eigen::Vector3f position;
            Eigen::Vector3f direction;
            float length;
            float width;
            VirtualRobot::VisualizationFactory::Color color;
        };
        struct RobotData : public DrawData
        {
            RobotData()
            {
                updateColor = false;
                updatePose = false;
                updateConfig = false;
            }
            std::string robotFile;
            std::string armarxProject;
 
            bool updateColor;
            VirtualRobot::VisualizationFactory::Color color;
 
            bool updateNodeColor;
            std::map < std::string, VirtualRobot::VisualizationFactory::Color > nodeColors;
 
            bool updatePose;
            Eigen::Matrix4f globalPose;
 
            bool updateConfig;
            std::map < std::string, float > configuration;
 
            armarx::DrawStyle drawStyle;
        };
 
        struct UpdateData
        {
            std::map<std::string, CoordData> coord;
            std::map<std::string, LineData> line;
            std::map<std::string, LineSetData> lineSet;
            std::map<std::string, BoxData> box;
            std::map<std::string, TextData> text;
            std::map<std::string, SphereData> sphere;
            std::map<std::string, CircleData> circle;
            std::map<std::string, CylinderData> cylinder;
            std::map<std::string, PointCloudData> pointcloud;
            std::map<std::string, ColoredPointCloudData> coloredpointcloud;
            std::map<std::string, Colored24BitPointCloudData> colored24Bitpointcloud;
            std::map<std::string, PolygonData> polygons;
            std::map<std::string, TriMeshData> triMeshes;
            std::map<std::string, ArrowData> arrows;
            std::map<std::string, RobotData> robots;
 
            std::set< std::string > clearLayers;
            std::set< std::string > removeLayers;
        };
 
        UpdateData accumulatedUpdateData;
 
        // Reads accumulatedUpdateData, writes all data to coin visualization and clears accumulatedUpdateData
        void updateVisualization();
 
        /*!
         * \brief onUpdateVisualization derived methods can overwrite this method to update custom visualizations.
         */
        virtual void onUpdateVisualization() {}
        virtual void onRemoveAccumulatedData(const std::string& layerName) {}
 
        virtual void removeCustomVisu(const std::string& layerName, const std::string& name) {}
 
        void drawCoordSystem(const CoordData& d);
        void drawLine(const LineData& d);
        void drawLineSet(const LineSetData& d);
        void drawBox(const BoxData& d);
        void drawText(const TextData& d);
        void drawSphere(const SphereData& d);
        void drawCylinder(const CylinderData& d);
        void drawCircle(const DebugDrawerComponent::CircleData& d);
        void drawPointCloud(const PointCloudData& d);
        void drawColoredPointCloud(const ColoredPointCloudData& d);
        void draw24BitColoredPointCloud(const Colored24BitPointCloudData& d);
        void drawPolygon(const PolygonData& d);
        void drawTriMesh(const TriMeshData& d);
        void drawArrow(const ArrowData& d);
        void drawRobot(const RobotData& d);
 
        void removeCoordSystem(const std::string& layerName, const std::string& name);
        void removeLine(const std::string& layerName, const std::string& name);
        void removeLineSet(const std::string& layerName, const std::string& name);
        void removeBox(const std::string& layerName, const std::string& name);
        void removeText(const std::string& layerName, const std::string& name);
        void removeSphere(const std::string& layerName, const std::string& name);
        void removeCylinder(const std::string& layerName, const std::string& name);
        void removeCircle(const std::string& layerName, const std::string& name);
        void removePointCloud(const std::string& layerName, const std::string& name);
        void removeColoredPointCloud(const std::string& layerName, const std::string& name);
        void remove24BitColoredPointCloud(const std::string& layerName, const std::string& name);
        void removePolygon(const std::string& layerName, const std::string& name);
        void removeTriMesh(const std::string& layerName, const std::string& name);
        void removeArrow(const std::string& layerName, const std::string& name);
        void removeRobot(const std::string& layerName, const std::string& name);
 
 
 
        /*!
         * QT safe version, should not be called from an ICE thread
         */
        void clearLayerQt(const std::string& layerName);
        void removeLayerQt(const std::string& layerName);
 
        void setLayerVisibility(const std::string& layerName, bool visible);
 
        /*!
         * \brief Contains data for a layer.
         */
        struct Layer
        {
            SoSeparator* mainNode;
            std::map<std::string, SoSeparator*> addedCoordVisualizations;
            std::map<std::string, SoSeparator*> addedLineVisualizations;
            std::map<std::string, SoSeparator*> addedLineSetVisualizations;
            std::map<std::string, SoSeparator*> addedBoxVisualizations;
            std::map<std::string, SoSeparator*> addedTextVisualizations;
            std::map<std::string, SoSeparator*> addedSphereVisualizations;
            std::map<std::string, SoSeparator*> addedCylinderVisualizations;
            std::map<std::string, SoSeparator*> addedCircleVisualizations;
            std::map<std::string, SoSeparator*> addedPointCloudVisualizations;
            std::map<std::string, SoSeparator*> addedColoredPointCloudVisualizations;
            std::map<std::string, SoSeparator*> added24BitColoredPointCloudVisualizations;
            std::map<std::string, SoSeparator*> addedPolygonVisualizations;
            std::map<std::string, SoSeparator*> addedTriMeshVisualizations;
            std::map<std::string, SoSeparator*> addedArrowVisualizations;
            std::map<std::string, SoSeparator*> addedRobotVisualizations;
            std::map<std::string, SoSeparator*> addedCustomVisualizations; // these separators are only used by derived classes and have to start with a material node
 
            bool visible;
        };
 
        /*!
         * \brief If a layer with layerName exists it is returned. Otherwise a new layer with given name is created and returned.
         * \param layerName The layer.
         * \return The requested layer.
         */
        Layer& requestLayer(const std::string& layerName);
        VirtualRobot::RobotPtr requestRobot(const RobotData& d);
 
        /*!
         * \brief ensureExistingRobotNodes Checks if all robotnodes of configuration exist in robot. If not the specific configuration entry is removed from the list.
         * \param d
         */
        void ensureExistingRobotNodes(DebugDrawerComponent::RobotData& d);
 
        std::map < std::string, VirtualRobot::RobotPtr > activeRobots;
        SoSeparator* coinVisu;
        SoSelection* selectionNode;
 
        bool verbose;
 
        /*!
         * \brief Main node for all layers
         */
        SoSeparator* layerMainNode;
 
        /*!
         * \brief All existing layers.
         */
        std::map<const std::string, Layer> layers;
 
        //! The coin mutex
        RecursiveMutexPtr mutex;
 
        //! The data update mutex
        RecursiveMutexPtr dataUpdateMutex;
 
        RecursiveMutexPtr topicMutex;
 
        static std::recursive_mutex selectionMutex;
        std::set<std::string> selectableLayers;
 
        RecursiveMutexLockPtr getScopedAccumulatedDataLock();
 
        DebugDrawerListenerPrx listenerPrx;
 
        float cycleTimeMS;
        //PeriodicTask<DebugDrawerComponent>::pointer_type execTaskVisuUpdates;
        SoTimerSensor* timerSensor;
        std::mutex timerSensorMutex;
        void removeAccumulatedData(const std::string& layerName);
 
        std::map<std::string, VirtualRobot::RobotPtr> loadedRobots;
 
        bool defaultLayerVisibility;
        std::map<std::string, bool> defaultLayerVisibilityPerLayer;
 
    };
 
    using DebugDrawerComponentPtr = IceInternal::Handle<DebugDrawerComponent>;
 
}