Draft: Make RobotStateMemory ready

Goal:

• Make stable (especially race conditions)
• Improve runtime
• Add profiling/monitoring
• Refactor code
• Add missing interfaces

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

@@ -114,13 +114,13 @@ namespace armarx::armem::base
         }
 
 
-        inline const std::map<Time, EntitySnapshotT>& history() const
+        inline const ContainerT& history() const
         {
             return this->_container;
         }
-        inline std::map<Time, EntitySnapshotT>& history()
+        inline ContainerT& history()
         {
-            return const_cast<std::map<Time, EntitySnapshotT>&>(const_cast<const EntityBase*>(this)->history());
+            return const_cast<ContainerT&>(const_cast<const EntityBase*>(this)->history());
         }
 
 
@@ -226,7 +226,7 @@ namespace armarx::armem::base
         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);
+            typename ContainerT::const_iterator refIt = this->_container.upper_bound(time);
 
             if (refIt == this->_container.begin())
             {
@@ -238,7 +238,7 @@ namespace armarx::armem::base
             }
 
             // last element less than
-            typename std::map<Time, EntitySnapshotT>::const_iterator refItPrev = std::prev(refIt);
+            typename ContainerT::const_iterator refItPrev = std::prev(refIt);
 
             // ... or we return the element before if possible
             if (refItPrev != this->_container.begin())
@@ -259,7 +259,7 @@ namespace armarx::armem::base
         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);
+            typename ContainerT::const_iterator refIt = this->_container.upper_bound(time);
 
             if (refIt == this->_container.begin())
             {
@@ -267,7 +267,7 @@ namespace armarx::armem::base
             }
 
             // last element less than
-            typename std::map<Time, EntitySnapshotT>::const_iterator refItPrev = std::prev(refIt);
+            typename ContainerT::const_iterator refItPrev = std::prev(refIt);
 
             // we return the element before if possible
             if (refItPrev != this->_container.begin())
@@ -311,7 +311,7 @@ namespace armarx::armem::base
         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);
+            typename ContainerT::const_iterator refIt = this->_container.upper_bound(time);
 
             if (refIt == this->_container.end())
             {
@@ -330,7 +330,7 @@ namespace armarx::armem::base
         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);
+            typename ContainerT::const_iterator refIt = this->_container.upper_bound(time);
 
             if (refIt == this->_container.end())
             {
@@ -343,7 +343,7 @@ namespace armarx::armem::base
             }
 
             // first element greater than
-            typename std::map<Time, EntitySnapshotT>::const_iterator refItNext = std::next(refIt);
+            typename ContainerT::const_iterator refItNext = std::next(refIt);
 
             if (refItNext != this->_container.end())
             {
@@ -418,7 +418,7 @@ namespace armarx::armem::base
 
         EntitySnapshotT& addSnapshot(EntitySnapshotT&& snapshot)
         {
-            auto it = this->_container.emplace(snapshot.time(), std::move(snapshot)).first;
+            auto it = this->_container.emplace_hint(this->_container.end(), snapshot.time(), std::move(snapshot));
             it->second.id().setEntityID(this->id());
             return it->second;
         }
@@ -473,7 +473,9 @@ namespace armarx::armem::base
          * @return The latest snapshot.
          * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
          */
-        virtual const typename std::map<Time, EntitySnapshotT>::value_type& getLatestItem() const
+        virtual
+        const typename ContainerT::value_type&
+        getLatestItem() const
         {
             if (this->_container.empty())
             {