From 4ba8639d18bdddd4cc22b0105f5fbc0a35a89391 Mon Sep 17 00:00:00 2001
From: Simon Ottenhaus <simon.ottenhaus@kit.edu>
Date: Thu, 16 Aug 2018 18:44:19 +0200
Subject: [PATCH] added DebugDrawerHelper
---
data/RobotAPI/VariantInfo-RobotAPI.xml | 4 +-
.../components/DebugDrawer/CMakeLists.txt | 12 ++-
.../DebugDrawer/DebugDrawerHelper.cpp | 86 +++++++++++++++++++
.../DebugDrawer/DebugDrawerHelper.h | 74 ++++++++++++++++
4 files changed, 172 insertions(+), 4 deletions(-)
create mode 100644 source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp
create mode 100644 source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h
diff --git a/data/RobotAPI/VariantInfo-RobotAPI.xml b/data/RobotAPI/VariantInfo-RobotAPI.xml
index 67402b87a..8ff3d3ca8 100644
--- a/data/RobotAPI/VariantInfo-RobotAPI.xml
+++ b/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"
diff --git a/source/RobotAPI/components/DebugDrawer/CMakeLists.txt b/source/RobotAPI/components/DebugDrawer/CMakeLists.txt
index 754e65a5f..171043962 100644
--- a/source/RobotAPI/components/DebugDrawer/CMakeLists.txt
+++ b/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}")
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp
new file mode 100644
index 000000000..a4ee8ad72
--- /dev/null
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp
@@ -0,0 +1,86 @@
+/*
+ * 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));
+}
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h
new file mode 100644
index 000000000..3bdaa4933
--- /dev/null
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h
@@ -0,0 +1,74 @@
+/*
+ * 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;
+ };
+}
--
GitLab