diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions.cpp b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions.cpp
index 71398d59dc8d00018bcc0d4fb5c1c7cfb6010465..e24325158807546c2e447db8a750a5e5413369f5 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions.cpp
@@ -7,177 +7,144 @@
// Ice
#include <IceUtil/Time.h>
-// Simox
-#include <SimoxUtility/shapes/OrientedBox.h>
+// boost
+#include <boost/bimap.hpp>
-// RobotAPI
-// TODO: ice dependency! header should be removed.
-#include <RobotAPI/interface/objectpose/object_pose_types.h>
+#include <ArmarXCore/core/exceptions/local/UnexpectedEnumValueException.h>
-#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
-#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
+void simox::fromAron(const arondto::OrientedBox& dto, OrientedBoxf& bo)
+{
+ bo = OrientedBoxf(dto.centerPose, dto.extends);
+}
-namespace armarx::objpose
+void simox::toAron(arondto::OrientedBox& dto, const OrientedBoxf& bo)
{
+ dto.centerPose = bo.transformation();
+ dto.extends = bo.dimensions();
+}
- ObjectID fromAron(const aron::ObjectID& id)
- {
- return ObjectID(id.dataset, id.className, id.instanceName);
- }
+void armarx::fromAron(const arondto::ObjectID& dto, ObjectID& bo)
+{
+ bo = ObjectID(dto.dataset, dto.className, dto.instanceName);
+}
+void armarx::toAron(arondto::ObjectID& dto, const ObjectID& bo)
+{
+ dto.dataset = bo.dataset();
+ dto.className = bo.className();
+ dto.instanceName = bo.instanceName();
+}
- ObjectTypeEnum fromAron(const aron::ObjectTypes& objectType)
- {
- using AronObjectType = aron::ObjectTypes::__ImplEnum;
-
- ObjectTypeEnum e{};
-
- switch (objectType.value)
- {
- case AronObjectType::ANY_OBJECT:
- e = ObjectTypeEnum::AnyObject;
- break;
- case AronObjectType::KNOWN_OBJECT:
- e = ObjectTypeEnum::KnownObject;
- break;
- case AronObjectType::UNKNOWN_OBJECT:
- e = ObjectTypeEnum::UnknownObject;
- break;
- }
-
- return e;
- }
- simox::OrientedBoxf fromAron(const aron::OrientedBoundingBox& obb)
- {
- return simox::OrientedBoxf(obb.centerPose, obb.extends);
- }
+void armarx::objpose::fromAron(const arondto::ObjectAttachmentInfo& dto, ObjectAttachmentInfo& bo)
+{
+ bo.frameName = dto.frameName;
+ bo.agentName = dto.agentName;
+ bo.poseInFrame = dto.poseInFrame;
+}
+void armarx::objpose::toAron(arondto::ObjectAttachmentInfo& dto, const ObjectAttachmentInfo& bo)
+{
+ dto.frameName = bo.frameName;
+ dto.agentName = bo.agentName;
+ dto.poseInFrame = bo.poseInFrame;
+}
- ObjectAttachmentInfo fromAron(const aron::ObjectAttachmentInfo& info)
+void armarx::objpose::fromAron(const arondto::ObjectType& dto, ObjectTypeEnum& bo)
+{
+ switch (dto.value)
{
- return
- {
- .frameName = info.frameName,
- .agentName = info.agentName,
- .poseInFrame = info.poseInFrame
- };
+ case arondto::ObjectType::AnyObject:
+ bo = ObjectTypeEnum::AnyObject;
+ return;
+ case arondto::ObjectType::KnownObject:
+ bo = ObjectTypeEnum::KnownObject;
+ return;
+ case arondto::ObjectType::UnknownObject:
+ bo = ObjectTypeEnum::UnknownObject;
+ return;
}
-
-
- void fromAron(const aron::ObjectPose& aronObjectPose, ObjectPose& objectPose)
+ ARMARX_UNEXPECTED_ENUM_VALUE(arondto::ObjectType, dto.value);
+}
+void armarx::objpose::toAron(arondto::ObjectType& dto, const ObjectTypeEnum& bo)
+{
+ switch (bo)
{
- objectPose.providerName = aronObjectPose.providerName;
-
- objectPose.objectType = fromAron(aronObjectPose.objectType);
-
- objectPose.objectID = fromAron(aronObjectPose.objectID);
-
- objectPose.objectPoseRobot = aronObjectPose.objectPoseRobot;
- objectPose.objectPoseGlobal = aronObjectPose.objectPoseGlobal;
- objectPose.objectPoseOriginal = aronObjectPose.objectPoseOriginal;
- objectPose.objectPoseOriginalFrame = aronObjectPose.objectPoseOriginalFrame;
-
- objectPose.robotConfig = aronObjectPose.robotConfig;
- objectPose.robotPose = aronObjectPose.robotPose;
-
- objectPose.attachment = fromAron(aronObjectPose.attachment);
-
- objectPose.confidence = aronObjectPose.confidence;
-
- objectPose.timestamp = IceUtil::Time::microSeconds(aronObjectPose.timestamp);
-
- objectPose.localOOBB = fromAron(aronObjectPose.localOOBB);
-
+ case ObjectTypeEnum::AnyObject:
+ dto.value = arondto::ObjectType::AnyObject;
+ return;
+ case ObjectTypeEnum::KnownObject:
+ dto.value = arondto::ObjectType::KnownObject;
+ return;
+ case ObjectTypeEnum::UnknownObject:
+ dto.value = arondto::ObjectType::UnknownObject;
+ return;
}
+ ARMARX_UNEXPECTED_ENUM_VALUE(ObjectTypeEnum, bo);
+}
- aron::ObjectID toAron(const ObjectID& id)
- {
- aron::ObjectID aronId;
+void armarx::objpose::fromAron(const arondto::ObjectPose& dto, ObjectPose& bo)
+{
+ bo.providerName = dto.providerName;
- aronId.className = id.className();
- aronId.dataset = id.dataset();
- aronId.instanceName = id.instanceName();
+ fromAron(dto.objectType, bo.objectType);
+ fromAron(dto.objectID, bo.objectID);
- return aronId;
- }
+ bo.objectPoseRobot = dto.objectPoseRobot;
+ bo.objectPoseGlobal = dto.objectPoseGlobal;
+ bo.objectPoseOriginal = dto.objectPoseOriginal;
+ bo.objectPoseOriginalFrame = dto.objectPoseOriginalFrame;
- aron::ObjectTypes toAron(const ObjectTypeEnum& objectType)
- {
- aron::ObjectTypes ot{};
-
- switch (objectType)
- {
- case ObjectTypeEnum::AnyObject:
- ot = aron::ObjectTypes::ANY_OBJECT;
- break;
- case ObjectTypeEnum::KnownObject:
- ot = aron::ObjectTypes::KNOWN_OBJECT;
- break;
- case ObjectTypeEnum::UnknownObject:
- ot = aron::ObjectTypes::UNKNOWN_OBJECT;
- break;
- }
-
- return ot;
- }
+ bo.robotConfig = dto.robotConfig;
+ bo.robotPose = dto.robotPose;
- aron::ObjectAttachmentInfo toAron(const ObjectAttachmentInfo& info)
+ if (dto.attachmentValid)
{
- aron::ObjectAttachmentInfo aronInfo;
-
- aronInfo.agentName = info.agentName;
- aronInfo.frameName = info.frameName;
- aronInfo.poseInFrame = info.poseInFrame;
-
- return aronInfo;
+ bo.attachment = ObjectAttachmentInfo();
+ fromAron(dto.attachment, *bo.attachment);
}
- aron::OrientedBoundingBox toAron(const simox::OrientedBoxf& box)
- {
- aron::OrientedBoundingBox aronBox;
- aronBox.centerPose = box.transformation();
- aronBox.extends = box.dimensions();
- return aronBox;
- }
+ bo.confidence = dto.confidence;
- template <typename T>
- auto toAron(const std::optional<T>& p)
+ bo.timestamp = IceUtil::Time::microSeconds(dto.timestamp);
+
+ if (dto.localOOBBValid)
{
- if (p)
- {
- return toAron(p.value());
- }
-
- // TODO(fabian.reister): handle optional properly
- // current fallback: default c'tor
- return decltype(toAron(p.value()))();
+ bo.localOOBB = simox::OrientedBoxf();
+ fromAron(dto.localOOBB, *bo.localOOBB);
}
+}
- void toAron(const ObjectPose& objectPose, aron::ObjectPose& aronObjectPose)
- {
- aronObjectPose.providerName = objectPose.providerName;
-
- aronObjectPose.objectType = toAron(objectPose.objectType);
+void armarx::objpose::toAron(arondto::ObjectPose& dto, const ObjectPose& bo)
+{
+ dto.providerName = bo.providerName;
- aronObjectPose.objectID = toAron(objectPose.objectID);
+ toAron(dto.objectType, bo.objectType);
+ toAron(dto.objectID, bo.objectID);
- aronObjectPose.objectPoseRobot = objectPose.objectPoseRobot;
- aronObjectPose.objectPoseGlobal = objectPose.objectPoseGlobal;
- aronObjectPose.objectPoseOriginal = objectPose.objectPoseOriginal;
- aronObjectPose.objectPoseOriginalFrame = objectPose.objectPoseOriginalFrame;
+ dto.objectPoseRobot = bo.objectPoseRobot;
+ dto.objectPoseGlobal = bo.objectPoseGlobal;
+ dto.objectPoseOriginal = bo.objectPoseOriginal;
+ dto.objectPoseOriginalFrame = bo.objectPoseOriginalFrame;
- aronObjectPose.robotConfig = objectPose.robotConfig;
- aronObjectPose.robotPose = objectPose.robotPose;
+ dto.robotConfig = bo.robotConfig;
+ dto.robotPose = bo.robotPose;
- aronObjectPose.attachment = toAron(objectPose.attachment);
+ if (bo.attachment)
+ {
+ dto.attachmentValid = true;
+ toAron(dto.attachment, *bo.attachment);
+ }
- aronObjectPose.confidence = objectPose.confidence;
+ dto.confidence = bo.confidence;
- aronObjectPose.timestamp = objectPose.timestamp.toMicroSeconds();
+ dto.timestamp = bo.timestamp.toMicroSeconds();
- aronObjectPose.localOOBB = toAron(objectPose.localOOBB);
+ if (bo.localOOBB)
+ {
+ dto.localOOBBValid = true;
+ toAron(dto.localOOBB, *bo.localOOBB);
}
+}
-} // namespace armarx::objpose
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions.h b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions.h
index 50e7fc9412d1a30ac166386a29723547ef04cbda..043ef05614aed294872283a96508295b146c8086 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions.h
@@ -1,16 +1,44 @@
#pragma once
+#include <SimoxUtility/shapes/OrientedBox.h>
-namespace armarx::objpose
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectID.aron.generated.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron/OrientedBox.aron.generated.h>
+
+
+namespace simox
+{
+ void fromAron(const arondto::OrientedBox& dto, OrientedBoxf& bo);
+ void toAron(arondto::OrientedBox& dto, const OrientedBoxf& bo);
+}
+
+
+namespace armarx
{
- struct ObjectPose;
+ void fromAron(const arondto::ObjectID& dto, ObjectID& bo);
+ void toAron(arondto::ObjectID& dto, const ObjectID& bo);
+}
- namespace aron
- {
- struct ObjectPose;
- }
- void fromAron(const aron::ObjectPose& aronObjectPose, ObjectPose& objectPose);
- void toAron(const ObjectPose& objectPose, aron::ObjectPose& aronObjectPose);
+namespace armarx
+{
+ void fromAron(const arondto::ObjectID& dto, ObjectID& bo);
+ void toAron(arondto::ObjectID& dto, const ObjectID& bo);
+}
+
+
+namespace armarx::objpose
+{
+ void fromAron(const arondto::ObjectAttachmentInfo& dto, ObjectAttachmentInfo& bo);
+ void toAron(arondto::ObjectAttachmentInfo& dto, const ObjectAttachmentInfo& bo);
+
+ void fromAron(const arondto::ObjectType& dto, ObjectTypeEnum& bo);
+ void toAron(arondto::ObjectType& dto, const ObjectTypeEnum& bo);
-} // namespace armarx::objpose
\ No newline at end of file
+ void fromAron(const arondto::ObjectPose& dto, ObjectPose& bo);
+ void toAron(arondto::ObjectPose& dto, const ObjectPose& bo);
+}