diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp index 6ca0338ce6c4c8edae44bb3248d9d4e8b1ea6c03..19bb9d5aac9be101f1c532279977f58a262eab72 100644 --- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp +++ b/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 } - - - - diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.h index d00e548b82bc9bdd72a46aeb997a6eb297e89ba4..cdfd1cfdece0570cf77162549de5da7d443c0e26 100644 --- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.h +++ b/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);