From ffa93a147058cd849ba0970a0c65e623c3f9cd7d Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@student.kit.edu>
Date: Fri, 15 Mar 2019 16:48:46 +0100
Subject: [PATCH] Added drawPose() with specific scale
---
.../core/visualization/DebugDrawerTopic.cpp | 25 +++++++++++++------
.../core/visualization/DebugDrawerTopic.h | 4 +++
2 files changed, 22 insertions(+), 7 deletions(-)
diff --git a/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.cpp b/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.cpp
index 04154b58e..e3dffe8c2 100644
--- a/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.cpp
+++ b/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.cpp
@@ -125,25 +125,36 @@ namespace armarx
void DebugDrawerTopic::drawPose(
const Eigen::Matrix4f& pose, const std::string& name, const std::string& layer)
+ {
+ drawPose(pose, poseScale, name, layer);
+ }
+
+ void DebugDrawerTopic::drawPose(
+ const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori,
+ const std::string& name, const std::string& layer)
+ {
+ drawPose(math::Helpers::Pose(pos, ori), poseScale, name, layer);
+ }
+
+ void DebugDrawerTopic::drawPose(
+ const Eigen::Matrix4f& pose, float scale, const std::string& name, const std::string& layer)
{
if (enabled())
{
- if (poseScale >= 1 && poseScale <= 1) // squelch compiler warning that == is unsafe
+ if (scale >= 1 && scale <= 1) // squelch compiler warning that == is unsafe
{
- topic->setPoseVisu(layer, name, new Pose(pose));
+ topic->setPoseVisu(theLayer(layer), name, new Pose(pose));
}
else
{
- topic->setScaledPoseVisu(layer, name, new Pose(pose), poseScale);
+ topic->setScaledPoseVisu(theLayer(layer), name, new Pose(pose), scale);
}
}
}
- void DebugDrawerTopic::drawPose(
- const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori,
- const std::string& name, const std::string& layer)
+ void DebugDrawerTopic::drawPose(const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori, float scale, const std::string& name, const std::string& layer)
{
- drawPose(math::Helpers::Pose(pos, ori), name, layer);
+ drawPose(math::Helpers::Pose(pos, ori), scale, name, layer);
}
void DebugDrawerTopic::drawArrow(
diff --git a/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.h b/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.h
index 103e9991f..0c5f0b7a1 100644
--- a/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.h
+++ b/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.h
@@ -103,6 +103,10 @@ namespace armarx
void drawPose(const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori,
const std::string& name, const std::string& layer = "");
+ void drawPose(const Eigen::Matrix4f& pose, float scale,
+ const std::string& name, const std::string& layer = "");
+ void drawPose(const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori, float scale,
+ const std::string& name, const std::string& layer = "");
void drawArrow(const Eigen::Vector3f& position, const Eigen::Vector3f& direction,
float length, float width, const DrawColor& color,
--
GitLab