DebugDrawerHelper.cpp

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
 */
 
#include "DebugDrawerHelper.h"
 
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/math/Helpers.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <RobotAPI/libraries/core/Pose.h>
 
using namespace math;
 
namespace armarx::detail::DebugDrawerHelper
{
    FrameView::FrameView(class DebugDrawerHelper& helper, const Eigen::Matrix4f frame) :
        _helper{&helper}, _fallbackFrame{frame}
    {
    }
 
    FrameView::FrameView(class DebugDrawerHelper& helper, const VirtualRobot::RobotNodePtr& rn) :
        _helper{&helper}, _rn{rn}
    {
    }
 
    //make global
    Eigen::Matrix4f
    FrameView::makeGlobalEigen(const Eigen::Matrix4f& pose) const
    {
        ARMARX_TRACE;
        if (_rn)
        {
            return _rn->getGlobalPose(pose);
        }
        return _fallbackFrame * pose;
    }
 
    Eigen::Vector3f
    FrameView::makeGlobalEigen(const Eigen::Vector3f& position) const
    {
        ARMARX_TRACE;
        if (_rn)
        {
            return _rn->getGlobalPosition(position);
        }
        return (_fallbackFrame * position.homogeneous()).eval().hnormalized();
    }
 
    Eigen::Vector3f
    FrameView::makeGlobalDirectionEigen(const Eigen::Vector3f& direction) const
    {
        ARMARX_TRACE;
        if (_rn)
        {
            return ::math::Helpers::TransformDirection(_rn->getGlobalPose(), direction);
        }
        return ::math::Helpers::TransformDirection(_fallbackFrame, direction);
    }
 
    PosePtr
    FrameView::makeGlobal(const Eigen::Matrix4f& pose) const
    {
        ARMARX_TRACE;
        return new Pose(makeGlobalEigen(pose));
    }
 
    Vector3Ptr
    FrameView::makeGlobal(const Eigen::Vector3f& position) const
    {
        ARMARX_TRACE;
        return new Vector3(makeGlobalEigen(position));
    }
 
    Vector3Ptr
    FrameView::makeGlobalDirection(const Eigen::Vector3f& direction) const
    {
        ARMARX_TRACE;
        return new Vector3(makeGlobalDirectionEigen(direction));
    }
 
    //1st order
    using DrawElementType = ::armarx::DebugDrawerHelper::DrawElementType;
#define CHECK_AND_ADD(name, type)                                                                  \
    if (!_helper->enableVisu)                                                                      \
    {                                                                                              \
        return;                                                                                    \
    }                                                                                              \
    _helper->addNewElement(name, type);
 
    void
    FrameView::drawPose(const std::string& name, const Eigen::Matrix4f& pose)
    {
        ARMARX_TRACE;
        CHECK_AND_ADD(name, DrawElementType::Pose)
        _helper->debugDrawerPrx->setPoseVisu(_helper->layerName, name, makeGlobal(pose));
    }
 
    void
    FrameView::drawPose(const std::string& name, const Eigen::Matrix4f& pose, float scale)
    {
        ARMARX_TRACE;
        CHECK_AND_ADD(name, DrawElementType::Pose)
        _helper->debugDrawerPrx->setScaledPoseVisu(
            _helper->layerName, name, makeGlobal(pose), scale);
    }
 
    void
    FrameView::drawBox(const std::string& name,
                       const Eigen::Matrix4f& pose,
                       const Eigen::Vector3f& size,
                       const DrawColor& color)
    {
        ARMARX_TRACE;
        CHECK_AND_ADD(name, DrawElementType::Box)
        _helper->debugDrawerPrx->setBoxVisu(
            _helper->layerName, name, makeGlobal(pose), new Vector3(size), color);
    }
 
    void
    FrameView::drawLine(const std::string& name,
                        const Eigen::Vector3f& p1,
                        const Eigen::Vector3f& p2,
                        float width,
                        const DrawColor& color)
    {
        ARMARX_TRACE;
        CHECK_AND_ADD(name, DrawElementType::Line)
        _helper->debugDrawerPrx->setLineVisu(
            _helper->layerName, name, makeGlobal(p1), makeGlobal(p2), width, color);
    }
 
    void
    FrameView::drawText(const std::string& name,
                        const Eigen::Vector3f& p1,
                        const std::string& text,
                        const DrawColor& color,
                        int size)
    {
        ARMARX_TRACE;
        CHECK_AND_ADD(name, DrawElementType::Text)
        _helper->debugDrawerPrx->setTextVisu(
            _helper->layerName, name, text, makeGlobal(p1), color, size);
    }
 
    void
    FrameView::drawArrow(const std::string& name,
                         const Eigen::Vector3f& pos,
                         const Eigen::Vector3f& direction,
                         const DrawColor& color,
                         float length,
                         float width)
    {
        ARMARX_TRACE;
        CHECK_AND_ADD(name, DrawElementType::Arrow)
        _helper->debugDrawerPrx->setArrowVisu(_helper->layerName,
                                              name,
                                              makeGlobal(pos),
                                              makeGlobalDirection(direction),
                                              color,
                                              length,
                                              width);
    }
 
    void
    FrameView::drawSphere(const std::string& name,
                          const Eigen::Vector3f& position,
                          float size,
                          const DrawColor& color)
    {
        ARMARX_TRACE;
        _helper->debugDrawerPrx->setSphereVisu(
            _helper->layerName, name, makeGlobal(position), color, size);
    }
 
    void
    FrameView::drawPointCloud(const std::string& name,
                              const std::vector<Eigen::Vector3f>& points,
                              float pointSize,
                              const DrawColor& color)
    {
        ARMARX_TRACE;
        CHECK_AND_ADD(name, DrawElementType::ColoredPointCloud)
        DebugDrawerColoredPointCloud pc;
        pc.pointSize = pointSize;
        for (const Eigen::Vector3f& p : points)
        {
            Eigen::Vector3f pg = makeGlobalEigen(p);
            DebugDrawerColoredPointCloudElement e;
            e.x = pg(0);
            e.y = pg(1);
            e.z = pg(2);
            e.color =
                color; //DrawColor24Bit {(Ice::Byte)(color.r * 255), (Ice::Byte)(color.g * 255), (Ice::Byte)(color.b * 255)};
            pc.points.push_back(e);
        }
        _helper->debugDrawerPrx->setColoredPointCloudVisu(_helper->layerName, name, pc);
    }
 
    void
    FrameView::drawRobot(const std::string& name,
                         const std::string& robotFile,
                         const std::string& armarxProject,
                         const Eigen::Matrix4f& pose,
                         const DrawColor& color)
    {
        ARMARX_TRACE;
        CHECK_AND_ADD(name, DrawElementType::Robot)
        _helper->debugDrawerPrx->setRobotVisu(
            _helper->layerName, name, robotFile, armarxProject, DrawStyle::FullModel);
        _helper->debugDrawerPrx->updateRobotPose(_helper->layerName, name, makeGlobal(pose));
        _helper->debugDrawerPrx->updateRobotColor(_helper->layerName, name, color);
    }
 
    void
    FrameView::setRobotConfig(const std::string& name, const std::map<std::string, float>& config)
    {
        ARMARX_TRACE;
        _helper->debugDrawerPrx->updateRobotConfig(_helper->layerName, name, config);
    }
 
    void
    FrameView::setRobotColor(const std::string& name, const DrawColor& color)
    {
        ARMARX_TRACE;
        _helper->debugDrawerPrx->updateRobotColor(_helper->layerName, name, color);
    }
 
    void
    FrameView::setRobotPose(const std::string& name, const Eigen::Matrix4f& pose)
    {
        ARMARX_TRACE;
        _helper->debugDrawerPrx->updateRobotPose(_helper->layerName, name, makeGlobal(pose));
    }
 
#undef CHECK_AND_ADD
 
    //2nd order
    void
    FrameView::drawPoses(const std::string& prefix, const std::vector<Eigen::Matrix4f>& poses)
    {
        ARMARX_TRACE;
        for (std::size_t idx = 0; idx < poses.size(); ++idx)
        {
            Eigen::Matrix4f const& pose = poses[idx];
            drawPose(prefix + std::to_string(idx), pose);
        }
    }
 
    void
    FrameView::drawPoses(const std::string& prefix,
                         const std::vector<Eigen::Matrix4f>& poses,
                         float scale)
    {
        ARMARX_TRACE;
        for (std::size_t idx = 0; idx < poses.size(); ++idx)
        {
            Eigen::Matrix4f const& pose = poses[idx];
            drawPose(prefix + std::to_string(idx), pose, scale);
        }
    }
 
    void
    FrameView::drawBox(const std::string& name,
                       const Eigen::Vector3f& position,
                       float size,
                       const DrawColor& color)
    {
        ARMARX_TRACE;
        drawBox(name,
                Helpers::CreatePose(position, Eigen::Matrix3f::Identity()),
                Eigen::Vector3f(size, size, size),
                color);
    }
 
    void
    FrameView::drawBox(const std::string& name,
                       const Eigen::Matrix4f& pose,
                       float size,
                       const DrawColor& color)
    {
        ARMARX_TRACE;
        drawBox(name, pose, Eigen::Vector3f(size, size, size), color);
    }
 
    void
    FrameView::drawLine(const std::string& name,
                        const Eigen::Vector3f& p1,
                        const Eigen::Vector3f& p2)
    {
        ARMARX_TRACE;
        drawLine(name, p1, p2, _helper->defaults.lineWidth, _helper->defaults.lineColor);
    }
 
    void
    FrameView::drawLines(const std::string& prefix,
                         const std::vector<Eigen::Vector3f>& ps,
                         float width,
                         const DrawColor& color)
    {
        ARMARX_TRACE;
        for (std::size_t idx = 1; idx < ps.size(); ++idx)
        {
            drawLine(prefix + std::to_string(idx), ps.at(idx - 1), ps.at(idx), width, color);
        }
    }
 
    void
    FrameView::drawLines(const std::string& prefix, const std::vector<Eigen::Vector3f>& ps)
    {
        ARMARX_TRACE;
        for (std::size_t idx = 1; idx < ps.size(); ++idx)
        {
            drawLine(prefix + std::to_string(idx), ps.at(idx - 1), ps.at(idx));
        }
    }
 
    void
    FrameView::drawLines(const std::string& prefix,
                         const std::vector<Eigen::Matrix4f>& ps,
                         float width,
                         const DrawColor& color)
    {
        ARMARX_TRACE;
        for (std::size_t idx = 1; idx < ps.size(); ++idx)
        {
            drawLine(prefix + std::to_string(idx),
                     ps.at(idx - 1).topRightCorner<3, 1>(),
                     ps.at(idx).topRightCorner<3, 1>(),
                     width,
                     color);
        }
    }
 
    void
    FrameView::drawLines(const std::string& prefix, const std::vector<Eigen::Matrix4f>& ps)
    {
        ARMARX_TRACE;
        for (std::size_t idx = 1; idx < ps.size(); ++idx)
        {
            drawLine(prefix + std::to_string(idx),
                     ps.at(idx - 1).topRightCorner<3, 1>(),
                     ps.at(idx).topRightCorner<3, 1>());
        }
    }
} // namespace armarx::detail::DebugDrawerHelper
 
namespace armarx
{
    DebugDrawerHelper::DebugDrawerHelper(const DebugDrawerInterfacePrx& debugDrawerPrx,
                                         const std::string& layerName,
                                         const VirtualRobot::RobotPtr& robot) :
        FrameView(*this, robot ? robot->getRootNode() : nullptr),
        debugDrawerPrx(debugDrawerPrx),
        layerName(layerName),
        robot(robot)
    {
        ARMARX_CHECK_NOT_NULL(debugDrawerPrx);
    }
 
    DebugDrawerHelper::DebugDrawerHelper(const std::string& layerName) :
        FrameView(*this, nullptr), layerName{layerName}
    {
    }
 
    //utility
    void
    DebugDrawerHelper::clearLayer()
    {
        ARMARX_TRACE;
        debugDrawerPrx->clearLayer(layerName);
        oldElements.clear();
        newElements.clear();
    }
 
    void
    DebugDrawerHelper::cyclicCleanup()
    {
        ARMARX_TRACE;
        /*
        // debug code...
        std::stringstream ss;
        ss << "OLD: ";
        for (const DrawElement& e : oldElements)
        {
            ss << e.name << ",";
        }
        ss << " NEW: ";
        for (const DrawElement& e : newElements)
        {
            ss << e.name << ",";
        }
        ARMARX_IMPORTANT << ss.str();*/
 
        for (const DrawElement& old : oldElements)
        {
            if (newElements.find(old) == newElements.end())
            {
                //ARMARX_IMPORTANT << "removing " << old.name;
                removeElement(old.name, old.type);
            }
        }
        oldElements = newElements;
        newElements.clear();
    }
 
    void
    DebugDrawerHelper::setVisuEnabled(bool enableVisu)
    {
        this->enableVisu = enableVisu;
    }
 
    void
    DebugDrawerHelper::removeElement(const std::string& name,
                                     DebugDrawerHelper::DrawElementType type)
    {
        ARMARX_TRACE;
        switch (type)
        {
            case DrawElementType::Pose:
                debugDrawerPrx->removePoseVisu(layerName, name);
                return;
            case DrawElementType::Box:
                debugDrawerPrx->removeBoxVisu(layerName, name);
                return;
            case DrawElementType::Line:
                debugDrawerPrx->removeLineVisu(layerName, name);
                return;
            case DrawElementType::Text:
                debugDrawerPrx->removeTextVisu(layerName, name);
                return;
            case DrawElementType::Arrow:
                debugDrawerPrx->removeArrowVisu(layerName, name);
                return;
            case DrawElementType::ColoredPointCloud:
                debugDrawerPrx->removeColoredPointCloudVisu(layerName, name);
                return;
            case DrawElementType::Robot:
                debugDrawerPrx->removeRobotVisu(layerName, name);
                return;
        }
    }
 
    const VirtualRobot::RobotPtr&
    DebugDrawerHelper::getRobot() const
    {
        return robot;
    }
 
    const DebugDrawerInterfacePrx&
    DebugDrawerHelper::getDebugDrawer() const
    {
        return debugDrawerPrx;
    }
 
    void
    DebugDrawerHelper::setDebugDrawer(const DebugDrawerInterfacePrx& drawer)
    {
        ARMARX_CHECK_NOT_NULL(drawer);
        debugDrawerPrx = drawer;
    }
 
    void
    DebugDrawerHelper::setRobot(const VirtualRobot::RobotPtr& rob)
    {
        robot = rob;
    }
 
    DebugDrawerHelper::FrameView
    DebugDrawerHelper::inFrame(const Eigen::Matrix4f& frame)
    {
        return {*this, frame};
    }
 
    DebugDrawerHelper::FrameView
    DebugDrawerHelper::inGlobalFrame()
    {
        return {*this, Eigen::Matrix4f::Identity()};
    }
 
    DebugDrawerHelper::FrameView
    DebugDrawerHelper::inFrame(const std::string& nodeName)
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(robot);
        ARMARX_CHECK_EXPRESSION(robot->hasRobotNode(nodeName));
        return {*this, robot->getRobotNode(nodeName)};
    }
 
    DebugDrawerHelper::FrameView
    DebugDrawerHelper::inFrame(const VirtualRobot::RobotNodePtr& node)
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(node);
        return {*this, node};
    }
 
    void
    DebugDrawerHelper::setDefaultFrame(const Eigen::Matrix4f& frame)
    {
        ARMARX_TRACE;
        _rn = nullptr;
        _fallbackFrame = frame;
    }
 
    void
    DebugDrawerHelper::setDefaultFrameToGlobal()
    {
        ARMARX_TRACE;
        _rn = nullptr;
        _fallbackFrame = Eigen::Matrix4f::Identity();
    }
 
    void
    DebugDrawerHelper::setDefaultFrame(const std::string& nodeName)
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(robot);
        ARMARX_CHECK_EXPRESSION(robot->hasRobotNode(nodeName));
        _rn = robot->getRobotNode(nodeName);
    }
 
    void
    DebugDrawerHelper::setDefaultFrame(const VirtualRobot::RobotNodePtr& node)
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(robot);
        ARMARX_CHECK_EXPRESSION(robot == node->getRobot());
        _rn = node;
    }
 
    void
    DebugDrawerHelper::addNewElement(const std::string& name,
                                     DebugDrawerHelper::DrawElementType type)
    {
        ARMARX_TRACE;
        newElements.insert(DrawElement{name, type});
    }
 
    const DrawColor DebugDrawerHelper::Colors::Red = DrawColor{1, 0, 0, 1};
    const DrawColor DebugDrawerHelper::Colors::Green = DrawColor{0, 1, 0, 1};
    const DrawColor DebugDrawerHelper::Colors::Blue = DrawColor{0, 0, 1, 1};
    const DrawColor DebugDrawerHelper::Colors::Yellow = DrawColor{1, 1, 0, 1};
    const DrawColor DebugDrawerHelper::Colors::Orange = DrawColor{1, 0.5f, 0, 1};
 
    DrawColor
    DebugDrawerHelper::Colors::Lerp(const DrawColor& a, const DrawColor& b, float f)
    {
        return DrawColor{::math::Helpers::Lerp(a.r, b.r, f),
                         ::math::Helpers::Lerp(a.g, b.g, f),
                         ::math::Helpers::Lerp(a.b, b.b, f),
                         ::math::Helpers::Lerp(a.a, b.a, f)};
    }
} // namespace armarx