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/components/ArViz/Client/Client.h>
#include <RobotAPI/interface/visualization/DebugDrawerToArViz.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;
    };
} // namespace armarx