diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
index 38845b9383215d30c1457a9c1a1180ac86d4db73..d5a7b55c3712b8ed984a0ee33e5c4b061e30f888 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
@@ -126,11 +126,9 @@ namespace armarx
}
- void ObjectPoseObserver::reportProviderAvailable(
- const std::string& providerName, const objpose::ObjectPoseProviderPrx& provider, const Ice::Current&)
+ void ObjectPoseObserver::reportProviderAvailable(const std::string& providerName, const objpose::ProviderInfo& info, const Ice::Current&)
{
- static const bool refresh = true;
- fetchProviderInfo(providerName, provider, refresh);
+ updateProviderInfo(providerName, info);
}
void ObjectPoseObserver::reportObjectPoses(
@@ -138,9 +136,6 @@ namespace armarx
{
ARMARX_VERBOSE << "Received object poses from '" << providerName << "'.";
- // static const bool refresh = false;
- // fetchProviderInfo(providerName, refresh);
-
objpose::ObjectPoseSeq objectPoses;
{
std::scoped_lock lock(dataMutex);
@@ -180,24 +175,14 @@ namespace armarx
}
- void ObjectPoseObserver::fetchProviderInfo(const std::string& providerName, objpose::ObjectPoseProviderPrx provider, bool refresh)
+ void ObjectPoseObserver::updateProviderInfo(const std::string& providerName, const objpose::ProviderInfo& info)
{
- {
- std::scoped_lock lock(dataMutex);
- if (!refresh && providers.count(providerName) > 0)
- {
- return; // Do nothing.
- }
- }
-
- if (!provider)
+ if (!info.proxy)
{
ARMARX_WARNING << "Received availability signal from provider '" << providerName << "' "
- << "with invalid provider proxy.";
+ << "with invalid provider proxy.\nIgnoring provider '" << providerName << "'.";
return;
}
- objpose::ProviderInfo info = provider->getProviderInfo();
-
{
std::scoped_lock lock(dataMutex);
if (providers.count(providerName) == 0)
@@ -346,7 +331,7 @@ namespace armarx
offerChannel(providerName, "Channel of provider '" + providerName + "'.");
}
offerOrUpdateDataField(providerName, "updateCounter", Variant(updateCounters.at(providerName)), "Counter incremented for each update.");
- offerOrUpdateDataField(providerName, "candidateCount", Variant(int(objectPoses.at(providerName).size())), "Number of provided object poses.");
+ offerOrUpdateDataField(providerName, "objectCount", Variant(int(objectPoses.at(providerName).size())), "Number of provided object poses.");
if (visu.enabled)
{
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
index 23017441dedf37ce0602c248fea30ed2dde3ebaf..894bf651a7e63fe969e9ca65ef6c5cb65c4d9073 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
@@ -82,7 +82,7 @@ namespace armarx
// ObjectPoseTopic interface
public:
- void reportProviderAvailable(const std::string& providerName, const objpose::ObjectPoseProviderPrx& provider, ICE_CURRENT_ARG) override;
+ void reportProviderAvailable(const std::string& providerName, const objpose::ProviderInfo& info, ICE_CURRENT_ARG) override;
void reportObjectPoses(const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& objectPoses, ICE_CURRENT_ARG) override;
// ObjectPoseObserverInterface interface
@@ -120,7 +120,7 @@ namespace armarx
private:
- void fetchProviderInfo(const std::string& providerName, objpose::ObjectPoseProviderPrx provider, bool refresh = false);
+ void updateProviderInfo(const std::string& providerName, const objpose::ProviderInfo& info);
void handleProviderUpdate(const std::string& providerName);
void visProviderUpdate(const std::string& providerName);
diff --git a/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseProviderPlugin.cpp b/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseProviderPlugin.cpp
index 38b31297ea1344bc6b454fa74017b91d12d17730..3112560f38f72cbb9b7777631f894ac7d7249de5 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseProviderPlugin.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseProviderPlugin.cpp
@@ -30,7 +30,12 @@ namespace armarx::plugins
objpose::ObjectPoseTopicPrx& topic = parent.objectPoseTopic;
if (topic)
{
- topic->reportProviderAvailable(parent.getName(), ::armarx::objpose::ObjectPoseProviderPrx::checkedCast(parent.getProxy()));
+ objpose::ProviderInfo info = parent.getProviderInfo();
+ if (!info.proxy)
+ {
+ info.proxy = ::armarx::objpose::ObjectPoseProviderPrx::checkedCast(parent.getProxy());
+ }
+ topic->reportProviderAvailable(parent.getName(), info);
}
else
{
diff --git a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp b/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
index e29c39a9e006c7279c86e4478440cbac99487ec2..4fba5d59663d3ec1b1fd569309938dce94746763 100644
--- a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
+++ b/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
@@ -54,25 +54,6 @@ namespace armarx
return "ObjectPoseProviderExample";
}
- objpose::ProviderInfo ObjectPoseProviderExample::getProviderInfo(const Ice::Current&)
- {
- objpose::ProviderInfo info;
- info.objectType = objpose::ObjectTypeEnum::KnownObject;
- std::vector<ObjectInfo> objects = objectFinder.findAllObjectsOfDataset("KIT");
- for (const auto& obj : objects)
- {
- info.supportedObjects.push_back(objpose::ObjectID{obj.id().dataset(), obj.id().className(), obj.id().instanceName()});
- }
- return info;
- }
-
- void ObjectPoseProviderExample::requestObjects(
- const objpose::ObjectIDSeq& objectIDs, Ice::Long relativeTimeoutMS, const Ice::Current&)
- {
- std::scoped_lock lock(requestedObjectsMutex);
- requestedObjects.requestObjects(objectIDs, relativeTimeoutMS);
- }
-
void ObjectPoseProviderExample::onInitComponent()
{
@@ -81,6 +62,15 @@ namespace armarx
// -1 -> infinitely
requestedObjects.requestObjects({ObjectID(initial)}, -1);
}
+
+ {
+ providerInfo.objectType = objpose::ObjectTypeEnum::KnownObject;
+ std::vector<ObjectInfo> objects = objectFinder.findAllObjectsOfDataset("KIT");
+ for (const auto& obj : objects)
+ {
+ providerInfo.supportedObjects.push_back(objpose::ObjectID{obj.id().dataset(), obj.id().className(), obj.id().instanceName()});
+ }
+ }
}
@@ -102,6 +92,20 @@ namespace armarx
{
}
+
+ objpose::ProviderInfo ObjectPoseProviderExample::getProviderInfo(const Ice::Current&)
+ {
+ return providerInfo;
+ }
+
+ void ObjectPoseProviderExample::requestObjects(
+ const objpose::ObjectIDSeq& objectIDs, Ice::Long relativeTimeoutMS, const Ice::Current&)
+ {
+ std::scoped_lock lock(requestedObjectsMutex);
+ requestedObjects.requestObjects(objectIDs, relativeTimeoutMS);
+ }
+
+
void ObjectPoseProviderExample::poseEstimationTaskRun()
{
CycleUtil cycle(50);
diff --git a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h b/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h
index 0d1cf3ad35a4f45cd0fbb05d3c4a600a2394903f..46fc68b8e503ecdfb906fbe419b0b8e363f57cb0 100644
--- a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h
+++ b/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h
@@ -107,11 +107,15 @@ namespace armarx
private:
+ // To be moved to plugin
std::mutex requestedObjectsMutex;
objpose::RequestedObjects requestedObjects;
- std::vector<std::string> initialObjectIDs = { "KIT/Amicelli", "KIT/YellowSaltCylinder" };
+
+ objpose::ProviderInfo providerInfo;
+
armarx::ObjectFinder objectFinder;
+ std::vector<std::string> initialObjectIDs = { "KIT/Amicelli", "KIT/YellowSaltCylinder" };
armarx::SimpleRunningTask<>::pointer_type poseEstimationTask;
diff --git a/source/RobotAPI/interface/objectpose/ObjectPoseProvider.ice b/source/RobotAPI/interface/objectpose/ObjectPoseProvider.ice
index 63a309c9305f2c0d945b76334f15577f9e4020ca..b3b47492743eaa13a9f0a4ca7b5121a07de92c66 100644
--- a/source/RobotAPI/interface/objectpose/ObjectPoseProvider.ice
+++ b/source/RobotAPI/interface/objectpose/ObjectPoseProvider.ice
@@ -33,8 +33,10 @@ module armarx
// A struct's name cannot cannot differ only in capitalization from its immediately enclosing module name.
module objpose
{
+ interface ObjectPoseProvider;
struct ProviderInfo
{
+ ObjectPoseProvider* proxy;
ObjectTypeEnum objectType = AnyObject;
ObjectIDSeq supportedObjects;
};
@@ -51,8 +53,8 @@ module armarx
interface ObjectPoseTopic
{
- /// Signal that a new provider is now available (and ready for `getProviderInfo()`.
- void reportProviderAvailable(string providerName, ObjectPoseProvider* provider);
+ /// Signal that a new provider is now available.
+ void reportProviderAvailable(string providerName, ProviderInfo info);
void reportObjectPoses(string providerName, data::ProvidedObjectPoseSeq candidates);
};
};