ArmarXVisualizer.cpp

Go to the documentation of this file.

#include "ArmarXVisualizer.h"
 
#include <RobotAPI/libraries/core/Pose.h>
 
namespace armarx::semantic
{
 
    static armarx::Vector3Ptr
    toArmarx(const Eigen::Vector3f& v)
    {
        return armarx::Vector3Ptr(new armarx::Vector3(v));
    }
 
    static armarx::QuaternionPtr
    toArmarx(const Eigen::Quaternionf& q)
    {
        return armarx::QuaternionPtr(new armarx::Quaternion(q));
    }
 
    static armarx::PosePtr
    toArmarx(const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori)
    {
        return armarx::PosePtr(new armarx::Pose(toArmarx(pos), toArmarx(ori)));
    }
 
    static armarx::DrawColor
    toArmarx(const semrel::DrawColor& color)
    {
        return armarx::DrawColor{color.r, color.g, color.b, color.a};
    }
 
    void
    ArmarXVisualizer::setAsImplementation(const DebugDrawerInterfacePrx& debugDrawer)
    {
        semrel::VisualizerInterface::setImplementation(
            std::make_shared<ArmarXVisualizer>(debugDrawer));
    }
 
    ArmarXVisualizer::ArmarXVisualizer(armarx::DebugDrawerInterfacePrx const& debugDrawer) :
        drawer(debugDrawer)
    {
    }
 
    void
    ArmarXVisualizer::clearAll()
    {
        drawer->clearAll();
    }
 
    void
    ArmarXVisualizer::clearLayer(const std::string& layer)
    {
        drawer->clearLayer(layer);
    }
 
    void
    ArmarXVisualizer::drawLine(semrel::VisuMetaInfo id,
                               const Eigen::Vector3f& start,
                               const Eigen::Vector3f& end,
                               float lineWidth,
                               semrel::DrawColor color)
    {
        drawer->setLineVisu(
            id.layer, id.name, toArmarx(start), toArmarx(end), lineWidth, toArmarx(color));
    }
 
    void
    ArmarXVisualizer::drawArrow(semrel::VisuMetaInfo id,
                                const Eigen::Vector3f& origin,
                                const Eigen::Vector3f& direction,
                                float length,
                                float width,
                                semrel::DrawColor color)
    {
        drawer->setArrowVisu(id.layer,
                             id.name,
                             toArmarx(origin),
                             toArmarx(direction),
                             toArmarx(color),
                             length,
                             width);
    }
 
    void
    ArmarXVisualizer::drawBox(semrel::VisuMetaInfo id,
                              const semrel::Box& box,
                              semrel::DrawColor color)
    {
        drawer->setBoxVisu(id.layer,
                           id.name,
                           toArmarx(box.getPosition(), box.getOrientation()),
                           toArmarx(box.getExtents()),
                           toArmarx(color));
    }
 
    void
    ArmarXVisualizer::drawCylinder(semrel::VisuMetaInfo id,
                                   const semrel::Cylinder& cylinder,
                                   semrel::DrawColor color)
    {
        drawer->setCylinderVisu(id.layer,
                                id.name,
                                toArmarx(cylinder.getPosition()),
                                toArmarx(cylinder.getAxisDirection()),
                                cylinder.getHeight(),
                                cylinder.getRadius(),
                                toArmarx(color));
    }
 
    void
    ArmarXVisualizer::drawSphere(semrel::VisuMetaInfo id,
                                 const semrel::Sphere& sphere,
                                 semrel::DrawColor color)
    {
        drawer->setSphereVisu(
            id.layer, id.name, toArmarx(sphere.getPosition()), toArmarx(color), sphere.getRadius());
    }
 
    void
    ArmarXVisualizer::drawPolygon(semrel::VisuMetaInfo id,
                                  const std::vector<Eigen::Vector3f>& polygonPoints,
                                  float lineWidth,
                                  semrel::DrawColor colorInner,
                                  semrel::DrawColor colorBorder)
    {
        armarx::PolygonPointList armarxPoints;
        for (const Eigen::Vector3f& v : polygonPoints)
        {
            armarxPoints.push_back(toArmarx(v));
        }
 
        drawer->setPolygonVisu(id.layer,
                               id.name,
                               armarxPoints,
                               toArmarx(colorInner),
                               toArmarx(colorBorder),
                               lineWidth);
    }
 
    void
    ArmarXVisualizer::drawTriMesh(semrel::VisuMetaInfo id,
                                  const semrel::TriMesh& mesh,
                                  semrel::DrawColor color)
    {
        /*
        // use variant to create armarx tri mesh (DebugDrawerTriMesh)
        MeshShapeVariant variant(semrel::MeshShape(Eigen::Vector3f::Zero(), Eigen::Quaternionf::Identity(), mesh));
        DebugDrawerTriMesh ddMesh = variant.getMesh();
 
        // replace default color
        ddMesh.colors.front() = toArmarx(color);
 
        drawer->setTriMeshVisu(id.layer, id.name, ddMesh);
        */
 
        int i = 0;
        for (semrel::TriMesh::Triangle triangle : mesh.getTriangles())
        {
            semrel::VisuMetaInfo triID = id;
            std::stringstream ss;
            ss << triID.name << "_" << i;
            triID.name = ss.str();
 
            Eigen::Vector3f v0 = mesh.getVertex(triangle.v0);
            Eigen::Vector3f v1 = mesh.getVertex(triangle.v1);
            Eigen::Vector3f v2 = mesh.getVertex(triangle.v2);
            std::vector<Eigen::Vector3f> points;
            points.push_back(v0);
            points.push_back(v1);
            points.push_back(v2);
 
            semrel::DrawColor inner = color;
            color.a = 0.1f;
 
            drawPolygon(triID, points, 1, inner, color);
            i++;
        }
    }
 
    void
    ArmarXVisualizer::drawText(semrel::VisuMetaInfo id,
                               const std::string& text,
                               const Eigen::Vector3f& position,
                               float size,
                               semrel::DrawColor color)
    {
        drawer->setTextVisu(
            id.layer, id.name, text, toArmarx(position), toArmarx(color), int(roundf(size)));
    }
 
    void
    ArmarXVisualizer::drawPointCloud(semrel::VisuMetaInfo id,
                                     const std::vector<Eigen::Vector3f>& cloud,
                                     float pointSize,
                                     semrel::DrawColor color)
    {
        armarx::DrawColor armarxColor = toArmarx(color);
 
        armarx::DebugDrawerColoredPointCloud ddPointcloud;
        ddPointcloud.pointSize = pointSize;
 
        for (const Eigen::Vector3f& v : cloud)
        {
            armarx::DebugDrawerColoredPointCloudElement el{v(0), v(1), v(2), armarxColor};
            ddPointcloud.points.push_back(el);
        }
 
        drawer->setColoredPointCloudVisu(id.layer, id.name, ddPointcloud);
    }
 
    const std::string semantic::properties::defaults::visualizationLevelName = "visu.level";
    const std::string semantic::properties::defaults::visualizationLevelDescription =
        "Visualization level: Determines what is shown in the debug drawer.\n"
        "Possible values: live, verbose, result, user, disabled";
 
    void
    semantic::properties::defineVisualizationLevel(ComponentPropertyDefinitions& defs,
                                                   const std::string& propertyName,
                                                   semrel::VisuLevel defaultLevel,
                                                   const std::string& description)
    {
        defs.defineOptionalProperty<semrel::VisuLevel>(propertyName, defaultLevel, description)
            .map("live", semrel::VisuLevel::LIVE_VISU)
            .map("verbose", semrel::VisuLevel::VERBOSE)
            .map("result", semrel::VisuLevel::RESULT)
            .map("user", semrel::VisuLevel::USER)
            .map("disabled", semrel::VisuLevel::DISABLED);
    }
 
    semrel::VisuLevel
    semantic::properties::getVisualizationLevel(PropertyUser& defs, const std::string& propertyName)
    {
        return defs.getProperty<semrel::VisuLevel>(propertyName);
    }
 
    void
    semantic::properties::setMinimumVisuLevel(PropertyUser& defs, const std::string& propertyName)
    {
        semrel::VisualizerInterface::setMinimumVisuLevel(getVisualizationLevel(defs, propertyName));
    }
 
} // namespace armarx::semantic