From 69b81532b3897042fdfca522331ce489c522ae94 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Tue, 20 Jul 2021 13:21:28 +0200
Subject: [PATCH] Add locking and non-locking getters

---
 .../armem/core/workingmemory/CoreSegment.cpp  | 68 +++++++++++++++++++
 .../armem/core/workingmemory/CoreSegment.h    | 62 +++++++++++++++++
 2 files changed, 130 insertions(+)

diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.cpp
index dd23bddb5..601f07935 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.cpp
+++ b/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);
+    }
+
 }
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h b/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h
index 351ab40f8..86b9b439e 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h
@@ -1,6 +1,7 @@
 #pragma once
 
 #include <mutex>
+#include <optional>
 
 #include "../base/CoreSegmentBase.h"
 
@@ -38,6 +39,67 @@ namespace armarx::armem::wm
         std::mutex& mutex() const;
 
 
+        // 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;
+            }
+        }
+
+
+        // Locking interface
+
+        std::optional<wm::EntitySnapshot> getLatestEntitySnapshotLocking(
+            const MemoryID& entityID) const;
+        std::optional<wm::EntityInstance> getLatestEntityInstanceLocking(
+            const MemoryID& entityID, int instanceIndex = 0) const;
+        armarx::aron::datanavigator::DictNavigatorPtr getLatestEntityInstanceDataLocking(
+            const MemoryID& entityID, int instanceIndex = 0) const;
+
+
+        template <class AronDtoT>
+        std::optional<AronDtoT> getLatestEntityInstanceDataLockingAs(
+            const MemoryID& entityID, int instanceIndex = 0) const
+        {
+            // Keep lock to a minimum.
+            wm::EntityInstanceDataPtr data = nullptr;
+            {
+                std::scoped_lock lock(_mutex);
+                data = getLatestEntityInstanceData(entityID, instanceIndex);
+            }
+            if (data)
+            {
+                AronDtoT aron;
+                aron.fromAron(data);
+                return aron;
+            }
+            else
+            {
+                return std::nullopt;
+            }
+        }
+
+
     protected:
 
         virtual void _copySelfWithoutData(CoreSegment& other) const override;
-- 
GitLab