DebugDrawerToArViz.cpp

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
 */
 
#include "DebugDrawerToArViz.h"
 
#include <SimoxUtility/color/interpolation.h>
#include <SimoxUtility/math/pose/pose.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
 
#include "BlackWhitelistUpdate.h"
 
 
#define FUNCTION_NOT_IMPLEMENTED_MESSAGE                                                           \
    "Function DebugDrawerToArViz::" << __FUNCTION__ << "(): Not implemented."
 
#define LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE() ARMARX_VERBOSE << FUNCTION_NOT_IMPLEMENTED_MESSAGE
 
namespace armarx
{
 
    namespace
    {
        Eigen::Vector3f
        toEigen(Vector3BasePtr v)
        {
            ARMARX_CHECK_NOT_NULL(v);
            return {v->x, v->y, v->z};
        }
 
        Eigen::Vector3f
        toEigen(DebugDrawerPointCloudElement e)
        {
            return {e.x, e.y, e.z};
        }
 
        Eigen::Quaternionf
        toEigen(QuaternionBasePtr q)
        {
            ARMARX_CHECK_NOT_NULL(q);
            return Eigen::Quaternionf(q->qw, q->qx, q->qy, q->qz);
        }
 
        Eigen::Matrix4f
        toEigen(PoseBasePtr pose)
        {
            ARMARX_CHECK_NOT_NULL(pose);
            return simox::math::pose(toEigen(pose->position), toEigen(pose->orientation));
        }
 
        simox::Color
        toSimox(DrawColor c)
        {
            return simox::Color(c.r, c.g, c.b, c.a);
        }
 
        viz::Color
        toViz(DrawColor c)
        {
            return viz::Color(toSimox(c));
        }
    } // namespace
 
    void
    DebugDrawerToArViz::setArViz(viz::Client arviz)
    {
        this->arviz = arviz;
    }
 
    void
    DebugDrawerToArViz::updateBlackWhitelist(const BlackWhitelistUpdate& update,
                                             const Ice::Current&)
    {
        std::scoped_lock lock(mutex);
 
        armarx::updateBlackWhitelist(layerBlackWhitelist, update);
        ARMARX_VERBOSE << "Updated layer black-whitelist: \n" << layerBlackWhitelist;
 
        // Remove all excluded layers.
        std::vector<viz::Layer> cleared;
        for (const auto& [name, layer] : layers)
        {
            if (layerBlackWhitelist.isExcluded(name))
            {
                cleared.push_back(arviz.layer(name));
            }
        }
        if (!cleared.empty())
        {
            arviz.commit(cleared);
        }
    }
 
    void
    DebugDrawerToArViz::exportScene(const std::string&, const Ice::Current&)
    {
        LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
    }
 
    void
    DebugDrawerToArViz::exportLayer(const std::string&, const std::string&, const Ice::Current&)
    {
        LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
    }
 
    void
    DebugDrawerToArViz::setPoseVisu(const std::string& layer,
                                    const std::string& name,
                                    const PoseBasePtr& globalPose,
                                    const Ice::Current&)
    {
        std::scoped_lock lock(mutex);
        if (layerBlackWhitelist.isExcluded(layer))
        {
            return;
        }
        setAndCommit(layer, viz::Pose(name).pose(toEigen(globalPose)));
    }
 
    void
    DebugDrawerToArViz::setScaledPoseVisu(const std::string& layer,
                                          const std::string& name,
                                          const PoseBasePtr& globalPose,
                                          Ice::Float scale,
                                          const Ice::Current&)
    {
        std::scoped_lock lock(mutex);
        if (layerBlackWhitelist.isExcluded(layer))
        {
            return;
        }
        setAndCommit(layer, viz::Pose(name).pose(toEigen(globalPose)).scale(scale));
    }
 
    void
    DebugDrawerToArViz::setLineVisu(const std::string& layer,
                                    const std::string& name,
                                    const Vector3BasePtr& globalPosition1,
                                    const Vector3BasePtr& globalPosition2,
                                    Ice::Float lineWidth,
                                    const DrawColor& color,
                                    const Ice::Current&)
    {
        std::scoped_lock lock(mutex);
        if (layerBlackWhitelist.isExcluded(layer))
        {
            return;
        }
        setAndCommit(layer,
                     viz::Path(name)
                         .addPoint(toEigen(globalPosition1))
                         .addPoint(toEigen(globalPosition2))
                         .color(toViz(color))
                         .width(lineWidth)
                         .color(simox::Color::black(0)));
    }
 
    void
    DebugDrawerToArViz::setLineSetVisu(const std::string& layerName,
                                       const std::string& name,
                                       const DebugDrawerLineSet& lineSet,
                                       const Ice::Current&)
    {
        std::scoped_lock lock(mutex);
        if (layerBlackWhitelist.isExcluded(layerName))
        {
            return;
        }
 
        viz::Layer& layer = getLayer(layerName);
        ARMARX_CHECK_EQUAL(lineSet.points.size() % 2, 0) << VAROUT(lineSet.points.size());
        ARMARX_CHECK_EQUAL(lineSet.intensities.size(), lineSet.points.size() / 2);
 
        if (lineSet.useHeatMap)
        {
            ARMARX_VERBOSE << "DebugDrawerToArViz::" << __FUNCTION__ << "(): "
                           << "'useHeatMap' not supported.";
        }
 
        simox::Color color0 = toSimox(lineSet.colorNoIntensity);
        simox::Color color1 = toSimox(lineSet.colorFullIntensity);
 
        for (size_t i = 0; i + 1 < lineSet.points.size(); i += 2)
        {
            const auto& p1 = lineSet.points[i];
            const auto& p2 = lineSet.points[i + 1];
            float intensity = lineSet.intensities[i / 2];
            simox::Color color = simox::color::interpol::linear(intensity, color0, color1);
 
            std::stringstream ss;
            ss << name << "/" << i << "_" << i + 1;
            setLayerElement(layer,
                            viz::Polygon(ss.str())
                                .addPoint(toEigen(p1))
                                .addPoint(toEigen(p2))
                                .lineColor(color)
                                .lineWidth(lineSet.lineWidth)
                                .color(simox::Color::black(0)));
        }
        arviz.commit({layer});
    }
 
    void
    DebugDrawerToArViz::setBoxVisu(const std::string& layer,
                                   const std::string& name,
                                   const PoseBasePtr& globalPose,
                                   const Vector3BasePtr& dimensions,
                                   const DrawColor& color,
                                   const Ice::Current&)
    {
        std::scoped_lock lock(mutex);
        if (layerBlackWhitelist.isExcluded(layer))
        {
            return;
        }
        setAndCommit(
            layer,
            viz::Box(name).pose(toEigen(globalPose)).size(toEigen(dimensions)).color(toViz(color)));
    }
 
    void
    DebugDrawerToArViz::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&)
    {
        std::scoped_lock lock(mutex);
        if (layerBlackWhitelist.isExcluded(layer))
        {
            return;
        }
        setAndCommit(layer,
                     viz::Text(name)
                         .text(text)
                         .position(toEigen(globalPosition))
                         .scale(size)
                         .color(toViz(color)));
    }
 
    void
    DebugDrawerToArViz::setSphereVisu(const std::string& layer,
                                      const std::string& name,
                                      const Vector3BasePtr& globalPosition,
                                      const DrawColor& color,
                                      Ice::Float radius,
                                      const Ice::Current&)
    {
        std::scoped_lock lock(mutex);
        if (layerBlackWhitelist.isExcluded(layer))
        {
            return;
        }
        setAndCommit(
            layer,
            viz::Sphere(name).position(toEigen(globalPosition)).radius(radius).color(toViz(color)));
    }
 
    void
    DebugDrawerToArViz::setPointCloudVisu(const std::string& layer,
                                          const std::string& name,
                                          const DebugDrawerPointCloud& pointCloud,
                                          const Ice::Current&)
    {
        std::scoped_lock lock(mutex);
        if (layerBlackWhitelist.isExcluded(layer))
        {
            return;
        }
 
        viz::PointCloud cloud(name);
        for (const auto& p : pointCloud.points)
        {
            cloud.addPoint(p.x, p.y, p.z);
        }
        setAndCommit(layer, cloud.pointSizeInPixels(pointCloud.pointSize));
    }
 
    void
    DebugDrawerToArViz::setColoredPointCloudVisu(const std::string& layer,
                                                 const std::string& name,
                                                 const DebugDrawerColoredPointCloud& pointCloud,
                                                 const Ice::Current&)
    {
        std::scoped_lock lock(mutex);
        if (layerBlackWhitelist.isExcluded(layer))
        {
            return;
        }
 
        viz::PointCloud cloud(name);
        for (const auto& p : pointCloud.points)
        {
            viz::ColoredPoint cp;
            cp.x = p.x;
            cp.y = p.y;
            cp.z = p.z;
            cp.color = toViz(p.color);
            cloud.addPoint(cp);
        }
        setAndCommit(layer, cloud.pointSizeInPixels(pointCloud.pointSize));
    }
 
    void
    DebugDrawerToArViz::set24BitColoredPointCloudVisu(
        const std::string& layer,
        const std::string& name,
        const DebugDrawer24BitColoredPointCloud& pointCloud,
        const Ice::Current&)
    {
        std::scoped_lock lock(mutex);
        if (layerBlackWhitelist.isExcluded(layer))
        {
            return;
        }
 
        viz::PointCloud cloud(name);
        for (const auto& p : pointCloud.points)
        {
            viz::ColoredPoint cp;
            cp.x = p.x;
            cp.y = p.y;
            cp.z = p.z;
            cp.color = viz::Color(simox::Color(p.color.r, p.color.g, p.color.b));
            cloud.addPoint(cp);
        }
        setAndCommit(layer, cloud.pointSizeInPixels(pointCloud.pointSize));
    }
 
    void
    DebugDrawerToArViz::setPolygonVisu(const std::string& layer,
                                       const std::string& name,
                                       const PolygonPointList& polygonPoints,
                                       const DrawColor& colorInner,
                                       const DrawColor& colorBorder,
                                       Ice::Float lineWidth,
                                       const Ice::Current&)
    {
        std::scoped_lock lock(mutex);
        if (layerBlackWhitelist.isExcluded(layer))
        {
            return;
        }
 
        viz::Polygon poly(name);
        for (const auto& p : polygonPoints)
        {
            poly.addPoint(toEigen(p));
        }
        setAndCommit(
            layer,
            poly.color(toViz(colorInner)).lineColor(toViz(colorBorder)).lineWidth(lineWidth));
    }
 
    void
    DebugDrawerToArViz::setTriMeshVisu(const std::string& layer,
                                       const std::string& name,
                                       const DebugDrawerTriMesh& triMesh,
                                       const Ice::Current&)
    {
        std::scoped_lock lock(mutex);
        if (layerBlackWhitelist.isExcluded(layer))
        {
            return;
        }
 
        std::vector<Eigen::Vector3f> vertices;
        std::vector<viz::data::Color> colors;
        std::vector<viz::data::Face> faces;
        for (const auto& v : triMesh.vertices)
        {
            vertices.emplace_back(v.x, v.y, v.z);
        }
        for (const auto& c : triMesh.colors)
        {
            colors.emplace_back(toViz(c));
        }
        for (const auto& f : triMesh.faces)
        {
            viz::data::Face& face = faces.emplace_back();
            face.v0 = f.vertex1.vertexID;
            face.v1 = f.vertex2.vertexID;
            face.v2 = f.vertex3.vertexID;
            face.c0 = f.vertex1.colorID;
            face.c1 = f.vertex2.colorID;
            face.c2 = f.vertex3.colorID;
        }
        setAndCommit(layer, viz::Mesh(name).vertices(vertices).colors(colors).faces(faces));
    }
 
    void
    DebugDrawerToArViz::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&)
    {
        std::scoped_lock lock(mutex);
        if (layerBlackWhitelist.isExcluded(layer))
        {
            return;
        }
        setAndCommit(layer,
                     viz::Arrow(name)
                         .position(toEigen(position))
                         .direction(toEigen(direction))
                         .color(toViz(color))
                         .width(width)
                         .length(length));
    }
 
    void
    DebugDrawerToArViz::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&)
    {
        std::scoped_lock lock(mutex);
        if (layerBlackWhitelist.isExcluded(layer))
        {
            return;
        }
        setAndCommit(layer,
                     viz::Cylinder(name)
                         .fromTo(toEigen(globalPosition),
                                 toEigen(globalPosition) + length * toEigen(direction))
                         .color(toViz(color))
                         .radius(radius));
    }
 
    void
    DebugDrawerToArViz::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&)
    {
        std::scoped_lock lock(mutex);
        if (layerBlackWhitelist.isExcluded(layer))
        {
            return;
        }
        setAndCommit(layer,
                     viz::ArrowCircle(name)
                         .position(toEigen(globalPosition))
                         .normal(toEigen(directionVec))
                         .radius(radius)
                         .completion(circleCompletion)
                         .color(toViz(color))
                         .width(width));
    }
 
    void
    DebugDrawerToArViz::setRobotVisu(const std::string& layer,
                                     const std::string& name,
                                     const std::string& robotFile,
                                     const std::string& armarxProject,
                                     DrawStyle drawStyleType,
                                     const Ice::Current&)
    {
        std::scoped_lock lock(mutex);
        if (layerBlackWhitelist.isExcluded(layer))
        {
            return;
        }
 
        viz::Robot robot = viz::Robot(name).file(armarxProject, robotFile);
        switch (drawStyleType)
        {
            case DrawStyle::CollisionModel:
                robot.useCollisionModel();
                break;
            case DrawStyle::FullModel:
                robot.useFullModel();
                break;
        }
 
        robots.emplace(std::make_pair(layer, name), robot);
        setAndCommit(layer, robot);
    }
 
    void
    DebugDrawerToArViz::updateRobotPose(const std::string& layer,
                                        const std::string& name,
                                        const PoseBasePtr& globalPose,
                                        const Ice::Current&)
    {
        std::scoped_lock lock(mutex);
        if (layerBlackWhitelist.isExcluded(layer))
        {
            return;
        }
        if (auto it = robots.find(std::make_pair(layer, name)); it != robots.end())
        {
            viz::Robot& robot = it->second;
            robot.pose(toEigen(globalPose));
        }
        arviz.commit({getLayer(layer)});
    }
 
    void
    DebugDrawerToArViz::updateRobotConfig(const std::string& layer,
                                          const std::string& name,
                                          const NameValueMap& configuration,
                                          const Ice::Current&)
    {
        std::scoped_lock lock(mutex);
        if (layerBlackWhitelist.isExcluded(layer))
        {
            return;
        }
        if (auto it = robots.find(std::make_pair(layer, name)); it != robots.end())
        {
            viz::Robot& robot = it->second;
            robot.joints(configuration);
        }
        arviz.commit({getLayer(layer)});
    }
 
    void
    DebugDrawerToArViz::updateRobotColor(const std::string& layer,
                                         const std::string& name,
                                         const DrawColor& color,
                                         const Ice::Current&)
    {
        std::scoped_lock lock(mutex);
        if (layerBlackWhitelist.isExcluded(layer))
        {
            return;
        }
        if (auto it = robots.find(std::make_pair(layer, name)); it != robots.end())
        {
            viz::Robot& robot = it->second;
            robot.overrideColor(toViz(color));
        }
        arviz.commit({getLayer(layer)});
    }
 
    void
    DebugDrawerToArViz::updateRobotNodeColor(const std::string& layer,
                                             const std::string& name,
                                             const std::string& robotNodeName,
                                             const DrawColor& color,
                                             const Ice::Current&)
    {
        (void)layer, (void)name, (void)robotNodeName, (void)color;
        LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
    }
 
    void
    DebugDrawerToArViz::removeRobotVisu(const std::string& layer,
                                        const std::string& name,
                                        const Ice::Current&)
    {
        std::scoped_lock lock(mutex);
        if (layerBlackWhitelist.isExcluded(layer))
        {
            return;
        }
        robots.erase(std::make_pair(layer, name));
        removeAndCommit(layer, name);
    }
 
    void
    DebugDrawerToArViz::setPoseDebugLayerVisu(const std::string& name,
                                              const PoseBasePtr& globalPose,
                                              const Ice::Current& c)
    {
        setPoseVisu(DEBUG_LAYER_NAME, name, globalPose, c);
    }
 
    void
    DebugDrawerToArViz::setScaledPoseDebugLayerVisu(const std::string& name,
                                                    const PoseBasePtr& globalPose,
                                                    Ice::Float scale,
                                                    const Ice::Current& c)
    {
        setScaledPoseVisu(DEBUG_LAYER_NAME, name, globalPose, scale, c);
    }
 
    void
    DebugDrawerToArViz::setLineDebugLayerVisu(const std::string& name,
                                              const Vector3BasePtr& globalPosition1,
                                              const Vector3BasePtr& globalPosition2,
                                              Ice::Float lineWidth,
                                              const DrawColor& color,
                                              const Ice::Current& c)
    {
        setLineVisu(DEBUG_LAYER_NAME, name, globalPosition1, globalPosition2, lineWidth, color, c);
    }
 
    void
    DebugDrawerToArViz::setLineSetDebugLayerVisu(const std::string& name,
                                                 const DebugDrawerLineSet& lineSet,
                                                 const Ice::Current& c)
    {
        setLineSetVisu(DEBUG_LAYER_NAME, name, lineSet, c);
    }
 
    void
    DebugDrawerToArViz::setBoxDebugLayerVisu(const std::string& name,
                                             const PoseBasePtr& globalPose,
                                             const Vector3BasePtr& dimensions,
                                             const DrawColor& color,
                                             const Ice::Current& c)
    {
        setBoxVisu(DEBUG_LAYER_NAME, name, globalPose, dimensions, color, c);
    }
 
    void
    DebugDrawerToArViz::setTextDebugLayerVisu(const std::string& name,
                                              const std::string& text,
                                              const Vector3BasePtr& globalPosition,
                                              const DrawColor& color,
                                              Ice::Int size,
                                              const Ice::Current& c)
    {
        setTextVisu(DEBUG_LAYER_NAME, name, text, globalPosition, color, size, c);
    }
 
    void
    DebugDrawerToArViz::setSphereDebugLayerVisu(const std::string& name,
                                                const Vector3BasePtr& globalPosition,
                                                const DrawColor& color,
                                                Ice::Float radius,
                                                const Ice::Current& c)
    {
        setSphereVisu(DEBUG_LAYER_NAME, name, globalPosition, color, radius, c);
    }
 
    void
    DebugDrawerToArViz::setPointCloudDebugLayerVisu(const std::string& name,
                                                    const DebugDrawerPointCloud& pointCloud,
                                                    const Ice::Current& c)
    {
        setPointCloudVisu(DEBUG_LAYER_NAME, name, pointCloud, c);
    }
 
    void
    DebugDrawerToArViz::set24BitColoredPointCloudDebugLayerVisu(
        const std::string& name,
        const DebugDrawer24BitColoredPointCloud& pointCloud,
        const Ice::Current& c)
    {
        set24BitColoredPointCloudVisu(DEBUG_LAYER_NAME, name, pointCloud, c);
    }
 
    void
    DebugDrawerToArViz::setPolygonDebugLayerVisu(const std::string& name,
                                                 const PolygonPointList& polygonPoints,
                                                 const DrawColor& colorInner,
                                                 const DrawColor& colorBorder,
                                                 Ice::Float lineWidth,
                                                 const Ice::Current& c)
    {
        setPolygonVisu(
            DEBUG_LAYER_NAME, name, polygonPoints, colorInner, colorBorder, lineWidth, c);
    }
 
    void
    DebugDrawerToArViz::setTriMeshDebugLayerVisu(const std::string& name,
                                                 const DebugDrawerTriMesh& triMesh,
                                                 const Ice::Current& c)
    {
        setTriMeshVisu(DEBUG_LAYER_NAME, name, triMesh, c);
    }
 
    void
    DebugDrawerToArViz::setArrowDebugLayerVisu(const std::string& name,
                                               const Vector3BasePtr& position,
                                               const Vector3BasePtr& direction,
                                               const DrawColor& color,
                                               Ice::Float length,
                                               Ice::Float width,
                                               const Ice::Current& c)
    {
        setArrowVisu(DEBUG_LAYER_NAME, name, position, direction, color, length, width, c);
    }
 
    void
    DebugDrawerToArViz::setCylinderDebugLayerVisu(const std::string& name,
                                                  const Vector3BasePtr& globalPosition,
                                                  const Vector3BasePtr& direction,
                                                  Ice::Float length,
                                                  Ice::Float radius,
                                                  const DrawColor& color,
                                                  const Ice::Current& c)
    {
        setCylinderVisu(
            DEBUG_LAYER_NAME, name, globalPosition, direction, length, radius, color, c);
    }
 
    void
    DebugDrawerToArViz::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&)
    {
        (void)name, (void)globalPosition, (void)directionVec, (void)radius, (void)circleCompletion,
            (void)width, (void)color;
        LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
    }
 
    void
    DebugDrawerToArViz::removePoseVisu(const std::string& layer,
                                       const std::string& name,
                                       const Ice::Current&)
    {
        std::scoped_lock lock(mutex);
        if (layerBlackWhitelist.isIncluded(layer))
        {
            removeAndCommit(layer, name);
        }
    }
 
    void
    DebugDrawerToArViz::removeLineVisu(const std::string& layer,
                                       const std::string& name,
                                       const Ice::Current&)
    {
        std::scoped_lock lock(mutex);
        if (layerBlackWhitelist.isIncluded(layer))
        {
            removeAndCommit(layer, name);
        }
    }
 
    void
    DebugDrawerToArViz::removeLineSetVisu(const std::string& layer,
                                          const std::string& name,
                                          const Ice::Current&)
    {
        std::scoped_lock lock(mutex);
        if (layerBlackWhitelist.isIncluded(layer))
        {
            removeAndCommit(layer, name);
        }
    }
 
    void
    DebugDrawerToArViz::removeBoxVisu(const std::string& layer,
                                      const std::string& name,
                                      const Ice::Current&)
    {
        std::scoped_lock lock(mutex);
        if (layerBlackWhitelist.isIncluded(layer))
        {
            removeAndCommit(layer, name);
        }
    }
 
    void
    DebugDrawerToArViz::removeTextVisu(const std::string& layer,
                                       const std::string& name,
                                       const Ice::Current&)
    {
        std::scoped_lock lock(mutex);
        if (layerBlackWhitelist.isIncluded(layer))
        {
            removeAndCommit(layer, name);
        }
    }
 
    void
    DebugDrawerToArViz::removeSphereVisu(const std::string& layer,
                                         const std::string& name,
                                         const Ice::Current&)
    {
        std::scoped_lock lock(mutex);
        if (layerBlackWhitelist.isIncluded(layer))
        {
            removeAndCommit(layer, name);
        }
    }
 
    void
    DebugDrawerToArViz::removePointCloudVisu(const std::string& layer,
                                             const std::string& name,
                                             const Ice::Current&)
    {
        std::scoped_lock lock(mutex);
        if (layerBlackWhitelist.isIncluded(layer))
        {
            removeAndCommit(layer, name);
        }
    }
 
    void
    DebugDrawerToArViz::removeColoredPointCloudVisu(const std::string& layer,
                                                    const std::string& name,
                                                    const Ice::Current&)
    {
        std::scoped_lock lock(mutex);
        if (layerBlackWhitelist.isIncluded(layer))
        {
            removeAndCommit(layer, name);
        }
    }
 
    void
    DebugDrawerToArViz::remove24BitColoredPointCloudVisu(const std::string& layer,
                                                         const std::string& name,
                                                         const Ice::Current&)
    {
        std::scoped_lock lock(mutex);
        if (layerBlackWhitelist.isIncluded(layer))
        {
            removeAndCommit(layer, name);
        }
    }
 
    void
    DebugDrawerToArViz::removePolygonVisu(const std::string& layer,
                                          const std::string& name,
                                          const Ice::Current&)
    {
        std::scoped_lock lock(mutex);
        if (layerBlackWhitelist.isIncluded(layer))
        {
            removeAndCommit(layer, name);
        }
    }
 
    void
    DebugDrawerToArViz::removeTriMeshVisu(const std::string& layer,
                                          const std::string& name,
                                          const Ice::Current&)
    {
        std::scoped_lock lock(mutex);
        if (layerBlackWhitelist.isIncluded(layer))
        {
            removeAndCommit(layer, name);
        }
    }
 
    void
    DebugDrawerToArViz::removeArrowVisu(const std::string& layer,
                                        const std::string& name,
                                        const Ice::Current&)
    {
        std::scoped_lock lock(mutex);
        if (layerBlackWhitelist.isIncluded(layer))
        {
            removeAndCommit(layer, name);
        }
    }
 
    void
    DebugDrawerToArViz::removeCylinderVisu(const std::string& layer,
                                           const std::string& name,
                                           const Ice::Current&)
    {
        std::scoped_lock lock(mutex);
        if (layerBlackWhitelist.isIncluded(layer))
        {
            removeAndCommit(layer, name);
        }
    }
 
    void
    DebugDrawerToArViz::removeCircleVisu(const std::string& layer,
                                         const std::string& name,
                                         const Ice::Current&)
    {
        std::scoped_lock lock(mutex);
        if (layerBlackWhitelist.isIncluded(layer))
        {
            removeAndCommit(layer, name);
        }
    }
 
    void
    DebugDrawerToArViz::removePoseDebugLayerVisu(const std::string& name, const Ice::Current&)
    {
        removePointCloudVisu(DEBUG_LAYER_NAME, name);
    }
 
    void
    DebugDrawerToArViz::removeLineDebugLayerVisu(const std::string& name, const Ice::Current&)
    {
        removeLineVisu(DEBUG_LAYER_NAME, name);
    }
 
    void
    DebugDrawerToArViz::removeLineSetDebugLayerVisu(const std::string& name, const Ice::Current&)
    {
        removeLineSetVisu(DEBUG_LAYER_NAME, name);
    }
 
    void
    DebugDrawerToArViz::removeBoxDebugLayerVisu(const std::string& name, const Ice::Current&)
    {
        removeBoxVisu(DEBUG_LAYER_NAME, name);
    }
 
    void
    DebugDrawerToArViz::removeTextDebugLayerVisu(const std::string& name, const Ice::Current&)
    {
        removeTextVisu(DEBUG_LAYER_NAME, name);
    }
 
    void
    DebugDrawerToArViz::removeSphereDebugLayerVisu(const std::string& name, const Ice::Current&)
    {
        removeSphereVisu(DEBUG_LAYER_NAME, name);
    }
 
    void
    DebugDrawerToArViz::removePointCloudDebugLayerVisu(const std::string& name, const Ice::Current&)
    {
        removePointCloudVisu(DEBUG_LAYER_NAME, name);
    }
 
    void
    DebugDrawerToArViz::removeColoredPointCloudDebugLayerVisu(const std::string& name,
                                                              const Ice::Current&)
    {
        removeColoredPointCloudVisu(DEBUG_LAYER_NAME, name);
    }
 
    void
    DebugDrawerToArViz::remove24BitColoredPointCloudDebugLayerVisu(const std::string& name,
                                                                   const Ice::Current&)
    {
        remove24BitColoredPointCloudVisu(DEBUG_LAYER_NAME, name);
    }
 
    void
    DebugDrawerToArViz::removePolygonDebugLayerVisu(const std::string& name, const Ice::Current&)
    {
        removePolygonVisu(DEBUG_LAYER_NAME, name);
    }
 
    void
    DebugDrawerToArViz::removeTriMeshDebugLayerVisu(const std::string& name, const Ice::Current&)
    {
        removeTriMeshVisu(DEBUG_LAYER_NAME, name);
    }
 
    void
    DebugDrawerToArViz::removeArrowDebugLayerVisu(const std::string& name, const Ice::Current&)
    {
        removeArrowVisu(DEBUG_LAYER_NAME, name);
    }
 
    void
    DebugDrawerToArViz::removeCylinderDebugLayerVisu(const std::string& name, const Ice::Current&)
    {
        removeCylinderVisu(DEBUG_LAYER_NAME, name);
    }
 
    void
    DebugDrawerToArViz::removeCircleDebugLayerVisu(const std::string& name, const Ice::Current&)
    {
        removeCircleVisu(DEBUG_LAYER_NAME, name);
    }
 
    void
    DebugDrawerToArViz::clearAll(const Ice::Current&)
    {
        std::scoped_lock lock(mutex);
 
        std::vector<viz::Layer> commit;
        commit.reserve(layers.size());
        for (auto& [name, layer] : layers)
        {
            layer.clear();
            commit.push_back(layer);
        }
        arviz.commit(commit);
    }
 
    void
    DebugDrawerToArViz::clearLayer(const std::string& layerName, const Ice::Current&)
    {
        std::scoped_lock lock(mutex);
 
        viz::Layer layer = getLayer(layerName);
        layer.clear();
        arviz.commit({layer});
    }
 
    void
    DebugDrawerToArViz::clearDebugLayer(const Ice::Current&)
    {
        clearLayer(DEBUG_LAYER_NAME);
    }
 
    void
    DebugDrawerToArViz::enableLayerVisu(const std::string& layer, bool visible, const Ice::Current&)
    {
        (void)layer, (void)visible;
        LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
    }
 
    void
    DebugDrawerToArViz::enableDebugLayerVisu(bool visible, const Ice::Current&)
    {
        enableLayerVisu(DEBUG_LAYER_NAME, visible);
    }
 
    Ice::StringSeq
    DebugDrawerToArViz::layerNames(const Ice::Current&)
    {
        LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
        return {};
    }
 
    LayerInformationSequence
    DebugDrawerToArViz::layerInformation(const Ice::Current&)
    {
        LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
        return {};
    }
 
    bool
    DebugDrawerToArViz::hasLayer(const std::string&, const Ice::Current&)
    {
        LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
        return false;
    }
 
    void
    DebugDrawerToArViz::removeLayer(const std::string&, const Ice::Current&)
    {
        LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
    }
 
    void
    DebugDrawerToArViz::disableAllLayers(const Ice::Current&)
    {
        LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
    }
 
    void
    DebugDrawerToArViz::enableAllLayers(const Ice::Current&)
    {
        LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
    }
 
    void
    DebugDrawerToArViz::enableSelections(const std::string&, const Ice::Current&)
    {
        LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
    }
 
    void
    DebugDrawerToArViz::disableSelections(const std::string&, const Ice::Current&)
    {
        LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
    }
 
    void
    DebugDrawerToArViz::clearSelections(const std::string&, const Ice::Current&)
    {
        LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
    }
 
    void
    DebugDrawerToArViz::select(const std::string& layer,
                               const std::string& elementName,
                               const Ice::Current&)
    {
        (void)layer, (void)elementName;
        LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
    }
 
    void
    DebugDrawerToArViz::deselect(const std::string& layer,
                                 const std::string& elementName,
                                 const Ice::Current&)
    {
        (void)layer, (void)elementName;
        LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
    }
 
    DebugDrawerSelectionList
    DebugDrawerToArViz::getSelections(const Ice::Current&)
    {
        LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
        return {};
    }
 
    viz::Layer&
    DebugDrawerToArViz::getLayer(const std::string& layerName)
    {
        if (auto it = layers.find(layerName); it != layers.end())
        {
            return it->second;
        }
        else
        {
            return layers.emplace(layerName, arviz.layer(layerName)).first->second;
        }
    }
 
    viz::data::ElementSeq::iterator
    DebugDrawerToArViz::findLayerElement(viz::Layer& layer, const std::string& elementName)
    {
        return std::find_if(layer.data_.elements.begin(),
                            layer.data_.elements.end(),
                            [&elementName](const viz::data::ElementPtr& e)
                            { return e->id == elementName; });
    }
 
    void
    DebugDrawerToArViz::removeLayerElement(viz::Layer& layer, const std::string& name)
    {
        auto it = findLayerElement(layer, name);
        if (it != layer.data_.elements.end())
        {
            layer.data_.elements.erase(it);
        }
    }
 
    void
    DebugDrawerToArViz::removeAndCommit(const std::string& layerName, const std::string& name)
    {
        viz::Layer& layer = getLayer(layerName);
        removeLayerElement(layer, name);
        arviz.commit({layer});
    }
 
} // namespace armarx