Move ProvidedObjectPose and ObjectPose to data:: namespace and add C++ type for ObjectPose

source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt

@@ -15,6 +15,7 @@ set(SOURCES
 
     ObjectFinder.cpp
     ice_conversions.cpp
+    ObjectPose.cpp
 )
 set(HEADERS
     ObjectPoseObserver.h
@@ -22,6 +23,7 @@ set(HEADERS
 
     ObjectFinder.h
     ice_conversions.h
+    ObjectPose.h
 )
 
 

source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp

+#include "ObjectPose.h"
+
+#include <RobotAPI/libraries/core/Pose.h>
+
+
+namespace armarx::objpose
+{
+    ObjectPose::ObjectPose()
+    {
+    }
+
+    ObjectPose::ObjectPose(const data::ObjectPose& ice)
+    {
+        fromIce(ice);
+    }
+
+    void ObjectPose::fromIce(const data::ObjectPose& ice)
+    {
+        providerName = ice.providerName;
+        objectType = ice.objectType;
+        objectID = ice.objectID;
+
+        objectPoseRobot = PosePtr::dynamicCast(ice.objectPoseRobot)->toEigen();
+        objectPoseGlobal = PosePtr::dynamicCast(ice.objectPoseGlobal)->toEigen();
+        objectPoseOriginal = PosePtr::dynamicCast(ice.objectPoseOriginal)->toEigen();
+        objectPoseOriginalFrame = ice.objectPoseOriginalFrame;
+
+        robotConfig = ice.robotConfig;
+        robotPose = PosePtr::dynamicCast(ice.robotPose)->toEigen();
+
+        confidence = ice.confidence;
+        timestamp = IceUtil::Time::microSeconds(ice.timestampMicroSeconds);
+
+        localOOBB = ice.localOOBB;
+    }
+
+    data::ObjectPose ObjectPose::toIce() const
+    {
+        data::ObjectPose ice;
+        toIce(ice);
+        return ice;
+    }
+
+    void ObjectPose::toIce(data::ObjectPose& ice) const
+    {
+        ice.providerName = providerName;
+        ice.objectType = objectType;
+        ice.objectID = objectID;
+
+        ice.objectPoseRobot = new Pose(objectPoseRobot);
+        ice.objectPoseGlobal = new Pose(objectPoseGlobal);
+        ice.objectPoseOriginal = new Pose(objectPoseOriginal);
+        ice.objectPoseOriginalFrame = objectPoseOriginalFrame;
+
+        ice.robotConfig = robotConfig;
+        ice.robotPose = new Pose(robotPose);
+
+        ice.confidence = confidence;
+        ice.timestampMicroSeconds = timestamp.toMicroSeconds();
+
+        ice.localOOBB = localOOBB;
+    }
+
+
+    void fromIce(const data::ObjectPose& ice, ObjectPose& pose)
+    {
+        pose.fromIce(ice);
+    }
+    ObjectPose fromIce(const data::ObjectPose& ice)
+    {
+        return ObjectPose(ice);
+    }
+
+    void fromIce(const data::ObjectPoseSeq& ice, ObjectPoseSeq& poses)
+    {
+        poses.clear();
+        poses.reserve(ice.size());
+        for (const auto& i : ice)
+        {
+            poses.emplace_back(i);
+        }
+    }
+    ObjectPoseSeq fromIce(const data::ObjectPoseSeq& ice)
+    {
+        ObjectPoseSeq poses;
+        fromIce(ice, poses);
+        return poses;
+    }
+
+
+    void toIce(data::ObjectPose& ice, const ObjectPose& pose)
+    {
+        pose.toIce(ice);
+    }
+    data::ObjectPose toIce(const ObjectPose& pose)
+    {
+        return pose.toIce();
+    }
+
+    void toIce(const ObjectPoseSeq& poses, data::ObjectPoseSeq& ice)
+    {
+        ice.clear();
+        ice.reserve(poses.size());
+        for (const auto& p : poses)
+        {
+            ice.emplace_back(p.toIce());
+        }
+    }
+    data::ObjectPoseSeq toIce(const ObjectPoseSeq& poses)
+    {
+        data::ObjectPoseSeq ice;
+        toIce(ice, poses);
+        return ice;
+    }
+
+}
+

source/RobotAPI/components/ObjectPoseObserver/ObjectPose.h

+#pragma once
+
+#include <IceUtil/Time.h>
+
+#include <Eigen/Core>
+
+#include <RobotAPI/interface/objectpose/types.h>
+
+
+namespace armarx::objpose
+{
+
+    /**
+     * @brief An object pose as stored by the ObjectPoseObserver.
+     */
+    struct ObjectPose
+    {
+        ObjectPose();
+        ObjectPose(const data::ObjectPose& ice);
+
+        void fromIce(const data::ObjectPose& ice);
+
+        data::ObjectPose toIce() const;
+        void toIce(data::ObjectPose& ice) const;
+
+
+        /// Name of the providing component.
+        std::string providerName;
+        /// Known or unknown object.
+        ObjectTypeEnum objectType = AnyObject;
+
+        /// The object ID, i.e. dataset and name.
+        ObjectID objectID;
+
+        Eigen::Matrix4f objectPoseRobot;
+        Eigen::Matrix4f objectPoseGlobal;
+        Eigen::Matrix4f objectPoseOriginal;
+        std::string objectPoseOriginalFrame;
+
+        std::map<std::string, float> robotConfig;
+        Eigen::Matrix4f robotPose;
+
+        /// Confidence in [0, 1] (1 = full, 0 = none).
+        float confidence = 0;
+        /// Source timestamp.
+        IceUtil::Time timestamp = IceUtil::Time::microSeconds(-1);
+
+        /// Object bounding box in object's local coordinate frame.
+        Box localOOBB;
+    };
+    using ObjectPoseSeq = std::vector<ObjectPose>;
+
+
+    void fromIce(const data::ObjectPose& ice, ObjectPose& pose);
+    ObjectPose fromIce(const data::ObjectPose& ice);
+
+    void fromIce(const data::ObjectPoseSeq& ice, ObjectPoseSeq& poses);
+    ObjectPoseSeq fromIce(const data::ObjectPoseSeq& ice);
+
+
+    void toIce(data::ObjectPose& ice, const ObjectPose& pose);
+    data::ObjectPose toIce(const ObjectPose& pose);
+
+    void toIce(data::ObjectPoseSeq& ice, const ObjectPoseSeq& poses);
+    data::ObjectPoseSeq toIce(const ObjectPoseSeq& poses);
+
+}

source/RobotAPI/interface/objectpose/types.ice

@@ -58,61 +58,63 @@ module armarx
         };
 
 
-        /// An object pose provided by an ObjectPoseProvider.
-        struct ProvidedObjectPose
+        module data
         {
-            /// Name of the providing component.
-            string providerName;
-            /// Known or unknown object.
-            ObjectTypeEnum objectType = AnyObject;
-
-            /// The object ID, i.e. dataset and name.
-            ObjectID objectID;
-
-            /// Pose in `objectPoseFrame`.
-            PoseBase objectPose;
-            string objectPoseFrame;
-
-            /// Confidence in [0, 1] (1 = full, 0 = none).
-            float confidence = 0;
-            /// Source timestamp.
-            long timestampMicroSeconds = -1;
-
-            /// Object bounding box in object's local coordinate frame.
-            Box localOOBB;
-        };
-        sequence<ProvidedObjectPose> ProvidedObjectPoseSeq;
-
-
-        /// An object pose as stored by the ObjectPoseObserver.
-        struct ObjectPose
-        {
-            /// Name of the providing component.
-            string providerName;
-            /// Known or unknown object.
-            ObjectTypeEnum objectType = AnyObject;
-
-            /// The object ID, i.e. dataset and name.
-            ObjectID objectID;
-
-            PoseBase objectPoseRobot;
-            PoseBase objectPoseGlobal;
-            PoseBase objectPoseOriginal;
-            string objectPoseOriginalFrame;
-
-            StringFloatDictionary robotConfig;
-            PoseBase robotPose;
-
-            /// Confidence in [0, 1] (1 = full, 0 = none).
-            float confidence = 0;
-            /// Source timestamp.
-            long timestampMicroSeconds = -1;
-
-            /// Object bounding box in object's local coordinate frame.
-            Box localOOBB;
-        };
-        sequence<ObjectPose> ObjectPoseSeq;
-
+            /// An object pose provided by an ObjectPoseProvider.
+            struct ProvidedObjectPose
+            {
+                /// Name of the providing component.
+                string providerName;
+                /// Known or unknown object.
+                ObjectTypeEnum objectType = AnyObject;
+
+                /// The object ID, i.e. dataset and name.
+                ObjectID objectID;
+
+                /// Pose in `objectPoseFrame`.
+                PoseBase objectPose;
+                string objectPoseFrame;
+
+                /// Confidence in [0, 1] (1 = full, 0 = none).
+                float confidence = 0;
+                /// Source timestamp.
+                long timestampMicroSeconds = -1;
+
+                /// Object bounding box in object's local coordinate frame.
+                Box localOOBB;
+            };
+            sequence<ProvidedObjectPose> ProvidedObjectPoseSeq;
+
+
+            /// An object pose as stored by the ObjectPoseObserver.
+            struct ObjectPose
+            {
+                /// Name of the providing component.
+                string providerName;
+                /// Known or unknown object.
+                ObjectTypeEnum objectType = AnyObject;
+
+                /// The object ID, i.e. dataset and name.
+                ObjectID objectID;
+
+                PoseBase objectPoseRobot;
+                PoseBase objectPoseGlobal;
+                PoseBase objectPoseOriginal;
+                string objectPoseOriginalFrame;
+
+                StringFloatDictionary robotConfig;
+                PoseBase robotPose;
+
+                /// Confidence in [0, 1] (1 = full, 0 = none).
+                float confidence = 0;
+                /// Source timestamp.
+                long timestampMicroSeconds = -1;
+
+                /// Object bounding box in object's local coordinate frame.
+                Box localOOBB;
+            };
+            sequence<ObjectPose> ObjectPoseSeq;
+        }
 
     };
 };