Revise entity get/find functions

source/RobotAPI/libraries/armem/CMakeLists.txt

@@ -51,6 +51,7 @@ set(LIB_FILES
     core/wm/ice_conversions.cpp
     core/wm/json_conversions.cpp
     core/wm/aron_conversions.cpp
+    core/wm/detail/data_lookup_mixins.cpp
     core/wm/visitor/Visitor.cpp
     core/wm/visitor/FunctionalVisitor.cpp
 
@@ -141,6 +142,7 @@ set(LIB_HEADERS
     core/base/detail/MemoryItem.h
     core/base/detail/MemoryContainerBase.h
     core/base/detail/AronTyped.h
+    core/base/detail/derived.h
     core/base/detail/iteration_mixins.h
     core/base/detail/lookup_mixins.h
     core/base/detail/negative_index_semantics.h
@@ -158,6 +160,7 @@ set(LIB_HEADERS
     core/wm/aron_conversions.h
     core/wm/ice_conversions.h
     core/wm/json_conversions.h
+    core/wm/detail/data_lookup_mixins.h
     core/wm/visitor/Visitor.h
     core/wm/visitor/FunctionalVisitor.h
 

source/RobotAPI/libraries/armem/core/base/EntityBase.h

@@ -188,7 +188,28 @@ namespace armarx::armem::base
          */
         Time getLatestTimestamp() const
         {
-            return getLatestItem().first;
+            return this->getLatestSnapshot().time();
+        }
+        /**
+         * @brief Get the oldest timestamp.
+         * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
+         */
+        Time getFirstTimestamp() const
+        {
+            return this->getFirstSnapshot().time();
+        }
+
+        /**
+         * @brief Return the snapshot with the most recent timestamp.
+         * @return The latest snapshot or nullptr if the entity is empty.
+         */
+        EntitySnapshotT* findLatestSnapshot()
+        {
+            return this->empty() ? nullptr : &this->_container.rbegin()->second;
+        }
+        const EntitySnapshotT* findLatestSnapshot() const
+        {
+            return this->empty() ? nullptr : &this->_container.rbegin()->second;
         }
 
         /**
@@ -202,12 +223,33 @@ namespace armarx::armem::base
         }
         const EntitySnapshotT& getLatestSnapshot() const
         {
-            return getLatestItem().second;
+            if (const EntitySnapshotT* snapshot = this->findLatestSnapshot())
+            {
+                return *snapshot;
+            }
+            else
+            {
+                throw armem::error::EntityHistoryEmpty(name(), "when getting the latest snapshot.");
+            }
         }
 
+
         /**
-         * @brief Return the snapshot with the most recent timestamp.
-         * @return The latest snapshot.
+         * @brief Return the snapshot with the least recent timestamp.
+         * @return The first snapshot or nullptr if the entity is empty.
+         */
+        EntitySnapshotT* findFirstSnapshot()
+        {
+            return this->empty() ? nullptr : &this->_container.begin()->second;
+        }
+        const EntitySnapshotT* findFirstSnapshot() const
+        {
+            return this->empty() ? nullptr : &this->_container.begin()->second;
+        }
+
+        /**
+         * @brief Return the snapshot with the least recent timestamp.
+         * @return The first snapshot.
          * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
          */
         EntitySnapshotT& getFirstSnapshot()
@@ -216,22 +258,27 @@ namespace armarx::armem::base
         }
         const EntitySnapshotT& getFirstSnapshot() const
         {
-            return getFirstItem().second;
+            if (const EntitySnapshotT* snapshot = this->findFirstSnapshot())
+            {
+                return *snapshot;
+            }
+            else
+            {
+                throw armem::error::EntityHistoryEmpty(name(), "when getting the first snapshot.");
+            }
         }
 
 
         /**
          * @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
+         * @return The latest snapshot < time or nullptr if none was found.
          */
-        const EntitySnapshotT& getLatestSnapshotBefore(const Time& time) const
+        const EntitySnapshotT* findLatestSnapshotBefore(const Time& time) const
         {
             if (this->empty())
             {
-                throw error::EntityHistoryEmpty(this->name());
+                return nullptr;
             }
 
             // We want the rightmost element < time
@@ -241,7 +288,7 @@ namespace armarx::armem::base
             // refIt = begin() => no element < time
             if (greaterEq == this->_container.begin())
             {
-                throw error::MissingEntry::create<EntitySnapshotT>(toDateTimeMilliSeconds(time), *this);
+                return nullptr;
             }
 
             // end(): No element >= time, we can still have one < time (rbegin()) => std::prev(end())
@@ -251,29 +298,25 @@ namespace armarx::armem::base
             typename ContainerT::const_iterator less = std::prev(greaterEq);
 
             // we return the element before if possible
-            return less->second;
+            return &less->second;
         }
 
         /**
-         * @brief Return the lastest snapshot before or at time.
+         * @brief Return the latest 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
+         * @return The latest snapshot <= time or nullptr if none was found.
          */
-        const EntitySnapshotT& getLatestSnapshotBeforeOrAt(const Time& time) const
+        const EntitySnapshotT* findLatestSnapshotBeforeOrAt(const Time& time) const
         {
-            return getLatestSnapshotBefore(time + Time::microSeconds(1));
+            return findLatestSnapshotBefore(time + Time::microSeconds(1));
         }
 
         /**
          * @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
+         * @return The first snapshot >= time or nullptr if none was found.
          */
-        const EntitySnapshotT& getFirstSnapshotAfterOrAt(const Time& time) const
+        const EntitySnapshotT* findFirstSnapshotAfterOrAt(const Time& time) const
         {
             // We want the leftmost element >= time.
             // That's lower bound.
@@ -281,21 +324,19 @@ namespace armarx::armem::base
 
             if (greaterEq == this->_container.end())
             {
-                throw error::MissingEntry::create<EntitySnapshotT>(toDateTimeMilliSeconds(time), *this);
+                return nullptr;
             }
-            return greaterEq->second;
+            return &greaterEq->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
+         * @return The first snapshot >= time or nullptr if none was found.
          */
-        const EntitySnapshotT& getFirstSnapshotAfter(const Time& time) const
+        const EntitySnapshotT* findFirstSnapshotAfter(const Time& time) const
         {
-            return getFirstSnapshotAfter(time - Time::microSeconds(1));
+            return findFirstSnapshotAfter(time - Time::microSeconds(1));
         }
 
 
@@ -544,39 +585,6 @@ namespace armarx::armem::base
             return "entity";
         }
 
-
-    protected:
-
-        /**
-         * @brief Return the snapshot with the most recent timestamp.
-         * @return The latest snapshot.
-         * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
-         */
-        const typename ContainerT::value_type&
-        getLatestItem() const
-        {
-            if (this->_container.empty())
-            {
-                throw armem::error::EntityHistoryEmpty(name(), "when getting the latest snapshot.");
-            }
-            return *this->_container.rbegin();
-        }
-
-        /**
-         * @brief Return the snapshot with the least recent timestamp.
-         * @return The first snapshot.
-         * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
-         */
-        const typename ContainerT::value_type&
-        getFirstItem() const
-        {
-            if (this->_container.empty())
-            {
-                throw armem::error::EntityHistoryEmpty(name(), "when getting the first snapshot.");
-            }
-            return *this->_container.begin();
-        }
-
     };
 
 }

source/RobotAPI/libraries/armem/core/base/detail/derived.h

+#pragma once
+
+
+namespace armarx::armem::base::detail
+{
+
+    template <class DerivedT, class ThisT>
+    DerivedT&
+    derived(ThisT* t)
+    {
+        return static_cast<DerivedT&>(*t);
+    }
+
+
+    template <class DerivedT, class ThisT>
+    const DerivedT&
+    derived(const ThisT* t)
+    {
+        return static_cast<const DerivedT&>(*t);
+    }
+
+}

source/RobotAPI/libraries/armem/core/base/detail/lookup_mixins.h

 #pragma once
 
+#include "derived.h"
+
 #include <RobotAPI/libraries/armem/core/MemoryID.h>
 #include <RobotAPI/libraries/armem/core/error/ArMemError.h>
 
@@ -53,20 +55,6 @@ namespace armarx::armem::base::detail
     }
 
 
-    template <class DerivedT, class ThisT>
-    DerivedT&
-    derived(ThisT* t)
-    {
-        return static_cast<DerivedT&>(*t);
-    }
-    template <class DerivedT, class ThisT>
-    const DerivedT&
-    derived(const ThisT* t)
-    {
-        return static_cast<const DerivedT&>(*t);
-    }
-
-
     template <class DerivedT>
     struct GetFindInstanceMixin
     {
@@ -158,6 +146,32 @@ namespace armarx::armem::base::detail
         {
             return derived<DerivedT>(this).getEntity(snapshotID).getSnapshot(snapshotID);
         }
+
+
+        // More elaborate cases
+
+        auto* findLatestEntitySnapshot(const MemoryID& entityID)
+        {
+            auto* entity = derived<DerivedT>(this).findEntity(entityID);
+            return entity ? entity->findLatestSnapshot() : nullptr;
+        }
+        const auto* findLatestEntitySnapshot(const MemoryID& entityID) const
+        {
+            auto* entity = derived<DerivedT>(this).findEntity(entityID);
+            return entity ? entity->findLatestSnapshot() : nullptr;
+        }
+
+        auto* findLatestInstance(const MemoryID& entityID, int instanceIndex = 0)
+        {
+            auto* snapshot = derived<DerivedT>(this).findLatestEntitySnapshot(entityID);
+            return snapshot ? snapshot->findInstance(instanceIndex) : nullptr;
+        }
+        const auto* findLatestInstance(const MemoryID& entityID, int instanceIndex = 0) const
+        {
+            auto* snapshot = derived<DerivedT>(this).findLatestEntitySnapshot(entityID);
+            return snapshot ? snapshot->findInstance(instanceIndex) : nullptr;
+        }
+
     };
 
 

source/RobotAPI/libraries/armem/core/longtermmemory/Entity.cpp

@@ -142,10 +142,4 @@ namespace armarx::armem::ltm
         return Base::getSnapshot(time);
     }
 
-    auto Entity::getLatestItem() const -> const ContainerT::value_type&
-    {
-        // Directly query mongodb (cache cant know whether it is the latest or not)
-        // TODO
-        return Base::getLatestItem();
-    }
 }

source/RobotAPI/libraries/armem/core/longtermmemory/Entity.h

@@ -50,11 +50,6 @@ namespace armarx::armem::ltm
         std::vector<Time> getTimestamps() const;
         const EntitySnapshot& getSnapshot(const Time& time) const;
 
-
-    protected:
-        // overrides for LTM storage
-        const ContainerT::value_type& getLatestItem() const;
-
     public:
         MongoDBConnectionManager::MongoDBSettings dbsettings;
 

source/RobotAPI/libraries/armem/core/wm/detail/data_lookup_mixins.cpp

+#include "data_lookup_mixins.h"
+
+
+namespace armarx::armem::base
+{
+
+}

source/RobotAPI/libraries/armem/core/wm/detail/data_lookup_mixins.h

+#pragma once
+
+#include <RobotAPI/libraries/armem/core/forward_declarations.h>
+#include <RobotAPI/libraries/armem/core/base/detail/derived.h>
+
+#include <RobotAPI/libraries/aron/core/navigator/data/forward_declarations.h>
+
+#include <optional>
+
+
+namespace armarx::armem::wm::detail
+{
+    using base::detail::derived;
+
+
+    template <class DerivedT>
+    struct FindInstanceDataMixin
+    {
+        armarx::aron::datanavigator::DictNavigatorPtr
+        findLatestInstanceData(const MemoryID& entityID, int instanceIndex = 0) const
+        {
+            const auto* instance = derived<DerivedT>(*this).findLatestInstance(entityID, instanceIndex);
+            return instance ? instance->data() : nullptr;
+        }
+
+
+        template <class AronDtoT>
+        std::optional<AronDtoT>
+        findLatestInstanceDataAs(const MemoryID& entityID, int instanceIndex = 0) const
+        {
+            if (auto data = derived<DerivedT>(*this).findLatestInstanceData(entityID, instanceIndex))
+            {
+                AronDtoT aron;
+                aron.fromAron(data);
+                return aron;
+            }
+            else
+            {
+                return std::nullopt;
+            }
+        }
+
+    };
+
+}

source/RobotAPI/libraries/armem/core/wm/memory_definitions.cpp

@@ -9,6 +9,7 @@
 #include <vector>
 
 
+
 namespace armarx::armem::wm
 {
 
@@ -41,52 +42,4 @@ namespace armarx::armem::wm
         this->_metadata.timeArrived = update.timeArrived;
     }
 
-
-
-    std::optional<wm::EntitySnapshot> CoreSegment::getLatestEntitySnapshot(const MemoryID& entityID) const
-    {
-        const wm::Entity& entity = this->getEntity(entityID);
-        if (entity.empty())
-        {
-            return std::nullopt;
-        }
-        else
-        {
-            return entity.getLatestSnapshot();
-        }
-    }
-
-
-    std::optional<wm::EntityInstance> CoreSegment::getLatestEntityInstance(
-        const MemoryID& entityID, int instanceIndex) const
-    {
-        auto snapshot = getLatestEntitySnapshot(entityID);
-        if (snapshot.has_value()
-            and instanceIndex >= 0
-            and static_cast<size_t>(instanceIndex) < snapshot->size())
-        {
-            return snapshot->getInstance(instanceIndex);
-        }
-        else
-        {
-            return std::nullopt;
-        }
-    }
-
-
-    armarx::aron::datanavigator::DictNavigatorPtr
-    CoreSegment::getLatestEntityInstanceData(const MemoryID& entityID, int instanceIndex) const
-    {
-        auto instance = getLatestEntityInstance(entityID, instanceIndex);
-        if (instance.has_value())
-        {
-            return instance->data();
-        }
-        else
-        {
-            return nullptr;
-        }
-    }
-
-
 }

source/RobotAPI/libraries/armem/core/wm/memory_definitions.h

@@ -7,7 +7,7 @@
 #include <RobotAPI/libraries/armem/core/base/CoreSegmentBase.h>
 #include <RobotAPI/libraries/armem/core/base/MemoryBase.h>
 
-#include <optional>
+#include "detail/data_lookup_mixins.h"
 
 
 namespace armarx::armem::wm
@@ -56,6 +56,7 @@ namespace armarx::armem::wm
     /// @see base::EntityBase
     class Entity :
         public base::EntityBase<EntitySnapshot, Entity>
+        , public detail::FindInstanceDataMixin<Entity>
     {
     public:
 
@@ -68,10 +69,13 @@ namespace armarx::armem::wm
     /// @see base::ProviderSegmentBase
     class ProviderSegment :
         public base::ProviderSegmentBase<Entity, ProviderSegment>
+        , public detail::FindInstanceDataMixin<ProviderSegment>
     {
+        using Base = base::ProviderSegmentBase<Entity, ProviderSegment>;
+
     public:
 
-        using base::ProviderSegmentBase<Entity, ProviderSegment>::ProviderSegmentBase;
+        using Base::ProviderSegmentBase;
 
     };
 
@@ -80,6 +84,7 @@ namespace armarx::armem::wm
     /// @see base::CoreSegmentBase
     class CoreSegment :
         public base::CoreSegmentBase<ProviderSegment, CoreSegment>
+        , public detail::FindInstanceDataMixin<CoreSegment>
     {
         using Base = base::CoreSegmentBase<ProviderSegment, CoreSegment>;
 
@@ -87,34 +92,6 @@ namespace armarx::armem::wm
 
         using Base::CoreSegmentBase;
 
-
-        // Non-locking interface
-
-        std::optional<wm::EntitySnapshot> getLatestEntitySnapshot(
-            const MemoryID& entityID) const;
-        std::optional<wm::EntityInstance> getLatestEntityInstance(
-            const MemoryID& entityID, int instanceIndex = 0) const;
-        armarx::aron::datanavigator::DictNavigatorPtr getLatestEntityInstanceData(
-            const MemoryID& entityID, int instanceIndex = 0) const;
-
-
-        template <class AronDtoT>
-        std::optional<AronDtoT> getLatestEntityInstanceDataAs(
-            const MemoryID& entityID, int instanceIndex = 0) const
-        {
-            wm::EntityInstanceDataPtr data = getLatestEntityInstanceData(entityID, instanceIndex);
-            if (data)
-            {
-                AronDtoT aron;
-                aron.fromAron(data);
-                return aron;
-            }
-            else
-            {
-                return std::nullopt;
-            }
-        }
-
     };
 
 
@@ -122,6 +99,7 @@ namespace armarx::armem::wm
     /// @see base::MemoryBase
     class Memory :
         public base::MemoryBase<CoreSegment, Memory>
+        , public detail::FindInstanceDataMixin<Memory>
     {
         using Base = base::MemoryBase<CoreSegment, Memory>;
 

source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h

@@ -184,14 +184,9 @@ namespace armarx::armem::server::query_proc::base
             const auto referenceTimestamp = fromIce<Time>(query.timestamp);
             ARMARX_CHECK(referenceTimestamp.toMicroSeconds() >= 0) << "Reference timestamp is negative!";
 
-            try
+            if (auto* beforeOrAt = entity.findLatestSnapshotBeforeOrAt(referenceTimestamp))
             {
-                auto beforeOrAt = entity.getLatestSnapshotBeforeOrAt(referenceTimestamp);
-                addResultSnapshot(result, beforeOrAt);
-            }
-            catch (const armem::error::MissingEntry&)
-            {
-                // Leave empty.
+                addResultSnapshot(result, *beforeOrAt);
             }
         }
 
@@ -231,11 +226,11 @@ namespace armarx::armem::server::query_proc::base
                      const armem::query::data::entity::TimeApprox& query,
                      const EntityT& entity) const
         {
-            const auto referenceTimestamp = fromIce<Time>(query.timestamp);
+            const Time referenceTimestamp = fromIce<Time>(query.timestamp);
             ARMARX_CHECK(referenceTimestamp.toMicroSeconds() >= 0) << "Reference timestamp is negative!";
 
-            const auto referenceTimestampMicroSeconds = referenceTimestamp.toMicroSeconds();
-            const auto epsDuration = fromIce<Time>(query.eps).toMicroSeconds();
+            const float referenceTimestampMicroSeconds = referenceTimestamp.toMicroSeconds();
+            const float epsDuration = fromIce<Time>(query.eps).toMicroSeconds();
 
             // elements have to be in range [t_ref - eps, t_ref + eps] if eps is positive
             const auto isInRange = [&](const Time & t) -> bool
@@ -248,15 +243,14 @@ namespace armarx::armem::server::query_proc::base
                 return std::abs(t.toMicroSeconds() - referenceTimestampMicroSeconds) <= epsDuration;
             };
 
-            try
+            // last element before or at timestamp
+            if (auto* beforeOrAt = entity.findLatestSnapshotBeforeOrAt(referenceTimestamp))
             {
-                // last element before or at timestamp
-                auto beforeOrAt = entity.getLatestSnapshotBeforeOrAt(referenceTimestamp);
-                const auto timestampOfMatchBefore = beforeOrAt.id().timestamp;
+                const auto timestampOfMatchBefore = beforeOrAt->id().timestamp;
                 const auto isPerfectMatch = timestampOfMatchBefore == referenceTimestamp;
                 if (isInRange(timestampOfMatchBefore))
                 {
-                    addResultSnapshot(result, beforeOrAt);
+                    addResultSnapshot(result, *beforeOrAt);
                 }
 
                 // earsly stop, not necessary to also get the next since the match is perfect
@@ -266,16 +260,16 @@ namespace armarx::armem::server::query_proc::base
                 }
 
                 // 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))
+                const auto* after = entity.findFirstSnapshotAfterOrAt(referenceTimestamp);
+                if (after)
                 {
-                    addResultSnapshot(result, after);
+                    const auto timestampOfMatchAfter = after->id().timestamp;
+                    if (isInRange(timestampOfMatchAfter))
+                    {
+                        addResultSnapshot(result, *after);
+                    }
                 }
             }
-            catch (const armem::error::MissingEntry&)
-            {
-            }
         }
 
 

source/RobotAPI/libraries/armem/server/wm/memory_definitions.cpp

@@ -44,74 +44,12 @@ namespace armarx::armem::server::wm
 
 
 
-    Entity& ProviderSegment::addEntity(Entity&& entity)
-    {
-        Entity& added = ProviderSegmentBase::addEntity(std::move(entity));
-        added.setMaxHistorySize(this->getMaxHistorySize());
-        return added;
-    }
-
-
-
     std::mutex& CoreSegment::mutex() const
     {
         return _mutex;
     }
 
 
-    std::optional<wm::EntitySnapshot> CoreSegment::getLatestEntitySnapshot(const MemoryID& entityID) const
-    {
-        const wm::Entity& entity = this->getEntity(entityID);
-        if (entity.empty())
-        {
-            return std::nullopt;
-        }
-        else
-        {
-            return entity.getLatestSnapshot();
-        }
-    }
-
-
-    std::optional<wm::EntityInstance> CoreSegment::getLatestEntityInstance(
-        const MemoryID& entityID, int instanceIndex) const
-    {
-        auto snapshot = getLatestEntitySnapshot(entityID);
-        if (snapshot.has_value()
-            and instanceIndex >= 0
-            and static_cast<size_t>(instanceIndex) < snapshot->size())
-        {
-            return snapshot->getInstance(instanceIndex);
-        }
-        else
-        {
-            return std::nullopt;
-        }
-    }
-
-
-    armarx::aron::datanavigator::DictNavigatorPtr
-    CoreSegment::getLatestEntityInstanceData(const MemoryID& entityID, int instanceIndex) const
-    {
-        auto instance = getLatestEntityInstance(entityID, instanceIndex);
-        if (instance.has_value())
-        {
-            return instance->data();
-        }
-        else
-        {
-            return nullptr;
-        }
-    }
-
-
-    ProviderSegment& CoreSegment::addProviderSegment(ProviderSegment&& providerSegment)
-    {
-        ProviderSegmentT& added = CoreSegmentBase::addProviderSegment(std::move(providerSegment));
-        added.setMaxHistorySize(this->getMaxHistorySize());
-        return added;
-    }
-
 
     std::optional<wm::EntitySnapshot>
     CoreSegment::getLatestEntitySnapshotLocking(const MemoryID& entityID) const