DebugDrawerToArViz_8h_source.html

/*

 * 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