From e732c5fc19a8b3abd9ca359f01a93ef3ecace552 Mon Sep 17 00:00:00 2001
From: armar-user <armar-user@kit.edu>
Date: Wed, 28 Jul 2021 14:58:38 +0200
Subject: [PATCH] fixed ltm query and consollidation
---
.../RobotStateMemory/RobotUnitReader.cpp | 9 ++--
.../RobotAPI/libraries/armem/client/Query.h | 32 +++++++++++++
.../libraries/armem/core/base/MemoryBase.h | 19 ++++++++
.../armem/core/longtermmemory/CoreSegment.cpp | 8 ++--
.../armem/core/longtermmemory/Entity.cpp | 8 ++--
.../core/longtermmemory/EntitySnapshot.cpp | 8 +---
.../armem/core/longtermmemory/Memory.cpp | 48 +++++++++++++++----
.../armem/core/longtermmemory/Memory.h | 5 +-
.../core/longtermmemory/ProviderSegment.cpp | 9 ++--
.../armem/server/MemoryToIceAdapter.cpp | 23 ++++++++-
10 files changed, 130 insertions(+), 39 deletions(-)
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitReader.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitReader.cpp
index 58b688557..94dc94f8b 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitReader.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitReader.cpp
@@ -116,12 +116,13 @@ namespace armarx::armem::server::robot_state
for (const auto& [nameEntry, dataEntry] : description.entries)
{
std::string name = nameEntry;
- std::string jointOrWhateverName;
+ std::string jointOrWhateverName = nameEntry;
std::string groupName = "";
if (auto it = configSensorMapping.find(name); it != configSensorMapping.end())
{
groupName = it->second;
+ jointOrWhateverName = it->second; // ???
}
else
{
@@ -134,10 +135,9 @@ namespace armarx::armem::server::robot_state
<< "and Y is the actual sensor. Ignoring this sensor.";
continue;
}
- groupName = name.substr(0, second_dot_pos);
+ groupName = name.substr(0, second_dot_pos);
jointOrWhateverName = name.substr(first_dot_pos + 1, second_dot_pos - first_dot_pos - 1);
-
name = name.substr(second_dot_pos + 1); // remove the groupName, TODO check if +1 is valid
}
@@ -161,7 +161,8 @@ namespace armarx::armem::server::robot_state
}
RobotUnitData::RobotUnitDataGroup& group = it->second;
- group.data->addElement(name, RobotUnitDataStreaming::toAron(data.value(), dataEntry));
+ const std::string escapedName = simox::alg::replace_all(name, ".", "/");
+ group.data->addElement(escapedName, RobotUnitDataStreaming::toAron(data.value(), dataEntry));
}
auto stop = std::chrono::high_resolution_clock::now();
diff --git a/source/RobotAPI/libraries/armem/client/Query.h b/source/RobotAPI/libraries/armem/client/Query.h
index 916278a8f..c6bb445bf 100644
--- a/source/RobotAPI/libraries/armem/client/Query.h
+++ b/source/RobotAPI/libraries/armem/client/Query.h
@@ -60,6 +60,38 @@ namespace armarx::armem::client
}
}
+ void replaceQueryTarget(const armem::query::data::QueryTarget search, const armem::query::data::QueryTarget replace)
+ {
+ for (const auto& memoryQuery : memoryQueries)
+ {
+ if (auto it = std::find(memoryQuery->targets.begin(), memoryQuery->targets.end(), search); it != memoryQuery->targets.end())
+ {
+ memoryQuery->targets.push_back(replace);
+ }
+ for (const auto& coreSegmentQuery : memoryQuery->coreSegmentQueries)
+ {
+ if (auto it = std::find(coreSegmentQuery->targets.begin(), coreSegmentQuery->targets.end(), search); it != coreSegmentQuery->targets.end())
+ {
+ coreSegmentQuery->targets.push_back(replace);
+ }
+ for (const auto& providerSegmentQuery : coreSegmentQuery->providerSegmentQueries)
+ {
+ if (auto it = std::find(providerSegmentQuery->targets.begin(), providerSegmentQuery->targets.end(), search); it != providerSegmentQuery->targets.end())
+ {
+ providerSegmentQuery->targets.push_back(replace);
+ }
+ for (const auto& entityQuery : providerSegmentQuery->entityQueries)
+ {
+ if (auto it = std::find(entityQuery->targets.begin(), entityQuery->targets.end(), search); it != entityQuery->targets.end())
+ {
+ entityQuery->targets.push_back(replace);
+ }
+ }
+ }
+ }
+ }
+ }
+
static QueryInput fromIce(const armem::query::data::Input& ice);
armem::query::data::Input toIce() const;
};
diff --git a/source/RobotAPI/libraries/armem/core/base/MemoryBase.h b/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
index 781322b54..1bc42b0cc 100644
--- a/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
@@ -336,6 +336,25 @@ namespace armarx::armem::base
return this->name();
}
+ std::string dump() const
+ {
+ std::stringstream ss;
+ ss << "Memory: " << this->name() << "\n";
+ for (const auto& [ckey, cseg] : this->container())
+ {
+ ss << " |- Found core seg: " << ckey << "\n";
+ for (const auto& [pkey, pseg] : cseg.container())
+ {
+ ss << " | |- Found prov seg: " << pkey << "\n";
+ for (const auto& [ekey, eseg] : pseg.container())
+ {
+ ss << " | | |- Found entity: " << ekey << "\n";
+ }
+ }
+ }
+ return ss.str();
+ }
+
protected:
virtual void _copySelf(DerivedT& other) const override
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.cpp
index a64a5982e..d47dc46cb 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.cpp
@@ -10,8 +10,6 @@ namespace armarx::armem::ltm
wm::CoreSegment CoreSegment::convert() const
{
- ARMARX_INFO << "CoreSegment: Converting with connection to: " << dbsettings.toString();
-
wm::CoreSegment m(id());
for (const auto& [_, s] : _container)
{
@@ -24,8 +22,6 @@ namespace armarx::armem::ltm
{
_container.clear();
- ARMARX_INFO << "CoreSegment: (Re)Establishing connection to: " << dbsettings.toString();
-
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
mongocxx::database db = client[dbsettings.database];
mongocxx::collection coll = db[id().str()];
@@ -47,7 +43,7 @@ namespace armarx::armem::ltm
if (const auto& it = _container.find(k); it != _container.end())
{
- throw error::ArMemError("Somehow after clearing the container a key k = " + k + " was found. Do you have double entries in mongodb?");
+ throw error::ArMemError("Somehow after clearing the (core) container a key k = " + k + " was found. Do you have double entries in mongodb?");
}
else
{
@@ -56,6 +52,8 @@ namespace armarx::armem::ltm
wms.first->second.reload();
}
}
+
+ ARMARX_INFO << "After reload has core segment " << id().str() << " size: " << _container.size();
}
void CoreSegment::append(const wm::CoreSegment& m)
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.cpp
index aeb2be76e..bccb79c9d 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.cpp
@@ -6,8 +6,6 @@ namespace armarx::armem::ltm
wm::Entity Entity::convert() const
{
- ARMARX_INFO << "Entity: Converting with connection to: " << dbsettings.toString();
-
wm::Entity m(id());
for (const auto& [_, s] : _container)
{
@@ -20,8 +18,6 @@ namespace armarx::armem::ltm
{
_container.clear();
- ARMARX_INFO << "Entity: (Re)Establishing connection to: " << dbsettings.toString();
-
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
mongocxx::database db = client[dbsettings.database];
mongocxx::collection coll = db[id().str()];
@@ -42,7 +38,7 @@ namespace armarx::armem::ltm
if (const auto& it = _container.find(k); it != _container.end())
{
- throw error::ArMemError("Somehow after clearing the container a key k = " + std::to_string(k.toMicroSeconds()) + " was found. Do you have double entries in mongodb?");
+ throw error::ArMemError("Somehow after clearing the (entity) container a key k = " + std::to_string(k.toMicroSeconds()) + " was found. Do you have double entries in mongodb?");
}
else
{
@@ -52,6 +48,8 @@ namespace armarx::armem::ltm
}
++i;
}
+
+ ARMARX_INFO << "After reload has entity " << id().str() << " size: " << _container.size();
}
void Entity::append(const wm::Entity& m)
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.cpp
index 47a680731..aae8a6d32 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.cpp
@@ -13,8 +13,6 @@ namespace armarx::armem::ltm
wm::EntitySnapshot EntitySnapshot::convert(const aron::typenavigator::NavigatorPtr& expectedStructure) const
{
- ARMARX_INFO << "EntitySnapshot: Converting with connection to: " << dbsettings.toString();
-
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
mongocxx::database db = client[dbsettings.database];
mongocxx::collection coll = db[id().getEntityID().str()];
@@ -52,8 +50,6 @@ namespace armarx::armem::ltm
{
_container.clear();
- ARMARX_INFO << "EntitySnapshot: (Re)Establishing connection to: " << dbsettings.toString();
-
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
mongocxx::database db = client[dbsettings.database];
mongocxx::collection coll = db[id().getEntityID().str()];
@@ -68,10 +64,8 @@ namespace armarx::armem::ltm
nlohmann::json json = nlohmann::json::parse(bsoncxx::to_json(*res));
for (unsigned int i = 0; i < json.at("instances").size(); ++i)
{
- auto wms = _container.emplace_back(id().withInstanceIndex(i));
+ _container.emplace_back(id().withInstanceIndex(i));
}
-
- ARMARX_INFO << "Entity '" + id().str() + "': Found instances in LTM: " << json.at("instances").size();
}
void EntitySnapshot::setTo(const wm::EntitySnapshot& m, const armem::Time& t)
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp
index 95ad5af75..2080879c5 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp
@@ -27,7 +27,8 @@ namespace armarx::armem::ltm
dbsettings(other.dbsettings),
alwaysTransferSettings(other.alwaysTransferSettings),
periodicTransferSettings(other.periodicTransferSettings),
- onFullTransferSettings(other.onFullTransferSettings)
+ onFullTransferSettings(other.onFullTransferSettings),
+ reloaded(other.reloaded)
{
// Do not move _mutex.
}
@@ -53,6 +54,7 @@ namespace armarx::armem::ltm
alwaysTransferSettings = std::move(other.alwaysTransferSettings);
periodicTransferSettings = std::move(other.periodicTransferSettings);
onFullTransferSettings = std::move(other.onFullTransferSettings);
+ reloaded = other.reloaded;
// Don't move _mutex.
return *this;
@@ -88,13 +90,14 @@ namespace armarx::armem::ltm
wm::Memory Memory::convert() const
{
+ std::lock_guard l(mongoDBMutex);
if (!checkConnection())
{
wm::Memory m(id());
return m;
}
- ARMARX_INFO << "Converting with connection to: " << dbsettings.toString();
+ ARMARX_INFO << "Converting Memory with connection to: " << dbsettings.toString();
TIMING_START(LTM_Convert);
@@ -110,18 +113,20 @@ namespace armarx::armem::ltm
void Memory::reload()
{
+ std::lock_guard l(mongoDBMutex);
+ reloaded = false;
+
if (!checkConnection())
{
return;
}
- std::scoped_lock l(mongoDBMutex);
ARMARX_INFO << "(Re)Establishing connection to: " << dbsettings.toString();
+
+ TIMING_START(LTM_Reload);
_container.clear();
- ARMARX_TRACE;
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
- ARMARX_TRACE;
if (!client)
{
ARMARX_ERROR << "A client has died. Could not reload.";
@@ -139,6 +144,8 @@ namespace armarx::armem::ltm
ARMARX_INFO << "Getting collection for id: " << id().str();
mongocxx::collection coll = db[id().str()];
+ ARMARX_IMPORTANT << "Memory Container size is: " << _container.size();
+
mongocxx::cursor cursor = coll.find({});
for (const auto& doc : cursor)
{
@@ -155,7 +162,7 @@ namespace armarx::armem::ltm
if (const auto& it = _container.find(k); it != _container.end())
{
- throw error::ArMemError("Somehow after clearing the container a key k = " + k + " was found. Do you have double entries in mongodb?");
+ throw error::ArMemError("Somehow after clearing the (memory) container a key k = " + k + " was found. Do you have double entries in mongodb?");
}
else
{
@@ -164,17 +171,26 @@ namespace armarx::armem::ltm
wms.first->second.reload();
}
}
+
+ reloaded = true;
+ for (const auto& m : toAppendQueue)
+ {
+ _append(m);
+ }
+
+ TIMING_END(LTM_Reload);
+ ARMARX_INFO << "After reload memory " << id().str() << " size: " << _container.size() << ". Setting reloaded: " << reloaded;
}
- void Memory::append(const wm::Memory& m)
+ void Memory::_append(const wm::Memory& m)
{
- if (!checkConnection())
+ if (!checkConnection() || !reloaded)
{
+ // We ignore if not fully loaded yet
return;
}
- std::scoped_lock l(mongoDBMutex);
- ARMARX_INFO << "Merge memory with name '" << m.name() << "' into the LTM with name '" << name() << "'";
+ //ARMARX_INFO << "Merge memory with name '" << m.name() << "' into the LTM with name '" << name() << "'";
TIMING_START(LTM_Append);
@@ -205,4 +221,16 @@ namespace armarx::armem::ltm
TIMING_END(LTM_Append);
}
+
+ void Memory::append(const wm::Memory& m)
+ {
+ std::lock_guard l(mongoDBMutex);
+ if (!checkConnection() || !reloaded)
+ {
+ toAppendQueue.push_back(m);
+ return;
+ }
+
+ _append(m);
+ }
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.h b/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.h
index 43b67c82c..b24f4a7a6 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.h
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.h
@@ -87,6 +87,8 @@ namespace armarx::armem::ltm
private:
bool checkConnection() const;
+ void _append(const wm::Memory&);
+
public:
MongoDBConnectionManager::MongoDBSettings dbsettings;
@@ -95,8 +97,9 @@ namespace armarx::armem::ltm
OnFullTransferSettings onFullTransferSettings;
private:
+ bool reloaded = false;
mutable std::mutex mongoDBMutex;
-
+ std::vector<wm::Memory> toAppendQueue;
};
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.cpp
index aac804599..5fcf4a135 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.cpp
@@ -10,8 +10,6 @@ namespace armarx::armem::ltm
wm::ProviderSegment ProviderSegment::convert() const
{
- ARMARX_INFO << "ProviderSegment: Converting with connection to: " << dbsettings.toString();
-
wm::ProviderSegment m(id());
for (const auto& [_, s] : _container)
{
@@ -24,8 +22,6 @@ namespace armarx::armem::ltm
{
_container.clear();
- ARMARX_INFO << "ProviderSegment: (Re)Establishing connection to: " << dbsettings.toString();
-
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
mongocxx::database db = client[dbsettings.database];
mongocxx::collection coll = db[id().str()];
@@ -34,7 +30,6 @@ namespace armarx::armem::ltm
for (auto doc : cursor)
{
nlohmann::json json = nlohmann::json::parse(bsoncxx::to_json(doc));
- ARMARX_INFO << "ProviderSegment: Found foreign key: " << json.at("foreign_key");
MemoryID i((std::string) json.at("foreign_key"));
if (i.providerSegmentName != id().providerSegmentName)
@@ -46,7 +41,7 @@ namespace armarx::armem::ltm
if (const auto& it = _container.find(k); it != _container.end())
{
- throw error::ArMemError("Somehow after clearing the container a key k = " + k + " was found. Do you have double entries in mongodb?");
+ throw error::ArMemError("Somehow after clearing the (provvider) container a key k = " + k + " was found. Do you have double entries in mongodb?");
}
else
{
@@ -55,6 +50,8 @@ namespace armarx::armem::ltm
wms.first->second.reload();
}
}
+
+ ARMARX_INFO << "After reload has provider segment " << id().str() << " size: " << _container.size();
}
void ProviderSegment::append(const wm::ProviderSegment& m)
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
index 801536883..6b0761f53 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
@@ -173,6 +173,7 @@ namespace armarx::armem::server
result.timeArrived = update.timeArrived;
// also store in ltm if transfermode is set to always
+ // TODO: Move outside of loop?
if (longtermMemory)
{
if (longtermMemory->alwaysTransferSettings.enabled)
@@ -243,16 +244,35 @@ namespace armarx::armem::server
armem::query::data::Result result;
if (ltmResult.hasData())
{
+ ARMARX_INFO << "The LTM returned data after query";
+
// ATTENTION: This code block moves data from LTM back into WM.
// However, since some segments are constrained, the WM might send data back to LTM.
// This may also affect the data returned by the current query.
// However, this is expected behavior, since we copy the data in the processor (copyEmpty) we can safely return the copy and
// remove the original memory reference from WM here.
wm::Memory ltmConverted = ltmResult.convert();
+ if (!ltmConverted.hasData())
+ {
+ ARMARX_ERROR << "A converted memory contains no data although the original memory contained data. This indicates that something is wrong.";
+ }
+
wmResult.append(ltmConverted);
+ if (!wmResult.hasData())
+ {
+ ARMARX_ERROR << "A merged Memory has no data although at least the LTM result contains data. This indicates that something is wrong.";
+ }
// query again to limit output size (TODO: Skip if querytype is all)
- wm::Memory merged_result = wmProcessor.process(input, wmResult);
+ auto queryInput = armem::client::QueryInput::fromIce(input);
+ queryInput.replaceQueryTarget(query::data::QueryTarget::LTM, query::data::QueryTarget::WM);
+
+ wm::Memory merged_result = wmProcessor.process(queryInput.toIce(), wmResult, /* execute if: */ { query::data::QueryTarget::WM });
+ if (!merged_result.hasData())
+ {
+ ARMARX_ERROR << "A merged and postprocessed Memory has no data although at least the LTM result contains data. This indicates that something is wrong.";
+ }
+
result.memory = toIce<data::MemoryPtr>(merged_result);
// also move results of ltm to wm
@@ -263,6 +283,7 @@ namespace armarx::armem::server
}
else
{
+ ARMARX_INFO << "The LTM did not return data after query";
result.memory = toIce<data::MemoryPtr>(wmResult);
}
--
GitLab