Add and use toProvidedObjectPose

source/RobotAPI/components/InteractiveMemoryEditor/Editor.cpp

@@ -105,18 +105,10 @@ namespace armarx
                 continue;
             }
 
-            objpose::ProvidedObjectPose& providing = providingPoses.emplace_back();
-            providing.providerName = properties.providerName;
-            providing.objectType = objpose::KnownObject;
-
-            providing.objectID = current.objectID;
-
-            providing.objectPose = current.objectPoseGlobal;
-            providing.objectPoseFrame = GlobalFrame;
+            providingPoses.push_back(current.toProvidedObjectPose());
+            objpose::ProvidedObjectPose& providing = providingPoses.back();
 
-            providing.objectPoseGaussian = current.objectPoseGlobalGaussian;
-
-            providing.confidence = current.confidence;
+            providing.providerName = properties.providerName;
             providing.timestamp = DateTime::Now();
 
             remainingPoses.push_back(current);

source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp

@@ -12,6 +12,7 @@
 #include <RobotAPI/libraries/core/Pose.h>
 #include <RobotAPI/libraries/core/FramedPose.h>
 #include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+#include <RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h>
 
 #include "ice_conversions.h"
 
@@ -171,6 +172,30 @@ namespace armarx::objpose
         objpose::fromIce(provided.localOOBB, localOOBB);
     }
 
+    objpose::ProvidedObjectPose ObjectPose::toProvidedObjectPose() const
+    {
+        objpose::ProvidedObjectPose providedObjectPose;
+
+        providedObjectPose.objectID = this->objectID;
+        providedObjectPose.providerName = this->providerName;
+        providedObjectPose.objectType = this->objectType;
+
+        providedObjectPose.isStatic = this->isStatic;
+
+        providedObjectPose.objectPoseFrame = GlobalFrame;
+        providedObjectPose.objectPose = this->objectPoseGlobal;
+        providedObjectPose.objectPoseGaussian = this->objectPoseGlobalGaussian;
+
+        providedObjectPose.objectJointValues = this->objectJointValues;
+
+        providedObjectPose.confidence = this->confidence;
+        providedObjectPose.timestamp = this->timestamp;
+
+        providedObjectPose.localOOBB = this->localOOBB;
+
+        return providedObjectPose;
+    }
+
     void ObjectPose::setObjectPoseRobot(
         const Eigen::Matrix4f& objectPoseRobot, bool updateObjectPoseGlobal)
     {

source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h

@@ -46,6 +46,7 @@ namespace armarx::objpose
         void toIce(data::ObjectPose& ice) const;
 
         void fromProvidedPose(const data::ProvidedObjectPose& provided, VirtualRobot::RobotPtr robot = nullptr);
+        objpose::ProvidedObjectPose toProvidedObjectPose() const;
 
         void setObjectPoseRobot(const Eigen::Matrix4f& objectPoseRobot, bool updateObjectPoseGlobal = true);
         void setObjectPoseGlobal(const Eigen::Matrix4f& objectPoseGlobal, bool updateObjectPoseRobot = true);