aron conversion

source/RobotAPI/libraries/ArmarXObjects/aron_conversions.cpp

 #include "aron_conversions.h"
 
+#include <SimoxUtility/shapes/OrientedBox.h>
+#include <exception>
 
+#include <RobotAPI/interface/objectpose/object_pose_types.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
+#include <stdexcept>
+#include <string>
 
 namespace armarx::objpose
 {
+
+    ObjectID fromAron(const aron::ObjectID& id)
+    {
+        return ObjectID(id.dataset, id.className, id.instanceName);
+    }
+
+    ObjectTypeEnum fromAron(const aron::ObjectTypeEnum& objectType)
+    {
+        switch (objectType.value)
+        {
+            case aron::ObjectTypeEnum::ANY_OBJECT:
+                return ObjectTypeEnum::AnyObject;
+            case aron::ObjectTypeEnum::KNOWN_OBJECT:
+                return ObjectTypeEnum::KnownObject;
+            case aron::ObjectTypeEnum::UNKNOWN_OBJECT:
+                return ObjectTypeEnum::UnknownObject;
+            default:
+                // TODO(fabian.reister): do it the ARMARX way ...
+                throw std::invalid_argument("Invalid ObjectTypeEnum value: " + std::to_string(objectType.value));
+        }
+    }
+
+    simox::OrientedBoxf fromAron(const aron::OrientedBoundingBox& obb)
+    {
+        return simox::OrientedBoxf(obb.centerPose, obb.extends);
+    }
+
+    ObjectAttachmentInfo fromAron(const aron::ObjectAttachmentInfo& info)
+    {
+        return
+        {
+            .frameName = info.frameName,
+            .agentName = info.agentName,
+            .poseInFrame = info.poseInFrame
+        };
+    }
+
     void fromAron(const aron::ObjectPose& aronObjectPose, ObjectPose& objectPose)
     {
+        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;
+
+        // TODO(fabian.reister): change timestamp type
+        // objectPose.timestamp = fromAron(aronObjectPose.timestampTODOtimestamptype);
+
+        objectPose.localOOBB = fromAron(aronObjectPose.localOOBB);
+
+    }
+
+
+    aron::ObjectID toAron(const ObjectID& id)
+    {
+        aron::ObjectID aronId;
+        aronId.className = id.className();
+        aronId.dataset = id.dataset();
+        aronId.instanceName = id.instanceName();
+        return aronId;
+    }
+
+    decltype(aron::ObjectTypeEnum::value) toAronValue(const ObjectTypeEnum& objectType)
+    {
+        switch (objectType)
+        {
+            case ObjectTypeEnum::AnyObject:
+                return aron::ObjectTypeEnum::ANY_OBJECT;
+            case ObjectTypeEnum::KnownObject:
+                return aron::ObjectTypeEnum::KNOWN_OBJECT;
+            case ObjectTypeEnum::UnknownObject:
+                return aron::ObjectTypeEnum::UNKNOWN_OBJECT;
+            default:
+                // TODO(fabian.reister): do it the ARMARX way ...
+                throw std::invalid_argument("Invalid ObjectTypeEnum value: " + std::to_string(objectType));
+                return -1; // oh gcc ...
+        }
+    }
+
+    aron::ObjectAttachmentInfo toAron(const ObjectAttachmentInfo& info)
+    {
+        aron::ObjectAttachmentInfo aronInfo;
+        aronInfo.agentName = info.agentName;
+        aronInfo.frameName = info.frameName;
+        aronInfo.poseInFrame = info.poseInFrame;
+        return aronInfo;
+    }
+
+    aron::OrientedBoundingBox toAron(const simox::OrientedBoxf& box)
+    {
+        aron::OrientedBoundingBox aronBox;
+        aronBox.centerPose = box.transformation();
+        aronBox.extends = box.dimensions();
+        return aronBox;
+    }
+
+    template <typename T>
+    auto toAron(const std::optional<T>& p)
+    {
+        if (p)
+        {
+            return toAron(p.value());
+        }
+
+        // TODO(fabian.reister): handle optional properly
+        // current fallback: default c'tor
+        return decltype(toAron(p.value()))();
     }
-    
-    
+
+
     void toAron(const ObjectPose& objectPose, aron::ObjectPose& aronObjectPose)
     {
+        aronObjectPose.providerName = objectPose.providerName;
+
+        aronObjectPose.objectType.value = toAronValue(objectPose.objectType);
+
+        aronObjectPose.objectID = toAron(objectPose.objectID);
+
+        aronObjectPose.objectPoseRobot = objectPose.objectPoseRobot;
+        aronObjectPose.objectPoseGlobal = objectPose.objectPoseGlobal;
+        aronObjectPose.objectPoseOriginal = objectPose.objectPoseOriginal;
+        aronObjectPose.objectPoseOriginalFrame = objectPose.objectPoseOriginalFrame;
+
+        aronObjectPose.robotConfig = objectPose.robotConfig;
+        aronObjectPose.robotPose = objectPose.robotPose;
+
+        aronObjectPose.attachment = toAron(objectPose.attachment);
+
+        aronObjectPose.confidence = objectPose.confidence;
+
+        // TODO(fabian.reister): change timestamp type
+        // aronObjectPose.timestamp = fromAron(aronObjectPose.timestampTODOtimestamptype);
 
+        aronObjectPose.localOOBB = toAron(objectPose.localOOBB);
     }
 
 } // namespace armarx::objpose
\ No newline at end of file