Add C++ type for ProvidedObjectPose

source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt

@@ -17,6 +17,8 @@ set(LIB_FILES
     ObjectFinder.cpp
     ObjectPose.cpp
     ObjectPoseClient.cpp
+    PoseCovariance.cpp
+    ProvidedObjectPose.cpp
 
     json_conversions.cpp
     ice_conversions.cpp
@@ -35,7 +37,10 @@ set(LIB_HEADERS
     ObjectFinder.h
     ObjectPose.h
     ObjectPoseClient.h
+    PoseCovariance.h
+    ProvidedObjectPose.h
 
+    forward_declarations.h
     json_conversions.h
     ice_conversions.h
 

source/RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.cpp

+#include "ProvidedObjectPose.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/core/Pose.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+
+#include "ice_conversions.h"
+
+
+namespace armarx::objpose
+{
+    ProvidedObjectPose::ProvidedObjectPose()
+    {
+    }
+
+    ProvidedObjectPose::ProvidedObjectPose(const data::ProvidedObjectPose& ice)
+    {
+        fromIce(ice);
+    }
+
+    void ProvidedObjectPose::fromIce(const data::ProvidedObjectPose& ice)
+    {
+        providerName = ice.providerName;
+        objectType = ice.objectType;
+        isStatic = ice.isStatic;
+        armarx::fromIce(ice.objectID, objectID);
+
+        armarx::fromIce(ice.objectPose, objectPose);
+        objectPoseFrame = ice.objectPoseFrame;
+        objpose::fromIce(ice.objectPoseCovariance, objectPoseCovariance);
+
+        objectJointValues = ice.objectJointValues;
+
+        confidence = ice.confidence;
+        timestamp = IceUtil::Time::microSeconds(ice.timestampMicroSeconds);
+
+        objpose::fromIce(ice.localOOBB, localOOBB);
+    }
+
+    data::ProvidedObjectPose ProvidedObjectPose::toIce() const
+    {
+        data::ProvidedObjectPose ice;
+        toIce(ice);
+        return ice;
+    }
+
+    void ProvidedObjectPose::toIce(data::ProvidedObjectPose& ice) const
+    {
+        ice.providerName = providerName;
+        ice.objectType = objectType;
+        ice.isStatic = isStatic;
+        armarx::toIce(ice.objectID, objectID);
+
+        armarx::toIce(ice.objectPose, objectPose);
+        ice.objectPoseFrame = objectPoseFrame;
+        objpose::toIce(ice.objectPoseCovariance, objectPoseCovariance);
+
+        ice.objectJointValues = objectJointValues;
+
+        ice.confidence = confidence;
+        ice.timestampMicroSeconds = timestamp.toMicroSeconds();
+
+        objpose::toIce(ice.localOOBB, localOOBB);
+    }
+}
+
+namespace armarx
+{
+
+    void objpose::fromIce(const data::ProvidedObjectPose& ice, ProvidedObjectPose& pose)
+    {
+        pose.fromIce(ice);
+    }
+    objpose::ProvidedObjectPose objpose::fromIce(const data::ProvidedObjectPose& ice)
+    {
+        return ProvidedObjectPose(ice);
+    }
+
+    void objpose::fromIce(const data::ProvidedObjectPoseSeq& ice, ProvidedObjectPoseSeq& poses)
+    {
+        poses.clear();
+        poses.reserve(ice.size());
+        for (const auto& i : ice)
+        {
+            poses.emplace_back(i);
+        }
+    }
+
+    objpose::ProvidedObjectPoseSeq objpose::fromIce(const data::ProvidedObjectPoseSeq& ice)
+    {
+        ProvidedObjectPoseSeq poses;
+        fromIce(ice, poses);
+        return poses;
+    }
+
+
+    void objpose::toIce(data::ProvidedObjectPose& ice, const ProvidedObjectPose& pose)
+    {
+        pose.toIce(ice);
+    }
+
+    objpose::data::ProvidedObjectPose objpose::toIce(const ProvidedObjectPose& pose)
+    {
+        return pose.toIce();
+    }
+
+
+    void objpose::toIce(data::ProvidedObjectPoseSeq& ice, const ProvidedObjectPoseSeq& poses)
+    {
+        ice.clear();
+        ice.reserve(poses.size());
+        for (const auto& p : poses)
+        {
+            ice.emplace_back(p.toIce());
+        }
+    }
+
+    objpose::data::ProvidedObjectPoseSeq objpose::toIce(const ProvidedObjectPoseSeq& poses)
+    {
+        data::ProvidedObjectPoseSeq ice;
+        toIce(ice, poses);
+        return ice;
+    }
+
+}
+
+
+

source/RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h

+#pragma once
+
+#include <optional>
+#include <map>
+#include <vector>
+
+#include <Eigen/Core>
+
+#include <SimoxUtility/shapes/OrientedBox.h>
+
+#include <IceUtil/Time.h>
+
+#include <RobotAPI/interface/objectpose/object_pose_types.h>
+#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+#include <RobotAPI/libraries/ArmarXObjects/PoseCovariance.h>
+
+
+namespace armarx::objpose
+{
+
+    /**
+     * @brief An object pose provided by an ObjectPoseProvider.
+     */
+    class ProvidedObjectPose
+    {
+    public:
+
+        ProvidedObjectPose();
+        ProvidedObjectPose(const data::ProvidedObjectPose& ice);
+
+        void fromIce(const data::ProvidedObjectPose& ice);
+
+        data::ProvidedObjectPose toIce() const;
+        void toIce(data::ProvidedObjectPose& ice) const;
+
+    public:
+
+        /// Name of the providing component.
+        std::string providerName;
+        /// Known or unknown object.
+        ObjectType objectType = AnyObject;
+        /// Whether object is static. Static objects don't decay.
+        bool isStatic = false;
+
+        /// The object ID, i.e. dataset, class name and instance name.
+        armarx::ObjectID objectID;
+
+        Eigen::Matrix4f objectPose;
+        std::string objectPoseFrame;
+        std::optional<PoseCovariance> objectPoseCovariance;
+
+        /// The object's joint values if it is articulated.
+        std::map<std::string, float> objectJointValues;
+
+
+        /// 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.
+        /// @see oobbRobot(), oobbGlobal()
+        std::optional<simox::OrientedBoxf> localOOBB;
+    };
+
+
+
+    void fromIce(const data::ProvidedObjectPose& ice, ProvidedObjectPose& pose);
+    ProvidedObjectPose fromIce(const data::ProvidedObjectPose& ice);
+
+    void fromIce(const data::ProvidedObjectPoseSeq& ice, ProvidedObjectPoseSeq& poses);
+    ProvidedObjectPoseSeq fromIce(const data::ProvidedObjectPoseSeq& ice);
+
+
+    void toIce(data::ProvidedObjectPose& ice, const ProvidedObjectPose& pose);
+    data::ProvidedObjectPose toIce(const ProvidedObjectPose& pose);
+
+    void toIce(data::ProvidedObjectPoseSeq& ice, const ProvidedObjectPoseSeq& poses);
+    data::ProvidedObjectPoseSeq toIce(const ProvidedObjectPoseSeq& poses);
+
+
+}