From c68d8ca98f35bc36ed9324a15f8802fd37f9068c Mon Sep 17 00:00:00 2001
From: Simon Ottenhaus <simon.ottenhaus@kit.edu>
Date: Fri, 28 Jun 2019 10:52:24 +0200
Subject: [PATCH] DebugDrawerHelper: added option to use without robot for
global frames
---
.../DebugDrawer/DebugDrawerHelper.cpp | 18 +++++++++++++-----
.../components/DebugDrawer/DebugDrawerHelper.h | 1 +
2 files changed, 14 insertions(+), 5 deletions(-)
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp
index f593cfb56..9e86cde0b 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp
@@ -34,7 +34,7 @@ using namespace armarx;
//#define CHECK_AND_ADD(name,type) if (!enableVisu) {return;}
DebugDrawerHelper::DebugDrawerHelper(const DebugDrawerInterfacePrx& debugDrawerPrx, const std::string& layerName, const VirtualRobot::RobotPtr& robot)
- : debugDrawerPrx(debugDrawerPrx), layerName(layerName), robot(robot), rn(robot->getRootNode())
+ : debugDrawerPrx(debugDrawerPrx), layerName(layerName), robot(robot), rn(robot ? robot->getRootNode() : VirtualRobot::RobotNodePtr())
{
}
@@ -99,7 +99,7 @@ void DebugDrawerHelper::drawPointCloud(const std::string& name, const std::vecto
pc.pointSize = pointSize;
for (const Eigen::Vector3f& p : points)
{
- Eigen::Vector3f pg = rn->getGlobalPosition(p);
+ Eigen::Vector3f pg = rn ? rn->getGlobalPosition(p) : p;
DebugDrawerColoredPointCloudElement e;
e.x = pg(0);
e.y = pg(1);
@@ -166,17 +166,25 @@ void DebugDrawerHelper::setVisuEnabled(bool enableVisu)
PosePtr DebugDrawerHelper::makeGlobal(const Eigen::Matrix4f& pose)
{
- return new Pose(rn->getGlobalPose(pose));
+ Eigen::Matrix4f gp = rn ? rn->getGlobalPose(pose) : pose;
+ return new Pose(gp);
}
Vector3Ptr DebugDrawerHelper::makeGlobal(const Eigen::Vector3f& position)
{
- return new Vector3(rn->getGlobalPosition(position));
+ Eigen::Vector3f gp = rn ? rn->getGlobalPosition(position) : position;
+ return new Vector3(gp);
}
Vector3Ptr DebugDrawerHelper::makeGlobalDirection(const Eigen::Vector3f& direction)
{
- return new Vector3(math::Helpers::TransformDirection(rn->getGlobalPose(), direction));
+ Eigen::Vector3f gd = rn ? math::Helpers::TransformDirection(rn->getGlobalPose(), direction) : direction;
+ return new Vector3(gd);
+}
+
+Eigen::Vector3f DebugDrawerHelper::makeGlobalEigen(const Eigen::Vector3f& position)
+{
+ return rn ? rn->getGlobalPosition(position) : position;
}
void DebugDrawerHelper::removeElement(const std::string& name, DebugDrawerHelper::DrawElementType type)
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h
index 93333cdcf..7b821a7e7 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h
@@ -99,6 +99,7 @@ namespace armarx
PosePtr makeGlobal(const Eigen::Matrix4f& pose);
Vector3Ptr makeGlobal(const Eigen::Vector3f& position);
Vector3Ptr makeGlobalDirection(const Eigen::Vector3f& direction);
+ Eigen::Vector3f makeGlobalEigen(const Eigen::Vector3f& position);
Defaults defaults;
--
GitLab