Add ObjectAttachmentInfo to ObjectPose in C++

source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp

@@ -56,6 +56,8 @@ namespace armarx::objpose
         robotConfig = ice.robotConfig;
         robotPose = armarx::objpose::toEigen(ice.robotPose);
 
+        objpose::fromIce(ice.attachment, this->attachment);
+
         confidence = ice.confidence;
         timestamp = IceUtil::Time::microSeconds(ice.timestampMicroSeconds);
 
@@ -83,6 +85,8 @@ namespace armarx::objpose
         ice.robotConfig = robotConfig;
         ice.robotPose = new Pose(robotPose);
 
+        objpose::toIce(ice.attachment, this->attachment);
+
         ice.confidence = confidence;
         ice.timestampMicroSeconds = timestamp.toMicroSeconds();
 
@@ -107,6 +111,8 @@ namespace armarx::objpose
         robotConfig = robot->getConfig()->getRobotNodeJointValueMap();
         robotPose = robot->getGlobalPose();
 
+        attachment = std::nullopt;
+
         confidence = provided.confidence;
         timestamp = IceUtil::Time::microSeconds(provided.timestampMicroSeconds);
 
@@ -137,6 +143,70 @@ namespace armarx::objpose
 namespace armarx
 {
 
+    void objpose::fromIce(const data::ObjectAttachmentInfo& ice, ObjectAttachmentInfo& attachment)
+    {
+        attachment.agentName = ice.agentName;
+        attachment.frameName = ice.frameName;
+        attachment.poseInFrame = toEigen(ice.poseInFrame);
+    }
+
+    void objpose::fromIce(const data::ObjectAttachmentInfoPtr& ice, std::optional<ObjectAttachmentInfo>& attachment)
+    {
+        if (ice)
+        {
+            attachment = ObjectAttachmentInfo();
+            fromIce(*ice, *attachment);
+        }
+        else
+        {
+            attachment = std::nullopt;
+        }
+    }
+
+    std::optional<objpose::ObjectAttachmentInfo> objpose::fromIce(const data::ObjectAttachmentInfoPtr& ice)
+    {
+        if (!ice)
+        {
+            return std::nullopt;
+        }
+        objpose::ObjectAttachmentInfo info;
+        fromIce(*ice, info);
+        return info;
+    }
+
+
+    void objpose::toIce(data::ObjectAttachmentInfo& ice, const ObjectAttachmentInfo& attachment)
+    {
+        ice.agentName = attachment.agentName;
+        ice.frameName = attachment.frameName;
+        ice.poseInFrame = new Pose(attachment.poseInFrame);
+    }
+
+    void objpose::toIce(data::ObjectAttachmentInfoPtr& ice, const std::optional<ObjectAttachmentInfo>& attachment)
+    {
+        if (attachment)
+        {
+            ice = new objpose::data::ObjectAttachmentInfo();
+            toIce(*ice, *attachment);
+        }
+        else
+        {
+            ice = nullptr;
+        }
+    }
+
+    objpose::data::ObjectAttachmentInfoPtr objpose::toIce(const std::optional<ObjectAttachmentInfo>& attachment)
+    {
+        if (!attachment)
+        {
+            return nullptr;
+        }
+        objpose::data::ObjectAttachmentInfoPtr ice = new objpose::data::ObjectAttachmentInfo();
+        toIce(*ice, *attachment);
+        return ice;
+    }
+
+
     void objpose::fromIce(const data::ObjectPose& ice, ObjectPose& pose)
     {
         pose.fromIce(ice);
@@ -190,7 +260,3 @@ namespace armarx
 
 }
 
-
-
-
-

source/RobotAPI/components/ObjectPoseObserver/ObjectPose.h

@@ -16,6 +16,14 @@
 namespace armarx::objpose
 {
 
+    struct ObjectAttachmentInfo
+    {
+        std::string frameName;
+        std::string agentName;
+        Eigen::Matrix4f poseInFrame;
+    };
+
+
     /**
      * @brief An object pose as stored by the ObjectPoseObserver.
      */
@@ -48,6 +56,8 @@ namespace armarx::objpose
         std::map<std::string, float> robotConfig;
         Eigen::Matrix4f robotPose;
 
+        std::optional<ObjectAttachmentInfo> attachment;
+
         /// Confidence in [0, 1] (1 = full, 0 = none).
         float confidence = 0;
         /// Source timestamp.
@@ -67,6 +77,10 @@ namespace armarx::objpose
     using ObjectPoseSeq = std::vector<ObjectPose>;
 
 
+    void fromIce(const data::ObjectAttachmentInfo& ice, ObjectAttachmentInfo& attachment);
+    void fromIce(const data::ObjectAttachmentInfoPtr& ice, std::optional<ObjectAttachmentInfo>& attachment);
+    std::optional<ObjectAttachmentInfo> fromIce(const data::ObjectAttachmentInfoPtr& ice);
+
     void fromIce(const data::ObjectPose& ice, ObjectPose& pose);
     ObjectPose fromIce(const data::ObjectPose& ice);
 
@@ -74,6 +88,10 @@ namespace armarx::objpose
     ObjectPoseSeq fromIce(const data::ObjectPoseSeq& ice);
 
 
+    void toIce(data::ObjectAttachmentInfo& ice, const ObjectAttachmentInfo& attachment);
+    void toIce(data::ObjectAttachmentInfoPtr& ice, const std::optional<ObjectAttachmentInfo>& attachment);
+    data::ObjectAttachmentInfoPtr toIce(const std::optional<ObjectAttachmentInfo>& ice);
+
     void toIce(data::ObjectPose& ice, const ObjectPose& pose);
     data::ObjectPose toIce(const ObjectPose& pose);