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
{
    /**
     * @brief A 3x2 matrix.
     *
     * Useful to represent axis-aligned bounding boxes (AABBs). When used as a
     * AABB, column 0 contains the minimal x, y, z values and column 1 the
     * maximal x, y, z values.
     * Accordingly, the rows each contain the limits in x, y, z direction.
     */
    using Matrix32f = Matrix<float, 3, 2>;
} // namespace Eigen
 
namespace VirtualRobot
{
    class TriMeshModel;
    class BoundingBox;
} // namespace VirtualRobot
 
namespace armarx
{
    // forward declaration
    class ManagedIceObject;
 
    /**
     * @brief The `DebugDrawerTopic` wraps a `DebugDrawerInterfacePrx` and
     * provides a more useful interface than the Ice interface.
     *
     * The methods by `DebugDrawerTopic` take "raw" types, such as Eigen or PCL
     * types, and take care of conversion to Ice variants or data structures.
     * In addition, this class provides useful overloads for different use cases.
     *
     * Drawing is enabled if the internal topic proxy is set and an optional
     * enabled flag is set (true by default). All methods check whether drawing
     * is enabled and do nothing if drawing is disabled. To disable
     * visualization by this class completely, use `setEnabled(true/false)` or
     * just do not set the topic. To check whether visualization is enabled,
     * use `enabled()` or just convert `*this` to bool:
     * @code
     * DebugDrawerTopic debugDrawer;
     * if (debugDrawer)  // Equivalent: if (debugDrawer.enabled())
     * {
     *     // Do stuff if visualization is enabled.
     * }
     * @endcode
     *
     * The `DebugDrawerTopic` allows to set a layer on constructor or via
     * `setLayer()`. This layer will be used if none is passed to a drawing
     * method. If no layer is passed or set, `DebugDrawerTopic::DEFAULT_LAYER`
     * is used.
     *
     *
     * @par Initialisation by Offering and Getting Topic
     *
     * A `DebugDrawerTopic` needs an underlying `DebugDrawerInterfacePrx` topic proxy.
     * This proxy can be passed on construction or set via `setTopic()`.
     * In a component (or any other `ManagedIceObject`), `DebugDrawerTopic`
     * provides convenience functions to register and fetch the topics.
     *
     * In `onInitComponent()` (or equivalent method), call:
     * @code
     * debugDrawer.offeringTopic(*this);
     * @endcode
     * In `onConnectComponent()` (or equivalent), call:
     * @code
     * debugDrawer.getTopic(*this);
     * @endcode
     * where `*this` is a `ManagedIceObject`.
     *
     * This will call `this->offeringTopic("...")` and `this->getTopic("...")`
     * with the correct topic name (`DebugDrawerTopic::TOPIC_NAME`) and
     * enable the `DebugDrawerTopic`.
     *
     *
     * @par Scaling
     *
     * `DebugDrawerTopic` supports length scaling and pose scaling.
     *
     * If a length scale is set, all visualizations will be scaled up or down
     * by this value. This scaling affects positions, sizes / extents, and
     * distances / lengths. This is useful when drawing quantities of
     * different sources using different scalings (such as meters vs
     * millimeters).
     * Length scale can be set via `setPoseScale()` or the short hands
     * `setPoseScaleMetersToMillimeters()` and `setPoseScaleMillimetersToMeters()`.
     *
     * All applicable methods offer a final argument called
     * `ignoreLengthScaling` (false by default, expect for robots, see below),
     * which can be set to true to ignore the set length scale for this
     * method call.
     *
     * @note Robots are always drawn in their native size (the
     *  `DebugDrawerInterface` offers no size scaling for robots).
     * (That is, robots are drawn in millimeters most of the time.)
     *
     *
     * In addition, this class allows to set a pose scale, which will be used
     * for all drawn poses (if no other value is passed to drawPose()).
     * This is useful when working with geometries scaled in meters, in which
     * case a pose scale of 0.001 can be used.
     *
     *
     * @par Argument Pattern
     *
     * All drawing methods take a `VisuID` as first argument, which specifies
     * the name of the visualization and the layer to draw on.
     * There are different ways to specify the first argument.
     * For example, to specify a only the name for a pose, pass just the name:
     *
     * @code
     * Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
     * std::string name = "pose";
     * debugDrawer.drawPose(name, pose);
     * debugDrawer.drawPose({name}, pose);  // equivalent to the line above
     * @endcode
     *
     * This will draw a pose on the preset layer (i.e. the layer passed to the
     * constructor or set via `setLayer()`, or "debug" by default).
     * To specify both name and layer of a single visualization, pass both in
     * an initializer list:
     *
     * @code
     * Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
     * std::string layer = "layer";
     * std::string name = "pose";
     * debugDrawer.drawPose({layer, name}, pose);
     * @endcode
     *
     *
     * After the VisuID, usually the essential geometric parameters follow,
     * (e.g. position, size, length, point list, ...), depending on the type
     * of visualization.
     * Finally, decorative parameters like colors and width can be passed.
     * Most of the time, they have sensible default values and can be omitted
     * for quick-and-dirty drawing.
     *
     * (Added methods should adhere to this pattern.)
     *
     * @see `DebugDrawerTopic::VisuID`
     */
    class DebugDrawerTopic
    {
    public:
        /**
         * @brief A visualisation ID.
         *
         * This constructor can be called in the following ways
         * (with `draw(const VisuID& id, ...)` being any drawing method):
         *
         * @code
         * std::string name = "pose";
         * std::string layer = "layer";
         * draw(name, ...);           // just the name, implicit call
         * draw({name}, ...);         // just the name, call with initializer list
         * draw({layer, name}, ...);  // layer and name, with initializer list
         * @endcode
         *
         * (And of course by an explicit call if you want to be really verbose.)
         * Not passing a layer will cause DebugDrawerTopic to use the
         * preset layer.
         */
        struct VisuID
        {
        public:
            /// Empty constructor.
            VisuID();
 
            /// Construct a VisuID with given name (for drawing to the preset layer).
            VisuID(const std::string& name);
            /// Construct a VisuID with given name and layer.
            VisuID(const std::string& layer, const std::string& name);
 
            /// Construct a VisuID from a non-std::string source (e.g. char[]).
            template <typename Source>
            VisuID(const Source& name) : VisuID(std::string(name))
            {
            }
 
            /// Get a `VisuID` with the given name and same layer as `*this.
            VisuID withName(const std::string& name) const;
 
            /// Streams a short human-readable description of `rhs` to `os`.
            friend std::ostream& operator<<(std::ostream& os, const VisuID& rhs);
 
        public:
            std::string layer = ""; ///< The layer name (empty by default).
            std::string name = ""; ///< The visu name (empty by default).
        };
 
        /// Default values for drawing functions.
        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
 
        /// Construct without topic, and optional layer.
        DebugDrawerTopic(const std::string& layer = DEFAULT_LAYER);
        /// Construct with given topic and optional layer.
        DebugDrawerTopic(const DebugDrawerInterfacePrx& topic,
                         const std::string& layer = DEFAULT_LAYER);
 
 
        /// Set the topic.
        void setTopic(const DebugDrawerInterfacePrx& topic);
        /// Get the topic.
        DebugDrawerInterfacePrx getTopic() const;
 
        /**
         * @brief Set whether drawing is enabled.
         * Visualization is only truly enabled if the topic is set.
         */
        void setEnabled(bool enabled);
        /// Indicate whether visualization is enabled, i.e. a topic is set and enabled flag is set.
        bool enabled() const;
 
        /**
         * @brief Call offeringTopic([topicName]) on the given component.
         * @param component The component (`*this` when called from a component).
         * @param topicNameOverride Optional override for the topic name. If left empty (default),
         *      uses the standard topic name (see `TOPIC_NAME`).
         */
        void offeringTopic(ManagedIceObject& component,
                           const std::string& topicNameOverride = "") const;
        /**
         * @brief Get the topic by calling getTopic([topicName]) on the given component.
         * @param component The component (`*this` when called from a component).
         * @param topicNameOverride Optional override for the topic name. If left empty (default),
         *      uses the standard topic name (see `TOPIC_NAME`).
         */
        void getTopic(ManagedIceObject& component, const std::string& topicNameOverride = "");
 
        /// Get the default layer (used if no layer is passed to a method).
        const std::string& getLayer() const;
        /// Set the default layer (used if no layer is passed to a method).
        void setLayer(const std::string& layer);
 
        /// Get the scaling for positions, lengths and distances.
        float getLengthScale() const;
        /// Set the scale for positions, lengths and distances.
        void setLengthScale(float scale);
        /// Set the scale for positions, lengths and distances to 1000.
        void setLengthScaleMetersToMillimeters();
        /// Set the scale for positions, lengths and distances to 0.001.
        void setLengthScaleMillimetersToMeters();
 
        /// Get the scale for pose visualization.
        float getPoseScale() const;
        /// Set the scale for pose visualization.
        /// This value will be used for all successive calls to drawPose().
        void setPoseScale(float scale);
        /// Set the pose scale to 0.001 (good when drawing in meters).
        void setPoseScaleMeters();
        /// Set the pose scale to 1 (good when drawing in millimeters).
        void setPoseScaleMillimeters();
 
 
        // SLEEP
 
        /// Sleep for the `shortSleepDuration`. Useful after clearing.
        void shortSleep();
 
        /// If enabled, sleep for the given duration (e.g. a chrono duration).
        template <typename DurationT>
        void sleepFor(const DurationT& duration);
 
        /// Set the duration for "short sleeps".
        template <typename DurationT>
        void setShortSleepDuration(const DurationT& duration);
 
 
        // CLEAR
 
        /// Clear all layers.
        /// @param sleep If true, do a short sleep clearing.
        void clearAll(bool sleep = false);
 
        /// Clear the (set default) layer.
        /// @param sleep If true, do a short sleep clearing.
        void clearLayer(bool sleep = false);
 
        /// Clear the given layer.
        /// @param sleep If true, do a short sleep clearing.
        void clearLayer(const std::string& layer, bool sleep = false);
 
 
        // PRIMITIVES
 
        /**
         * @brief Draw text at the specified position.
         * @param size the text size (in pixels, not affected by length scaling)
         */
        void drawText(const VisuID& id,
                      const Eigen::Vector3f& position,
                      const std::string& text,
                      int size = 10,
                      const DrawColor color = DEFAULTS.colorText,
                      bool ignoreLengthScale = false);
 
 
        /// Draw a box.
        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);
        /// Draw a box.
        void drawBox(const VisuID& id,
                     const Eigen::Matrix4f& pose,
                     const Eigen::Vector3f& extents,
                     const DrawColor& color = DEFAULTS.colorBox,
                     bool ignoreLengthScale = false);
 
        /// Draw an axis aligned bounding box.
        void drawBox(const VisuID& id,
                     const VirtualRobot::BoundingBox& boundingBox,
                     const DrawColor& color = DEFAULTS.colorBox,
                     bool ignoreLengthScale = false);
 
        /// Draw a locally axis aligned bounding box, transformed by the given pose.
        void drawBox(const VisuID& id,
                     const VirtualRobot::BoundingBox& boundingBox,
                     const Eigen::Matrix4f& pose,
                     const DrawColor& color = DEFAULTS.colorBox,
                     bool ignoreLengthScale = false);
 
 
        /// Remove a box.
        void removeBox(const VisuID& id);
 
 
        /// Draw box edges (as a line set).
        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);
 
        /// Draw box edges (as a line set).
        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);
 
        /// Draw edges of an axis aligned bounding box (as a line set).
        void drawBoxEdges(const VisuID& id,
                          const VirtualRobot::BoundingBox& boundingBox,
                          float width = DEFAULTS.boxEdgesWidth,
                          const DrawColor& color = DEFAULTS.boxEdgesColor,
                          bool ignoreLengthScale = false);
 
        /// Draw edges of an axis aligned bounding box (as a line set).
        void drawBoxEdges(const VisuID& id,
                          const Eigen::Matrix32f& aabb,
                          float width = DEFAULTS.boxEdgesWidth,
                          const DrawColor& color = DEFAULTS.boxEdgesColor,
                          bool ignoreLengthScale = false);
 
        /// Draw edges of a locally axis-aligned bounding box, transformed by the given pose (as a line set).
        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);
 
        /// Draw edges of a locally axis-aligned bounding box, transformed by the given pose (as a line set).
        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);
 
        /// Remove box edges (as a line set).
        void removeboxEdges(const VisuID& id);
 
 
        /**
         * @brief Draw a cylinder with center and direction.
         * @param length The full length (not half-length).
         */
        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);
 
        /**
         * @brief Draw a cylinder with center and orientation.
         * An identity orientation represents a cylinder with an axis aligned to the Y-axis.
         * @param length The full length (not half-length).
         */
        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);
 
        /// Draw a cylinder from start to end.
        void drawCylinderFromTo(const VisuID& id,
                                const Eigen::Vector3f& from,
                                const Eigen::Vector3f& to,
                                float radius,
                                const DrawColor& color = DEFAULTS.colorCylinder,
                                bool ignoreLengthScale = false);
 
        /// Remove a cylinder.
        void removeCylinder(const VisuID& id);
 
 
        /// Draw a sphere.
        void drawSphere(const VisuID& id,
                        const Eigen::Vector3f& center,
                        float radius,
                        const DrawColor& color = DEFAULTS.colorSphere,
                        bool ignoreLengthScale = false);
 
        /// Remove a sphere.
        void removeSphere(const VisuID& id);
 
 
        /// Draw an arrow with position (start) and direction.
        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);
 
        /// Draw an arrow with start and end.
        void drawArrowFromTo(const VisuID& id,
                             const Eigen::Vector3f& from,
                             const Eigen::Vector3f& to,
                             float width,
                             const DrawColor& color = DEFAULTS.colorArrow,
                             bool ignoreLengthScale = false);
 
        /// Remove an arrow.
        void removeArrow(const VisuID& id);
 
 
        /// Draw a polygon.
        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);
 
        /// Remove a polygon.
        void removePolygon(const VisuID& id);
 
 
        /// Draw a line from start to end.
        void drawLine(const VisuID& id,
                      const Eigen::Vector3f& from,
                      const Eigen::Vector3f& to,
                      float width,
                      const DrawColor& color = DEFAULTS.colorLine,
                      bool ignoreLengthScale = false);
 
        /// Remove a line.
        void removeLine(const VisuID& id);
 
 
        /// Draw a line set.
        void drawLineSet(const VisuID& id,
                         const DebugDrawerLineSet& lineSet,
                         bool ignoreLengthScale = false);
 
        /**
         * @brief Draw a set of lines with constant color.
         * @param points List of start and end points [ s1, e1, s2, e2, ..., sn, en ].
         */
        void drawLineSet(const VisuID& id,
                         const std::vector<Eigen::Vector3f>& points,
                         float width,
                         const DrawColor& color = DEFAULTS.colorLine,
                         bool ignoreLengthScale = false);
 
        /**
         * @brief Draw a set of lines with constant color.
         * @param points List of start and end points [ s1, e1, s2, e2, ..., sn, en ].
         * @param colors List of line colors [ c1, c2, ..., cn ].
         */
        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);
 
        /// Remove a line set.
        void removeLineSet(const VisuID& id);
 
 
        // POSE
 
        /// Draw a pose (with the preset scale).
        void
        drawPose(const VisuID& id, const Eigen::Matrix4f& pose, bool ignoreLengthScale = false);
        /// Draw a pose (with the preset scale).
        void drawPose(const VisuID& id,
                      const Eigen::Vector3f& pos,
                      const Eigen::Quaternionf& ori,
                      bool ignoreLengthScale = false);
 
        /// Draw a pose with the given scale.
        void drawPose(const VisuID& id,
                      const Eigen::Matrix4f& pose,
                      float scale,
                      bool ignoreLengthScale = false);
        /// Draw a pose with the given scale.
        void drawPose(const VisuID& id,
                      const Eigen::Vector3f& pos,
                      const Eigen::Quaternionf& ori,
                      float scale,
                      bool ignoreLengthScale = false);
 
        /// Remove a pose.
        void removePose(const VisuID& id);
 
 
        // ROBOT
 
        /**
         * @brief Draw a robot.
         * Afterwards, `updateRobot*()` methods can be used with the same VisuID.
         *
         * @param robotFile The robot file. Either an absolute path, or a path
         *  relative to the data directory of the ArmarX project specified by `armarxProject`.
         * @param armarxProject The name of the ArmarX project keeping the robot model.
         * @param drawStyle The draw style (full or collision model).
         */
        void drawRobot(const VisuID& id,
                       const std::string& robotFile,
                       const std::string& armarxProject,
                       armarx::DrawStyle drawStyle = armarx::DrawStyle::FullModel);
 
        /// Update / set the robot pose.
        void
        updateRobotPose(const VisuID& id, const Eigen::Matrix4f& pose, bool ignoreScale = false);
 
        /// Update / set the robot pose.
        void updateRobotPose(const VisuID& id,
                             const Eigen::Vector3f& pos,
                             const Eigen::Quaternionf& ori,
                             bool ignoreScale = false);
 
        /// Update / set the robot configuration (joint angles).
        void updateRobotConfig(const VisuID& id, const std::map<std::string, float>& config);
 
        /// Update / set the robot color.
        void updateRobotColor(const VisuID& id, const DrawColor& color);
        /// Update / set the color of a robot node.
        void
        updateRobotNodeColor(const VisuID& id, const std::string& nodeName, const DrawColor& color);
 
        /// Remove a robot visualization.
        void removeRobot(const VisuID& id);
 
 
        // TRI MESH
 
        /// Draw a TriMeshModel as DebugDrawerTriMesh.
        void drawTriMesh(const VisuID& id,
                         const VirtualRobot::TriMeshModel& triMesh,
                         const DrawColor& color = {.5, .5, .5, 1},
                         bool ignoreLengthScale = false);
 
        /// Draw a TriMeshModel as individual polygons.
        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);
 
        /// Draw a TriMeshModel as individual polygons, transformed by the given pose.
        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);
 
        /// Draw a TriMeshModel as individual polygons with individual face colors.
        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.)
         */
 
        /**
         * @brief Draw a unicolored point cloud.
         *
         * `pointCloud` must be iterable and its elements must provide members `x, y, z`.
         */
        template <class PointCloudT>
        void drawPointCloud(const VisuID& id,
                            const PointCloudT& pointCloud,
                            const DrawColor& color = DEFAULTS.colorPointCloud,
                            float pointSize = DEFAULTS.pointCloudPointSize,
                            bool ignoreLengthScale = false);
 
        /**
         * @brief Draw a colored point cloud with RGBA information.
         *
         * `pointCloud` must be iterable and its elements must provide
         * members `x, y, z, r, g, b, a`.
         */
        template <class PointCloudT>
        void drawPointCloudRGBA(const VisuID& id,
                                const PointCloudT& pointCloud,
                                float pointSize = DEFAULTS.pointCloudPointSize,
                                bool ignoreLengthScale = false);
 
        /**
         * @brief Draw a colored point cloud with colors according to labels.
         *
         * `pointCloud` must be iterable and its elements must provide
         * members `x, y, z, label`.
         */
        template <class PointCloudT>
        void drawPointCloudLabeled(const VisuID& id,
                                   const PointCloudT& pointCloud,
                                   float pointSize = DEFAULTS.pointCloudPointSize,
                                   bool ignoreLengthScale = false);
 
        /**
         * @brief Draw a colored point cloud with custom colors.
         *
         * `pointCloud` must be iterable and its elements must provide
         * members `x, y, z`.
         * The color of a point is specified by `colorFunc`, which must be
         * a callable taking an element of `pointCloud` and returning its
         * color as `armarx::DrawColor`.
         */
        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
 
        /// Draw a non-colored point cloud.
        void drawPointCloud(const VisuID& id, const DebugDrawerPointCloud& pointCloud);
 
        /// Draw a colored point cloud.
        void drawPointCloud(const VisuID& id, const DebugDrawerColoredPointCloud& pointCloud);
 
        /// Draw a 24 bit colored point cloud.
        void drawPointCloud(const VisuID& id, const DebugDrawer24BitColoredPointCloud& pointCloud);
 
 
        /// Forces the "deletion" of a point cloud by drawing an empty one.
        void clearColoredPointCloud(const VisuID& id);
 
 
        // CUSTOM
 
        /**
         * @brief Draw a quad representing the floor.
         * @param at the quad'S center
         * @param up the up direction (normal of the floor plane)
         * @param size the quad's edge length
         */
        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
 
        /// Indicate whether visualization is enabled.
        /// @see `enabled()`
        operator bool() const;
 
        /// Conversion operator to DebugDrawerInterfacePrx.
        operator DebugDrawerInterfacePrx&();
        operator const DebugDrawerInterfacePrx&() const;
 
        /// Pointer member access operator to access the raw debug drawer proxy.
        DebugDrawerInterfacePrx& operator->();
        const DebugDrawerInterfacePrx& operator->() const;
 
 
    public: // STATIC
        /**
         * @brief Convert a RGB color to HSV.
         * @param rgb RGB color as [r, g, b] with r, g, b in [0, 1].
         * @return HSV color as [h, s, v] with h in [0 deg, 360 deg] and s, v in [0, 1].
         */
        static Eigen::Vector3f rgb2hsv(const Eigen::Vector3f& rgb);
 
        /**
         * @brief Convert a HSV color RGB.
         * @param HSV color as [h, s, v] with h in [0 deg, 360 deg] and s, v in [0, 1].
         * @return RGB color as [r, g, b] with r, g, b in [0, 1].
         */
        static Eigen::Vector3f hsv2rgb(const Eigen::Vector3f& hsv);
 
 
        /**
         * @brief Construct a DrawColor from the given color type.
         *
         * The used color type must have members named `r`, `g` and `b`.
         * Applicable types include:
         *  - pcl::RGB (byteToFloat = true)
         *  - armarx::DrawColor (useful to get a color with a different alpha)
         *
         * @param alpha the alpha (default: 1)
         * @param byteToFloat If true, scale from range [0, 255] to [0, 1]
         */
        template <class ColorT>
        static DrawColor
        toDrawColor(const ColorT& color, float alpha = 1, bool byteToFloat = false);
 
 
        /// Get a color from the Glasbey look-up-table.
        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:
        /// Get the layer to draw on. (passedLayer if not empty, _layer otherwise).
        const std::string& layer(const std::string& passedLayer) const;
 
        /// Get the layer to draw on. (id.layer if not empty, _layer otherwise).
        const std::string& layer(const VisuID& id) const;
 
 
        /// Return _lengthScale if ignore is false (default), 1 otherwise.
        float lengthScale(bool ignore = false) const;
 
        /// Scale the value.
        static float scaled(float scale, float value);
        /// Scale the given vector and return it as Vector3 pointer.
        static Vector3BasePtr scaled(float scale, const Eigen::Vector3f& vector);
        /// Scale the translation of the given pose and return it as Pose pointer.
        static PoseBasePtr scaled(float scale, const Eigen::Matrix4f& pose);
 
        /// Scale the given vector and return it as `ScaledT`.
        /// The constructor of `ScaledT` must take the x, y and z coordinates.
        template <class ScaledT>
        static ScaledT scaledT(float scale, const Eigen::Vector3f& vector);
 
        /// Scale a value with .x, .y and .z attributes in-place.
        template <class XYZT>
        static void scaleXYZ(float scale, XYZT& xyz);
 
 
    private:
        /// The name of the debug drawer topic.
        static const std::string TOPIC_NAME;
        /// The default layer ("debug").
        static const std::string DEFAULT_LAYER;
 
 
        /// The debug drawer topic.
        DebugDrawerInterfacePrx topic = nullptr;
 
        /// Whether drawing is enabled. (In comination with `topic`.
        bool _enabled = true;
 
        /// The default layer (used if none is passed to the method).
        std::string _layer = DEFAULT_LAYER;
 
        /// Scaling for positions, lengths and distances.
        float _lengthScale = 1;
 
        /// Scaling for pose visualization (1 is good when drawing in millimeters).
        float _poseScale = 1;
 
        /// The duration for shortSleep().
        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;
    }
 
} // namespace armarx