DebugDrawerTopic.h

Go to the documentation of this file.

#pragma once
 
#include <chrono>
#include <functional>
#include <thread>
 
#include <Eigen/Geometry>
 
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
 
namespace Eigen
{
    using Matrix32f = Matrix<float, 3, 2>;
}
 
namespace VirtualRobot
{
    class TriMeshModel;
    class BoundingBox;
}
 
namespace armarx
{
    // forward declaration
    class ManagedIceObject;
 
 
    class DebugDrawerTopic
    {
    public:
 
 
        struct VisuID
        {
        public:
 
            VisuID();
 
            VisuID(const std::string& name);
            VisuID(const std::string& layer, const std::string& name);
 
            template <typename Source>
            VisuID(const Source& name) : VisuID(std::string(name))
            {}
 
 
            VisuID withName(const std::string& name) const;
 
            friend std::ostream& operator<<(std::ostream& os, const VisuID& rhs);
 
        public:
 
            std::string layer = "";  
            std::string name = "";   
        };
 
 
        struct Defaults
        {
            DrawColor colorText { 0, 0, 0, 1 };
 
            DrawColor colorArrow { 1, .5, 0, 1 };
            DrawColor colorBox { 1, 0, 0, 1 };
            DrawColor colorCylinder { 0, 1, 0, 1 };
            DrawColor colorLine { .5, 0, 0, 1 };
            DrawColor colorSphere { 0, 0, 1, 1 };
 
            DrawColor colorPolygonFace { 0, 1, 1, 1 };
            DrawColor colorPolygonEdge { .75, .75, .75, 1 };
 
            DrawColor colorFloor { .1f, .1f, .1f, 1 };
 
            DrawColor colorPointCloud { .5, .5, .5, 1. };
 
            // Default value of DebugDrawerColoredPointCloud etc.
            float pointCloudPointSize = 3.0f;
 
            float lineWidth = 2;
 
            DrawColor boxEdgesColor { 0, 1, 1, 1 };
            float boxEdgesWidth = 2;
        };
        static const Defaults DEFAULTS;
 
 
    public:
 
        // CONSTRUCTION & SETUP
 
        DebugDrawerTopic(const std::string& layer = DEFAULT_LAYER);
        DebugDrawerTopic(const DebugDrawerInterfacePrx& topic, const std::string& layer = DEFAULT_LAYER);
 
 
        void setTopic(const DebugDrawerInterfacePrx& topic);
        DebugDrawerInterfacePrx getTopic() const;
 
        void setEnabled(bool enabled);
        bool enabled() const;
 
        void offeringTopic(ManagedIceObject& component, const std::string& topicNameOverride = "") const;
        void getTopic(ManagedIceObject& component, const std::string& topicNameOverride = "");
 
        const std::string& getLayer() const;
        void setLayer(const std::string& layer);
 
        float getLengthScale() const;
        void setLengthScale(float scale);
        void setLengthScaleMetersToMillimeters();
        void setLengthScaleMillimetersToMeters();
 
        float getPoseScale() const;
        void setPoseScale(float scale);
        void setPoseScaleMeters();
        void setPoseScaleMillimeters();
 
 
 
        // SLEEP
 
        void shortSleep();
 
        template <typename DurationT>
        void sleepFor(const DurationT& duration);
 
        template <typename DurationT>
        void setShortSleepDuration(const DurationT& duration);
 
 
        // CLEAR
 
        void clearAll(bool sleep = false);
 
        void clearLayer(bool sleep = false);
 
        void clearLayer(const std::string& layer, bool sleep = false);
 
 
        // PRIMITIVES
 
        void drawText(const VisuID& id, const Eigen::Vector3f& position, const std::string& text,
                      int size = 10, const DrawColor color = DEFAULTS.colorText,
                      bool ignoreLengthScale = false);
 
 
        void drawBox(const VisuID& id, const Eigen::Vector3f& position, const Eigen::Quaternionf& orientation,
                     const Eigen::Vector3f& extents, const DrawColor& color = DEFAULTS.colorBox,
                     bool ignoreLengthScale = false);
        void drawBox(const VisuID& id, const Eigen::Matrix4f& pose, const Eigen::Vector3f& extents,
                     const DrawColor& color = DEFAULTS.colorBox,
                     bool ignoreLengthScale = false);
 
        void drawBox(const VisuID& id,
                     const VirtualRobot::BoundingBox& boundingBox,
                     const DrawColor& color = DEFAULTS.colorBox,
                     bool ignoreLengthScale = false);
 
        void drawBox(const VisuID& id,
                     const VirtualRobot::BoundingBox& boundingBox, const Eigen::Matrix4f& pose,
                     const DrawColor& color = DEFAULTS.colorBox,
                     bool ignoreLengthScale = false);
 
 
        void removeBox(const VisuID& id);
 
 
        void drawBoxEdges(const VisuID& id,
                          const Eigen::Vector3f& position, const Eigen::Quaternionf& orientation,
                          const Eigen::Vector3f& extents,
                          float width = DEFAULTS.boxEdgesWidth, const DrawColor& color = DEFAULTS.boxEdgesColor,
                          bool ignoreLengthScale = false);
 
        void drawBoxEdges(const VisuID& id,
                          const Eigen::Matrix4f& pose, const Eigen::Vector3f& extents,
                          float width = DEFAULTS.boxEdgesWidth, const DrawColor& color = DEFAULTS.boxEdgesColor,
                          bool ignoreLengthScale = false);
 
        void drawBoxEdges(const VisuID& id,
                          const VirtualRobot::BoundingBox& boundingBox,
                          float width = DEFAULTS.boxEdgesWidth, const DrawColor& color = DEFAULTS.boxEdgesColor,
                          bool ignoreLengthScale = false);
 
        void drawBoxEdges(const VisuID& id,
                          const Eigen::Matrix32f& aabb,
                          float width = DEFAULTS.boxEdgesWidth, const DrawColor& color = DEFAULTS.boxEdgesColor,
                          bool ignoreLengthScale = false);
 
        void drawBoxEdges(const VisuID& id,
                          const VirtualRobot::BoundingBox& boundingBox, const Eigen::Matrix4f& pose,
                          float width = DEFAULTS.boxEdgesWidth, const DrawColor& color = DEFAULTS.boxEdgesColor,
                          bool ignoreLengthScale = false);
 
        void drawBoxEdges(const VisuID& id,
                          const Eigen::Matrix32f& aabb, const Eigen::Matrix4f& pose,
                          float width = DEFAULTS.boxEdgesWidth, const DrawColor& color = DEFAULTS.boxEdgesColor,
                          bool ignoreLengthScale = false);
 
        void removeboxEdges(const VisuID& id);
 
 
        void drawCylinder(
            const VisuID& id,
            const Eigen::Vector3f& center, const Eigen::Vector3f& direction, float length, float radius,
            const DrawColor& color = DEFAULTS.colorCylinder,
            bool ignoreLengthScale = false);
 
        void drawCylinder(
            const VisuID& id,
            const Eigen::Vector3f& center, const Eigen::Quaternionf& orientation, float length, float radius,
            const DrawColor& color = DEFAULTS.colorCylinder,
            bool ignoreLengthScale = false);
 
        void drawCylinderFromTo(
            const VisuID& id,
            const Eigen::Vector3f& from, const Eigen::Vector3f& to, float radius,
            const DrawColor& color = DEFAULTS.colorCylinder,
            bool ignoreLengthScale = false);
 
        void removeCylinder(const VisuID& id);
 
 
        void drawSphere(
            const VisuID& id,
            const Eigen::Vector3f& center, float radius,
            const DrawColor& color = DEFAULTS.colorSphere,
            bool ignoreLengthScale = false);
 
        void removeSphere(const VisuID& id);
 
 
        void drawArrow(
            const VisuID& id,
            const Eigen::Vector3f& position, const Eigen::Vector3f& direction, float length,
            float width, const DrawColor& color = DEFAULTS.colorArrow,
            bool ignoreLengthScale = false);
 
        void drawArrowFromTo(
            const VisuID& id,
            const Eigen::Vector3f& from, const Eigen::Vector3f& to,
            float width, const DrawColor& color = DEFAULTS.colorArrow,
            bool ignoreLengthScale = false);
 
        void removeArrow(const VisuID& id);
 
 
        void drawPolygon(
            const VisuID& id,
            const std::vector<Eigen::Vector3f>& points,
            const DrawColor& colorFace,
            float lineWidth = 0, const DrawColor& colorEdge = DEFAULTS.colorPolygonEdge,
            bool ignoreLengthScale = false);
 
        void removePolygon(const VisuID& id);
 
 
        void drawLine(
            const VisuID& id,
            const Eigen::Vector3f& from, const Eigen::Vector3f& to,
            float width, const DrawColor& color = DEFAULTS.colorLine,
            bool ignoreLengthScale = false);
 
        void removeLine(const VisuID& id);
 
 
        void drawLineSet(
            const VisuID& id,
            const DebugDrawerLineSet& lineSet,
            bool ignoreLengthScale = false);
 
        void drawLineSet(
            const VisuID& id,
            const std::vector<Eigen::Vector3f>& points,
            float width, const DrawColor& color = DEFAULTS.colorLine,
            bool ignoreLengthScale = false);
 
        void drawLineSet(
            const VisuID& id,
            const std::vector<Eigen::Vector3f>& points,
            float width, const DrawColor& colorA, const DrawColor& colorB,
            const std::vector<float>& intensitiesB,
            bool ignoreLengthScale = false);
 
        void removeLineSet(const VisuID& id);
 
 
        // POSE
 
        void drawPose(const VisuID& id, const Eigen::Matrix4f& pose,
                      bool ignoreLengthScale = false);
        void drawPose(const VisuID& id, const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori,
                      bool ignoreLengthScale = false);
 
        void drawPose(const VisuID& id, const Eigen::Matrix4f& pose, float scale,
                      bool ignoreLengthScale = false);
        void drawPose(const VisuID& id, const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori,
                      float scale,
                      bool ignoreLengthScale = false);
 
        void removePose(const VisuID& id);
 
 
        // ROBOT
 
        void drawRobot(
            const VisuID& id,
            const std::string& robotFile, const std::string& armarxProject,
            armarx::DrawStyle drawStyle = armarx::DrawStyle::FullModel);
 
        void updateRobotPose(
            const VisuID& id,
            const Eigen::Matrix4f& pose,
            bool ignoreScale = false);
 
        void updateRobotPose(
            const VisuID& id,
            const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori,
            bool ignoreScale = false);
 
        void updateRobotConfig(
            const VisuID& id,
            const std::map<std::string, float>& config);
 
        void updateRobotColor(const VisuID& id, const DrawColor& color);
        void updateRobotNodeColor(
            const VisuID& id, const std::string& nodeName, const DrawColor& color);
 
        void removeRobot(const VisuID& id);
 
 
        // TRI MESH
 
        void drawTriMesh(
            const VisuID& id,
            const VirtualRobot::TriMeshModel& triMesh,
            const DrawColor& color = {.5, .5, .5, 1},
            bool ignoreLengthScale = false);
 
        void drawTriMeshAsPolygons(
            const VisuID& id,
            const VirtualRobot::TriMeshModel& trimesh,
            const DrawColor& colorFace = DEFAULTS.colorPolygonFace,
            float lineWidth = 0, const DrawColor& colorEdge = DEFAULTS.colorPolygonEdge,
            bool ignoreLengthScale = false);
 
        void drawTriMeshAsPolygons(
            const VisuID& id,
            const VirtualRobot::TriMeshModel& trimesh, const Eigen::Matrix4f& pose,
            const DrawColor& colorFace = DEFAULTS.colorPolygonFace,
            float lineWidth = 0, const DrawColor& colorEdge = DEFAULTS.colorPolygonEdge,
            bool ignoreLengthScale = false);
 
        void drawTriMeshAsPolygons(
            const VisuID& id,
            const VirtualRobot::TriMeshModel& trimesh,
            const std::vector<DrawColor>& faceColorsInner,
            float lineWidth = 0, const DrawColor& colorEdge = DEFAULTS.colorPolygonEdge,
            bool ignoreLengthScale = false);
 
 
        // POINT CLOUD
        /* (By templating these functions, we can make them usable for PCL
         * point clouds without a dependency on PCL.)
         */
 
        template <class PointCloudT>
        void drawPointCloud(
            const VisuID& id,
            const PointCloudT& pointCloud,
            const DrawColor& color = DEFAULTS.colorPointCloud,
            float pointSize = DEFAULTS.pointCloudPointSize,
            bool ignoreLengthScale = false);
 
        template <class PointCloudT>
        void drawPointCloudRGBA(
            const VisuID& id,
            const PointCloudT& pointCloud,
            float pointSize = DEFAULTS.pointCloudPointSize,
            bool ignoreLengthScale = false);
 
        template <class PointCloudT>
        void drawPointCloudLabeled(
            const VisuID& id,
            const PointCloudT& pointCloud,
            float pointSize = DEFAULTS.pointCloudPointSize,
            bool ignoreLengthScale = false);
 
        template <class PointCloudT, class ColorFuncT>
        void drawPointCloud(
            const VisuID& id,
            const PointCloudT& pointCloud,
            const ColorFuncT& colorFunc,
            float pointSize = DEFAULTS.pointCloudPointSize,
            bool ignoreLengthScale = false);
 
 
        // Debug Drawer Point Cloud Types
 
        void drawPointCloud(const VisuID& id, const DebugDrawerPointCloud& pointCloud);
 
        void drawPointCloud(const VisuID& id, const DebugDrawerColoredPointCloud& pointCloud);
 
        void drawPointCloud(const VisuID& id, const DebugDrawer24BitColoredPointCloud& pointCloud);
 
 
        void clearColoredPointCloud(const VisuID& id);
 
 
        // CUSTOM
 
        void drawFloor(
            const VisuID& id = { "floor" },
            const Eigen::Vector3f& at = Eigen::Vector3f::Zero(),
            const Eigen::Vector3f& up = Eigen::Vector3f::UnitZ(),
            float size = 5, const DrawColor& color = DEFAULTS.colorFloor,
            bool ignoreLengthScale = false);
 
 
        // OPERATORS
 
        operator bool() const;
 
        operator DebugDrawerInterfacePrx& ();
        operator const DebugDrawerInterfacePrx& () const;
 
        DebugDrawerInterfacePrx& operator->();
        const DebugDrawerInterfacePrx& operator->() const;
 
 
    public:  // STATIC
 
        static Eigen::Vector3f rgb2hsv(const Eigen::Vector3f& rgb);
 
        static Eigen::Vector3f hsv2rgb(const Eigen::Vector3f& hsv);
 
 
        template <class ColorT>
        static DrawColor toDrawColor(const ColorT& color, float alpha = 1, bool byteToFloat = false);
 
 
        static DrawColor getGlasbeyLUTColor(int id, float alpha = 1.f);
        static DrawColor getGlasbeyLUTColor(uint32_t id, float alpha = 1.f);
        static DrawColor getGlasbeyLUTColor(std::size_t id, float alpha = 1.f);
 
 
    private:
 
        const std::string& layer(const std::string& passedLayer) const;
 
        const std::string& layer(const VisuID& id) const;
 
 
        float lengthScale(bool ignore = false) const;
 
        static float scaled(float scale, float value);
        static Vector3BasePtr scaled(float scale, const Eigen::Vector3f& vector);
        static PoseBasePtr scaled(float scale, const Eigen::Matrix4f& pose);
 
        template <class ScaledT>
        static ScaledT scaledT(float scale, const Eigen::Vector3f& vector);
 
        template <class XYZT>
        static void scaleXYZ(float scale, XYZT& xyz);
 
 
    private:
 
        static const std::string TOPIC_NAME;
        static const std::string DEFAULT_LAYER;
 
 
        DebugDrawerInterfacePrx topic = nullptr;
 
        bool _enabled = true;
 
        std::string _layer = DEFAULT_LAYER;
 
        float _lengthScale = 1;
 
        float _poseScale = 1;
 
        std::chrono::milliseconds _shortSleepDuration { 100 };
 
    };
 
 
    template <typename DurationT>
    void DebugDrawerTopic::sleepFor(const DurationT& duration)
    {
        if (enabled())
        {
            std::this_thread::sleep_for(duration);
        }
    }
 
    template <typename DurationT>
    void DebugDrawerTopic::setShortSleepDuration(const DurationT& duration)
    {
        this->_shortSleepDuration = std::chrono::duration_cast<std::chrono::milliseconds>(duration);
    }
 
    template <class ColorT>
    DrawColor DebugDrawerTopic::toDrawColor(const ColorT& color, float alpha, bool byteToFloat)
    {
        const float scale = byteToFloat ? (1 / 255.f) : 1;
        return { color.r * scale, color.g * scale, color.b * scale, alpha };
    }
 
 
    template <class PointCloudT>
    void DebugDrawerTopic::drawPointCloud(
        const VisuID& id,
        const PointCloudT& pointCloud,
        const DrawColor& color,
        float pointSize,
        bool ignoreLengthScale)
    {
        drawPointCloud(id, pointCloud, [&color](const auto&)
        {
            return color;
        },
        pointSize, ignoreLengthScale);
    }
 
    template<class PointCloudT>
    void DebugDrawerTopic::drawPointCloudRGBA(
        const VisuID& id,
        const PointCloudT& pointCloud,
        float pointSize,
        bool ignoreLengthScale)
    {
        drawPointCloud(id, pointCloud, [](const auto & p)
        {
            return toDrawColor(p, p.a);
        },
        pointSize, ignoreLengthScale);
    }
 
    template <class PointCloudT>
    void DebugDrawerTopic::drawPointCloudLabeled(
        const VisuID& id,
        const PointCloudT& pointCloud,
        float pointSize,
        bool ignoreLengthScale)
    {
        drawPointCloud(id, pointCloud, [](const auto & p)
        {
            return getGlasbeyLUTColor(p.label);
        },
        pointSize, ignoreLengthScale);
    }
 
    template <class PointCloudT, class ColorFuncT>
    void DebugDrawerTopic::drawPointCloud(
        const VisuID& id,
        const PointCloudT& pointCloud,
        const ColorFuncT& colorFn,
        float pointSize,
        bool ignoreLengthScale)
    {
        if (!enabled())
        {
            return;
        }
 
        const float lf = ignoreLengthScale ? 1.0 : _lengthScale;
 
        DebugDrawerColoredPointCloud dd;
        dd.points.reserve(pointCloud.size());
 
        dd.pointSize = pointSize;
 
        for (const auto& p : pointCloud)
        {
            dd.points.push_back(DebugDrawerColoredPointCloudElement
            {
                lf * p.x, lf * p.y, lf * p.z, colorFn(p)
            });
        }
 
        drawPointCloud(id, dd);
    }
 
 
    template <class ScaledT>
    ScaledT DebugDrawerTopic::scaledT(float scale, const Eigen::Vector3f& vector)
    {
        auto scaled = vector * scale;
        return { scaled.x(), scaled.y(), scaled.z() };
    }
 
    template <class XYZT>
    void DebugDrawerTopic::scaleXYZ(float scale, XYZT& xyz)
    {
        xyz.x *= scale;
        xyz.y *= scale;
        xyz.z *= scale;
    }
 
}