Refactor Provider->requestObjects() interface

source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp

@@ -28,6 +28,7 @@
 
 #include <RobotAPI/libraries/core/Pose.h>
 #include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/components/ObjectPoseObserver/ice_conversions.h>
 
 
 namespace armarx
@@ -98,11 +99,23 @@ namespace armarx
         return providerInfo;
     }
 
-    void ObjectPoseProviderExample::requestObjects(
-        const objpose::ObjectIDSeq& objectIDs, Ice::Long relativeTimeoutMS, const Ice::Current&)
+    objpose::provider::RequestObjectsOutput ObjectPoseProviderExample::requestObjects(
+        const objpose::provider::RequestObjectsInput& input, const Ice::Current&)
     {
-        std::scoped_lock lock(requestedObjectsMutex);
-        requestedObjects.requestObjects(objectIDs, relativeTimeoutMS);
+        ARMARX_INFO << "Requested object IDs for " << input.relativeTimeoutMS << " ms: "
+                    << input.objectIDs;
+        {
+            std::scoped_lock lock(requestedObjectsMutex);
+            requestedObjects.requestObjects(input.objectIDs, input.relativeTimeoutMS);
+        }
+
+        objpose::provider::RequestObjectsOutput output;
+        // All requests are successful.
+        for (const auto& id : input.objectIDs)
+        {
+            output.results[id].success = true;
+        }
+        return output;
     }
 
 
@@ -111,34 +124,42 @@ namespace armarx
         CycleUtil cycle(50);
         IceUtil::Time start = TimeUtil::GetTime();
 
-        std::vector<ObjectID> requestedIDs;
-        {
-            std::scoped_lock lock(requestedObjectsMutex);
-            requestedIDs = requestedObjects.updateRequestedObjects().current;
-        }
-        std::vector<ObjectInfo> requested;
-        for (const auto& initial : requestedObjects.updateRequestedObjects().current)
+        std::map<ObjectID, ObjectInfo> objectInfos;
+
+        while (poseEstimationTask && !poseEstimationTask->isStopped())
         {
-            ObjectID id(initial);
-            if (std::optional<ObjectInfo> info = objectFinder.findObject(initial))
+            IceUtil::Time now = TimeUtil::GetTime();
+            float t = float((now - start).toSecondsDouble());
+
+            objpose::RequestedObjects::Update update;
             {
-                requested.push_back(*info);
+                std::scoped_lock lock(requestedObjectsMutex);
+                update = requestedObjects.updateRequestedObjects(now);
             }
-            else
+
+            if (update.added.size() > 0 || update.removed.size() > 0)
             {
-                ARMARX_WARNING << "Could not find object class '" << initial << "'";
+                ARMARX_INFO << "Added: " << update.added
+                            << "Removed: " << update.removed;
             }
-        }
-
-
-        while (poseEstimationTask && poseEstimationTask->isRunning())
-        {
-            float t = float((TimeUtil::GetTime() - start).toSecondsDouble());
 
             int i = 0;
             armarx::objpose::data::ProvidedObjectPoseSeq poses;
-            for (const ObjectInfo& info : requested)
+            for (const ObjectID& id : update.current)
             {
+                if (objectInfos.count(id) == 0)
+                {
+                    if (std::optional<ObjectInfo> info = objectFinder.findObject(id))
+                    {
+                        objectInfos.emplace(id, *info);
+                    }
+                    else
+                    {
+                        ARMARX_WARNING << "Could not find object class '" << id << "'.";
+                    }
+                }
+                const ObjectInfo& info = objectInfos.at(id);
+
                 armarx::objpose::data::ProvidedObjectPose& pose = poses.emplace_back();
                 pose.providerName = getName();
                 pose.objectType = objpose::ObjectTypeEnum::KnownObject;
@@ -157,7 +178,8 @@ namespace armarx
 
                 i++;
             }
-            ARMARX_IMPORTANT << "Reporting " << poses.size() << " object poses";
+
+            ARMARX_VERBOSE << "Reporting " << poses.size() << " object poses";
             objectPoseTopic->reportObjectPoses(getName(), poses);
 
             cycle.waitForCycleDuration();

source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h

@@ -79,7 +79,7 @@ namespace armarx
         // Implement the ObjectPoseProvider interface
     public:
         objpose::ProviderInfo getProviderInfo(const Ice::Current& = Ice::emptyCurrent) override;
-        void requestObjects(const objpose::ObjectIDSeq& objectIDs, Ice::Long relativeTimeoutMS, const Ice::Current&) override;
+        objpose::provider::RequestObjectsOutput requestObjects(const objpose::provider::RequestObjectsInput& input, const Ice::Current&) override;
 
 
     protected:

source/RobotAPI/components/ObjectPoseProviderExample/RequestedObjects.cpp

@@ -13,7 +13,7 @@ namespace armarx::objpose
     void RequestedObjects::requestObjects(const std::vector<armarx::objpose::ObjectID>& objectIDs, long relativeTimeOutMS)
     {
         std::vector<armarx::ObjectID> ids;
-        ids.resize(objectIDs.size());
+        ids.reserve(objectIDs.size());
         for (const auto& id : objectIDs)
         {
             ids.emplace_back(id.dataset, id.className, id.instanceName);

source/RobotAPI/interface/objectpose/ObjectPoseProvider.ice

@@ -43,18 +43,45 @@ module armarx
         dictionary<string, ProviderInfo> ProviderInfoMap;
 
 
+        module provider
+        {
+            struct RequestObjectsInput
+            {
+                /// Object IDs.
+                ObjectIDSeq objectIDs;
+                /// For how long to request localization. Negative for no timeout.
+                long relativeTimeoutMS;
+            };
+
+            struct ObjectRequestResult
+            {
+                bool success;
+            };
+            dictionary<ObjectID, ObjectRequestResult> ObjectRequestResultMap;
+            struct RequestObjectsOutput
+            {
+                ObjectRequestResultMap results;
+            };
+        };
         interface ObjectPoseProvider
         {
             /// Get the provider info.
             ProviderInfo getProviderInfo();
             /// Request to track the specified objects for some time.
-            void requestObjects(ObjectIDSeq objectIDs, long relativeTimeoutMS);
+            provider::RequestObjectsOutput requestObjects(provider::RequestObjectsInput input);
         };
 
         interface ObjectPoseTopic
         {
             /// Signal that a new provider is now available.
             void reportProviderAvailable(string providerName, ProviderInfo info);
+
+            /**
+             * @brief Report new object poses.
+             *
+             * This will overwrite all object poses previously provided
+             * with the same provider name.
+             */
             void reportObjectPoses(string providerName, data::ProvidedObjectPoseSeq candidates);
         };
     };