From 3b00d3eaf6f573bd259764e083f523f96bee5fef Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Mon, 25 May 2020 15:42:53 +0200
Subject: [PATCH] Add toIce() functions for AABB and OOBB
---
.../ObjectPoseObserver/ObjectPoseObserver.cpp | 1 -
.../ObjectPoseObserver/ice_conversions.cpp | 23 +++++++++++++++++++
.../ObjectPoseObserver/ice_conversions.h | 13 +++++++++++
3 files changed, 36 insertions(+), 1 deletion(-)
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
index 959fda807..e7202cd5a 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
@@ -30,7 +30,6 @@
#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/libraries/ArmarXObjects/ObjectInfo.h>
#include "ice_conversions.h"
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.cpp b/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.cpp
index e40d29d70..0085e76c8 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.cpp
@@ -3,8 +3,12 @@
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotConfig.h>
+#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
+#include <SimoxUtility/shapes/OrientedBox.h>
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/core/FramedPose.h>
@@ -23,6 +27,25 @@ namespace armarx
{ objpose::ObjectTypeEnum::UnknownObject, "UnknownObject" }
};
+
+ objpose::AABB objpose::toIce(const simox::AxisAlignedBoundingBox& aabb)
+ {
+ objpose::AABB ice;
+ ice.center = new Vector3(aabb.center());
+ ice.extents = new Vector3(aabb.extents());
+ return ice;
+ }
+
+ objpose::Box objpose::toIce(const simox::OrientedBox<float>& oobb)
+ {
+ objpose::Box ice;
+ ice.position = new Vector3(oobb.center());
+ ice.orientation = new Quaternion(oobb.rotation().eval());
+ ice.extents = new Vector3(oobb.dimensions());
+ return ice;
+ }
+
}
+
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.h b/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.h
index e9a0da541..d71b34cae 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.h
@@ -7,6 +7,15 @@
#include "ObjectPose.h"
+namespace simox
+{
+ // #include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
+ struct AxisAlignedBoundingBox;
+ // #include <SimoxUtility/shapes/OrientedBox.h>
+ template<class FloatT> class OrientedBox;
+}
+
+
namespace armarx::objpose
{
@@ -14,4 +23,8 @@ namespace armarx::objpose
extern const simox::meta::EnumNames<objpose::ObjectTypeEnum> ObjectTypeEnumNames;
+
+ objpose::AABB toIce(const simox::AxisAlignedBoundingBox& aabb);
+ objpose::Box toIce(const simox::OrientedBox<float>& oobb);
+
}
--
GitLab