Reduce includes in Reader.h, add special queries

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

@@ -9,8 +9,16 @@
 #include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
 
 
+namespace armarx::armem::client::query
+{
+    // #include <RobotAPI/libraries/armem/client/query/Builder.h>
+    class Builder;
+}
+
 namespace armarx::armem::client
 {
+    using QueryBuilder = query::Builder;
+
 
     /**
      * @brief An update of an entity for a specific point in time.

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

@@ -4,6 +4,13 @@
 
 #include <ArmarXCore/core/logging/Logging.h>
 
+#include <RobotAPI/libraries/armem/core/MemoryID_operators.h>
+#include <RobotAPI/libraries/armem/core/longtermmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/visitor.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
+#include <RobotAPI/libraries/armem/util/util.h>
+
 #include "query/Builder.h"
 #include "query/query_fns.h"
 
@@ -22,6 +29,7 @@ namespace armarx::armem::client
         return QueryResult::fromIce(query(input.toIce()));
     }
 
+
     armem::query::data::Result
     Reader::query(const armem::query::data::Input& input) const
     {
@@ -52,11 +60,13 @@ namespace armarx::armem::client
         return result;
     }
 
+
     QueryResult Reader::query(armem::query::data::MemoryQueryPtr query, DataMode dataMode) const
     {
         return this->query(armem::query::data::MemoryQuerySeq{query}, dataMode);
     }
 
+
     QueryResult Reader::query(const armem::query::data::MemoryQuerySeq& queries, DataMode dataMode) const
     {
         QueryInput input;
@@ -65,24 +75,10 @@ namespace armarx::armem::client
         return this->query(input);
     }
 
-    QueryResult Reader::getAll(DataMode dataMode) const
-    {
-        using namespace client::query_fns;
-
-        query::Builder qb(dataMode);
-        qb.coreSegments(all()).providerSegments(all()).entities(all()).snapshots(all());
 
-        return this->query(qb.buildQueryInput());
-    }
-
-    QueryResult Reader::getLatestSnapshots(DataMode dataMode) const
+    QueryResult Reader::query(const QueryBuilder& queryBuilder) const
     {
-        using namespace client::query_fns;
-
-        query::Builder qb(dataMode);
-        qb.coreSegments(all()).providerSegments(all()).entities(all()).snapshots(latest());
-
-        return this->query(qb.buildQueryInput());
+        return this->query(queryBuilder.buildQueryInput());
     }
 
 
@@ -104,7 +100,120 @@ namespace armarx::armem::client
                 entity.snapshots(latest());
             }
         }
-        return query(qb.buildQueryInput());
+        return query(qb);
+    }
+
+
+
+    std::optional<wm::EntitySnapshot> Reader::getLatestSnapshotOf(const std::vector<MemoryID>& _snapshotIDs) const
+    {
+        std::vector<MemoryID> snapshotIDs = _snapshotIDs;
+
+        client::QueryResult result = this->queryMemoryIDs(snapshotIDs);
+        if (result.success)
+        {
+            std::sort(snapshotIDs.begin(), snapshotIDs.end(), compareTimestampDecreasing);
+            for (const MemoryID& snapshotID : snapshotIDs)
+            {
+                try
+                {
+                    wm::EntitySnapshot& snapshot = result.memory.getEntitySnapshot(snapshotID);
+                    return snapshot;
+                }
+                catch (const armem::error::ArMemError&)
+                {
+                }
+            }
+            return std::nullopt;
+        }
+        else
+        {
+            ARMARX_INFO << "Error querying " << snapshotIDs.size() << " STT snapshots:\n"
+                        << result.errorMessage;
+            return std::nullopt;
+        }
+    }
+
+
+    QueryResult Reader::getLatestSnapshotsIn(const MemoryID& id, DataMode dataMode) const
+    {
+        using namespace client::query_fns;
+        if (!id.isWellDefined())
+        {
+            throw armem::error::InvalidMemoryID(id, "ID must be well defined, but was not.");
+        }
+
+        query::Builder qb(dataMode);
+        query::CoreSegmentSelector& core =
+            id.hasCoreSegmentName()
+            ? qb.coreSegments(withID(id))
+            : qb.coreSegments(all());
+        query::ProviderSegmentSelector& prov =
+            id.hasProviderSegmentName()
+            ? core.providerSegments(withID(id))
+            : core.providerSegments(all());
+        query::EntitySelector& entity =
+            id.hasEntityName()
+            ? prov.entities(withID(id))
+            : prov.entities(all());
+        entity.snapshots(latest());
+
+        return query(qb);
+    }
+
+
+    struct FindLatestSnapshotVisitor : public wm::Visitor
+    {
+        std::optional<wm::EntitySnapshot> latest = std::nullopt;
+
+        bool visitEnter(const wm::EntitySnapshot& snapshot) override;
+    };
+    bool FindLatestSnapshotVisitor::visitEnter(const wm::EntitySnapshot& snapshot)
+    {
+        if (not latest.has_value() or snapshot.time() < latest->time())
+        {
+            latest = snapshot;
+        }
+        return true;
+    }
+
+
+    std::optional<wm::EntitySnapshot> Reader::getLatestSnapshotIn(const MemoryID& id, DataMode dataMode) const
+    {
+        client::QueryResult result = getLatestSnapshotsIn(id, dataMode);
+        if (result.success)
+        {
+            FindLatestSnapshotVisitor visitor;
+            visitor.applyTo(result.memory);
+            return visitor.latest;
+        }
+        else
+        {
+            ARMARX_INFO << "Error querying latest snapshot in " << id;
+            return std::nullopt;
+        }
+    }
+
+
+    QueryResult Reader::getAllLatestSnapshots(DataMode dataMode) const
+    {
+        using namespace client::query_fns;
+
+        query::Builder qb(dataMode);
+        qb.coreSegments(all()).providerSegments(all()).entities(all()).snapshots(latest());
+
+        return this->query(qb);
+    }
+
+
+    QueryResult Reader::getAll(DataMode dataMode) const
+    {
+        using namespace client::query_fns;
+
+        query::Builder qb(dataMode);
+        qb.coreSegments(all()).providerSegments(all()).entities(all()).snapshots(all());
+
+        return this->query(qb);
     }
 
 
@@ -129,5 +238,4 @@ namespace armarx::armem::client
     {
         this->memoryPrx = memory;
     }
-
 }

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

@@ -2,15 +2,14 @@
 
 
 // STD/STL
+#include <optional>
 #include <vector>
 
 // RobotAPI
 #include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
 #include <RobotAPI/interface/armem/server/StoringMemoryInterface.h>
-#include <RobotAPI/interface/armem/client/MemoryListenerInterface.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
-#include <RobotAPI/libraries/armem/core/longtermmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.h>
+// #include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
 
 #include "Query.h"
 
@@ -34,15 +33,14 @@ namespace armarx::armem::client
 
         void setReadingMemory(server::ReadingMemoryInterfacePrx memory);
 
+        /// Perform a query.
         QueryResult query(const QueryInput& input) const;
         armem::query::data::Result query(const armem::query::data::Input& input) const;
 
         QueryResult query(armem::query::data::MemoryQueryPtr query, DataMode dataMode = DataMode::WithData) const;
         QueryResult query(const armem::query::data::MemoryQuerySeq& queries, DataMode dataMode = DataMode::WithData) const;
 
-
-        QueryResult getAll(DataMode dataMode = DataMode::WithData) const;
-        QueryResult getLatestSnapshots(DataMode dataMode = DataMode::WithData) const;
+        QueryResult query(const QueryBuilder& queryBuilder) const;
 
 
         /**
@@ -60,7 +58,54 @@ namespace armarx::armem::client
          * @param dataMode Whether to include instance data or just meta data.
          * @return The query result.
          */
-        QueryResult queryMemoryIDs(const std::vector<MemoryID>& ids, DataMode dataMode = DataMode::WithData) const;
+        QueryResult
+        queryMemoryIDs(const std::vector<MemoryID>& ids, DataMode dataMode = DataMode::WithData) const;
+
+
+        /**
+         * @brief Query the given snapshot and return the latest existing snapshot.
+         * @param snapshotIDs The snapshot (or entity) IDs.
+         * @return The latest snapshot contained in the query result, if any.
+         */
+        std::optional<wm::EntitySnapshot>
+        getLatestSnapshotOf(const std::vector<MemoryID>& snapshotIDs) const;
+
+
+        /**
+         * @brief Get the latest snapshots under the given memory ID.
+         * @param id A memory, core segment, provider segment or entity ID.
+         * @param dataMode With or without data.
+         * @return The query result.
+         */
+        QueryResult
+        getLatestSnapshotsIn(const MemoryID& id, DataMode dataMode = DataMode::WithData) const;
+
+        /**
+         * @brief Get the latest snapshot under the given memory ID.
+         * @param id A memory, core segment, provider segment or entity ID.
+         * @param dataMode With or without data.
+         * @return The latest contained snapshot, if any.
+         */
+        std::optional<wm::EntitySnapshot>
+        getLatestSnapshotIn(const MemoryID& id, DataMode dataMode = DataMode::WithData) const;
+
+
+        /**
+         * @brief Get all latest snapshots in the memory.
+         * @param dataMode With or without data.
+         * @return The query result.
+         */
+        QueryResult
+        getAllLatestSnapshots(DataMode dataMode = DataMode::WithData) const;
+
+
+        /**
+         * @brief Get the whole memory content.
+         * @param dataMode With or without data.
+         * @return The query result.
+         */
+        QueryResult
+        getAll(DataMode dataMode = DataMode::WithData) const;
 
 
         //data::StoreResult readAndStore(data::StoreInputSeq& input);