From f12abb3055e632359a089bb57e3691cb5118d3c7 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Fri, 11 Jun 2021 14:50:32 +0200
Subject: [PATCH] Add resolveEntityInstance(s)
---
.../armem/client/MemoryNameSystem.cpp | 70 +++++++++++++++++++
.../libraries/armem/client/MemoryNameSystem.h | 23 ++++++
.../libraries/armem/client/Reader.cpp | 23 ++++++
.../RobotAPI/libraries/armem/client/Reader.h | 20 ++++++
4 files changed, 136 insertions(+)
diff --git a/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp
index 98c6f0cca..8f077e0fc 100644
--- a/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp
+++ b/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;
diff --git a/source/RobotAPI/libraries/armem/client/MemoryNameSystem.h b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.h
index 0d6a3f3b6..dad7cde70 100644
--- a/source/RobotAPI/libraries/armem/client/MemoryNameSystem.h
+++ b/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;
diff --git a/source/RobotAPI/libraries/armem/client/Reader.cpp b/source/RobotAPI/libraries/armem/client/Reader.cpp
index 011948cc2..fc4775bc3 100644
--- a/source/RobotAPI/libraries/armem/client/Reader.cpp
+++ b/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
{
diff --git a/source/RobotAPI/libraries/armem/client/Reader.h b/source/RobotAPI/libraries/armem/client/Reader.h
index 5cbeff84a..ae2548f29 100644
--- a/source/RobotAPI/libraries/armem/client/Reader.h
+++ b/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);
/**
--
GitLab