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/math/Helpers.h>
 
#include <ArmarXCore/util/CPPUtility/trace.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.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
{
    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)
        };
    }
}