From abd8770e2380c5094543a56783d16a9d9df63a22 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@student.kit.edu>
Date: Wed, 6 Mar 2019 14:36:35 +0100
Subject: [PATCH] Added drawBox
---
.../libraries/core/DebugDrawerTopic.cpp | 20 ++++++++++++++++++-
.../libraries/core/DebugDrawerTopic.h | 13 +++++++++---
2 files changed, 29 insertions(+), 4 deletions(-)
diff --git a/source/RobotAPI/libraries/core/DebugDrawerTopic.cpp b/source/RobotAPI/libraries/core/DebugDrawerTopic.cpp
index 71e827fcc..7de001fe7 100644
--- a/source/RobotAPI/libraries/core/DebugDrawerTopic.cpp
+++ b/source/RobotAPI/libraries/core/DebugDrawerTopic.cpp
@@ -82,7 +82,25 @@ namespace armarx
}
}
-
+ void DebugDrawerTopic::drawBox(
+ const Eigen::Vector3f& position, const Eigen::Quaternionf& orientation,
+ const Eigen::Vector3f& extents,
+ const DrawColor& color, const std::string& name, const std::string& layer)
+ {
+ if (topic)
+ {
+ topic->setBoxVisu(layer, name, new Pose(new Vector3(position), new Quaternion(orientation)),
+ new Vector3(extents), color);
+ }
+ }
+
+ void DebugDrawerTopic::drawBox(
+ const Eigen::Matrix4f& pose, const Eigen::Vector3f& extents,
+ const DrawColor& color, const std::string& name, const std::string& layer)
+ {
+ drawBox(math::Helpers::Position(pose), Eigen::Quaternionf(math::Helpers::Orientation(pose)),
+ extents, color, name, layer);
+ }
void DebugDrawerTopic::drawPose(
diff --git a/source/RobotAPI/libraries/core/DebugDrawerTopic.h b/source/RobotAPI/libraries/core/DebugDrawerTopic.h
index 644352e71..dad8e8c19 100644
--- a/source/RobotAPI/libraries/core/DebugDrawerTopic.h
+++ b/source/RobotAPI/libraries/core/DebugDrawerTopic.h
@@ -25,7 +25,8 @@ namespace armarx
* Methods arguments are ordered from specific to general. That is, common
* arguments such as layer, name and color always come last. Less common
* parameters (e.g. width of arrow/line), come before them. The first
- * arguments are those specific to the respective function.
+ * arguments are those specific to the respective function (although
+ * pose-related arguments typically come first).
* (Added methods should follow this pattern.)
*
* All methods check whether the internal topic proxy is set, and do
@@ -75,8 +76,14 @@ namespace armarx
void sleepFor(const DurationT& duration);
- void drawPose(const Eigen::Matrix4f& pose, const
- std::string& name, const std::string& layer = "pose");
+ void drawBox(const Eigen::Vector3f& position, const Eigen::Quaternionf& orientation,
+ const Eigen::Vector3f& extents,
+ const DrawColor& color, const std::string& name, const std::string& layer);
+ void drawBox(const Eigen::Matrix4f& pose, const Eigen::Vector3f& extents,
+ const DrawColor& color, const std::string& name, const std::string& layer);
+
+ void drawPose(const Eigen::Matrix4f& pose,
+ const std::string& name, const std::string& layer = "pose");
void drawPose(const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori,
const std::string& name, const std::string& layer = "pose");
--
GitLab