DebugDrawerHelper.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
 * Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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/>.
 *
 * @author     Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <SimoxUtility/shapes/OrientedBox.h>
#include <SimoxUtility/shapes/XYConstrainedOrientedBox.h>
#include <VirtualRobot/VirtualRobot.h>
 
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/libraries/core/Pose.h>
 
namespace armarx
{
    class DebugDrawerHelper;
    using DebugDrawerHelperPtr = std::shared_ptr<DebugDrawerHelper>;
} // namespace armarx
 
namespace armarx::detail::DebugDrawerHelper
{
    /**
     * @brief This class provides the draw options for a given frame (static Matrix, or a robot node).
     * It is used by the \ref DebugDrawerHelper.
     *
     * \note Do not use this class directly. Use it via \ref DebugDrawerHelper.
     */
    class FrameView
    {
    public:
        FrameView(class DebugDrawerHelper& helper,
                  const Eigen::Matrix4f frame = Eigen::Matrix4f::Identity());
        FrameView(class DebugDrawerHelper& helper, const VirtualRobot::RobotNodePtr& rn);
        //make global
    public:
        Eigen::Matrix4f makeGlobalEigen(const Eigen::Matrix4f& pose) const;
        Eigen::Vector3f makeGlobalEigen(const Eigen::Vector3f& position) const;
        Eigen::Vector3f makeGlobalDirectionEigen(const Eigen::Vector3f& direction) const;
        PosePtr makeGlobal(const Eigen::Matrix4f& pose) const;
        Vector3Ptr makeGlobal(const Eigen::Vector3f& position) const;
        Vector3Ptr makeGlobalDirection(const Eigen::Vector3f& direction) const;
 
        //1st order draw functions (direct calls to proxy)
    public:
        void drawPose(const std::string& name, const Eigen::Matrix4f& pose);
        void drawPose(const std::string& name, const Eigen::Matrix4f& pose, float scale);
 
        void drawBox(const std::string& name,
                     const Eigen::Matrix4f& pose,
                     const Eigen::Vector3f& size,
                     const DrawColor& color);
 
        void drawLine(const std::string& name,
                      const Eigen::Vector3f& p1,
                      const Eigen::Vector3f& p2,
                      float width,
                      const DrawColor& color);
 
        void drawText(const std::string& name,
                      const Eigen::Vector3f& p1,
                      const std::string& text,
                      const DrawColor& color,
                      int size);
 
        void drawArrow(const std::string& name,
                       const Eigen::Vector3f& pos,
                       const Eigen::Vector3f& direction,
                       const DrawColor& color,
                       float length,
                       float width);
 
        void drawSphere(const std::string& name,
                        const Eigen::Vector3f& position,
                        float size,
                        const DrawColor& color);
 
        void drawPointCloud(const std::string& name,
                            const std::vector<Eigen::Vector3f>& points,
                            float pointSize,
                            const DrawColor& color);
 
        void drawRobot(const std::string& name,
                       const std::string& robotFile,
                       const std::string& armarxProject,
                       const Eigen::Matrix4f& pose,
                       const DrawColor& color);
        void setRobotConfig(const std::string& name, const std::map<std::string, float>& config);
        void setRobotColor(const std::string& name, const DrawColor& color);
        void setRobotPose(const std::string& name, const Eigen::Matrix4f& pose);
 
        //2nd order draw functions (call 1st order)
    public:
        void drawPoses(const std::string& prefix, const std::vector<Eigen::Matrix4f>& poses);
        void drawPoses(const std::string& prefix,
                       const std::vector<Eigen::Matrix4f>& poses,
                       float scale);
 
        void drawBox(const std::string& name,
                     const Eigen::Vector3f& position,
                     float size,
                     const DrawColor& color);
        void drawBox(const std::string& name,
                     const Eigen::Matrix4f& pose,
                     float size,
                     const DrawColor& color);
 
        template <class FloatT>
        void
        drawBox(const std::string& name,
                const simox::OrientedBox<FloatT>& box,
                const DrawColor& color)
        {
            drawBox(name,
                    box.template transformation_centered<float>(),
                    box.template dimensions<float>(),
                    color);
        }
 
        template <class FloatT>
        void
        drawBox(const std::string& name,
                const simox::XYConstrainedOrientedBox<FloatT>& box,
                const DrawColor& color)
        {
            drawBox(name,
                    box.template transformation_centered<float>(),
                    box.template dimensions<float>(),
                    color);
        }
 
        void
        drawLine(const std::string& name, const Eigen::Vector3f& p1, const Eigen::Vector3f& p2);
 
        void drawLines(const std::string& prefix,
                       const std::vector<Eigen::Vector3f>& ps,
                       float width,
                       const DrawColor& color);
        void drawLines(const std::string& prefix, const std::vector<Eigen::Vector3f>& ps);
        void drawLines(const std::string& prefix,
                       const std::vector<Eigen::Matrix4f>& ps,
                       float width,
                       const DrawColor& color);
        void drawLines(const std::string& prefix, const std::vector<Eigen::Matrix4f>& ps);
 
    protected:
        class DebugDrawerHelper* _helper;
        Eigen::Matrix4f _fallbackFrame = Eigen::Matrix4f::Identity();
        VirtualRobot::RobotNodePtr _rn;
    };
} // namespace armarx::detail::DebugDrawerHelper
 
namespace armarx
{
    /**
     * @brief The DebugDrawerHelper class provides draw functions
     * in a given frame (static matrix or robot node) and accepts eigen data types
     *
     *
     * \note Implement new draw functions in \ref armarx::detail::DebugDrawerHelper::FrameView
     */
    class DebugDrawerHelper : public armarx::detail::DebugDrawerHelper::FrameView
    {
        using FrameView = armarx::detail::DebugDrawerHelper::FrameView;
        friend class armarx::detail::DebugDrawerHelper::FrameView;
 
    public:
        struct Defaults
        {
            float lineWidth = 2;
            DrawColor lineColor = DrawColor{0, 1, 0, 1};
        };
        enum class DrawElementType
        {
            Pose,
            Box,
            Line,
            Text,
            Arrow,
            ColoredPointCloud,
            Robot
        };
 
        struct DrawElement
        {
            std::string name;
            DrawElementType type;
 
            bool
            operator<(const DrawElement& rhs) const
            {
                int cmp = name.compare(rhs.name);
                return cmp < 0 || (cmp == 0 && type < rhs.type);
            }
        };
 
        class Colors
        {
        public:
            static const DrawColor Red;
            static const DrawColor Green;
            static const DrawColor Blue;
            static const DrawColor Yellow;
            static const DrawColor Orange;
            static DrawColor Lerp(const DrawColor& a, const DrawColor& b, float f);
        };
 
        DebugDrawerHelper(const DebugDrawerInterfacePrx& debugDrawerPrx,
                          const std::string& layerName,
                          const VirtualRobot::RobotPtr& robot);
        DebugDrawerHelper(const std::string& layerName);
 
        //utility
        void clearLayer();
        void cyclicCleanup();
 
        void setVisuEnabled(bool enableVisu);
        void removeElement(const std::string& name, DrawElementType type);
 
        Defaults defaults;
 
        const VirtualRobot::RobotPtr& getRobot() const;
        const DebugDrawerInterfacePrx& getDebugDrawer() const;
        void setDebugDrawer(const DebugDrawerInterfacePrx& drawer);
        void setRobot(const VirtualRobot::RobotPtr& rob);
 
        FrameView inFrame(const Eigen::Matrix4f& frame = Eigen::Matrix4f::Identity());
        FrameView inGlobalFrame();
        FrameView inFrame(const std::string& nodeName);
        FrameView inFrame(const VirtualRobot::RobotNodePtr& node);
 
        void setDefaultFrame(const Eigen::Matrix4f& frame = Eigen::Matrix4f::Identity());
        void setDefaultFrameToGlobal();
        void setDefaultFrame(const std::string& nodeName);
        void setDefaultFrame(const VirtualRobot::RobotNodePtr& node);
 
    private:
        void addNewElement(const std::string& name, DrawElementType type);
 
    private:
        std::set<DrawElement> newElements;
        std::set<DrawElement> oldElements;
        bool enableVisu = true;
        DebugDrawerInterfacePrx debugDrawerPrx;
        std::string layerName;
        VirtualRobot::RobotPtr robot;
    };
} // namespace armarx