Add resolveEntityInstance(s)

source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp

@@ -224,6 +224,76 @@ namespace armarx::armem::client
     }
 
 
+    std::optional<wm::EntityInstance> MemoryNameSystem::resolveEntityInstance(const MemoryID& id)
+    {
+        auto result = resolveEntityInstances({id});
+        if (result.size() > 0)
+        {
+            return result.begin()->second;
+        }
+        else
+        {
+            return std::nullopt;
+        }
+    }
+
+
+    std::map<MemoryID, wm::EntityInstance> MemoryNameSystem::resolveEntityInstances(const std::vector<MemoryID>& ids)
+    {
+        std::stringstream errors;
+
+        std::map<std::string, std::vector<MemoryID>> idsPerMemory;
+        for (const auto& id : ids)
+        {
+            idsPerMemory[id.memoryName].push_back(id);
+        }
+
+        std::map<MemoryID, wm::EntityInstance> result;
+        for (const auto& [memoryName, ids] : idsPerMemory)
+        {
+            Reader reader = getReader(MemoryID().withMemoryName(memoryName));
+            QueryResult queryResult = reader.queryMemoryIDs(ids);
+            if (queryResult.success)
+            {
+                for (const MemoryID& id : ids)
+                {
+                    try
+                    {
+                        if (id.hasInstanceIndex())
+                        {
+                            result[id] = queryResult.memory.getEntityInstance(id);
+                        }
+                        else if (id.hasTimestamp())
+                        {
+                            result[id] = queryResult.memory.getEntitySnapshot(id).getInstance(0);
+                        }
+                        else // must: id.hasEntityName()
+                        {
+                            result[id] = queryResult.memory.getEntity(id).getLatestSnapshot().getInstance(0);
+                        }
+                    }
+                    catch (const error::ArMemError& e)
+                    {
+                        errors << "# Failed to retrieve " << id << " from query result: \n" << e.what() << "\n";
+                    }
+                }
+            }
+            else
+            {
+                errors << "# Failed to query '" << memoryName << "': \n" << queryResult.errorMessage << "\n";
+            }
+        }
+
+        if (errors.str().size() > 0)
+        {
+            ARMARX_INFO << "MemoryNameSystem::" << __FUNCTION__ << ": The following errors may affect your result: "
+                        << "\n\n" << errors.str();
+        }
+
+        return result;
+    }
+
+
     void MemoryNameSystem::registerServer(const MemoryID& memoryID, server::MemoryInterfacePrx proxy)
     {
         data::RegisterMemoryInput input;

source/RobotAPI/libraries/armem/client/MemoryNameSystem.h

@@ -22,6 +22,9 @@
 
 #pragma once
 
+#include <map>
+#include <optional>
+
 #include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
 #include <RobotAPI/interface/armem/server/MemoryInterface.h>
 
@@ -38,6 +41,11 @@ namespace armarx::armem::client
     class Writer;
 }
 
+namespace armarx::armem::wm
+{
+    class EntityInstance;
+}
+
 namespace armarx::armem::client
 {
 
@@ -190,6 +198,20 @@ namespace armarx::armem::client
 
         // ToDo: commit() and query()
 
+        /**
+         * @brief Resolve a memory ID to an EntityInstance.
+         *
+         * The ID can refer to an entity, an entity snapshot, or an entity
+         * instance. When not referring to an entity instance, the latest
+         * snapshot and first instance will be queried.
+         *
+         * @param id The entity, snapshot or instance ID.
+         * @return
+         */
+        std::optional<wm::EntityInstance> resolveEntityInstance(const MemoryID& id);
+
+        std::map<MemoryID, wm::EntityInstance> resolveEntityInstances(const std::vector<MemoryID>& ids);
+
 
 
         // Registration - only for memory servers
@@ -234,6 +256,7 @@ namespace armarx::armem::client
 
         /// The MNS proxy.
         mns::MemoryNameSystemInterfacePrx mns = nullptr;
+
         /// The component to which dependencies will be added.
         ManagedIceObject* component = nullptr;
 

source/RobotAPI/libraries/armem/client/Reader.cpp

@@ -83,6 +83,29 @@ namespace armarx::armem::client
         return this->query(qb.buildQueryInput());
     }
 
+
+    QueryResult Reader::queryMemoryIDs(const std::vector<MemoryID>& ids, DataMode dataMode) const
+    {
+        using namespace client::query_fns;
+
+        query::Builder qb(dataMode);
+        for (const auto& id : ids)
+        {
+            auto entity = qb.coreSegments(withID(id)).providerSegments(withID(id)).entities(withID(id));
+
+            if (id.hasTimestamp())
+            {
+                entity.snapshots(withID(id));
+            }
+            else
+            {
+                entity.snapshots(latest());
+            }
+        }
+        return query(qb.buildQueryInput());
+    }
+
+
     void
     Reader::updated(const std::vector<MemoryID>& updatedSnapshotIDs) const
     {

source/RobotAPI/libraries/armem/client/Reader.h

@@ -55,9 +55,29 @@ namespace armarx::armem::client
         QueryResult getAll(DataMode dataMode = DataMode::WithData) const;
         QueryResult getLatestSnapshots(DataMode dataMode = DataMode::WithData) const;
 
+
+        /**
+         * @brief Qeury a specific set of memory IDs.
+         *
+         * Each ID can refer to an entity, a snapshot or an instance. When not
+         * referring to an entity instance, the latest snapshot and first
+         * instance will be queried, respectively.
+         *
+         * All memory IDs must refer to the memory this reader is reading from.
+         * If an ID refers to another memory, the query will not find it and it
+         * will not be part of the result.
+         *
+         * @param ids The entity, snapshot or instance IDs.
+         * @param dataMode Whether to include instance data or just meta data.
+         * @return The query result.
+         */
+        QueryResult queryMemoryIDs(const std::vector<MemoryID>& ids, DataMode dataMode = DataMode::WithData) const;
+
+
         //data::StoreResult readAndStore(data::StoreInputSeq& input);
         data::StoreResult readAndStore(const data::StoreInput& input) const;
 
+
         void subscribe(const MemoryID& subscriptionID, callback callback);
         void subscribe(const MemoryID& subscriptionID, callback_updated_only callback);
         /**