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));
    }
 
}