diff --git a/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt b/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt
index d5760158331a6f3dc664f3116a9f571e6830d37d..a2587dc0c09e5329484dceb1838ea097e016283f 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt
+++ b/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt
@@ -11,17 +11,23 @@ set(COMPONENT_LIBS
set(SOURCES
ObjectPoseObserver.cpp
- ObjectPoseProviderPlugin.cpp
+
+ plugins/ObjectPoseProviderPlugin.cpp
+ plugins/ObjectPoseClientPlugin.cpp
ObjectFinder.cpp
ice_conversions.cpp
+ ObjectPose.cpp
)
set(HEADERS
ObjectPoseObserver.h
- ObjectPoseProviderPlugin.h
+
+ plugins/ObjectPoseProviderPlugin.h
+ plugins/ObjectPoseClientPlugin.h
ObjectFinder.h
ice_conversions.h
+ ObjectPose.h
)
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.cpp
index 4779f70a41dda97a309ed7606b37f192f142a29a..eef7faa2231ea91333ed9b4da81ff402f74a1cdc 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.cpp
@@ -3,6 +3,7 @@
#include <SimoxUtility/filesystem/list_directory.h>
#include <SimoxUtility/json.h>
#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
+#include <SimoxUtility/shapes/OrientedBox.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
@@ -85,17 +86,29 @@ namespace armarx
std::vector<std::string> ObjectFinder::getDatasets() const
{
- init();
- const bool local = true;
- std::vector<path> paths = simox::fs::list_directory(_rootDirAbs(), local);
- return std::vector<std::string>(paths.begin(), paths.end());
+ // init(); // Done by called methods.
+ std::vector<std::string> datasets;
+ for (const auto& dir : getDatasetDirectories())
+ {
+ datasets.push_back(dir.filename());
+ }
+ return datasets;
}
std::vector<ObjectFinder::path> ObjectFinder::getDatasetDirectories() const
{
init();
const bool local = false;
- return simox::fs::list_directory(_rootDirAbs(), local);
+ std::vector<path> dirs = simox::fs::list_directory(_rootDirAbs(), local);
+ std::vector<path> datasetDirs;
+ for (const auto& p : dirs)
+ {
+ if (std::filesystem::is_directory(p))
+ {
+ datasetDirs.push_back(p);
+ }
+ }
+ return datasetDirs;
}
std::vector<ObjectInfo> ObjectFinder::findAllObjects(bool checkPaths) const
@@ -117,6 +130,18 @@ namespace armarx
return objects;
}
+ std::map<std::string, std::vector<ObjectInfo>>
+ ObjectFinder::findAllObjectsByDataset(bool checkPaths) const
+ {
+ // init(); // Done by called methods.
+ std::map<std::string, std::vector<ObjectInfo>> objects;
+ for (const std::string& dataset : getDatasets())
+ {
+ objects[dataset] = findAllObjectsOfDataset(dataset, checkPaths);
+ }
+ return objects;
+ }
+
std::vector<ObjectInfo> ObjectFinder::findAllObjectsOfDataset(const std::string& dataset, bool checkPaths) const
{
init();
@@ -227,6 +252,23 @@ namespace armarx
return simox::AxisAlignedBoundingBox(min, max);
}
+ simox::OrientedBox<float> ObjectInfo::oobb() const
+ {
+ nlohmann::json j = nlohmann::read_json(boundingBoxJson().absolutePath);
+ nlohmann::json joobb = j.at("oobb");
+ auto ori = joobb.at("ori").get<Eigen::Quaternionf>().toRotationMatrix();
+ auto min = joobb.at("min").get<Eigen::Vector3f>();
+ auto extents = joobb.at("extents").get<Eigen::Vector3f>();
+
+ Eigen::Vector3f corner = ori * min;
+
+ simox::OrientedBox<float> oobb(corner,
+ ori.col(0) * extents(0),
+ ori.col(1) * extents(1),
+ ori.col(2) * extents(2));
+ return oobb;
+ }
+
bool ObjectInfo::checkPaths() const
{
namespace fs = std::filesystem;
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.h
index e7e8fdf98b9669d2f52ac746200d53113a3c7333..7bbaa7c2282029ed78399027154f392f84ccad8a 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.h
@@ -7,6 +7,7 @@
namespace simox
{
struct AxisAlignedBoundingBox;
+ template<class FloatT> class OrientedBox;
}
namespace armarx
@@ -37,6 +38,7 @@ namespace armarx
std::vector<path> getDatasetDirectories() const;
std::vector<ObjectInfo> findAllObjects(bool checkPaths = true) const;
+ std::map<std::string, std::vector<ObjectInfo>> findAllObjectsByDataset(bool checkPaths = true) const;
std::vector<ObjectInfo> findAllObjectsOfDataset(const std::string& dataset, bool checkPaths = true) const;
@@ -99,6 +101,7 @@ namespace armarx
PackageFileLocation boundingBoxJson() const;
simox::AxisAlignedBoundingBox aabb() const;
+ simox::OrientedBox<float> oobb() const;
/**
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..dd24a1ecc8cc9cf390684902112e5ac44a49c624
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp
@@ -0,0 +1,160 @@
+#include "ObjectPose.h"
+
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotConfig.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/core/Pose.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+
+
+namespace armarx::objpose
+{
+
+ Eigen::Matrix4f toEigen(const PoseBasePtr pose)
+ {
+ auto cast = PosePtr::dynamicCast(pose);
+ ARMARX_CHECK_NOT_NULL(cast);
+ return cast->toEigen();
+ }
+
+ 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 = toEigen(ice.objectPoseRobot);
+ objectPoseGlobal = toEigen(ice.objectPoseGlobal);
+ objectPoseOriginal = toEigen(ice.objectPoseOriginal);
+ objectPoseOriginalFrame = ice.objectPoseOriginalFrame;
+
+ robotConfig = ice.robotConfig;
+ robotPose = toEigen(ice.robotPose);
+
+ 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 ObjectPose::fromProvidedPose(const data::ProvidedObjectPose& provided, VirtualRobot::RobotPtr robot)
+ {
+ providerName = provided.providerName;
+ objectType = provided.objectType;
+
+ objectID = provided.objectID;
+
+ objectPoseOriginal = toEigen(provided.objectPose);
+ objectPoseOriginalFrame = provided.objectPoseFrame;
+
+ armarx::FramedPose framed(objectPoseOriginal, objectPoseOriginalFrame, robot->getName());
+ framed.changeFrame(robot, robot->getRootNode()->getName());
+ objectPoseRobot = framed.toEigen();
+ framed.changeToGlobal(robot);
+ objectPoseGlobal = framed.toEigen();
+
+ robotConfig = robot->getConfig()->getRobotNodeJointValueMap();
+ robotPose = robot->getGlobalPose();
+
+ confidence = provided.confidence;
+ timestamp = IceUtil::Time::microSeconds(provided.timestampMicroSeconds);
+
+ localOOBB = provided.localOOBB;
+ }
+
+}
+
+namespace armarx
+{
+
+ void objpose::fromIce(const data::ObjectPose& ice, ObjectPose& pose)
+ {
+ pose.fromIce(ice);
+ }
+ objpose::ObjectPose objpose::fromIce(const data::ObjectPose& ice)
+ {
+ return ObjectPose(ice);
+ }
+
+ void objpose::fromIce(const data::ObjectPoseSeq& ice, ObjectPoseSeq& poses)
+ {
+ poses.clear();
+ poses.reserve(ice.size());
+ for (const auto& i : ice)
+ {
+ poses.emplace_back(i);
+ }
+ }
+ objpose::ObjectPoseSeq objpose::fromIce(const data::ObjectPoseSeq& ice)
+ {
+ ObjectPoseSeq poses;
+ fromIce(ice, poses);
+ return poses;
+ }
+
+
+ void objpose::toIce(data::ObjectPose& ice, const ObjectPose& pose)
+ {
+ pose.toIce(ice);
+ }
+ objpose::data::ObjectPose objpose::toIce(const ObjectPose& pose)
+ {
+ return pose.toIce();
+ }
+
+ void objpose::toIce(data::ObjectPoseSeq& ice, const ObjectPoseSeq& poses)
+ {
+ ice.clear();
+ ice.reserve(poses.size());
+ for (const auto& p : poses)
+ {
+ ice.emplace_back(p.toIce());
+ }
+ }
+ objpose::data::ObjectPoseSeq objpose::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..6a237e319daaae4c6e43ff0d6cd933990d2ae2f2
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.h
@@ -0,0 +1,71 @@
+#pragma once
+
+#include <Eigen/Core>
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <IceUtil/Time.h>
+
+#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;
+
+ void fromProvidedPose(const data::ProvidedObjectPose& provided, VirtualRobot::RobotPtr robot);
+
+
+ /// 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/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
index b969a58931b1d3242a0ebbbf5176c1ccc413810e..e7202cd5ae606072e92b776949bcb5b15f2d9270 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
@@ -31,18 +31,12 @@
#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/core/FramedPose.h>
+#include "ice_conversions.h"
+
namespace armarx
{
- const simox::meta::EnumNames<objpose::ObjectTypeEnum> ObjectTypeEnumNames =
- {
- { objpose::ObjectTypeEnum::AnyObject, "AnyObject" },
- { objpose::ObjectTypeEnum::KnownObject, "KnownObject" },
- { objpose::ObjectTypeEnum::UnknownObject, "UnknownObject" }
- };
-
-
ObjectPoseObserverPropertyDefinitions::ObjectPoseObserverPropertyDefinitions(std::string prefix) :
armarx::ObserverPropertyDefinitions(prefix)
{
@@ -121,11 +115,11 @@ namespace armarx
{
offerChannel(providerName, "Channel of provider '" + providerName + "'.");
}
- offerOrUpdateDataField(providerName, "objectType", ObjectTypeEnumNames.to_name(info.objectType), "");
+ offerOrUpdateDataField(providerName, "objectType", objpose::ObjectTypeEnumNames.to_name(info.objectType), "");
}
void ObjectPoseObserver::reportObjectPoses(
- const std::string& providerName, const objpose::ProvidedObjectPoseSeq& providedPoses, const Ice::Current&)
+ const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& providedPoses, const Ice::Current&)
{
ARMARX_VERBOSE << "Received object poses from '" << providerName << "'.";
@@ -137,28 +131,7 @@ namespace armarx
for (const auto& provided : providedPoses)
{
objpose::ObjectPose& pose = objectPoses.emplace_back();
- pose.objectType = provided.objectType;
- pose.objectID = provided.objectID;
-
- pose.objectPoseOriginal = provided.objectPose;
- pose.objectPoseOriginalFrame = provided.objectPoseFrame;
-
- armarx::PosePtr p = armarx::PosePtr::dynamicCast(provided.objectPose);
- ARMARX_CHECK_NOT_NULL(p);
- {
- armarx::FramedPose framed(p->toEigen(), provided.objectPoseFrame, robot->getName());
- framed.changeFrame(robot, robot->getRootNode()->getName());
- pose.objectPoseRobot = new Pose(framed.toEigen());
- framed.changeToGlobal(robot);
- pose.objectPoseGlobal = new Pose(framed.toEigen());
-
- pose.robotConfig = robot->getConfig()->getRobotNodeJointValueMap();
- pose.robotPose = new Pose(robot->getGlobalPose());
- }
-
- pose.confidence = provided.confidence;
- pose.timestampMicroSeconds = provided.timestampMicroSeconds;
- pose.providerName = provided.providerName;
+ pose.fromProvidedPose(provided, robot);
}
}
@@ -170,24 +143,24 @@ namespace armarx
}
- objpose::ObjectPoseSeq ObjectPoseObserver::getObjectPoses(const Ice::Current&)
+ objpose::data::ObjectPoseSeq ObjectPoseObserver::getObjectPoses(const Ice::Current&)
{
std::scoped_lock lock(dataMutex);
- objpose::ObjectPoseSeq result;
+ objpose::data::ObjectPoseSeq result;
for (const auto& [name, poses] : objectPoses)
{
for (const auto& pose : poses)
{
- result.push_back(pose);
+ result.push_back(pose.toIce());
}
}
return result;
}
- objpose::ObjectPoseSeq ObjectPoseObserver::getObjectPosesByProvider(const std::string& providerName, const Ice::Current&)
+ objpose::data::ObjectPoseSeq ObjectPoseObserver::getObjectPosesByProvider(const std::string& providerName, const Ice::Current&)
{
std::scoped_lock lock(dataMutex);
- return objectPoses.at(providerName);
+ return objpose::toIce(objectPoses.at(providerName));
}
@@ -325,11 +298,11 @@ namespace armarx
continue;
}
- PoseBasePtr pose = visu.inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
+ Eigen::Matrix4f pose = visu.inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
viz::Object object = viz::Object(id.dataset + "/" + id.name)
.file(objectInfo->package(), objectInfo->simoxXML().relativePath)
- .pose(armarx::PosePtr::dynamicCast(pose)->toEigen());
+ .pose(pose);
if (visu.alpha < 1)
{
object.overrideColor(simox::Color::white().with_alpha(visu.alpha));
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
index d06d2e2915b1e959dce91b727b498ba1140bf059..23484b0b377b7d2fad5b56c8aab7f87f613624a3 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
@@ -33,7 +33,7 @@
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
#include "ObjectFinder.h"
-#include "ice_conversions.h"
+#include "ObjectPose.h"
#define ICE_CURRENT_ARG const Ice::Current& = Ice::emptyCurrent
@@ -83,13 +83,13 @@ namespace armarx
// ObjectPoseTopic interface
public:
void reportProviderAvailable(const std::string& providerName, ICE_CURRENT_ARG) override;
- void reportObjectPoses(const std::string& providerName, const objpose::ProvidedObjectPoseSeq& objectPoses, ICE_CURRENT_ARG) override;
+ void reportObjectPoses(const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& objectPoses, ICE_CURRENT_ARG) override;
// ObjectPoseObserverInterface interface
public:
- objpose::ObjectPoseSeq getObjectPoses(ICE_CURRENT_ARG) override;
- objpose::ObjectPoseSeq getObjectPosesByProvider(const std::string& providerName, ICE_CURRENT_ARG) override;
+ objpose::data::ObjectPoseSeq getObjectPoses(ICE_CURRENT_ARG) override;
+ objpose::data::ObjectPoseSeq getObjectPosesByProvider(const std::string& providerName, ICE_CURRENT_ARG) override;
void requestObjects(const objpose::ObjectIDSeq& objectIDs, Ice::Long relativeTimeoutMS, ICE_CURRENT_ARG) override;
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ProvidedObjectPose.cpp b/source/RobotAPI/components/ObjectPoseObserver/ProvidedObjectPose.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ecf05de2b31c102245e4fccfbf9663d43ab19f9b
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseObserver/ProvidedObjectPose.cpp
@@ -0,0 +1,12 @@
+#include "ProvidedObjectPose.h"
+
+
+namespace armarx::objpose
+{
+ ProvidedObjectPose::ProvidedObjectPose(const data::ProvidedObjectPose& ice)
+ {
+
+ }
+}
+
+
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ProvidedObjectPose.h b/source/RobotAPI/components/ObjectPoseObserver/ProvidedObjectPose.h
new file mode 100644
index 0000000000000000000000000000000000000000..afc47fe515c61511c1ab225df37b2c96a48eaa2f
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseObserver/ProvidedObjectPose.h
@@ -0,0 +1,41 @@
+#pragma once
+
+#include <Eigen/Core>
+
+#include <RobotAPI/interface/objectpose/types.h>
+
+
+namespace armarx::objpose
+{
+
+ struct ProvidedObjectPose
+ {
+ ProvidedObjectPose();
+ ProvidedObjectPose(const data::ProvidedObjectPose& ice);
+
+
+ /// 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;
+
+ /// Pose in `objectPoseFrame`.
+ Eigen::Matrix4f objectPose;
+ std::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;
+ };
+
+
+
+
+}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.cpp b/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.cpp
index 0502b05d46ce9747707522052a40c1a69df0048c..e40d29d709de816fedbd44e84d54f08961542eff 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.cpp
@@ -1,5 +1,12 @@
#include "ice_conversions.h"
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotConfig.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/core/FramedPose.h>
+
namespace armarx
{
@@ -9,5 +16,13 @@ namespace armarx
return os << "'" << id.dataset << "/" << id.name << "'";
}
+ const simox::meta::EnumNames<objpose::ObjectTypeEnum> objpose::ObjectTypeEnumNames =
+ {
+ { objpose::ObjectTypeEnum::AnyObject, "AnyObject" },
+ { objpose::ObjectTypeEnum::KnownObject, "KnownObject" },
+ { objpose::ObjectTypeEnum::UnknownObject, "UnknownObject" }
+ };
+
}
+
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.h b/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.h
index fc73211f96f470557795667cbc62e8405f33bdd6..e9a0da5413706e3e53eea0f4fb4fee267c5f8354 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.h
@@ -1,7 +1,10 @@
#pragma once
+#include <SimoxUtility/meta/enum/EnumNames.hpp>
+
#include <RobotAPI/interface/objectpose/types.h>
-#include <RobotAPI/interface/objectpose/ObjectPoseProvider.h>
+
+#include "ObjectPose.h"
namespace armarx::objpose
@@ -9,5 +12,6 @@ namespace armarx::objpose
std::ostream& operator<<(std::ostream& os, const ObjectID& id);
+ extern const simox::meta::EnumNames<objpose::ObjectTypeEnum> ObjectTypeEnumNames;
}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseClientPlugin.cpp b/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseClientPlugin.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bb921e04a106971c501849d28fec3dfc18db415f
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseClientPlugin.cpp
@@ -0,0 +1,59 @@
+#include "ObjectPoseClientPlugin.h"
+
+
+namespace armarx::plugins
+{
+
+ void ObjectPoseClientPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
+ {
+ if (!properties->hasDefinition(makePropertyName(PROPERTY_NAME)))
+ {
+ properties->defineOptionalProperty<std::string>(
+ makePropertyName(PROPERTY_NAME),
+ "ObjectPoseObserver",
+ "Name of the object pose observer.");
+ }
+ }
+
+ void ObjectPoseClientPlugin::preOnInitComponent()
+ {
+ parent<Component>().usingProxyFromProperty(makePropertyName(PROPERTY_NAME));
+ }
+
+ void ObjectPoseClientPlugin::preOnConnectComponent()
+ {
+ parent<ObjectPoseClientPluginUser>().objectPoseObserver = createObjectPoseObserver();
+ }
+
+ objpose::ObjectPoseObserverInterfacePrx ObjectPoseClientPlugin::createObjectPoseObserver()
+ {
+ return parent<Component>().getTopicFromProperty<objpose::ObjectPoseObserverInterfacePrx>(makePropertyName(PROPERTY_NAME));
+ }
+
+}
+
+
+namespace armarx
+{
+
+ ObjectPoseClientPluginUser::ObjectPoseClientPluginUser()
+ {
+ addPlugin(plugin);
+ }
+
+ objpose::ObjectPoseObserverInterfacePrx ObjectPoseClientPluginUser::createObjectPoseObserver()
+ {
+ return plugin->createObjectPoseObserver();
+ }
+
+ objpose::ObjectPoseSeq ObjectPoseClientPluginUser::getObjectPoses()
+ {
+ if (!objectPoseObserver)
+ {
+ ARMARX_WARNING << "No object pose observer.";
+ return {};
+ }
+ return objpose::fromIce(objectPoseObserver->getObjectPoses());
+ }
+
+}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseClientPlugin.h b/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseClientPlugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..667c22b4ed44a887c9dcd453836fcb67e50d4c9f
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseClientPlugin.h
@@ -0,0 +1,63 @@
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+
+#include <RobotAPI/interface/objectpose/ObjectPoseObserver.h>
+
+#include "../ObjectPose.h"
+
+
+namespace armarx::plugins
+{
+
+ class ObjectPoseClientPlugin : public ComponentPlugin
+ {
+ public:
+ using ComponentPlugin::ComponentPlugin;
+
+ void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
+
+ void preOnInitComponent() override;
+ void preOnConnectComponent() override;
+
+ objpose::ObjectPoseObserverInterfacePrx createObjectPoseObserver();
+
+
+ private:
+
+ static constexpr const char* PROPERTY_NAME = "ObjectPoseTopicName";
+
+ };
+
+}
+
+
+namespace armarx
+{
+
+ /**
+ * @brief Provides an `objpose::ObjectPoseTopicPrx objectPoseTopic` as member variable.
+ */
+ class ObjectPoseClientPluginUser :
+ virtual public ManagedIceObject
+ {
+ public:
+
+ /// Allow usage like: ObjectPose::getObjects()
+ using ObjectPose = ObjectPoseClientPluginUser;
+
+ ObjectPoseClientPluginUser();
+
+ objpose::ObjectPoseObserverInterfacePrx createObjectPoseObserver();
+ objpose::ObjectPoseObserverInterfacePrx objectPoseObserver;
+
+
+ objpose::ObjectPoseSeq getObjectPoses();
+
+
+ private:
+
+ armarx::plugins::ObjectPoseClientPlugin* plugin = nullptr;
+
+ };
+}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseProviderPlugin.cpp b/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseProviderPlugin.cpp
similarity index 100%
rename from source/RobotAPI/components/ObjectPoseObserver/ObjectPoseProviderPlugin.cpp
rename to source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseProviderPlugin.cpp
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseProviderPlugin.h b/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseProviderPlugin.h
similarity index 100%
rename from source/RobotAPI/components/ObjectPoseObserver/ObjectPoseProviderPlugin.h
rename to source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseProviderPlugin.h
diff --git a/source/RobotAPI/interface/objectpose/ObjectPoseObserver.ice b/source/RobotAPI/interface/objectpose/ObjectPoseObserver.ice
index 3eddae79a480fa3aee046da4572e552e052fba50..7a8060dbabbb206be75243df6f579b90a203993d 100644
--- a/source/RobotAPI/interface/objectpose/ObjectPoseObserver.ice
+++ b/source/RobotAPI/interface/objectpose/ObjectPoseObserver.ice
@@ -36,39 +36,10 @@ module armarx
module objpose
{
- 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;
-
-
interface ObjectPoseObserverInterface extends ObserverInterface, ObjectPoseTopic
{
- ObjectPoseSeq getObjectPoses();
- ObjectPoseSeq getObjectPosesByProvider(string providerName);
+ data::ObjectPoseSeq getObjectPoses();
+ data::ObjectPoseSeq getObjectPosesByProvider(string providerName);
void requestObjects(ObjectIDSeq objectIDs, long relativeTimeoutMS);
diff --git a/source/RobotAPI/interface/objectpose/ObjectPoseProvider.ice b/source/RobotAPI/interface/objectpose/ObjectPoseProvider.ice
index 61f9a99fb6872ba20a9cd47dd8976130914c257b..4dcfb2d2f699107a2fbf95a4cf0baf996726a33b 100644
--- a/source/RobotAPI/interface/objectpose/ObjectPoseProvider.ice
+++ b/source/RobotAPI/interface/objectpose/ObjectPoseProvider.ice
@@ -33,31 +33,6 @@ module armarx
// A struct's name cannot cannot differ only in capitalization from its immediately enclosing module name.
module objpose
{
- 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;
-
-
struct ProviderInfo
{
ObjectTypeEnum objectType = AnyObject;
@@ -70,7 +45,7 @@ module armarx
{
/// Signal that a new provider is now available (and ready for `getProviderInfo()`.
void reportProviderAvailable(string providerName);
- void reportObjectPoses(string providerName, ProvidedObjectPoseSeq candidates);
+ void reportObjectPoses(string providerName, data::ProvidedObjectPoseSeq candidates);
};
interface ObjectPoseProvider
diff --git a/source/RobotAPI/interface/objectpose/types.ice b/source/RobotAPI/interface/objectpose/types.ice
index e91ac3fcf82b7a3548e44ecdc3ce6a43572f1bc9..eaf7adf779b0ac6d51c952f343f08d75bd87d93c 100644
--- a/source/RobotAPI/interface/objectpose/types.ice
+++ b/source/RobotAPI/interface/objectpose/types.ice
@@ -57,6 +57,65 @@ module armarx
Vector3Base extents;
};
+
+ module data
+ {
+ /// 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;
+ }
+
};
};