diff --git a/source/RobotAPI/interface/armem/query.ice b/source/RobotAPI/interface/armem/query.ice
index bfe0764272dbee12270de46cfe7cf0b90f851bb0..2a0eb6e217a769fe4bab33dd0ab896e2faeda8e5 100644
--- a/source/RobotAPI/interface/armem/query.ice
+++ b/source/RobotAPI/interface/armem/query.ice
@@ -11,9 +11,17 @@ module armarx
{
module data
{
+ enum QueryTarget
+ {
+ WMAndLTM,
+ WM,
+ LTM // only for debug. remove later
+ };
+
/// Which entity snapshots to get from an entity?
class EntityQuery
{
+ QueryTarget target = QueryTarget::WMAndLTM;
};
sequence<EntityQuery> EntityQuerySeq;
@@ -107,6 +115,7 @@ module armarx
class ProviderSegmentQuery
{
EntityQuerySeq entityQueries;
+ QueryTarget target = QueryTarget::WMAndLTM;
};
sequence<ProviderSegmentQuery> ProviderSegmentQuerySeq;
module provider
@@ -129,6 +138,7 @@ module armarx
class CoreSegmentQuery
{
ProviderSegmentQuerySeq providerSegmentQueries;
+ QueryTarget target = QueryTarget::WMAndLTM;
};
sequence<CoreSegmentQuery> CoreSegmentQuerySeq;
@@ -152,6 +162,7 @@ module armarx
class MemoryQuery
{
CoreSegmentQuerySeq coreSegmentQueries;
+ QueryTarget target = QueryTarget::WMAndLTM;
};
sequence<MemoryQuery> MemoryQuerySeq;
dictionary<string, MemoryQuerySeq> MemoryQueriesDict;
@@ -177,6 +188,7 @@ module armarx
{
/// Dict of memory name to
MemoryQueriesDict memoryQueries;
+ QueryTarget target = QueryTarget::WMAndLTM;
};
struct Input
diff --git a/source/RobotAPI/libraries/armem/client/query/Builder.cpp b/source/RobotAPI/libraries/armem/client/query/Builder.cpp
index e7ce598a68d393d6da88a9e4ec52a801d10999c5..5979ea365a863dc0ce1c0c0b91a0d77fc3f331e9 100644
--- a/source/RobotAPI/libraries/armem/client/query/Builder.cpp
+++ b/source/RobotAPI/libraries/armem/client/query/Builder.cpp
@@ -8,6 +8,12 @@ namespace armarx::armem::client::query
{
}
+ Builder& Builder::queryTarget(const armem::query::data::QueryTarget target)
+ {
+ this->_target = target;
+ return *this;
+ }
+
QueryInput Builder::buildQueryInput() const
{
QueryInput input;
@@ -27,6 +33,7 @@ namespace armarx::armem::client::query
{
for (const armem::query::data::MemoryQueryPtr& query : child.buildQueries())
{
+ query->target = _target;
memoryQueries.push_back(query);
}
}
diff --git a/source/RobotAPI/libraries/armem/client/query/Builder.h b/source/RobotAPI/libraries/armem/client/query/Builder.h
index 7f4c2b7c63440f8182d2e489e74ea33ab98e8317..6cf3e8929effb7640224589de0062d9eee72bd01 100644
--- a/source/RobotAPI/libraries/armem/client/query/Builder.h
+++ b/source/RobotAPI/libraries/armem/client/query/Builder.h
@@ -28,7 +28,7 @@ namespace armarx::armem::client::query
public:
Builder(DataMode dataMode = DataMode::WithData);
-
+ Builder& queryTarget(const armem::query::data::QueryTarget target);
/// Start specifying core segments.
CoreSegmentSelector& coreSegments();
@@ -66,7 +66,7 @@ namespace armarx::armem::client::query
public:
-
+ armem::query::data::QueryTarget _target;
DataMode dataMode;
};
diff --git a/source/RobotAPI/libraries/armem/core/Commit.cpp b/source/RobotAPI/libraries/armem/core/Commit.cpp
index 5c1946ed0f55591457c7ce7fcf7a8a5b9a325ee4..fed7ed660b450bb0865a018f9c8d7119dd96a7b4 100644
--- a/source/RobotAPI/libraries/armem/core/Commit.cpp
+++ b/source/RobotAPI/libraries/armem/core/Commit.cpp
@@ -86,4 +86,17 @@ namespace armarx::armem
return updates.emplace_back(update);
}
+ void Commit::add(const std::vector<EntityUpdate>& updates)
+ {
+ for (const auto& update : updates)
+ {
+ add(update);
+ }
+ }
+
+ void Commit::append(const Commit& c)
+ {
+ add(c.updates);
+ }
+
}
diff --git a/source/RobotAPI/libraries/armem/core/Commit.h b/source/RobotAPI/libraries/armem/core/Commit.h
index 48707600ab242ad3a624c6e26965acdef18839fa..fc69a4b53dcaecc52d703aa42c00405f4162825f 100644
--- a/source/RobotAPI/libraries/armem/core/Commit.h
+++ b/source/RobotAPI/libraries/armem/core/Commit.h
@@ -10,6 +10,16 @@
namespace armarx::armem
{
+
+ /**
+ * @brief The type of an update
+ */
+ enum class UpdateType
+ {
+ UpdatedExisting,
+ InsertedNew
+ };
+
/**
* @brief An update of an entity for a specific point in time.
*/
@@ -87,6 +97,8 @@ namespace armarx::armem
EntityUpdate& add();
EntityUpdate& add(const EntityUpdate& update);
+ void add(const std::vector<EntityUpdate>& updates);
+ void append(const Commit& c);
friend std::ostream& operator<<(std::ostream& os, const Commit& rhs);
};
diff --git a/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h b/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
index 3c3c0ef2c3ffa6f1020f6c3ff82dfce8e4650f5b..b3fae618c36bb67920c346f14162383c9f2ec845 100644
--- a/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
@@ -33,6 +33,22 @@ namespace armarx::armem::base
using EntitySnapshotT = typename EntityT::EntitySnapshotT;
using EntityInstanceT = typename EntitySnapshotT::EntityInstanceT;
+ struct UpdateResult
+ {
+ armarx::armem::UpdateType coreSegmentUpdateType;
+ armarx::armem::UpdateType providerSegmentUpdateType;
+ armarx::armem::UpdateType entityUpdateType;
+ MemoryID id;
+ std::vector<EntitySnapshotT> removedSnapshots;
+
+ UpdateResult() = default;
+ UpdateResult(const typename ProviderSegmentT::UpdateResult& c) :
+ providerSegmentUpdateType(c.providerSegmentUpdateType),
+ entityUpdateType(c.entityUpdateType),
+ id(c.id),
+ removedSnapshots(c.removedSnapshots)
+ {}
+ };
public:
@@ -129,25 +145,61 @@ namespace armarx::armem::base
}
}
- virtual MemoryID update(const EntityUpdate& update) override
+ /**
+ * @brief Updates an entity's history.
+ *
+ * Missing entity entries are added before updating.
+ */
+ UpdateResult update(const EntityUpdate& update)
{
this->_checkContainerName(update.entityID.coreSegmentName, this->name());
+ ProviderSegmentT* provSeg;
+ UpdateType updateType = UpdateType::InsertedNew;
+
auto it = this->_container.find(update.entityID.providerSegmentName);
- if (it != this->_container.end())
+ if (it == this->_container.end())
{
- return it->second.update(update);
+ if (_addMissingProviderSegmentDuringUpdate)
+ {
+ // Insert into map.
+ provSeg = &addProviderSegment(update.entityID.providerSegmentName);
+ provSeg->setMaxHistorySize(_maxHistorySize);
+ updateType = UpdateType::InsertedNew;
+ }
+ else
+ {
+ throw armem::error::MissingEntry("provider segment", update.entityID.providerSegmentName, getLevelName(), this->getKeyString());
+ }
}
else
{
- if (_addMissingProviderSegmentDuringUpdate)
+ provSeg = &it->second;
+ updateType = UpdateType::UpdatedExisting;
+ }
+
+ // Update entry.
+ UpdateResult ret(provSeg->update(update));
+ ret.coreSegmentUpdateType = updateType;
+ return ret;
+ }
+
+ void append(const _Derived& m)
+ {
+ ARMARX_INFO << "CoreSegment: Merge name '" << m.name() << "' into '" << name() << "'";
+
+ for (const auto& [k, s] : m.container())
+ {
+ if (const auto& it = this->_container.find(k); it != this->_container.end())
{
- // Add the missing provider segment (with this core segment's type).
- ProviderSegmentT& provSeg = addProviderSegment(update.entityID.providerSegmentName);
- return provSeg.update(update);
+ // segment already exists
+ it->second.append(s);
+ }
+ else
+ {
+ auto wms = this->_container.emplace(std::make_pair(k, this->id().withProviderSegmentName(k)));
+ wms.first->second.append(s);
}
- throw armem::error::MissingEntry("provider segment", update.entityID.providerSegmentName, getLevelName(), this->getKeyString());
-
}
}
diff --git a/source/RobotAPI/libraries/armem/core/base/EntityBase.h b/source/RobotAPI/libraries/armem/core/base/EntityBase.h
index a798cb09eec72b3a368955ebb219fe15f9308cdc..ef1834a8a72352048b39f99e370a09ff623630d6 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntityBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/EntityBase.h
@@ -52,6 +52,14 @@ namespace armarx::armem::base
using EntitySnapshotT = _EntitySnapshotT;
using EntityInstanceT = typename EntitySnapshotT::EntityInstanceT;
+ struct UpdateResult
+ {
+ armarx::armem::UpdateType entityUpdateType;
+ MemoryID id;
+ std::vector<EntitySnapshotT> removedSnapshots;
+
+ UpdateResult() = default;
+ };
public:
@@ -119,7 +127,12 @@ namespace armarx::armem::base
/**
* @brief Indicates whether a history entry for the given time exists.
*/
- virtual bool hasSnapshot(Time time) const
+ virtual bool hasSnapshot(const Time& time)
+ {
+ return const_cast<const EntityBase*>(this)->hasSnapshot(time);
+ }
+
+ virtual bool hasSnapshot(const Time& time) const
{
return this->_container.count(time) > 0;
}
@@ -128,6 +141,11 @@ namespace armarx::armem::base
* @brief Get the latest timestamp.
* @throw `armem::error::EntityHistoryEmpty` If the history is empty.
*/
+ Time getLatestTimestamp()
+ {
+ return const_cast<const EntityBase*>(this)->getLatestTimestamp();
+ }
+
Time getLatestTimestamp() const
{
return getLatestItem().first;
@@ -136,6 +154,11 @@ namespace armarx::armem::base
/**
* @brief Get all timestamps in the history.
*/
+ virtual std::vector<Time> getTimestamps()
+ {
+ return const_cast<const EntityBase*>(this)->getTimestamps();
+ }
+
virtual std::vector<Time> getTimestamps() const
{
return simox::alg::get_keys(this->_container);
@@ -148,14 +171,13 @@ namespace armarx::armem::base
* @return The snapshot, if it exists.
*
* @throws `armem::error::MissingEntry` If there is no such entry.
- * @throws `armem::error::MissingData` If the entry has no data.
*/
- EntitySnapshotT& getSnapshot(Time time)
+ virtual EntitySnapshotT& getSnapshot(const Time& time)
{
return const_cast<EntitySnapshotT&>(const_cast<const EntityBase*>(this)->getSnapshot(time));
}
- virtual const EntitySnapshotT& getSnapshot(Time time) const
+ virtual const EntitySnapshotT& getSnapshot(const Time& time) const
{
auto it = this->_container.find(time);
if (it != this->_container.end())
@@ -175,7 +197,7 @@ namespace armarx::armem::base
const EntitySnapshotT& getSnapshot(const MemoryID& id) const
{
- checkEntityName(id.entityName);
+ this->_checkContainerName(id.entityName, this->name());
return getSnapshot(id.timestamp);
}
@@ -183,7 +205,6 @@ namespace armarx::armem::base
* @brief Return the snapshot with the most recent timestamp.
* @return The latest snapshot.
* @throw `armem::error::EntityHistoryEmpty` If the history is empty.
- * @throw `armem::error::MissingData` If the latest snapshot has no data.
*/
EntitySnapshotT& getLatestSnapshot()
{
@@ -195,16 +216,153 @@ namespace armarx::armem::base
return getLatestItem().second;
}
+ /**
+ * @brief Return the lastest snapshot before or at time.
+ * @param time The time.
+ * @return The latest snapshot.
+ * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
+ * @throw `armem::error::MissingEntry` If no such snapshot
+ */
+ virtual const EntitySnapshotT& getLatestSnapshotBeforeOrAt(const Time& time) const
+ {
+ // first element equal or greater
+ typename std::map<Time, EntitySnapshotT>::const_iterator refIt = this->_container.upper_bound(time);
+
+ if (refIt == this->_container.begin())
+ {
+ if (refIt->first == time)
+ {
+ return refIt->second;
+ }
+ throw error::MissingEntry("entity snapshot", toDateTimeMilliSeconds(time), getLevelName(), this->id().str());
+ }
+
+ // last element less than
+ typename std::map<Time, EntitySnapshotT>::const_iterator refItPrev = std::prev(refIt);
+
+ // ... or we return the element before if possible
+ if (refItPrev != this->_container.begin())
+ {
+ return refItPrev->second;
+ }
+
+ throw error::MissingEntry("entity snapshot", toDateTimeMilliSeconds(time), getLevelName(), this->id().str());
+ }
+
+ /**
+ * @brief Return the lastest snapshot before time.
+ * @param time The time.
+ * @return The latest snapshot.
+ * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
+ * @throw `armem::error::MissingEntry` If no such snapshot
+ */
+ virtual const EntitySnapshotT& getLatestSnapshotBefore(const Time& time) const
+ {
+ // first element equal or greater
+ typename std::map<Time, EntitySnapshotT>::const_iterator refIt = this->_container.upper_bound(time);
+
+ if (refIt == this->_container.begin())
+ {
+ throw error::MissingEntry("entity snapshot", toDateTimeMilliSeconds(time), getLevelName(), this->id().str());
+ }
+
+ // last element less than
+ typename std::map<Time, EntitySnapshotT>::const_iterator refItPrev = std::prev(refIt);
+
+ // we return the element before if possible
+ if (refItPrev != this->_container.begin())
+ {
+ return refItPrev->second;
+ }
+
+ throw error::MissingEntry("entity snapshot", toDateTimeMilliSeconds(time), getLevelName(), this->id().str());
+ }
+
+ /**
+ * @brief Return all snapshots before or at time.
+ * @param time The time.
+ * @return The latest snapshots.
+ * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
+ * @throw `armem::error::MissingEntry` If no such snapshots
+ */
+ virtual const std::vector<std::reference_wrapper<const EntitySnapshotT>> getSnapshotsBefore(const Time& time) const
+ {
+ std::vector<std::reference_wrapper<const EntitySnapshotT>> ret;
+ const EntitySnapshotT& latest = getLatestSnapshotBefore(time);
+
+ for (const auto& [timestamp, snapshot] : this->_container)
+ {
+ ret.emplace_back(snapshot);
+ if (timestamp == latest.id().timestamp)
+ {
+ break;
+ }
+ }
+ return ret;
+ }
+
+ /**
+ * @brief Return first snapshot after or at time.
+ * @param time The time.
+ * @return The first snapshot.
+ * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
+ * @throw `armem::error::MissingEntry` If no such snapshot
+ */
+ virtual const EntitySnapshotT& getFirstSnapshotAfterOrAt(const Time& time) const
+ {
+ // first element equal or greater
+ typename std::map<Time, EntitySnapshotT>::const_iterator refIt = this->_container.upper_bound(time);
+
+ if (refIt == this->_container.end())
+ {
+ throw error::MissingEntry("entity snapshot", toDateTimeMilliSeconds(time), getLevelName(), this->id().str());
+ }
+ return refIt->second;
+ }
+
+ /**
+ * @brief Return first snapshot after time.
+ * @param time The time.
+ * @return The first snapshot.
+ * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
+ * @throw `armem::error::MissingEntry` If no such snapshot
+ */
+ virtual const EntitySnapshotT& getFirstSnapshotAfter(const Time& time) const
+ {
+ // first element equal or greater
+ typename std::map<Time, EntitySnapshotT>::const_iterator refIt = this->_container.upper_bound(time);
+
+ if (refIt == this->_container.end())
+ {
+ throw error::MissingEntry("entity snapshot", toDateTimeMilliSeconds(time), getLevelName(), this->id().str());
+ }
+
+ if (refIt->first > time)
+ {
+ return refIt->second;
+ }
+
+ // first element greater than
+ typename std::map<Time, EntitySnapshotT>::const_iterator refItNext = std::next(refIt);
+
+ if (refItNext != this->_container.end())
+ {
+ return refItNext->second;
+ }
+
+ throw error::MissingEntry("entity snapshot", toDateTimeMilliSeconds(time), getLevelName(), this->id().str());
+ }
+
/**
* @brief Add the given update to this entity's history.
* @param update The update.
* @return The snapshot ID of the update.
*/
- virtual MemoryID update(const EntityUpdate& update)
+ UpdateResult update(const EntityUpdate& update)
{
- checkEntityName(update.entityID.entityName);
- this->id() = update.entityID;
+ this->_checkContainerName(update.entityID.entityName, this->name());
+ UpdateResult ret;
EntitySnapshotT* snapshot;
@@ -213,16 +371,39 @@ namespace armarx::armem::base
{
// Insert into history.
snapshot = &addSnapshot(update.timeCreated);
- truncateHistoryToSize();
+ ret.removedSnapshots = truncate();
+ ret.entityUpdateType = UpdateType::InsertedNew;
}
else
{
snapshot = &it->second;
+ ret.entityUpdateType = UpdateType::UpdatedExisting;
}
// Update entry.
- snapshot->update(update);
+ snapshot->update(update, this->id());
+ ret.id = snapshot->id();
+
+ return ret;
+ }
- return snapshot->id();
+ void append(const DerivedT& m)
+ {
+ ARMARX_INFO << "Entity: Merge name '" << m.name() << "' into '" << name() << "'";
+
+ for (const auto& [k, s] : m.container())
+ {
+ if (const auto& it = this->_container.find(k); it != this->_container.end())
+ {
+ // segment already exists
+ // We assume that a snapshot does not change, so ignore
+ }
+ else
+ {
+ auto snapshot = s.copy();
+ snapshot.id() = this->id().withTimestamp(k); // update id (e.g. memory name) if necessary
+ this->_container.insert(std::make_pair(k, std::move(snapshot)));
+ }
+ }
}
/**
@@ -256,7 +437,7 @@ namespace armarx::armem::base
void setMaxHistorySize(long maxSize) override
{
MaxHistorySize::setMaxHistorySize(maxSize);
- truncateHistoryToSize();
+ truncate();
}
std::string getKeyString() const override
@@ -272,24 +453,19 @@ namespace armarx::armem::base
protected:
/// If maximum size is set, ensure `history`'s is not higher.
- void truncateHistoryToSize()
+ std::vector<EntitySnapshotT> truncate()
{
+ std::vector<EntitySnapshotT> removedElements;
if (_maxHistorySize >= 0)
{
while (this->_container.size() > size_t(_maxHistorySize))
{
+ removedElements.push_back(std::move(this->_container.begin()->second));
this->_container.erase(this->_container.begin());
}
ARMARX_CHECK_LESS_EQUAL(this->_container.size(), _maxHistorySize);
}
- }
-
- void checkEntityName(const std::string& name) const
- {
- if (name != this->name())
- {
- throw armem::error::ContainerNameMismatch(name, getLevelName(), getKeyString());
- }
+ return removedElements;
}
/**
diff --git a/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.h b/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.h
index aab70193435c7eec54b9e2127e7c57528ebba842..8dd474c667687e6235a96a191e577163ca194cba 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.h
@@ -53,7 +53,6 @@ namespace armarx::armem::base
*/
virtual void update(const EntityUpdate& update, int index) = 0;
-
virtual bool equalsDeep(const DerivedT& other) const = 0;
virtual DerivedT copy() const
diff --git a/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h b/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
index defdf1c049fa5274b0d112f42d78037730d9eff5..33a35ed15cd3edb67ad532256a0a3e556fe67dc5 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
@@ -49,7 +49,7 @@ namespace armarx::armem::base
EntitySnapshotBase& operator=(EntitySnapshotBase&& other) = default;
- virtual bool equalsDeep(const EntitySnapshotBase& other) const
+ bool equalsDeep(const EntitySnapshotBase& other) const
{
//std::cout << "EntitySnapshot::equalsDeep" << std::endl;
if (this->size() != other.size())
diff --git a/source/RobotAPI/libraries/armem/core/base/MemoryBase.h b/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
index 713ba580089443b9fc2ca6f982e72970aa28dde7..7ee2999727463e51fad2163a3fdd18123ff1d8c3 100644
--- a/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
@@ -33,6 +33,25 @@ namespace armarx::armem::base
using EntityInstanceT = typename EntitySnapshotT::EntityInstanceT;
+ struct UpdateResult
+ {
+ armarx::armem::UpdateType memoryUpdateType;
+ armarx::armem::UpdateType coreSegmentUpdateType;
+ armarx::armem::UpdateType providerSegmentUpdateType;
+ armarx::armem::UpdateType entityUpdateType;
+ MemoryID id;
+ std::vector<EntitySnapshotT> removedSnapshots;
+
+ UpdateResult() = default;
+ UpdateResult(const typename CoreSegmentT::UpdateResult& c) :
+ coreSegmentUpdateType(c.coreSegmentUpdateType),
+ providerSegmentUpdateType(c.providerSegmentUpdateType),
+ entityUpdateType(c.entityUpdateType),
+ id(c.id),
+ removedSnapshots(c.removedSnapshots)
+ {}
+ };
+
public:
MemoryBase()
@@ -173,14 +192,37 @@ namespace armarx::armem::base
return segments;
}
- virtual MemoryID update(const EntityUpdate& update) override
+
+ /**
+ * @brief Store all updates in `commit`.
+ * @param commit The commit.
+ * @return The resulting memory IDs.
+ */
+ std::vector<UpdateResult> update(const Commit& commit)
+ {
+ std::vector<UpdateResult> results;
+ for (const auto& update : commit.updates)
+ {
+ results.push_back(this->update(update));
+ }
+ return results;
+ }
+
+ /**
+ * @brief Store the given update.
+ * @param update The update.
+ * @return The resulting entity snapshot's ID.
+ */
+ UpdateResult update(const EntityUpdate& update)
{
this->_checkContainerName(update.entityID.memoryName, this->name());
auto it = this->_container.find(update.entityID.coreSegmentName);
if (it != this->_container.end())
{
- return it->second.update(update);
+ UpdateResult ret(it->second.update(update));
+ ret.memoryUpdateType = UpdateType::UpdatedExisting;
+ return ret;
}
else
{
@@ -188,6 +230,29 @@ namespace armarx::armem::base
}
}
+ /**
+ * @brief Merge another memory into this one. Append all data
+ * @param m The other memory
+ */
+ void append(const _Derived& m)
+ {
+ ARMARX_INFO << "Memory: Merge name '" << m.name() << "' into '" << name() << "'";
+
+ for (const auto& [k, s] : m.container())
+ {
+ if (const auto& it = this->_container.find(k); it != this->_container.end())
+ {
+ // segment already exists
+ it->second.append(s);
+ }
+ else
+ {
+ auto wms = this->_container.emplace(std::make_pair(k, this->id().withCoreSegmentName(k)));
+ wms.first->second.append(s);
+ }
+ }
+ }
+
virtual bool equalsDeep(const MemoryBase& other) const
{
//std::cout << "Memory::equalsDeep" << std::endl;
diff --git a/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h b/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
index 503650498d9afb5ddcdbbf22ecb84baaae3201f8..df5f3efc17a904775bce0790112e7e283573d7d0 100644
--- a/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
@@ -32,6 +32,20 @@ namespace armarx::armem::base
using EntitySnapshotT = typename EntityT::EntitySnapshotT;
using EntityInstanceT = typename EntitySnapshotT::EntityInstanceT;
+ struct UpdateResult
+ {
+ armarx::armem::UpdateType providerSegmentUpdateType;
+ armarx::armem::UpdateType entityUpdateType;
+ MemoryID id;
+ std::vector<EntitySnapshotT> removedSnapshots;
+
+ UpdateResult() = default;
+ UpdateResult(const typename EntityT::UpdateResult& c) :
+ entityUpdateType(c.entityUpdateType),
+ id(c.id),
+ removedSnapshots(c.removedSnapshots)
+ {}
+ };
public:
@@ -128,24 +142,49 @@ namespace armarx::armem::base
*
* Missing entity entries are added before updating.
*/
- virtual MemoryID update(const EntityUpdate& update) override
+ UpdateResult update(const EntityUpdate& update)
{
this->_checkContainerName(update.entityID.providerSegmentName, this->name());
EntityT* entity;
- auto it = this->_container.find(update.entityID.providerSegmentName);
+ UpdateType updateType = UpdateType::InsertedNew;
+
+ auto it = this->_container.find(update.entityID.entityName);
if (it == this->_container.end())
{
// Add entity entry.
entity = &addEntity(update.entityID.entityName);
entity->setMaxHistorySize(_maxHistorySize);
+ updateType = UpdateType::InsertedNew;
}
else
{
entity = &it->second;
+ updateType = UpdateType::UpdatedExisting;
}
// Update entity.
- return entity->update(update);
+ UpdateResult ret(entity->update(update));
+ ret.providerSegmentUpdateType = updateType;
+ return ret;
+ }
+
+ void append(const _Derived& m)
+ {
+ ARMARX_INFO << "ProviderSegment: Merge name '" << m.name() << "' into '" << name() << "'";
+
+ for (const auto& [k, s] : m.container())
+ {
+ if (const auto& it = this->_container.find(k); it != this->_container.end())
+ {
+ // segment already exists
+ it->second.append(s);
+ }
+ else
+ {
+ auto wms = this->_container.emplace(std::make_pair(k, this->id().withEntityName(k)));
+ wms.first->second.append(s);
+ }
+ }
}
/// Add an empty entity with the given name.
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/EntityContainerBase.h b/source/RobotAPI/libraries/armem/core/base/detail/EntityContainerBase.h
index 09733b648d096c7e33179c2903f3dbe241dca8d2..95993d560c6435fd147ddfc7a2a7aa9267d94855 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/EntityContainerBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/detail/EntityContainerBase.h
@@ -32,42 +32,18 @@ namespace armarx::armem::base::detail
using EntitySnapshotT = typename EntityT::EntitySnapshotT;
using EntityInstanceT = typename EntitySnapshotT::EntityInstanceT;
-
public:
using Base::MemoryContainerBase;
using Base::operator=;
-
- /**
- * @brief Store all updates in `commit`.
- * @param commit The commit.
- * @return The resulting memory IDs.
- */
- std::vector<MemoryID> update(const Commit& commit)
- {
- std::vector<MemoryID> ids;
- for (const auto& update : commit.updates)
- {
- ids.push_back(this->update(update));
- }
- return ids;
- }
- /**
- * @brief Store the given update.
- * @param update The update.
- * @return The resulting entity snapshot's ID.
- */
- virtual MemoryID update(const EntityUpdate& update) = 0;
-
-
/**
* @brief Retrieve an entity.
* @param id The entity ID.
* @return The entity.
* @throw An exception deriving from `armem::error::ArMemError` if the entity is missing.
*/
- virtual EntityT& getEntity(const MemoryID& id)
+ EntityT& getEntity(const MemoryID& id)
{
return const_cast<EntityT&>(const_cast<const EntityContainerBase*>(this)->getEntity(id));
}
@@ -97,12 +73,12 @@ namespace armarx::armem::base::detail
* @return The entity snapshot.
* @throw An exception deriving from `armem::error::ArMemError` if the snapshot is missing.
*/
- virtual EntitySnapshotT& getEntitySnapshot(const MemoryID& id)
+ EntitySnapshotT& getEntitySnapshot(const MemoryID& id)
{
return const_cast<EntitySnapshotT&>(const_cast<const EntityContainerBase*>(this)->getEntitySnapshot(id));
}
- virtual const EntitySnapshotT& getEntitySnapshot(const MemoryID& id) const
+ const EntitySnapshotT& getEntitySnapshot(const MemoryID& id) const
{
const EntityT& entity = getEntity(id);
@@ -116,12 +92,12 @@ namespace armarx::armem::base::detail
}
}
- virtual EntityInstanceT& getEntityInstance(const MemoryID& id)
+ EntityInstanceT& getEntityInstance(const MemoryID& id)
{
return getEntitySnapshot(id).getInstance(id);
}
- virtual const EntityInstanceT& getEntityInstance(const MemoryID& id) const
+ const EntityInstanceT& getEntityInstance(const MemoryID& id) const
{
return getEntitySnapshot(id).getInstance(id);
}
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/MaxHistorySize.h b/source/RobotAPI/libraries/armem/core/base/detail/MaxHistorySize.h
index 69539df5c4063b8d27cd751b79ee82eb90ff5b7e..1fc07ed85e54716d24d2447fa17cb95b291f2766 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/MaxHistorySize.h
+++ b/source/RobotAPI/libraries/armem/core/base/detail/MaxHistorySize.h
@@ -4,6 +4,7 @@
namespace armarx::armem::base::detail
{
+ // TODO: Replace by ConstrainedHistorySize (not only max entries, e.g. delete oldest / delete least accessed / ...)
class MaxHistorySize
{
public:
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/MemoryContainerBase.h b/source/RobotAPI/libraries/armem/core/base/detail/MemoryContainerBase.h
index b2155c70dbf645ebe0e2783fd3189bf9b58800af..5b61dc1eaf6a76948f86f3065be980c901777d17 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/MemoryContainerBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/detail/MemoryContainerBase.h
@@ -7,7 +7,6 @@
namespace armarx::armem::base::detail
{
-
/**
* @class Provides default implmentations of `MemoryContainer`, as well as
* iterators (which requires a template).
@@ -19,7 +18,6 @@ namespace armarx::armem::base::detail
using Base = MemoryItem;
public:
-
using DerivedT = _DerivedT;
using ContainerT = _ContainerT;
@@ -42,15 +40,19 @@ namespace armarx::armem::base::detail
// Container methods
- virtual bool empty() const
+ bool empty() const
{
return _container.empty();
}
- virtual std::size_t size() const
+ std::size_t size() const
{
return _container.size();
}
- virtual void clear()
+ bool hasData() const
+ {
+ return size() > 0;
+ }
+ void clear()
{
return _container.clear();
}
@@ -81,9 +83,7 @@ namespace armarx::armem::base::detail
return _container;
}
-
// Copying
-
virtual DerivedT copy() const
{
DerivedT d;
@@ -128,7 +128,7 @@ namespace armarx::armem::base::detail
protected:
- ContainerT _container;
+ mutable ContainerT _container;
};
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.cpp
index a99518db24ed00819be69f8085670272281daf58..c2b6a617e19ed8dfe6705b99f3e611d7b62b4dbe 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.cpp
@@ -8,23 +8,23 @@
namespace armarx::armem::ltm
{
- wm::CoreSegment CoreSegment::convert(const MongoDBConnectionManager::MongoDBSettings& dbsettings) const
+ wm::CoreSegment CoreSegment::convert() const
{
wm::CoreSegment m(id());
for (const auto& [_, s] : _container)
{
- m.addProviderSegment(s.convert(dbsettings));
+ m.addProviderSegment(s.convert());
}
return m;
}
- void CoreSegment::reload(const MongoDBConnectionManager::MongoDBSettings& dbsettings)
+ void CoreSegment::reload()
{
_container.clear();
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
mongocxx::database db = client[dbsettings.database];
- mongocxx::collection coll = db[MONGO_DB_COLLECTION_PREFIX + id().coreSegmentName];
+ mongocxx::collection coll = db[id().str()];
mongocxx::cursor cursor = coll.find({});
@@ -48,22 +48,23 @@ namespace armarx::armem::ltm
else
{
auto wms = _container.emplace(std::make_pair(k, id().withProviderSegmentName(k)));
- wms.first->second.reload(dbsettings);
+ wms.first->second.dbsettings = dbsettings;
+ wms.first->second.reload();
}
}
}
- void CoreSegment::append(const wm::CoreSegment& m, const MongoDBConnectionManager::MongoDBSettings& dbsettings)
+ void CoreSegment::append(const wm::CoreSegment& m)
{
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
mongocxx::database db = client[dbsettings.database];
- mongocxx::collection coll = db[MONGO_DB_COLLECTION_PREFIX + id().coreSegmentName];
+ mongocxx::collection coll = db[id().str()];
for (const auto& [k, s] : m.container())
{
if (const auto& it = _container.find(k); it != _container.end())
{
- it->second.append(s, dbsettings);
+ it->second.append(s);
}
else
{
@@ -74,7 +75,8 @@ namespace armarx::armem::ltm
coll.insert_one(foreign_key.view());
auto wms = _container.emplace(std::make_pair(k, id().withProviderSegmentName(k)));
- wms.first->second.append(s, dbsettings);
+ wms.first->second.dbsettings = dbsettings;
+ wms.first->second.append(s);
}
}
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.h b/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.h
index 1bbcd26b7a056907498c8b78607f1d77ebc0f5a4..f49db5d75cc8ba2469433f265c8096863fae4459 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.h
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.h
@@ -25,14 +25,14 @@ namespace armarx::armem::ltm
// Conversion
- wm::CoreSegment convert(const MongoDBConnectionManager::MongoDBSettings&) const;
+ wm::CoreSegment convert() const;
// MongoDB connection
- void reload(const MongoDBConnectionManager::MongoDBSettings&);
- void append(const wm::CoreSegment&, const MongoDBConnectionManager::MongoDBSettings&);
+ void reload();
+ void append(const wm::CoreSegment&);
- private:
- static const constexpr char* MONGO_DB_COLLECTION_PREFIX = "CoreSegment__";
+ public:
+ MongoDBConnectionManager::MongoDBSettings dbsettings;
};
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.cpp
index 00dbd2ae85c2d8fac7ab2ff73131c17da41eb7fe..a2e68a9e216355962d534ee9928d3547cc90774e 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.cpp
@@ -3,17 +3,17 @@
namespace armarx::armem::ltm
{
- wm::Entity Entity::convert(const MongoDBConnectionManager::MongoDBSettings& dbsettings) const
+ wm::Entity Entity::convert() const
{
wm::Entity m(id());
for (const auto& [_, s] : _container)
{
- m.addSnapshot(s.convert(dbsettings));
+ m.addSnapshot(s.convert());
}
return m;
}
- void Entity::reload(const MongoDBConnectionManager::MongoDBSettings& dbsettings)
+ void Entity::reload()
{
_container.clear();
@@ -21,15 +21,21 @@ namespace armarx::armem::ltm
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
mongocxx::database db = client[dbsettings.database];
- mongocxx::collection coll = db[MONGO_DB_COLLECTION_PREFIX + id().entityName];
+ mongocxx::collection coll = db[id().str()];
mongocxx::cursor cursor = coll.find({});
+ int i = 0;
for (auto doc : cursor)
{
+ if (i > MAX_HISTORY_SIZE)
+ {
+ // TODO: Add logic for most queried data?
+ break;
+ }
nlohmann::json json = nlohmann::json::parse(bsoncxx::to_json(doc));
auto k = armem::Time::microSeconds(json.at("timestamp"));
- ARMARX_INFO << "Entity: Found timestamp: " << std::to_string(k.toMicroSeconds());
+ //ARMARX_INFO << "Entity: Found timestamp: " << std::to_string(k.toMicroSeconds());
if (const auto& it = _container.find(k); it != _container.end())
{
@@ -38,29 +44,106 @@ namespace armarx::armem::ltm
else
{
auto wms = _container.emplace(std::make_pair(k, id().withTimestamp(k)));
- wms.first->second.reload(dbsettings);
+ wms.first->second.dbsettings = dbsettings;
+ wms.first->second.reload();
}
+ ++i;
}
}
- void Entity::append(const wm::Entity& m, const MongoDBConnectionManager::MongoDBSettings& dbsettings)
+ void Entity::append(const wm::Entity& m)
{
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
mongocxx::database db = client[dbsettings.database];
- mongocxx::collection coll = db[MONGO_DB_COLLECTION_PREFIX + id().entityName];
- coll.drop();
+ mongocxx::collection coll = db[id().str()];
for (const auto& [k, s] : m.container())
{
if (const auto& it = _container.find(k); it != _container.end())
{
- it->second.setTo(s, k, dbsettings);
+ // segment already exists
+ // We assume that a snapshot does not change, so ignore
}
else
{
auto wms = _container.emplace(std::make_pair(k, id().withTimestamp(k)));
- wms.first->second.setTo(s, k, dbsettings);
+ wms.first->second.dbsettings = dbsettings;
+ wms.first->second.setTo(s, k);
+ //truncateHistoryToSize();
}
}
}
+
+ bool Entity::hasSnapshot(const Time& time) const
+ {
+ // check cache
+ if (Base::hasSnapshot(time))
+ {
+ return true;
+ }
+ // check mongodb
+ mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
+ mongocxx::database db = client[dbsettings.database];
+ mongocxx::collection coll = db[id().str()];
+
+ bsoncxx::stdx::optional<bsoncxx::document::value> maybe_result = coll.find_one(document{} << "timestamp" << time.toMicroSeconds() << finalize);
+ if (!maybe_result)
+ {
+ return false;
+ }
+
+ auto json = nlohmann::json::parse(bsoncxx::to_json(*maybe_result));
+ auto id = MemoryID::fromString(json["id"].get<std::string>());
+ auto instances = json["instances"];
+ EntitySnapshot snapshot(id);
+ snapshot.dbsettings = dbsettings;
+
+ for (unsigned int i = 0; i < instances.size(); ++i)
+ {
+ EntityInstance instance(id.withInstanceIndex(i));
+ snapshot.addInstance(instance);
+ }
+
+ _container.insert({time, snapshot});
+ //truncateHistoryToSize();
+ return true;
+ }
+
+ std::vector<Time> Entity::getTimestamps() const
+ {
+ // get from cache
+ std::vector<Time> ret; // = Base::getTimestamps();
+
+ // get missing from mongodb
+ mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
+ mongocxx::database db = client[dbsettings.database];
+ mongocxx::collection coll = db[id().str()];
+
+ auto cursor = coll.find({});
+ for (auto doc : cursor)
+ {
+ auto json = nlohmann::json::parse(bsoncxx::to_json(doc));
+ auto ts = json["timestamp"].get<long>();
+ ret.push_back(Time::microSeconds(ts));
+ }
+ return ret;
+ }
+
+ const EntitySnapshot& Entity::getSnapshot(const Time& time) const
+ {
+ if (!hasSnapshot(time))
+ {
+ throw error::MissingEntry("ltm entity snapshot", toDateTimeMilliSeconds(time), getLevelName(), this->id().str());
+ }
+
+ // the above command already puts the reference to the cache
+ return Base::getSnapshot(time);
+ }
+
+ const std::map<Time, EntitySnapshot>::value_type& Entity::getLatestItem() const
+ {
+ // Directly query mongodb (cache cant know whether it is the latest or not)
+ // TODO
+ return Base::getLatestItem();
+ }
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.h b/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.h
index 54e004f4875242a30b538864695d13fe1f553582..9f26b865c4bbc4541fa408731d8ed6aba07f5fdf 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.h
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.h
@@ -41,62 +41,30 @@ namespace armarx::armem::ltm
Entity()
{
// the history of snapshots is just a cache of frequently used elements
- setMaxHistorySize(20);
+ setMaxHistorySize(MAX_HISTORY_SIZE);
}
// Conversion
- wm::Entity convert(const MongoDBConnectionManager::MongoDBSettings&) const;
+ wm::Entity convert() const;
// MongoDB connection
- void reload(const MongoDBConnectionManager::MongoDBSettings&);
- void append(const wm::Entity&, const MongoDBConnectionManager::MongoDBSettings&);
+ void reload();
+ void append(const wm::Entity&);
- // virtual overrides for LTM storage
- /*virtual bool hasSnapshot(Time time) const override
- {
- // check cache
- if (Base::hasSnapshot(time))
- {
- return true;
- }
- // check mongodb
- return false;
- // TODO
- }
- virtual std::vector<Time> getTimestamps() const override
- {
- // get from cache
- std::vector<Time> ret = Base::getTimestamps();
- // get missing from mongodb
- // TODO
- return ret;
- }
- virtual const EntitySnapshotT& getSnapshot(Time time) const override
- {
- try
- {
- return Base::getSnapshot(time);
- }
- catch (const error::MissingEntry& _)
- {
- // if not in cache then get from mongo db
- // TODO
- return Base::getSnapshot(time);
- }
- }
+ // virtual overrides for LTM lookups
+ virtual bool hasSnapshot(const Time& time) const override;
+ virtual std::vector<Time> getTimestamps() const override;
+ virtual const EntitySnapshot& getSnapshot(const Time& time) const override;
- protected:
+ protected:
// virtual overrides for LTM storage
- virtual const typename std::map<Time, EntitySnapshotT>::value_type& getLatestItem() const override
- {
- // Directly query mongodb (cache cant know whether it is the latest or not)
- // TODO
- return Base::getLatestItem();
- }*/
+ virtual const std::map<Time, EntitySnapshot>::value_type& getLatestItem() const override;
+ public:
+ MongoDBConnectionManager::MongoDBSettings dbsettings;
private:
- static const constexpr char* MONGO_DB_COLLECTION_PREFIX = "Entity__";
+ const static constexpr int MAX_HISTORY_SIZE = 200;
};
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.cpp
index 412dbe1c41603f80960f9087c2a6277274f67c89..081347cafd19fc688292ca05eee6ee7e42dc201e 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.cpp
@@ -11,11 +11,11 @@
namespace armarx::armem::ltm
{
- wm::EntitySnapshot EntitySnapshot::convert(const MongoDBConnectionManager::MongoDBSettings& dbsettings, const aron::typenavigator::NavigatorPtr& expectedStructure) const
+ wm::EntitySnapshot EntitySnapshot::convert(const aron::typenavigator::NavigatorPtr& expectedStructure) const
{
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
mongocxx::database db = client[dbsettings.database];
- mongocxx::collection coll = db[MONGO_DB_COLLECTION_PREFIX + id().coreSegmentName];
+ mongocxx::collection coll = db[id().getEntityID().str()];
auto res = coll.find_one(document{} << "id" << id().getEntitySnapshotID().str() << finalize);
@@ -46,13 +46,13 @@ namespace armarx::armem::ltm
return m;
}
- void EntitySnapshot::reload(const MongoDBConnectionManager::MongoDBSettings& dbsettings)
+ void EntitySnapshot::reload()
{
_container.clear();
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
mongocxx::database db = client[dbsettings.database];
- mongocxx::collection coll = db[MONGO_DB_COLLECTION_PREFIX + id().coreSegmentName];
+ mongocxx::collection coll = db[id().getEntityID().str()];
mongocxx::cursor cursor = coll.find({});
for (auto doc : cursor)
@@ -67,14 +67,14 @@ namespace armarx::armem::ltm
}
}
- void EntitySnapshot::setTo(const wm::EntitySnapshot& m, const armem::Time& t, const MongoDBConnectionManager::MongoDBSettings& dbsettings)
+ void EntitySnapshot::setTo(const wm::EntitySnapshot& m, const armem::Time& t)
{
// We remove the contente here and reset it with new values
_container.clear();
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
mongocxx::database db = client[dbsettings.database];
- mongocxx::collection coll = db[MONGO_DB_COLLECTION_PREFIX + id().entityName];
+ mongocxx::collection coll = db[id().getEntityID().str()];
bsoncxx::builder::stream::document builder{};
auto in_array = builder
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.h b/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.h
index b81f0f8e1140eaaa6d0e916ac38d86655ab86140..be1992708516f74d95ec799946bbcba547b9348f 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.h
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.h
@@ -25,13 +25,13 @@ namespace armarx::armem::ltm
// Conversion
- wm::EntitySnapshot convert(const MongoDBConnectionManager::MongoDBSettings&, const aron::typenavigator::NavigatorPtr& = nullptr) const;
+ wm::EntitySnapshot convert(const aron::typenavigator::NavigatorPtr& = nullptr) const;
// MongoDB connection
- void reload(const MongoDBConnectionManager::MongoDBSettings&);
- void setTo(const wm::EntitySnapshot&, const armem::Time& t, const MongoDBConnectionManager::MongoDBSettings&);
+ void reload();
+ void setTo(const wm::EntitySnapshot&, const armem::Time& t);
- private:
- static const constexpr char* MONGO_DB_COLLECTION_PREFIX = "Entity__";
+ public:
+ MongoDBConnectionManager::MongoDBSettings dbsettings;
};
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp
index 4079ebde68babd7d77fc01c1bea827c99e2d11a5..f143f814f0432fe1b43572e44e9bd9d592ffe977 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp
@@ -15,7 +15,7 @@ namespace armarx::armem::ltm
wm::Memory m(id());
for (const auto& [_, s] : _container)
{
- m.addCoreSegment(s.convert(dbsettings));
+ m.addCoreSegment(s.convert());
}
return m;
}
@@ -27,7 +27,7 @@ namespace armarx::armem::ltm
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
mongocxx::database db = client[dbsettings.database];
- mongocxx::collection coll = db[MONGO_DB_COLLECTION_PREFIX + id().memoryName];
+ mongocxx::collection coll = db[id().str()];
mongocxx::cursor cursor = coll.find({});
for (auto doc : cursor)
@@ -50,27 +50,28 @@ namespace armarx::armem::ltm
else
{
auto wms = _container.emplace(std::make_pair(k, id().withCoreSegmentName(k)));
- wms.first->second.reload(dbsettings);
+ wms.first->second.dbsettings = dbsettings;
+ wms.first->second.reload();
}
}
}
void Memory::append(const wm::Memory& m)
{
- ARMARX_INFO << "Appending Memory with name: " << m.name();
+ ARMARX_INFO << "Merge memory with name '" << m.name() << "' into the LTM with name '" << name() << "'";
TIMING_START(LTM_Append);
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
mongocxx::database db = client[dbsettings.database];
- mongocxx::collection coll = db[MONGO_DB_COLLECTION_PREFIX + id().memoryName];
+ mongocxx::collection coll = db[id().str()];
for (const auto& [k, s] : m.container())
{
if (const auto& it = _container.find(k); it != _container.end())
{
// TODO check if foreign key exists
- it->second.append(s, dbsettings);
+ it->second.append(s);
}
else
{
@@ -81,7 +82,8 @@ namespace armarx::armem::ltm
coll.insert_one(foreign_key.view());
auto wms = _container.emplace(std::make_pair(k, id().withCoreSegmentName(k)));
- wms.first->second.append(s, dbsettings);
+ wms.first->second.dbsettings = dbsettings;
+ wms.first->second.append(s);
}
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.h b/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.h
index 9caa6b795732dc9b0ab82254027519c5e014ae71..f7b816449a29d1a5c97367429d4690b3c13794e2 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.h
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.h
@@ -58,8 +58,5 @@ namespace armarx::armem::ltm
PeriodicTransfer periodic_transfer;
FilledTransferMode filled_transfer;
-
- private:
- static const constexpr char* MONGO_DB_COLLECTION_PREFIX = "Memory__";
};
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.cpp
index d8ca92c97755c4f26f1553d5b98479cb87b19d00..2ebcd5237ec98ed8b105b4003be94921dbdf7c67 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.cpp
@@ -8,23 +8,23 @@
namespace armarx::armem::ltm
{
- wm::ProviderSegment ProviderSegment::convert(const MongoDBConnectionManager::MongoDBSettings& dbsettings) const
+ wm::ProviderSegment ProviderSegment::convert() const
{
wm::ProviderSegment m(id());
for (const auto& [_, s] : _container)
{
- m.addEntity(s.convert(dbsettings));
+ m.addEntity(s.convert());
}
return m;
}
- void ProviderSegment::reload(const MongoDBConnectionManager::MongoDBSettings& dbsettings)
+ void ProviderSegment::reload()
{
_container.clear();
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
mongocxx::database db = client[dbsettings.database];
- mongocxx::collection coll = db[MONGO_DB_COLLECTION_PREFIX + id().providerSegmentName];
+ mongocxx::collection coll = db[id().str()];
mongocxx::cursor cursor = coll.find({});
for (auto doc : cursor)
@@ -47,22 +47,23 @@ namespace armarx::armem::ltm
else
{
auto wms = _container.emplace(std::make_pair(k, id().withEntityName(k)));
- wms.first->second.reload(dbsettings);
+ wms.first->second.dbsettings = dbsettings;
+ wms.first->second.reload();
}
}
}
- void ProviderSegment::append(const wm::ProviderSegment& m, const MongoDBConnectionManager::MongoDBSettings& dbsettings)
+ void ProviderSegment::append(const wm::ProviderSegment& m)
{
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
mongocxx::database db = client[dbsettings.database];
- mongocxx::collection coll = db[MONGO_DB_COLLECTION_PREFIX + id().providerSegmentName];
+ mongocxx::collection coll = db[id().str()];
for (const auto& [k, s] : m.container())
{
if (const auto& it = _container.find(k); it != _container.end())
{
- it->second.append(s, dbsettings);
+ it->second.append(s);
}
else
{
@@ -73,7 +74,8 @@ namespace armarx::armem::ltm
coll.insert_one(foreign_key.view());
auto wms = _container.emplace(std::make_pair(k, id().withEntityName(k)));
- wms.first->second.append(s, dbsettings);
+ wms.first->second.dbsettings = dbsettings;
+ wms.first->second.append(s);
}
}
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.h b/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.h
index 3c19ec5816ceb2c629a252e45d0b25d14cb8219e..5aecdb21ab0166a6d637c6f4e80ac4dbdae34377 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.h
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.h
@@ -25,14 +25,14 @@ namespace armarx::armem::ltm
// Conversion
- wm::ProviderSegment convert(const MongoDBConnectionManager::MongoDBSettings&) const;
+ wm::ProviderSegment convert() const;
// MongoDB connection
- void reload(const MongoDBConnectionManager::MongoDBSettings&);
- void append(const wm::ProviderSegment&, const MongoDBConnectionManager::MongoDBSettings&);
+ void reload();
+ void append(const wm::ProviderSegment&);
- private:
- static const constexpr char* MONGO_DB_COLLECTION_PREFIX = "ProviderSegment__";
+ public:
+ MongoDBConnectionManager::MongoDBSettings dbsettings;
};
}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.cpp
index b4341c9da6b9bf6c1c9be6e1141ff712d9e00132..285235b76e397952301c23d8218abb11f1f7aeaf 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.cpp
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.cpp
@@ -7,6 +7,17 @@
namespace armarx::armem::wm
{
+
+ Commit CoreSegment::toCommit() const
+ {
+ Commit c;
+ for (const auto& [k, s] : _container)
+ {
+ c.append(s.toCommit());
+ }
+ return c;
+ }
+
void CoreSegment::_copySelfWithoutData(CoreSegment& other) const
{
other.id() = _id;
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h b/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h
index 1eaf327338929b1290e476951aa69bdde0d3723d..7847bbb71cc6a1a269556aeec07738c396e42a68 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h
@@ -27,8 +27,11 @@ namespace armarx::armem::wm
CoreSegment& operator=(const CoreSegment& other) = default;
CoreSegment& operator=(CoreSegment&& other) = default;
- virtual ~CoreSegment() override = default;
-
+ /**
+ * @brief Convert the content of this segmnet into a commit
+ * @return The resulting commit
+ */
+ Commit toCommit() const;
protected:
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/Entity.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/Entity.cpp
index 022864c19d9ea98583d9d3998bec46643921cdc8..e704e96d5454d27bfb095120c1901e000611122a 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/Entity.cpp
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/Entity.cpp
@@ -2,6 +2,20 @@
namespace armarx::armem::wm
{
+
+ Commit Entity::toCommit() const
+ {
+ Commit c;
+ for (const auto& [k, s] : _container)
+ {
+ EntityUpdate& up = c.add();
+ up.entityID = id();
+ up.timeCreated = k;
+ up.instancesData = s.getAronData();
+ }
+ return c;
+ }
+
void Entity::_copySelfWithoutData(Entity& other) const
{
other.id() = _id;
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/Entity.h b/source/RobotAPI/libraries/armem/core/workingmemory/Entity.h
index 6a31286722e0e78d53423f6aa6b517583f83ca09..a6a7621e21272db6447e9e09b695900eefd263c1 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/Entity.h
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/Entity.h
@@ -42,8 +42,11 @@ namespace armarx::armem::wm
Entity& operator=(const Entity& other) = default;
Entity& operator=(Entity&& other) = default;
- virtual ~Entity() override = default;
-
+ /**
+ * @brief Convert the content of this segmnet into a commit
+ * @return The resulting commit
+ */
+ Commit toCommit() const;
protected:
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.cpp
index fb68d050a0ad19e1e2627e4cb5732a23a2ce4493..76d5400131a7ec6a57478f13afb0dfc38a4da174 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.cpp
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.cpp
@@ -7,6 +7,16 @@
namespace armarx::armem::wm
{
+ std::vector<aron::datanavigator::DictNavigatorPtr> EntitySnapshot::getAronData() const
+ {
+ std::vector<aron::datanavigator::DictNavigatorPtr> ret;
+ for (const auto& aron : _container)
+ {
+ ret.push_back(aron.data());
+ }
+ return ret;
+ }
+
void EntitySnapshot::_copySelfWithoutData(EntitySnapshot& other) const
{
other.id() = _id;
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.h b/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.h
index ee0c78c5d201ed2e9fb1fe5c333efc728a9dccfc..3eedb48d0b4aaa5191541d8ca1586ffc59ab5998 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.h
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.h
@@ -23,6 +23,7 @@ namespace armarx::armem::wm
using Base::EntitySnapshotBase;
using Base::operator=;
+ std::vector<aron::datanavigator::DictNavigatorPtr> getAronData() const;
protected:
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/Memory.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/Memory.cpp
index b7af1a6456687b1741710d46fa157b9091295db7..3d2efd2249f969308b126608d01533500f3bfa73 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/Memory.cpp
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/Memory.cpp
@@ -3,6 +3,16 @@
namespace armarx::armem::wm
{
+ Commit Memory::toCommit() const
+ {
+ Commit c;
+ for (const auto& [k, s] : _container)
+ {
+ c.append(s.toCommit());
+ }
+ return c;
+ }
+
void Memory::_copySelfWithoutData(Memory& other) const
{
other.id() = _id;
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/Memory.h b/source/RobotAPI/libraries/armem/core/workingmemory/Memory.h
index e8b7cb35349837820c3dd47f2d584ef6b9760461..3c64c0841b46a5c95f7f7365af26a1b20226f4dc 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/Memory.h
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/Memory.h
@@ -27,6 +27,11 @@ namespace armarx::armem::wm
Memory& operator=(const Memory& other) = default;
Memory& operator=(Memory&& other) = default;
+ /**
+ * @brief Convert the content of this memory into a commit
+ * @return The resulting commit
+ */
+ Commit toCommit() const;
protected:
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.cpp
index 517b35bfad666cdac02041694e787523c4c1e894..bd03cf21cd8b616a3afcc4d531377288e55c5f2f 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.cpp
@@ -8,6 +8,16 @@
namespace armarx::armem::wm
{
+ Commit ProviderSegment::toCommit() const
+ {
+ Commit c;
+ for (const auto& [k, s] : _container)
+ {
+ c.append(s.toCommit());
+ }
+ return c;
+ }
+
void ProviderSegment::_copySelfWithoutData(ProviderSegment& other) const
{
other.id() = _id;
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h b/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h
index 0b703617d9b1b642153da505882e71aab8aa3723..7d1e9db335ee39433351a8e15113059836bf825d 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h
@@ -28,7 +28,11 @@ namespace armarx::armem::wm
ProviderSegment& operator=(const ProviderSegment& other) = default;
ProviderSegment& operator=(ProviderSegment&& other) = default;
- virtual ~ProviderSegment() override = default;
+ /**
+ * @brief Convert the content of this segmnet into a commit
+ * @return The resulting commit
+ */
+ Commit toCommit() const;
protected:
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
index 75e38944378144ee47fe13cbd733445555fbe48f..95976635370ee454e9d1bde89cc36ca2d99afb65 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
@@ -5,9 +5,8 @@
#include "../error.h"
#include "../core/workingmemory/ice_conversions.h"
#include "query_proc/workingmemory/MemoryQueryProcessor.h"
+#include "query_proc/longtermmemory/MemoryQueryProcessor.h"
-//#include "../core/io/diskWriter/NlohmannJSON/NlohmannJSONDiskWriter.h"
-//#include "../core/io/diskReader/NlohmannJSON/NlohmannJSONDiskReader.h"
namespace armarx::armem::server
{
@@ -151,16 +150,22 @@ namespace armarx::armem::server
EntityUpdateResult& result = commitResult.results.emplace_back();
try
{
- MemoryID snapshotID = workingMemory->update(update);
+ auto updateResult = workingMemory->update(update);
result.success = true;
- result.snapshotID = snapshotID;
+ result.snapshotID = updateResult.id;
result.timeArrived = update.timeArrived;
+ // TODO: Add if and param here (fabian.peller)
+ for (const auto& snapshot : updateResult.removedSnapshots)
+ {
+ ARMARX_IMPORTANT << "The id " << snapshot.id() << " was removed from wm";
+ }
+
if (publishUpdates)
{
data::MemoryID& id = updatedIDs.emplace_back();
- armem::toIce(id, snapshotID);
+ armem::toIce(id, result.snapshotID);
}
}
catch (const error::ArMemError& e)
@@ -195,13 +200,36 @@ namespace armarx::armem::server
{
ARMARX_CHECK_NOT_NULL(workingMemory);
- armem::wm::query_proc::MemoryQueryProcessor processor(
+ armem::wm::query_proc::MemoryQueryProcessor wm_processor(
input.withData ? armem::DataMode::WithData
: armem::DataMode::NoData);
- return processor.process(input, *workingMemory);
+
+ armem::query::data::Result result;
+ wm::Memory wm_result = wm_processor.process(input, *workingMemory, /* ignore if: */ { query::data::QueryTarget::LTM });
+
+ armem::ltm::query_proc::MemoryQueryProcessor ltm_processor;
+ ltm::Memory ltm_result = ltm_processor.process(input, *longtermMemory, /* ignore if: */ { query::data::QueryTarget::WM });
+
+ if (ltm_result.hasData())
+ {
+ // 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 ltm_converted = ltm_result.convert();
+ wm_result.append(ltm_converted);
+
+ this->commit(ltm_converted.toCommit());
+ }
+
+ result.memory = toIce<data::MemoryPtr>(wm_result);
+ result.success = true;
+
+ return result;
}
- // LTM LOADING
+ // LTM LOADING FROM LTM
query::data::Result MemoryToIceAdapter::load(const armem::query::data::Input& query)
{
ARMARX_CHECK_NOT_NULL(longtermMemory);
@@ -218,6 +246,13 @@ namespace armarx::armem::server
ARMARX_CHECK_NOT_NULL(longtermMemory);
data::StoreResult output;
+ for (const auto& query : input.query.memoryQueries)
+ {
+ // force query to only query WM
+ // TODO: Think about other query class (fabian.peller)
+ query->target = query::data::QueryTarget::WM;
+ }
+
armem::query::data::Result queryResult = this->query(input.query);
output.success = queryResult.success;
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.h
index 3ea2b0bdd45230dab810bd628cf338d2b685662b..6499cadc7bab7869e75fa2c054f32cad12b0ad5c 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.h
@@ -2,8 +2,6 @@
#include <RobotAPI/interface/armem/query.h>
-#include <RobotAPI/libraries/armem/core/DataMode.h>
-
namespace armarx::armem::base::query_proc
{
@@ -20,29 +18,37 @@ namespace armarx::armem::base::query_proc
public:
- DataT process(const QueryT& query, const DataT& data) const
+ DataT process(const QueryT& query, const DataT& data, const std::vector<query::data::QueryTarget>& ignoreTargets = {}) const
{
DataT result = data.copyEmpty();
+ if (std::find(ignoreTargets.begin(), ignoreTargets.end(), query.target) != ignoreTargets.end())
+ {
+ return result;
+ }
this->process(result, query, data);
return result;
}
- DataT process(const QueryPtrT& query, const DataT& data) const
+ DataT process(const QueryPtrT& query, const DataT& data, const std::vector<query::data::QueryTarget>& ignoreTargets = {}) const
{
- return this->process(*query, *data);
+ return this->process(*query, *data, ignoreTargets);
}
- DataT process(const QuerySeqT& queries, const DataT& data) const
+ DataT process(const QuerySeqT& queries, const DataT& data, const std::vector<query::data::QueryTarget>& ignoreTargets = {}) const
{
DataT result = data.copyEmpty();
- this->process(result, queries, data);
+ this->process(result, queries, data, ignoreTargets);
return result;
}
- void process(DataT& result, const QuerySeqT& queries, const DataT& data) const
+ void process(DataT& result, const QuerySeqT& queries, const DataT& data, const std::vector<query::data::QueryTarget>& ignoreTargets = {}) const
{
for (const auto& query : queries)
{
+ if (std::find(ignoreTargets.begin(), ignoreTargets.end(), query->target) != ignoreTargets.end())
+ {
+ continue;
+ }
this->process(result, *query, data);
}
}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
index 2a4308ebdc837255a9f6a816dc62880dc09e8c21..a39e66e7745d3da8e6335aae99d3d1230cdefca2 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
@@ -104,7 +104,8 @@ namespace armarx::armem::base::query_proc
Time time = fromIce<Time>(query.timestamp);
try
{
- addResultSnapshot(result, entity.getSnapshot(time));
+ auto snapshot = entity.getSnapshot(time);
+ addResultSnapshot(result, snapshot);
}
catch (const armem::error::MissingEntry&)
{
@@ -170,61 +171,6 @@ namespace armarx::armem::base::query_proc
}
- inline auto lastElementBeforeOrAt(const auto& history, const auto timestamp) const
- {
- // first element equal or greater
- typename std::map<Time, EntitySnapshotT>::const_iterator refItFwd = history.upper_bound(timestamp);
-
- // last element less than
- typename std::map<Time, EntitySnapshotT>::const_iterator refItFwdLt = std::prev(refItFwd);
-
- // last element not greater than => if this is invalid, we have no suitable elements
- typename std::map<Time, EntitySnapshotT>::const_reverse_iterator refIt(refItFwd);
- if (refIt == history.rend())
- {
- return history.end();
- }
-
- // now either refItFwd is a perfect match ...
- if (refItFwd->first == timestamp)
- {
- return refItFwd;
- }
-
- // ... or we return the element before if possible
- if (refItFwdLt != history.begin())
- {
- return refItFwdLt;
- }
-
- return history.end();
- }
-
- inline auto lastElementBefore(const auto& history, const auto& timestamp) const
- {
- // first element equal or greater
- typename std::map<Time, EntitySnapshotT>::const_iterator refItFwd = history.upper_bound(timestamp);
-
- // last element less than
- typename std::map<Time, EntitySnapshotT>::const_iterator refItFwdLt = std::prev(refItFwd);
-
- // last element not greater than => if this is invalid, we have no suitable elements
- typename std::map<Time, EntitySnapshotT>::const_reverse_iterator refIt(refItFwd);
- if (refIt == history.rend())
- {
- return history.end();
- }
-
- // we return the element before if possible
- if (refItFwdLt != history.begin())
- {
- return refItFwdLt;
- }
-
- return history.end();
- }
-
-
void process(_EntityT& result,
const armem::query::data::entity::BeforeOrAtTime& query,
const _EntityT& entity) const
@@ -232,12 +178,15 @@ namespace armarx::armem::base::query_proc
const auto referenceTimestamp = fromIce<Time>(query.timestamp);
ARMARX_CHECK(referenceTimestamp.toMicroSeconds() >= 0) << "Reference timestamp is negative!";
- const auto beforeOrAt = lastElementBeforeOrAt(entity.history(), referenceTimestamp);
-
- if (beforeOrAt != entity.history().end())
+ try
{
+ auto beforeOrAt = entity.getLatestSnapshotBeforeOrAt(referenceTimestamp);
addResultSnapshot(result, beforeOrAt);
}
+ catch (const armem::error::MissingEntry&)
+ {
+ // Leave empty.
+ }
}
@@ -250,31 +199,24 @@ namespace armarx::armem::base::query_proc
const auto maxEntries = fromIce<std::int64_t>(query.maxEntries);
- const auto itBefore = lastElementBefore(entity.history(), referenceTimestamp);
- if (itBefore == entity.history().end())
+ try
{
- ARMARX_WARNING << "No suitable element found";
- return;
- }
-
- const auto nEntriesBefore = std::distance(entity.history().begin(), itBefore);
+ const auto before = entity.getSnapshotsBefore(referenceTimestamp);
- const int nEntries = [&]()
- {
- // see query.ice
- if (maxEntries > 0)
+ int i = 0;
+ for (const auto& snapshot : before)
{
- return std::min(nEntriesBefore, maxEntries);
+ if (maxEntries >= 0 && i >= maxEntries)
+ {
+ break;
+ }
+ addResultSnapshot(result, snapshot);
+ ++i;
}
-
- return nEntriesBefore; // all elements before timestamp
}
- ();
-
- auto it = itBefore;
- for (std::int64_t i = 0; i < nEntries; i++, --it)
+ catch (const error::MissingEntry&)
{
- addResultSnapshot(result, it);
+ // Leave empty.
}
}
@@ -282,7 +224,6 @@ namespace armarx::armem::base::query_proc
const armem::query::data::entity::TimeApprox& query,
const _EntityT& entity) const
{
-
const auto referenceTimestamp = fromIce<Time>(query.timestamp);
ARMARX_CHECK(referenceTimestamp.toMicroSeconds() >= 0) << "Reference timestamp is negative!";
@@ -300,64 +241,35 @@ namespace armarx::armem::base::query_proc
return std::abs(t.toMicroSeconds() - referenceTimestampMicroSeconds) <= epsDuration;
};
- // first element before or at timestamp
- const auto beforeOrAt = lastElementBeforeOrAt(entity.history(), referenceTimestamp);
- const bool isBeforeOrAtValid = beforeOrAt != entity.history().end();
- const auto isPerfectMatch = isBeforeOrAtValid && beforeOrAt->first == referenceTimestamp;
-
- // first element greater
- const auto after = entity.history().upper_bound(referenceTimestamp);
- const bool isAfterValid = after != entity.history().end();
-
- // if both are invalid, there is nothing to be gained here.
- if ((not isBeforeOrAtValid) and (not isAfterValid))
+ try
{
- const armem::Time dt = referenceTimestamp - entity.getLatestTimestamp();
-
- ARMARX_WARNING << "Lookup " << dt.toMilliSeconds() << " ms into the future.";
- return;
- }
- // -> now one or both are valid ...
-
- // is 'before' a perfect match?
- if(isPerfectMatch)
- {
- addResultSnapshot(result, beforeOrAt);
- return;
- }
-
- // only 'before' valid?
- if (not isAfterValid)
- {
- if (isInRange(beforeOrAt->first))
+ // last element before or at timestamp
+ auto beforeOrAt = entity.getLatestSnapshotBeforeOrAt(referenceTimestamp);
+ const auto timestampOfMatchBefore = beforeOrAt.id().timestamp;
+ const auto isPerfectMatch = timestampOfMatchBefore == referenceTimestamp;
+ if (isInRange(timestampOfMatchBefore))
{
addResultSnapshot(result, beforeOrAt);
}
- return;
- }
- // only 'after' valid?
- if (not isBeforeOrAtValid)
- {
- if (isInRange(after->first))
+ // earsly stop, not necessary to also get the next since the match is perfect
+ if (isPerfectMatch)
+ {
+ return;
+ }
+
+ // first element after or at timestamp (or at because of fewer checks, we can assure that there is not element at)
+ const auto after = entity.getFirstSnapshotAfterOrAt(referenceTimestamp);
+ const auto timestampOfMatchAfter = after.id().timestamp;
+ if (isInRange(timestampOfMatchAfter))
{
addResultSnapshot(result, after);
}
- return;
}
- // -> now both are valid
-
- // return both (if in range) => user can interpolate
- if (isInRange(beforeOrAt->first))
+ catch (const armem::error::MissingEntry&)
{
- addResultSnapshot(result, beforeOrAt);
- }
- if (isInRange(after->first))
- {
- addResultSnapshot(result, after);
}
-
}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h
index c50086b553164b70f10ed1559a18420649eb3892..5de194d7ba2debada4ef9363005b1a7f2e927e94 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h
@@ -32,6 +32,13 @@ namespace armarx::armem::base::query_proc
using EntityT = typename ProviderSegmentT::EntityT;
using EntitySnapshotT = typename EntityT::EntitySnapshotT;
+
+ using Base::process;
+ _MemoryT process(const armem::query::data::Input& input, const _MemoryT& memory, const std::vector<query::data::QueryTarget>& ignoreTargets = {}) const
+ {
+ return this->process(input.memoryQueries, memory, ignoreTargets);
+ }
+
void process(_MemoryT& result,
const armem::query::data::MemoryQuery& query,
const _MemoryT& memory) const override
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/MemoryQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/MemoryQueryProcessor.h
index b017c3b561569c1da5d3d7f661ee604bbb1cae12..b0616c658a7915568fd910a8f35c9d4701aa1d0b 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/MemoryQueryProcessor.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/MemoryQueryProcessor.h
@@ -21,28 +21,6 @@ namespace armarx::armem::wm::query_proc
Base(dataMode), coreSegmentProcessor(dataMode)
{}
- armem::query::data::Result process(const armem::query::data::Input& input,
- const wm::Memory& memory) const
- {
- armem::query::data::Result result;
- result.memory = processToIce(input.memoryQueries, memory);
- result.success = true;
- return result;
- }
-
- using Base::process;
- data::MemoryPtr processToIce(const armem::query::data::MemoryQuery& query,
- const wm::Memory& memory) const
- {
- return toIce<data::MemoryPtr>(process(query, memory));
- }
-
- data::MemoryPtr processToIce(const armem::query::data::MemoryQuerySeq& queries,
- const wm::Memory& memory) const
- {
- return toIce<data::MemoryPtr>(process(queries, memory));
- }
-
protected:
virtual CoreSegmentT coreSegmentProcessorProcess(const armem::query::data::CoreSegmentQuerySeq& q, const CoreSegmentT& s) const override
{
diff --git a/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp
index 789600c2fe82857b865c845d6892fc2da44d3d6a..b2d07571d619531f45381d1238ca8201b8e0175e 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp
@@ -134,7 +134,7 @@ BOOST_AUTO_TEST_CASE(test_entity_Single_latest)
BOOST_AUTO_TEST_CASE(test_entity_Single_existing)
{
- addResults(query::entity::Single { 3000 });
+ addResults(query::entity::Single { query::QueryTarget::WMAndLTM, 3000 });
BOOST_REQUIRE_GT(results.size(), 0);
for (const armem::wm::Entity& result : results)
@@ -148,7 +148,7 @@ BOOST_AUTO_TEST_CASE(test_entity_Single_existing)
BOOST_AUTO_TEST_CASE(test_entity_Single_non_existing)
{
- addResults(query::entity::Single { 3500 });
+ addResults(query::entity::Single { query::QueryTarget::WMAndLTM, 3500 });
BOOST_REQUIRE_GT(results.size(), 0);
for (const armem::wm::Entity& result : results)
@@ -182,7 +182,7 @@ BOOST_AUTO_TEST_CASE(test_entity_All)
BOOST_AUTO_TEST_CASE(test_entity_TimeRange_slice)
{
- addResults(query::entity::TimeRange{ 1500, 3500 });
+ addResults(query::entity::TimeRange{ query::QueryTarget::WMAndLTM, 1500, 3500 });
BOOST_REQUIRE_GT(results.size(), 0);
for (const armem::wm::Entity& result : results)
@@ -203,7 +203,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_slice)
BOOST_AUTO_TEST_CASE(test_entity_TimeRange_exact)
{
- addResults(query::entity::TimeRange{ 2000, 4000 });
+ addResults(query::entity::TimeRange{ query::QueryTarget::WMAndLTM, 2000, 4000 });
BOOST_REQUIRE_GT(results.size(), 0);
for (const armem::wm::Entity& result : results)
@@ -223,8 +223,8 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_exact)
BOOST_AUTO_TEST_CASE(test_entity_TimeRange_all)
{
- addResults(query::entity::TimeRange{ 0, 10000 });
- addResults(query::entity::TimeRange{ -1, -1 });
+ addResults(query::entity::TimeRange{ query::QueryTarget::WMAndLTM, 0, 10000 });
+ addResults(query::entity::TimeRange{ query::QueryTarget::WMAndLTM, -1, -1 });
BOOST_REQUIRE_GT(results.size(), 0);
for (const armem::wm::Entity& result : results)
@@ -242,8 +242,8 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_all)
BOOST_AUTO_TEST_CASE(test_entity_TimeRange_empty)
{
- addResults(query::entity::TimeRange{ 2400, 2600 });
- addResults(query::entity::TimeRange{ 6000, 1000 });
+ addResults(query::entity::TimeRange{ query::QueryTarget::WMAndLTM, 2400, 2600 });
+ addResults(query::entity::TimeRange{ query::QueryTarget::WMAndLTM, 6000, 1000 });
BOOST_REQUIRE_GT(results.size(), 0);
for (const armem::wm::Entity& result : results)
@@ -257,7 +257,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_empty)
BOOST_AUTO_TEST_CASE(test_entity_TimeRange_from_start)
{
- addResults(query::entity::TimeRange{ -1, 2500 });
+ addResults(query::entity::TimeRange{ query::QueryTarget::WMAndLTM, -1, 2500 });
BOOST_REQUIRE_GT(results.size(), 0);
for (const armem::wm::Entity& result : results)
@@ -278,7 +278,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_from_start)
BOOST_AUTO_TEST_CASE(test_entity_TimeRange_to_end)
{
- addResults(query::entity::TimeRange{ 2500, -1 });
+ addResults(query::entity::TimeRange{ query::QueryTarget::WMAndLTM, 2500, -1 });
BOOST_REQUIRE_GT(results.size(), 0);
for (const armem::wm::Entity& result : results)
@@ -302,7 +302,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_to_end)
BOOST_AUTO_TEST_CASE(test_entity_BeforeTime_1)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::BeforeTime{ 3500, 1 });
+ addResults(query::entity::BeforeTime{ query::QueryTarget::WMAndLTM, 3500, 1 });
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -318,7 +318,7 @@ BOOST_AUTO_TEST_CASE(test_entity_BeforeTime_1)
BOOST_AUTO_TEST_CASE(test_entity_BeforeTime_2)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::BeforeTime{ 3500, 2});
+ addResults(query::entity::BeforeTime{ query::QueryTarget::WMAndLTM, 3500, 2});
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -343,7 +343,7 @@ BOOST_AUTO_TEST_CASE(test_entity_BeforeTime_2)
BOOST_AUTO_TEST_CASE(test_entity_BeforeOrAtTime_before)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::BeforeOrAtTime{ 3500 });
+ addResults(query::entity::BeforeOrAtTime{ query::QueryTarget::WMAndLTM, 3500 });
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -358,7 +358,7 @@ BOOST_AUTO_TEST_CASE(test_entity_BeforeOrAtTime_before)
BOOST_AUTO_TEST_CASE(test_entity_BeforeOrAtTime_at)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::BeforeOrAtTime{ 3000 });
+ addResults(query::entity::BeforeOrAtTime{ query::QueryTarget::WMAndLTM, 3000 });
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -373,7 +373,7 @@ BOOST_AUTO_TEST_CASE(test_entity_BeforeOrAtTime_at)
BOOST_AUTO_TEST_CASE(test_entity_BeforeOrAtTime_lookup_past)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::BeforeOrAtTime{ 1 });
+ addResults(query::entity::BeforeOrAtTime{ query::QueryTarget::WMAndLTM, 1 });
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -393,7 +393,7 @@ BOOST_AUTO_TEST_CASE(test_entity_BeforeOrAtTime_lookup_past)
BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_no_limit)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::TimeApprox{ 3500, -1});
+ addResults(query::entity::TimeApprox{ query::QueryTarget::WMAndLTM, 3500, -1});
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -419,7 +419,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_no_limit)
BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_600)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::TimeApprox{ 3500, 600});
+ addResults(query::entity::TimeApprox{ query::QueryTarget::WMAndLTM, 3500, 600});
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -445,7 +445,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_600)
BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_too_small)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::TimeApprox{ 3500, 100});
+ addResults(query::entity::TimeApprox{ query::QueryTarget::WMAndLTM, 3500, 100});
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -464,7 +464,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_too_small)
BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_only_next)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::TimeApprox{ 3700, 400});
+ addResults(query::entity::TimeApprox{ query::QueryTarget::WMAndLTM, 3700, 400});
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -489,7 +489,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_only_next)
BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_only_previous)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::TimeApprox{ 3300, 400});
+ addResults(query::entity::TimeApprox{ query::QueryTarget::WMAndLTM, 3300, 400});
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -514,7 +514,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_only_previous)
BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_perfect_match)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::TimeApprox{ 3000, -1});
+ addResults(query::entity::TimeApprox{ query::QueryTarget::WMAndLTM, 3000, -1});
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -539,7 +539,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_perfect_match)
BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_past)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::TimeApprox{ 1, 1});
+ addResults(query::entity::TimeApprox{ query::QueryTarget::WMAndLTM, 1, 1});
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -557,7 +557,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_past)
BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_future)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::TimeApprox{ 10'000, 1});
+ addResults(query::entity::TimeApprox{ query::QueryTarget::WMAndLTM, 10'000, 1});
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -575,7 +575,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_future)
BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_future_valid)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::TimeApprox{ 10'000, -1});
+ addResults(query::entity::TimeApprox{ query::QueryTarget::WMAndLTM, 10'000, -1});
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -594,7 +594,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_future_valid)
BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_invalid_timestamp)
{
- BOOST_REQUIRE_THROW(addResults(query::entity::TimeApprox{ -1, 1}), ::armarx::LocalException);
+ BOOST_REQUIRE_THROW(addResults(query::entity::TimeApprox{ query::QueryTarget::WMAndLTM, -1, 1}), ::armarx::LocalException);
}
@@ -634,7 +634,7 @@ BOOST_AUTO_TEST_CASE(test_negative_index_semantics)
BOOST_AUTO_TEST_CASE(test_entity_IndexRange_all_default)
{
addResults(query::entity::IndexRange());
- addResults(query::entity::IndexRange(0, -1));
+ addResults(query::entity::IndexRange(query::QueryTarget::WMAndLTM, 0, -1));
BOOST_REQUIRE_GT(results.size(), 0);
for (const armem::wm::Entity& result : results)
@@ -654,10 +654,10 @@ BOOST_AUTO_TEST_CASE(test_entity_IndexRange_all_default)
BOOST_AUTO_TEST_CASE(test_entity_IndexRange_slice)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::IndexRange{ 1, 3 }); // => [1, 2, 3]
- addResults(query::entity::IndexRange{ 1, -2 }); // 5 - 2 = 3
- addResults(query::entity::IndexRange{ -4, 3 }); // 5 - 4 = 1
- addResults(query::entity::IndexRange{ -4, -2 });
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, 1, 3 }); // => [1, 2, 3]
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, 1, -2 }); // 5 - 2 = 3
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, -4, 3 }); // 5 - 4 = 1
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, -4, -2 });
BOOST_REQUIRE_GT(results.size(), 0);
for (const armem::wm::Entity& result : results)
@@ -679,12 +679,12 @@ BOOST_AUTO_TEST_CASE(test_entity_IndexRange_slice)
BOOST_AUTO_TEST_CASE(test_entity_IndexRange_empty_range)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::IndexRange{ 1, 0 });
- addResults(query::entity::IndexRange{ 2, 1 });
- addResults(query::entity::IndexRange{ 5, 3 });
- addResults(query::entity::IndexRange{ 4, -3 }); // 5-3 = 2
- addResults(query::entity::IndexRange{ 3, -3 });
- addResults(query::entity::IndexRange{ 1, -5 });
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, 1, 0 });
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, 2, 1 });
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, 5, 3 });
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, 4, -3 }); // 5-3 = 2
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, 3, -3 });
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, 1, -5 });
BOOST_REQUIRE_GT(results.size(), 0);
@@ -701,12 +701,12 @@ BOOST_AUTO_TEST_CASE(test_entity_IndexRange_empty_entity)
{
entity.clear();
BOOST_REQUIRE_EQUAL(entity.size(), 0);
- addResults(query::entity::IndexRange{ 0, 0 });
- addResults(query::entity::IndexRange{ 0, 10 });
- addResults(query::entity::IndexRange{-10, -1 });
- addResults(query::entity::IndexRange{ 2, 5 });
- addResults(query::entity::IndexRange{ 3, -3 });
- addResults(query::entity::IndexRange{ -1, 10 });
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, 0, 0 });
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, 0, 10 });
+ addResults(query::entity::IndexRange{query::QueryTarget::WMAndLTM, -10, -1 });
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, 2, 5 });
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, 3, -3 });
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, -1, 10 });
BOOST_REQUIRE_GT(results.size(), 0);
diff --git a/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp b/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp
index 6ac42aee3f0870279f8f8fe823fdb9ca5340c4c7..ae9af3c213fbbea4d4a44852e8e02d0af15b8de9 100644
--- a/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp
@@ -45,6 +45,7 @@ namespace armarx::armem::gui
{
armem::client::query::Builder qb(dataMode());
qb
+ .queryTarget(_snapshotSelectorWidget->queryTarget())
.coreSegments().all()
.providerSegments().all()
.entities().all()
diff --git a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.cpp b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.cpp
index 762421d132997ffaec145d170cc71b708e2cb414..87d94376e673492d07a7fe2f0206359259ea327a 100644
--- a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.cpp
@@ -15,6 +15,11 @@ namespace armarx::armem::gui
return _selector;
}
+ query::data::QueryTarget SnapshotSelectorWidget::queryTarget() const
+ {
+ return _queryTarget;
+ }
+
SnapshotSelectorWidget::SnapshotSelectorWidget()
{
@@ -22,19 +27,30 @@ namespace armarx::armem::gui
setLayout(_pageLayout);
connect(this, &This::queryOutdated, this, &This::updateSelector);
+ connect(this, &This::queryTargetOutdated, this, &This::updateQueryTarget);
{
QHBoxLayout* typeLayout = new QHBoxLayout();
+ // snapshot select box
_queryComboBox = new QComboBox();
_queryComboBox->addItems({"All", "Single", "Time Range", "Index Range"});
_queryComboBox->setCurrentIndex(3);
typeLayout->addWidget(_queryComboBox);
- connect(_queryComboBox, &QComboBox::currentTextChanged, this, &This::showSelectedForm);
+ connect(_queryComboBox, &QComboBox::currentTextChanged, this, &This::showSelectedFormForQuery);
connect(_queryComboBox, &QComboBox::currentTextChanged, this, &This::queryOutdated);
+ // query type select box
+ _queryTargetComboBox = new QComboBox();
+ _queryTargetComboBox->addItems({"WM", "WM & LTM", "LTM (debug only)"});
+ _queryTargetComboBox->setCurrentIndex(0);
+
+ typeLayout->addWidget(_queryTargetComboBox);
+
+ connect(_queryTargetComboBox, &QComboBox::currentTextChanged, this, &This::queryTargetOutdated);
+
_pageLayout->addLayout(typeLayout);
}
@@ -45,8 +61,14 @@ namespace armarx::armem::gui
addForm("Time Range", new SnapshotFormTimeRange());
addForm("Index Range", new SnapshotFormIndexRange());
- showSelectedForm(_queryComboBox->currentText());
+ // Add query targets
+ _queryTargets.insert({"WM", query::data::QueryTarget::WM});
+ _queryTargets.insert({"WM & LTM", query::data::QueryTarget::WMAndLTM});
+ _queryTargets.insert({"LTM (debug only)", query::data::QueryTarget::LTM});
+
+ showSelectedFormForQuery(_queryComboBox->currentText());
updateSelector();
+ updateQueryTarget();
}
void SnapshotSelectorWidget::updateSelector()
@@ -55,7 +77,13 @@ namespace armarx::armem::gui
emit queryChanged();
}
- void SnapshotSelectorWidget::showSelectedForm(QString selected)
+ void SnapshotSelectorWidget::updateQueryTarget()
+ {
+ this->_queryTarget = _queryTargets.at(_queryTargetComboBox->currentText());
+ emit queryTargetChanged();
+ }
+
+ void SnapshotSelectorWidget::showSelectedFormForQuery(QString selected)
{
for (auto& [name, form] : _queryForms)
{
diff --git a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.h b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.h
index 2241b67e810292b685fe0837f43ea621cfb2fb93..92db5fbb13c74b011ca0f36a64fbf82a58536717 100644
--- a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.h
+++ b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.h
@@ -36,6 +36,7 @@ namespace armarx::armem::gui
armem::DataMode dataMode() const;
client::query::SnapshotSelector& selector();
+ query::data::QueryTarget queryTarget() const;
public slots:
@@ -43,19 +44,22 @@ namespace armarx::armem::gui
signals:
void queryChanged();
+ void queryTargetChanged();
private slots:
void updateSelector();
+ void updateQueryTarget();
void hideAllForms();
- void showSelectedForm(QString selected);
+ void showSelectedFormForQuery(QString selected);
signals:
void queryOutdated();
+ void queryTargetOutdated();
private:
@@ -66,11 +70,14 @@ namespace armarx::armem::gui
public:
client::query::SnapshotSelector _selector;
+ query::data::QueryTarget _queryTarget;
QVBoxLayout* _pageLayout;
QComboBox* _queryComboBox;
+ QComboBox* _queryTargetComboBox;
/// The forms for the different query types. Hidden when not selected.
std::map<QString, SnapshotForm*> _queryForms;
+ std::map<QString, query::data::QueryTarget> _queryTargets;
};
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/detail/PrimitiveNavigatorBase.h b/source/RobotAPI/libraries/aron/core/navigator/data/detail/PrimitiveNavigatorBase.h
index d18112d5fb4d9e77465d714484f5f0759cf0ff91..4e91d3235c4f4234d7a9e3db943571a3dfb438fb 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/detail/PrimitiveNavigatorBase.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/detail/PrimitiveNavigatorBase.h
@@ -62,6 +62,12 @@ namespace armarx::aron::datanavigator::detail
return *this;
}
+ // Already implemented through thge constructor of a primitive navigator
+ /*bool operator==(const ValueT& x) const
+ {
+ return this->aron->value == x;
+ }*/
+
// virtual implementations
virtual std::vector<NavigatorPtr> getChildren() const override
{
diff --git a/source/RobotAPI/libraries/aron/core/test/CMakeLists.txt b/source/RobotAPI/libraries/aron/core/test/CMakeLists.txt
index 59892d744e814a6c0ece1b32cf208b6ba9e064a5..239c92ecff42e75ea7be37b5e43c8d442983f678 100644
--- a/source/RobotAPI/libraries/aron/core/test/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/core/test/CMakeLists.txt
@@ -1,9 +1,21 @@
-######################
-# ARON OPERATOR TEST #
-######################
find_package(Simox QUIET)
armarx_build_if(Simox_FOUND "Simox not available")
+find_package(Eigen3 QUIET)
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+
+find_package(IVT COMPONENTS ivt ivtopencv QUIET)
+armarx_build_if(IVT_FOUND "IVT not available")
+
+find_package(OpenCV QUIET)
+armarx_build_if(OpenCV_FOUND "OpenCV not available")
+
+find_package(PCL QUIET)
+armarx_build_if(PCL_FOUND "PCL not available")
+
+######################
+# ARON OPERATOR TEST #
+######################
armarx_add_test(
TEST_NAME
aronOperatorTest
@@ -18,11 +30,47 @@ armarx_add_test(
)
######################
-# ARON NAVIGATE TEST #
+# ARON CODE GEN TEST #
######################
-find_package(Simox QUIET)
-armarx_build_if(Simox_FOUND "Simox not available")
+armarx_add_test(
+ TEST_NAME
+ aronCodeGenerationTest
+ TEST_FILE
+ aronCodeGenerationTest.cpp
+ LIBS
+ Simox::SimoxUtility
+ ArmarXCore
+ RobotAPI::aron
+ ARON_FILES
+ # xmls/BaseClass.xml
+ # xmls/DerivedClassTest.xml
+ xmls/DictTest.xml
+ xmls/EigenMatrixTest.xml
+ xmls/EigenQuaternionTest.xml
+ xmls/EnumTest.xml
+ xmls/HumanPoseTest.xml
+ xmls/IVTCByteImageTest.xml
+ xmls/ListTest.xml
+ xmls/NaturalIKTest.xml
+ xmls/ObjectTest.xml
+ xmls/OpenCVMatTest.xml
+ xmls/OrientationTest.xml
+ xmls/PCLPointCloudTest.xml
+ xmls/PoseTest.xml
+ xmls/PositionTest.xml
+ xmls/PrimitiveTest.xml
+ xmls/OptionalTest.xml
+ INCLUDE_DIRECTORIES
+ ${Simox_INCLUDE_DIR}
+ ${Eigen3_INCLUDE_DIR}
+ ${IVT_INCLUDE_DIRS}
+ ${OpenCV_INCLUDE_DIRS}
+ ${PCL_INCLUDE_DIRS}
+)
+######################
+# ARON NAVIGATE TEST #
+######################
armarx_add_test(
TEST_NAME
aronNavigateTest
@@ -42,18 +90,6 @@ armarx_add_test(
########################
# ARON RANDOMIZED TEST #
########################
-find_package(Eigen3 QUIET)
-armarx_build_if(Eigen3_FOUND "Eigen3 not available")
-
-find_package(IVT COMPONENTS ivt ivtopencv QUIET)
-armarx_build_if(IVT_FOUND "IVT not available")
-
-find_package(OpenCV QUIET)
-armarx_build_if(OpenCV_FOUND "OpenCV not available")
-
-find_package(PCL QUIET)
-armarx_build_if(PCL_FOUND "PCL not available")
-
armarx_add_test(
TEST_NAME
aronRandomizedTest
diff --git a/source/RobotAPI/libraries/aron/core/test/aronCodeGenerationTest.cpp b/source/RobotAPI/libraries/aron/core/test/aronCodeGenerationTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c1db4b859aa464794c3bba2e1025feb5436a5bba
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/test/aronCodeGenerationTest.cpp
@@ -0,0 +1,212 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::aron
+ * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date 2019
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE RobotAPI::ArmarXLibraries::aron
+
+#define ARMARX_BOOST_TEST
+
+// STD/STL
+#include <iostream>
+#include <cstdlib>
+#include <ctime>
+#include <numeric>
+
+// Boost
+#include <boost/algorithm/string.hpp>
+
+// Test
+#include <RobotAPI/Test.h>
+
+// ArmarX
+#include <ArmarXCore/libraries/cppgen/CppMethod.h>
+#include <ArmarXCore/libraries/cppgen/CppClass.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
+
+// Aron
+#include <RobotAPI/libraries/aron/core/Debug.h>
+#include <RobotAPI/libraries/aron/core/Randomizer.h>
+
+// Generated File
+#include <RobotAPI/libraries/aron/core/test/aron/ListTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/DictTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/PrimitiveTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/ObjectTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/IVTCByteImageTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/EigenMatrixTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/EigenQuaternionTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/OpenCVMatTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/PCLPointCloudTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/PositionTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/OrientationTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/PoseTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/EnumTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/OptionalTest.aron.generated.h>
+
+using namespace armarx;
+using namespace aron;
+
+BOOST_AUTO_TEST_CASE(AronCodeGenerationListTest)
+{
+ std::cout << "Running Code Gen List test" << std::endl;
+ ListTest p_tmp;
+ ListTest p = p_tmp; // test assignment
+ BOOST_CHECK_EQUAL(p_tmp == p, true);
+
+ BOOST_CHECK_EQUAL(typeid(p.intList) == typeid(std::vector<int>), true);
+ BOOST_CHECK_EQUAL(typeid(p.longList) == typeid(std::vector<long>), true);
+ BOOST_CHECK_EQUAL(typeid(p.floatList) == typeid(std::vector<float>), true);
+ BOOST_CHECK_EQUAL(typeid(p.doubleList) == typeid(std::vector<double>), true);
+ BOOST_CHECK_EQUAL(typeid(p.stringList) == typeid(std::vector<std::string>), true);
+ BOOST_CHECK_EQUAL(typeid(p.boolList) == typeid(std::vector<bool>), true);
+ //BOOST_CHECK_EQUAL(typeid(p.objectList) == typeid(std::vector<ListTest::ListClass>), true);
+
+ BOOST_CHECK_EQUAL(p.intList.size() == 0, true);
+ BOOST_CHECK_EQUAL(p.longList.size() == 0, true);
+ BOOST_CHECK_EQUAL(p.floatList.size() == 0, true);
+ BOOST_CHECK_EQUAL(p.doubleList.size() == 0, true);
+ BOOST_CHECK_EQUAL(p.stringList.size() == 0, true);
+ BOOST_CHECK_EQUAL(p.boolList.size() == 0, true);
+ BOOST_CHECK_EQUAL(p.objectList.size() == 0, true);
+}
+
+BOOST_AUTO_TEST_CASE(AronCodeGenerationDictTest)
+{
+ std::cout << "Running Code Gen Dict test" << std::endl;
+ DictTest p_tmp;
+ DictTest p = p_tmp; // test assignment
+ BOOST_CHECK_EQUAL(p_tmp == p, true);
+
+ BOOST_CHECK_EQUAL(p.dict.size() == 0, true);
+}
+
+BOOST_AUTO_TEST_CASE(AronCodeGenerationEigenMatrixTest)
+{
+ std::cout << "Running Code Gen EigenMatrix test" << std::endl;
+ EigenMatrixTest p_tmp;
+ EigenMatrixTest p = p_tmp; // test assignment
+ BOOST_CHECK_EQUAL(p_tmp == p, true);
+
+ BOOST_CHECK_EQUAL(p.the_short_eigen_matrix.rows() == 5, true);
+ BOOST_CHECK_EQUAL(p.the_short_eigen_matrix.cols() == 7, true);
+ BOOST_CHECK_EQUAL(p.the_int_eigen_matrix.rows() == 7, true);
+ BOOST_CHECK_EQUAL(p.the_int_eigen_matrix.cols() == 7, true);
+ BOOST_CHECK_EQUAL(p.the_long_eigen_matrix.rows() == 7, true);
+ BOOST_CHECK_EQUAL(p.the_long_eigen_matrix.cols() == 5, true);
+ BOOST_CHECK_EQUAL(p.the_float_eigen_matrix.rows() == 1, true);
+ BOOST_CHECK_EQUAL(p.the_float_eigen_matrix.cols() == 9, true);
+ BOOST_CHECK_EQUAL(p.the_double_eigen_matrix.rows() == 25, true);
+ BOOST_CHECK_EQUAL(p.the_double_eigen_matrix.cols() == 1, true);
+}
+
+BOOST_AUTO_TEST_CASE(AronCodeGenerationEigenQuaternionTest)
+{
+ std::cout << "Running Code Gen EigenQuaternion test" << std::endl;
+ EigenQuaternionTest p_tmp;
+ EigenQuaternionTest p = p_tmp; // test assignment
+ BOOST_CHECK_EQUAL(p_tmp == p, true);
+
+ //BOOST_CHECK_EQUAL(p.the_double_eigen_matrix.w() == 0, true);
+ //BOOST_CHECK_EQUAL(p.the_double_eigen_matrix.x() == 0, true);
+ //BOOST_CHECK_EQUAL(p.the_double_eigen_matrix.y() == 0, true);
+ //BOOST_CHECK_EQUAL(p.the_double_eigen_matrix.z() == 0, true);
+}
+
+BOOST_AUTO_TEST_CASE(AronCodeGenerationEigenPositionTest)
+{
+ std::cout << "Running Code Gen EigenPosition test" << std::endl;
+ PositionTest p_tmp;
+ PositionTest p = p_tmp; // test assignment
+ BOOST_CHECK_EQUAL(p_tmp == p, true);
+
+ BOOST_CHECK_EQUAL(p.position.rows() == 3, true);
+ BOOST_CHECK_EQUAL(p.position.cols() == 1, true);
+}
+
+BOOST_AUTO_TEST_CASE(AronCodeGenerationEigenPoseTest)
+{
+ std::cout << "Running Code Gen EigenPose test" << std::endl;
+ PoseTest p_tmp;
+ PoseTest p = p_tmp; // test assignment
+ BOOST_CHECK_EQUAL(p_tmp == p, true);
+
+ BOOST_CHECK_EQUAL(p.pose.rows() == 4, true);
+ BOOST_CHECK_EQUAL(p.pose.cols() == 4, true);
+}
+
+BOOST_AUTO_TEST_CASE(AronCodeGenerationIntEnumTest)
+{
+ std::cout << "Running Code Gen IntEnum test" << std::endl;
+ TheObjectThatUsesTheIntEnum p_tmp;
+ TheObjectThatUsesTheIntEnum p = p_tmp; // test assignment
+ BOOST_CHECK_EQUAL(p_tmp == p, true);
+
+ BOOST_CHECK_EQUAL(p.the_int_enum.value == TheIntEnum::INT_ENUM_VALUE_0, true);
+}
+
+BOOST_AUTO_TEST_CASE(AronCodeGenerationPrimitiveTest)
+{
+ std::cout << "Running Code Gen Primitive test" << std::endl;
+ PrimitiveTest p_tmp;
+ PrimitiveTest p = p_tmp; // test assignment
+ BOOST_CHECK_EQUAL(p_tmp == p, true);
+
+ BOOST_CHECK_EQUAL(p.the_bool == false, true);
+ BOOST_CHECK_EQUAL(p.the_double == 0.0, true);
+ BOOST_CHECK_EQUAL(p.the_float == 0.0, true);
+ BOOST_CHECK_EQUAL(p.the_int == 0, true);
+ BOOST_CHECK_EQUAL(p.the_long == 0, true);
+ BOOST_CHECK_EQUAL(p.the_string == "", true);
+ BOOST_CHECK_EQUAL(p.the_time == IceUtil::Time(), true);
+}
+
+BOOST_AUTO_TEST_CASE(AronCodeGenerationOptionalTest)
+{
+ std::cout << "Running Code Gen Primitive test" << std::endl;
+ OptionalTest p_tmp;
+ OptionalTest p = p_tmp; // test assignment
+ BOOST_CHECK_EQUAL(p_tmp == p, true);
+
+ BOOST_CHECK_EQUAL(typeid(p.some_dict) == typeid(std::optional<std::map<std::string, float>>), true);
+ BOOST_CHECK_EQUAL(typeid(p.some_dict_with_optional_type) == typeid(std::map<std::string, std::optional<float>>), true);
+ BOOST_CHECK_EQUAL(typeid(p.some_eigen_matrix) == typeid(std::optional<Eigen::Matrix<long, 25, 10>>), true);
+ BOOST_CHECK_EQUAL(typeid(p.some_float) == typeid(std::optional<float>), true);
+ BOOST_CHECK_EQUAL(typeid(p.some_list) == typeid(std::optional<std::vector<double>>), true);
+ BOOST_CHECK_EQUAL(typeid(p.some_list_with_optional_list) == typeid(std::optional<std::vector<std::optional<std::vector<std::optional<float>>>>>), true);
+ BOOST_CHECK_EQUAL(typeid(p.some_list_with_optional_type) == typeid(std::vector<std::optional<double>>), true);
+ BOOST_CHECK_EQUAL(typeid(p.some_obj) == typeid(std::optional<armarx::OptionalTestElement>), true);
+ BOOST_CHECK_EQUAL(typeid(p.some_string) == typeid(std::optional<std::string>), true);
+
+
+ auto aronType = p.toAronType();
+ BOOST_CHECK_EQUAL(aronType->getMemberType("some_float")->getMaybe() == aron::type::Maybe::eOptional, true);
+
+ BOOST_CHECK_EQUAL(p.some_float.has_value() == false, true);
+ auto aron = p.toAron();
+ BOOST_CHECK_EQUAL(aron->getElement("some_float") == nullptr, true);
+
+ p.some_float = 5.0f;
+ BOOST_CHECK_EQUAL(p.some_float.has_value() == true, true);
+ aron = p.toAron();
+ BOOST_CHECK_EQUAL(*aron->getElement("some_float") == std::make_shared<datanavigator::FloatNavigator>(5.0f), true);
+ BOOST_CHECK_EQUAL(*aron->getElement("some_float") == nullptr, false);
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/test/xmls/OptionalTest.xml b/source/RobotAPI/libraries/aron/core/test/xmls/OptionalTest.xml
index 8e27d3c0263af6decff1d945acb40cc1d3081df4..7b402cb38e4b904723c31ce7097ebc7623c296ba 100644
--- a/source/RobotAPI/libraries/aron/core/test/xmls/OptionalTest.xml
+++ b/source/RobotAPI/libraries/aron/core/test/xmls/OptionalTest.xml
@@ -15,7 +15,7 @@
<Float optional="1"/>
</ObjectChild>
- <!--<ObjectChild key='some_string'>
+ <ObjectChild key='some_string'>
<String optional="true"/>
</ObjectChild>
@@ -57,7 +57,7 @@
<Float optional="ja" />
</List>
</List>
- </ObjectChild>-->
+ </ObjectChild>