diff --git a/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt b/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt
index d5760158331a6f3dc664f3116a9f571e6830d37d..b1644c6be2adf25870aa73f2bc63849550188771 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt
+++ b/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
)
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f7ca54aae57a8c789cb5b425dd5408308542a391
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp
@@ -0,0 +1,117 @@
+#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;
+ }
+
+}
+
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.h
new file mode 100644
index 0000000000000000000000000000000000000000..76be353c997825fbd2a15a19fa820cfd8021f74a
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.h
@@ -0,0 +1,67 @@
+#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);
+
+}
diff --git a/source/RobotAPI/interface/objectpose/types.ice b/source/RobotAPI/interface/objectpose/types.ice
index 16f28f3d5a4c74d8f7e62cbeb9452bf5e94c7e83..eaf7adf779b0ac6d51c952f343f08d75bd87d93c 100644
--- a/source/RobotAPI/interface/objectpose/types.ice
+++ b/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;
+ }
};
};