diff --git a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt index b2e71660ae1552d33eeaddb37f93a6f6b5d72491..e5f0ac6000c424874a81b987ec550eb7c4d5f1db 100644 --- a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt +++ b/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 diff --git a/source/RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.cpp b/source/RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.cpp new file mode 100644 index 0000000000000000000000000000000000000000..8de3f49520155f16b91fe586719f6c06db8fbb47 --- /dev/null +++ b/source/RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.cpp @@ -0,0 +1,130 @@ +#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; + } + +} + + + diff --git a/source/RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h b/source/RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h new file mode 100644 index 0000000000000000000000000000000000000000..b7ecab4d585a0d6c38a4abef0e2840fdcfa9c310 --- /dev/null +++ b/source/RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h @@ -0,0 +1,83 @@ +#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); + + +}