Merge branch 'feature/object-provider-skills' into 'master'

Feature/object provider skills

See merge request !488

source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.cpp

@@ -2,43 +2,48 @@
 
 #include <ArmarXCore/core/Component.h>
 
-
 namespace armarx::plugins
 {
 
-    void ObjectPoseProviderPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
+    void
+    ObjectPoseProviderPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
     {
         if (!properties->hasDefinition(makePropertyName(PROPERTY_NAME)))
         {
             properties->defineOptionalProperty<std::string>(
-                makePropertyName(PROPERTY_NAME),
-                "ObjectPoseTopic",
-                "Name of the object pose topic.");
+                makePropertyName(PROPERTY_NAME), "ObjectMemory", "Name of the object memory.");
         }
     }
 
-    void ObjectPoseProviderPlugin::preOnInitComponent()
+    void
+    ObjectPoseProviderPlugin::preOnInitComponent()
     {
-        parent<Component>().offeringTopicFromProperty(makePropertyName(PROPERTY_NAME));
+        parent<Component>().usingProxyFromProperty(makePropertyName(PROPERTY_NAME));
     }
 
-    void ObjectPoseProviderPlugin::preOnConnectComponent()
+    void
+    ObjectPoseProviderPlugin::preOnConnectComponent()
     {
-        parent<ObjectPoseProviderPluginUser>().objectPoseTopic = createObjectPoseTopic();
+        parent<ObjectPoseProviderPluginUser>().objectPoseTopic =
+            parent<Component>().getProxyFromProperty<objpose::ObjectPoseStorageInterfacePrx>(
+                makePropertyName(PROPERTY_NAME));
     }
 
-    void ObjectPoseProviderPlugin::postOnConnectComponent()
+    void
+    ObjectPoseProviderPlugin::postOnConnectComponent()
     {
-        ObjectPoseProviderPluginUser& parent = this->parent<ObjectPoseProviderPluginUser>();
-        objpose::ObjectPoseTopicPrx& topic = parent.objectPoseTopic;
-        if (topic)
+        auto& parent = this->parent<ObjectPoseProviderPluginUser>();
+        if (parent.objectPoseTopic)
         {
             objpose::ProviderInfo info = parent.getProviderInfo();
             if (!info.proxy)
             {
-                info.proxy = ::armarx::objpose::ObjectPoseProviderPrx::checkedCast(parent.getProxy());
+                info.proxy =
+                    ::armarx::objpose::ObjectPoseProviderPrx::checkedCast(parent.getProxy());
             }
-            topic->reportProviderAvailable(parent.getName(), info);
+
+            ARMARX_INFO << "Reporting provider `" << parent.getName() << "` availability.";
+            parent.objectPoseTopic->reportProviderAvailable(parent.getName(), info);
         }
         else
         {
@@ -46,13 +51,7 @@ namespace armarx::plugins
         }
     }
 
-    objpose::ObjectPoseTopicPrx ObjectPoseProviderPlugin::createObjectPoseTopic()
-    {
-        return parent<Component>().getTopicFromProperty<objpose::ObjectPoseTopicPrx>(makePropertyName(PROPERTY_NAME));
-    }
-
-}
-
+} // namespace armarx::plugins
 
 namespace armarx
 {
@@ -62,7 +61,10 @@ namespace armarx
         addPlugin(plugin);
     }
 
-    objpose::provider::RequestObjectsOutput ObjectPoseProviderPluginUser::requestObjects(const objpose::provider::RequestObjectsInput& input, const Ice::Current&)
+    objpose::provider::RequestObjectsOutput
+    ObjectPoseProviderPluginUser::requestObjects(
+        const objpose::provider::RequestObjectsInput& input,
+        const Ice::Current&)
     {
         // ARMARX_INFO << "Requested " << input.objectIDs.size() << " objects for " << input.relativeTimeoutMS << " ms.";
         objpose::provider::RequestObjectsOutput output;
@@ -73,9 +75,5 @@ namespace armarx
         return output;
     }
 
-    objpose::ObjectPoseTopicPrx ObjectPoseProviderPluginUser::createObjectPoseTopic()
-    {
-        return plugin->createObjectPoseTopic();
-    }
 
-}
+} // namespace armarx

source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h

@@ -3,7 +3,7 @@
 #include <ArmarXCore/core/ComponentPlugin.h>
 
 #include <RobotAPI/interface/objectpose/ObjectPoseProvider.h>
-
+#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
 
 namespace armarx::plugins
 {
@@ -11,7 +11,6 @@ namespace armarx::plugins
     class ObjectPoseProviderPlugin : public ComponentPlugin
     {
     public:
-
         using ComponentPlugin::ComponentPlugin;
 
         void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
@@ -20,17 +19,12 @@ namespace armarx::plugins
         void preOnConnectComponent() override;
         void postOnConnectComponent() override;
 
-        objpose::ObjectPoseTopicPrx createObjectPoseTopic();
-
-
     private:
-
-        static constexpr const char* PROPERTY_NAME = "ObjectPoseTopicName";
+        static constexpr const char* PROPERTY_NAME = "ObjectMemoryName";
 
     };
 
-}
-
+} // namespace armarx::plugins
 
 #include <ArmarXCore/core/ManagedIceObject.h>
 
@@ -41,24 +35,22 @@ namespace armarx
      * @brief Provides an `objpose::ObjectPoseTopicPrx objectPoseTopic` as member variable.
      */
     class ObjectPoseProviderPluginUser :
-        virtual public ManagedIceObject
-        , virtual public objpose::ObjectPoseProvider
+        virtual public ManagedIceObject,
+        virtual public objpose::ObjectPoseProvider
     {
     public:
-
         ObjectPoseProviderPluginUser();
 
         /// Implement to process object requests (empty default implementation).
-        objpose::provider::RequestObjectsOutput requestObjects(const objpose::provider::RequestObjectsInput& input, const Ice::Current&) override;
-
-        objpose::ObjectPoseTopicPrx createObjectPoseTopic();
+        objpose::provider::RequestObjectsOutput
+        requestObjects(const objpose::provider::RequestObjectsInput& input,
+                       const Ice::Current&) override;
 
-        objpose::ObjectPoseTopicPrx objectPoseTopic;
+        // objpose::ObjectPoseStorageInterfacePrx createObjectPoseStorage();
 
+        objpose::ObjectPoseStorageInterfacePrx objectPoseTopic;
 
     private:
-
         armarx::plugins::ObjectPoseProviderPlugin* plugin = nullptr;
-
     };
-}
+} // namespace armarx

source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp

@@ -32,6 +32,7 @@
 
 #include "ArmarXCore/core/time/Clock.h"
 #include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
+#include <ArmarXCore/core/logging/Logging.h>
 #include <ArmarXCore/core/time/CycleUtil.h>
 #include <ArmarXCore/core/time/ice_conversions.h>
 #include <ArmarXCore/observers/variant/Variant.h>
@@ -110,6 +111,7 @@ namespace armarx::armem::server::obj::instance
 
     void SegmentAdapter::reportProviderAvailable(const std::string& providerName, const objpose::ProviderInfo& info, const Ice::Current&)
     {
+        ARMARX_IMPORTANT << "Provider `" << providerName << "` is (now) available."; 
         updateProviderInfo(providerName, info);
     }
 
@@ -266,6 +268,8 @@ namespace armarx::armem::server::obj::instance
     objpose::observer::RequestObjectsOutput SegmentAdapter::requestObjects(
         const objpose::observer::RequestObjectsInput& input, const Ice::Current&)
     {
+        ARMARX_INFO << "Requesting objects ...";
+
         std::map<std::string, objpose::provider::RequestObjectsInput> providerRequests;
         std::map<std::string, objpose::ObjectPoseProviderPrx> proxies;
 
@@ -287,7 +291,7 @@ namespace armarx::armem::server::obj::instance
             }
         };
 
-        if (input.provider.size() > 0)
+        if (not input.provider.empty())
         {
             providerRequests[input.provider] = input.request;
             updateProxy(input.provider);
@@ -296,6 +300,9 @@ namespace armarx::armem::server::obj::instance
         {
             segment.doLocked([&]()
             {
+
+                ARMARX_INFO << "Known providers are: " << simox::alg::get_keys(segment.providers);
+
                 for (const auto& objectID : input.request.objectIDs)
                 {
                     bool found = true;
@@ -304,6 +311,8 @@ namespace armarx::armem::server::obj::instance
                         // ToDo: optimize look up.
                         if (std::find(info.supportedObjects.begin(), info.supportedObjects.end(), objectID) != info.supportedObjects.end())
                         {
+                            ARMARX_INFO << "Found provider for " << objectID << ": " << providerName;
+                            providerRequests[providerName].relativeTimeoutMS = input.request.relativeTimeoutMS;
                             providerRequests[providerName].objectIDs.push_back(objectID);
                             updateProxy(providerName);
                             break;
@@ -318,6 +327,8 @@ namespace armarx::armem::server::obj::instance
             });
         }
 
+        ARMARX_INFO << VAROUT(simox::alg::get_keys(providerRequests));
+
         for (const auto& [providerName, request] : providerRequests)
         {
             if (objpose::ObjectPoseProviderPrx proxy = proxies.at(providerName); proxy)