DebugDrawerToArViz.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    RobotAPI::ArmarXObjects::DebugDrawerToArViz
 * @author     Rainer Kartmann ( rainer dot kartmann at kit dot edu )
 * @date       2020
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <map>
#include <mutex>
 
#include <RobotAPI/interface/visualization/DebugDrawerToArViz.h>
#include <RobotAPI/components/ArViz/Client/Client.h>
 
#include "BlackWhitelist.h"
 
 
namespace armarx
{
 
    /**
     * @brief Passes updates from DebugDrawerInterface to ArViz.
     */
    class DebugDrawerToArViz : virtual public armarx::DebugDrawerToArvizInterface
    {
    public:
 
        void setArViz(viz::Client arviz);
 
 
        // BlackWhitelistTopic interface
    public:
        void updateBlackWhitelist(const BlackWhitelistUpdate& update, const Ice::Current& = Ice::emptyCurrent) override;
 
 
        // DebugDrawerInterface interface
    public:
 
        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& layer, const std::string& name, const PoseBasePtr& globalPose, const Ice::Current& = Ice::emptyCurrent) override;
        void setScaledPoseVisu(const std::string& layer, const std::string& name, const PoseBasePtr& globalPose, Ice::Float scale, const Ice::Current& = Ice::emptyCurrent) override;
        void setLineVisu(const std::string& layer, const std::string& name, const Vector3BasePtr& globalPosition1, const Vector3BasePtr& globalPosition2, Ice::Float lineWidth, const DrawColor& color, const Ice::Current& = Ice::emptyCurrent) override;
        void setLineSetVisu(const std::string& layer, const std::string& name, const DebugDrawerLineSet& lineSet, const Ice::Current& = Ice::emptyCurrent) override;
        void setBoxVisu(const std::string& layer, const std::string& name, const PoseBasePtr& globalPose, const Vector3BasePtr& dimensions, const DrawColor& color, const Ice::Current& = Ice::emptyCurrent) override;
        void setTextVisu(const std::string& layer, const std::string& name, const std::string& text, const Vector3BasePtr& globalPosition, const DrawColor& color, Ice::Int size, const Ice::Current& = Ice::emptyCurrent) override;
        void setSphereVisu(const std::string& layer, const std::string& name, const Vector3BasePtr& globalPosition, const DrawColor& color, Ice::Float radius, const Ice::Current& = Ice::emptyCurrent) override;
        void setPointCloudVisu(const std::string& layer, const std::string& name, const DebugDrawerPointCloud& pointCloud, const Ice::Current& = Ice::emptyCurrent) override;
        void setColoredPointCloudVisu(const std::string& layer, const std::string& name, const DebugDrawerColoredPointCloud& pointCloud, const Ice::Current& = Ice::emptyCurrent) override;
        void set24BitColoredPointCloudVisu(const std::string& layer, const std::string& name, const DebugDrawer24BitColoredPointCloud& pointCloud, const Ice::Current& = Ice::emptyCurrent) override;
        void setPolygonVisu(const std::string& layer, const std::string& name, const PolygonPointList& polygonPoints, const DrawColor& colorInner, const DrawColor& colorBorder, Ice::Float lineWidth, const Ice::Current& = Ice::emptyCurrent) override;
        void setTriMeshVisu(const std::string& layer, const std::string& name, const DebugDrawerTriMesh& triMesh, const Ice::Current& = Ice::emptyCurrent) override;
        void setArrowVisu(const std::string& layer, const std::string& name, const Vector3BasePtr& position, const Vector3BasePtr& direction, const DrawColor& color, Ice::Float length, Ice::Float width, const Ice::Current& = Ice::emptyCurrent) override;
        void setCylinderVisu(const std::string& layer, const std::string& name, const Vector3BasePtr& globalPosition, const Vector3BasePtr& direction, Ice::Float length, Ice::Float radius, const DrawColor& color, const Ice::Current& = Ice::emptyCurrent) override;
        void setCircleArrowVisu(const std::string& layer, const std::string& name, 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 setRobotVisu(const std::string& layer, const std::string& name, const std::string& robotFile, const std::string& armarxProject, DrawStyle drawStyleType, const Ice::Current& = Ice::emptyCurrent) override;
        void updateRobotPose(const std::string& layer, const std::string& name, const PoseBasePtr& globalPose, const Ice::Current& = Ice::emptyCurrent) override;
        void updateRobotConfig(const std::string& layer, const std::string& name, const NameValueMap& configuration, const Ice::Current& = Ice::emptyCurrent) override;
        void updateRobotColor(const std::string& layer, const std::string& name, const DrawColor& color, const Ice::Current& = Ice::emptyCurrent) override;
        void updateRobotNodeColor(const std::string& layer, const std::string& name, const std::string& robotNodeName, const DrawColor& color, const Ice::Current& = Ice::emptyCurrent) override;
        void removeRobotVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
 
        void setPoseDebugLayerVisu(const std::string& name, const PoseBasePtr& globalPose, const Ice::Current& = Ice::emptyCurrent) override;
        void setScaledPoseDebugLayerVisu(const std::string& name, const PoseBasePtr& globalPose, Ice::Float scale, const Ice::Current& = Ice::emptyCurrent) override;
        void setLineDebugLayerVisu(const std::string& name, const Vector3BasePtr& globalPosition1, const Vector3BasePtr& globalPosition2, Ice::Float lineWidth, const DrawColor& color, const Ice::Current& = Ice::emptyCurrent) override;
        void setLineSetDebugLayerVisu(const std::string& name, const DebugDrawerLineSet& lineSet, const Ice::Current& = Ice::emptyCurrent) override;
        void setBoxDebugLayerVisu(const std::string& name, const PoseBasePtr& globalPose, const Vector3BasePtr& dimensions, const DrawColor& color, const Ice::Current& = Ice::emptyCurrent) override;
        void setTextDebugLayerVisu(const std::string& name, const std::string& text, const Vector3BasePtr& globalPosition, const DrawColor& color, Ice::Int size, const Ice::Current& = Ice::emptyCurrent) override;
        void setSphereDebugLayerVisu(const std::string& name, const Vector3BasePtr& globalPosition, const DrawColor& color, Ice::Float radius, const Ice::Current& = Ice::emptyCurrent) override;
        void setPointCloudDebugLayerVisu(const std::string& name, const DebugDrawerPointCloud& pointCloud, const Ice::Current& = Ice::emptyCurrent) override;
        void set24BitColoredPointCloudDebugLayerVisu(const std::string& name, const DebugDrawer24BitColoredPointCloud& pointCloud, const Ice::Current& = Ice::emptyCurrent) override;
        void setPolygonDebugLayerVisu(const std::string& name, const PolygonPointList& polygonPoints, const DrawColor& colorInner, const DrawColor& colorBorder, Ice::Float lineWidth, const Ice::Current& = Ice::emptyCurrent) override;
        void setTriMeshDebugLayerVisu(const std::string& name, const DebugDrawerTriMesh& triMesh, const Ice::Current& = Ice::emptyCurrent) override;
        void setArrowDebugLayerVisu(const std::string& name, const Vector3BasePtr& position, const Vector3BasePtr& direction, const DrawColor& color, Ice::Float length, Ice::Float width, const Ice::Current& = Ice::emptyCurrent) override;
        void setCylinderDebugLayerVisu(const std::string& name, const Vector3BasePtr& globalPosition, const Vector3BasePtr& direction, Ice::Float length, Ice::Float radius, const DrawColor& color, const Ice::Current& = Ice::emptyCurrent) override;
        void setCircleDebugLayerVisu(const std::string& name, 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 removePoseVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
        void removeLineVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
        void removeLineSetVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
        void removeBoxVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
        void removeTextVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
        void removeSphereVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
        void removePointCloudVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
        void removeColoredPointCloudVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
        void remove24BitColoredPointCloudVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
        void removePolygonVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
        void removeTriMeshVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
        void removeArrowVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
        void removeCylinderVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
        void removeCircleVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
 
        void removePoseDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
        void removeLineDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
        void removeLineSetDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
        void removeBoxDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
        void removeTextDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
        void removeSphereDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
        void removePointCloudDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
        void removeColoredPointCloudDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
        void remove24BitColoredPointCloudDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
        void removePolygonDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
        void removeTriMeshDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
        void removeArrowDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
        void removeCylinderDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
        void removeCircleDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
 
        void clearAll(const Ice::Current& = Ice::emptyCurrent) override;
        void clearLayer(const std::string& layer, const Ice::Current& = Ice::emptyCurrent) override;
        void clearDebugLayer(const Ice::Current& = Ice::emptyCurrent) override;
 
        void enableLayerVisu(const std::string& layer, 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;
        LayerInformationSequence layerInformation(const Ice::Current& = Ice::emptyCurrent) override;
 
        bool hasLayer(const std::string&, const Ice::Current& = Ice::emptyCurrent) override;
        void removeLayer(const std::string&, const Ice::Current& = Ice::emptyCurrent) override;
 
        void disableAllLayers(const Ice::Current& = Ice::emptyCurrent) override;
        void enableAllLayers(const Ice::Current& = Ice::emptyCurrent) override;
 
        void enableSelections(const std::string&, const Ice::Current& = Ice::emptyCurrent) override;
        void disableSelections(const std::string&, const Ice::Current& = Ice::emptyCurrent) override;
        void clearSelections(const std::string&, const Ice::Current& = Ice::emptyCurrent) override;
 
        void select(const std::string& layer, const std::string& elementName, const Ice::Current& = Ice::emptyCurrent) override;
        void deselect(const std::string& layer, const std::string& elementName, const Ice::Current& = Ice::emptyCurrent) override;
 
        DebugDrawerSelectionList getSelections(const Ice::Current& = Ice::emptyCurrent) override;
 
 
    private:
 
        viz::Layer& getLayer(const std::string& layerName);
        viz::data::ElementSeq::iterator
        findLayerElement(viz::Layer& layer, const std::string& elementName);
 
 
        template <class Element>
        void setLayerElement(viz::Layer& layer, Element element)
        {
            bool found = false;
            for (size_t i = 0; i < layer.data_.elements.size(); ++i)
            {
                if (layer.data_.elements[i]->id == element.data_->id)
                {
                    // Replace existing element with given element.
                    layer.data_.elements[i] = element.data_;
                    found = true;
                    break;
                }
            }
            if (!found)
            {
                // Add given element.
                layer.add(element);
            }
        }
        template <class Element>
        void setLayerElement(const std::string& layerName, Element element)
        {
            setLayerElement(getLayer(layerName), element);
        }
 
        template <class Element>
        void setAndCommit(const std::string& layerName, Element element)
        {
            viz::Layer& layer = getLayer(layerName);
            setLayerElement(layer, element);
            arviz.commit({layer});
        }
 
        void removeLayerElement(viz::Layer& layer, const std::string& name);
        void removeAndCommit(const std::string& layerName, const std::string& name);
 
 
    public:
 
        armarx::StringBlackWhitelist layerBlackWhitelist;
 
 
    private:
 
        const std::string DEBUG_LAYER_NAME = "debug";
 
        std::mutex mutex;
 
        viz::Client arviz;
 
        std::map<std::string, viz::Layer> layers;
 
        std::map<std::pair<std::string, std::string>, viz::Robot> robots;
 
    };
}