added DebugDrawerHelper

data/RobotAPI/VariantInfo-RobotAPI.xml

@@ -210,7 +210,9 @@
             getterName="getDebugDrawerTopic"
             propertyName="DebugDrawerTopicName"
             propertyIsOptional="true"
-            propertyDefaultValue="DebugDrawerUpdates" />
+            propertyDefaultValue="DebugDrawerUpdates">
+            <library>DebugDrawer</library>
+        </Topic>
 
         <Topic include="RobotAPI/interface/speech/SpeechInterface.h"
             humanName="Text to Speech"

source/RobotAPI/components/DebugDrawer/CMakeLists.txt

@@ -18,9 +18,15 @@ endif()
 
 set(COMPONENT_LIBS ArmarXCore RobotAPIInterfaces RobotAPICore ${Simox_LIBRARIES})
 
-set(SOURCES DebugDrawerComponent.cpp)
-set(HEADERS DebugDrawerComponent.h
-    DebugDrawerUtils.h)
+set(SOURCES
+    DebugDrawerComponent.cpp
+    DebugDrawerHelper.cpp
+    )
+set(HEADERS
+    DebugDrawerComponent.h
+    DebugDrawerHelper.h
+    DebugDrawerUtils.h
+    )
 
 
 armarx_add_component("${SOURCES}" "${HEADERS}")

source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp

+/*
+ * 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 <RobotAPI/libraries/core/Pose.h>
+
+using namespace math;
+using namespace armarx;
+
+DebugDrawerHelper::DebugDrawerHelper(const DebugDrawerInterfacePrx& debugDrawerPrx, const std::string& layerName, const VirtualRobot::RobotPtr& robot)
+    : debugDrawerPrx(debugDrawerPrx), robot(robot), rn(robot->getRootNode())
+{
+}
+
+void DebugDrawerHelper::drawPose(const std::string& name, const Eigen::Matrix4f& pose)
+{
+    debugDrawerPrx->setPoseVisu(layerName, name, makeGlobal(pose));
+}
+
+void DebugDrawerHelper::drawPose(const std::string& name, const Eigen::Matrix4f& pose, float scale)
+{
+    debugDrawerPrx->setScaledPoseVisu(layerName, name, makeGlobal(pose), scale);
+}
+
+void DebugDrawerHelper::drawBox(const std::string& name, const Eigen::Vector3f& position, float size, const DrawColor& color)
+{
+    drawBox(name, Helpers::CreatePose(position, Eigen::Matrix3f::Identity()), Eigen::Vector3f(size, size, size), color);
+}
+
+void DebugDrawerHelper::drawBox(const std::string& name, const Eigen::Matrix4f& pose, float size, const DrawColor& color)
+{
+    drawBox(name, pose, Eigen::Vector3f(size, size, size), color);
+}
+
+void DebugDrawerHelper::drawBox(const std::string& name, const Eigen::Matrix4f& pose, const Eigen::Vector3f& size, const DrawColor& color)
+{
+    debugDrawerPrx->setBoxVisu(layerName, name, makeGlobal(pose), new Vector3(size), color);
+}
+
+void DebugDrawerHelper::drawLine(const std::string& name, const Eigen::Vector3f& p1, const Eigen::Vector3f& p2, float width, const DrawColor& color)
+{
+    debugDrawerPrx->setLineVisu(layerName, name, makeGlobal(p1), makeGlobal(p2), width, color);
+}
+
+void DebugDrawerHelper::drawLine(const std::string& name, const Eigen::Vector3f& p1, const Eigen::Vector3f& p2)
+{
+    drawLine(name, p1, p2, defaults.lineWidth, defaults.lineColor);
+}
+
+void DebugDrawerHelper::drawText(const std::string& name, const Eigen::Vector3f& p1, const std::string& text, const DrawColor& color, int size)
+{
+    debugDrawerPrx->setTextVisu(layerName, name, text, makeGlobal(p1), color, size);
+}
+
+PosePtr DebugDrawerHelper::makeGlobal(const Eigen::Matrix4f& pose)
+{
+    return new Pose(rn->getGlobalPose(pose));
+}
+
+Vector3Ptr DebugDrawerHelper::makeGlobal(const Eigen::Vector3f& position)
+{
+    return new Vector3(rn->getGlobalPosition(position));
+}

source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h

+/*
+ * 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 <boost/shared_ptr.hpp>
+
+#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
+
+#include <VirtualRobot/Robot.h>
+
+#include <RobotAPI/libraries/core/Pose.h>
+
+namespace armarx
+{
+    class DebugDrawerHelper;
+    typedef boost::shared_ptr<DebugDrawerHelper> DebugDrawerHelperPtr;
+
+    class DebugDrawerHelper
+    {
+    public:
+        struct Defaults
+        {
+            float lineWidth = 2;
+            DrawColor lineColor = DrawColor {0, 1, 0, 1};
+        };
+
+        DebugDrawerHelper(const DebugDrawerInterfacePrx& debugDrawerPrx, const std::string& layerName, const VirtualRobot::RobotPtr& robot);
+
+        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::Vector3f& position, float size, const DrawColor& color);
+        void drawBox(const std::string& name, const Eigen::Matrix4f& pose, float size, const DrawColor& color);
+        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 drawLine(const std::string& name, const Eigen::Vector3f& p1, const Eigen::Vector3f& p2);
+
+        void drawText(const std::string& name, const Eigen::Vector3f& p1, const std::string& text, const DrawColor& color, int size);
+
+        PosePtr makeGlobal(const Eigen::Matrix4f& pose);
+        Vector3Ptr makeGlobal(const Eigen::Vector3f& position);
+
+
+        Defaults defaults;
+
+    private:
+        DebugDrawerInterfacePrx debugDrawerPrx;
+        std::string layerName;
+        VirtualRobot::RobotPtr robot;
+        VirtualRobot::RobotNodePtr rn;
+    };
+}