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/workingmemory/CoreSegment.cpp

@@ -65,4 +65,72 @@ namespace armarx::armem::wm
         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;
+        }
+    }
+
+
+    std::optional<wm::EntitySnapshot>
+    CoreSegment::getLatestEntitySnapshotLocking(const MemoryID& entityID) const
+    {
+        std::scoped_lock lock(_mutex);
+        return getLatestEntitySnapshot(entityID);
+    }
+
+    std::optional<wm::EntityInstance>
+    CoreSegment::getLatestEntityInstanceLocking(const MemoryID& entityID, int instanceIndex) const
+    {
+        std::scoped_lock lock(_mutex);
+        return getLatestEntityInstance(entityID, instanceIndex);
+    }
+
+    armarx::aron::datanavigator::DictNavigatorPtr
+    CoreSegment::getLatestEntityInstanceDataLocking(const MemoryID& entityID, int instanceIndex) const
+    {
+        std::scoped_lock lock(_mutex);
+        return getLatestEntityInstanceData(entityID, instanceIndex);
+    }
+
 }