Add CoreSegmentQueryProcessor

source/RobotAPI/interface/armem/query/query.ice

@@ -32,6 +32,7 @@ module armarx
                 };
             }
 
+
             /// Which entities to get from a provider segment?
             class ProviderSegmentQuery
             {
@@ -53,30 +54,39 @@ module armarx
                 };
             }
 
-            module data
+
+            /// Which provider segments to get from a core segment?
+            class CoreSegmentQuery
             {
-                /// Which provider segments to get from a core segment?
-                class CoreSegmentQuery
-                {
-                    ProviderSegmentQuerySeq providerSegmentQueries;
-                };
-                sequence<CoreSegmentQuery> CoreSegmentQuerySeq;
+                ProviderSegmentQuerySeq providerSegmentQueries;
+            };
+            sequence<CoreSegmentQuery> CoreSegmentQuerySeq;
 
-                class AllProviderSegmentsQuery extends CoreSegmentQuery
+            module core
+            {
+                class All extends CoreSegmentQuery
                 {
                 };
-                class SingleProviderSegmentsQuery extends CoreSegmentQuery
+                class Single extends CoreSegmentQuery
                 {
                     string providerSegmentName;
                 };
-
-
-                /// Which core segments to get from a memory?
-                class MemoryQuery
+                class Regex extends CoreSegmentQuery
                 {
-                    CoreSegmentQuerySeq coreSegmentQueries;
+                    string providerSegmentNameRegex;
                 };
+            }
+
+
+            /// Which core segments to get from a memory?
+            class MemoryQuery
+            {
+                CoreSegmentQuerySeq coreSegmentQueries;
+            };
+            sequence<MemoryQuery> MemoryQuerySeq;
 
+            module memory
+            {
                 class AllCoreSegmentsQuery extends MemoryQuery
                 {
                 };
@@ -84,6 +94,10 @@ module armarx
                 {
                     string coreSegmentName;
                 };
+                class RegexCoreSegmentsQuery extends ProviderSegmentQuery
+                {
+                    string coreSegmentNameRegex;
+                };
             }
         }
 

source/RobotAPI/libraries/armem/CMakeLists.txt

@@ -44,6 +44,7 @@ set(LIB_FILES
     query/BaseQueryProcessor.cpp
     query/EntityQueryProcessor.cpp
     query/ProviderSegmentQueryProcessor.cpp
+    query/CoreSegmentQueryProcessor.cpp
 
     legacy/ArMemBase.cpp
 )
@@ -83,6 +84,7 @@ set(LIB_HEADERS
     query/BaseQueryProcessor.h
     query/EntityQueryProcessor.h
     query/ProviderSegmentQueryProcessor.h
+    query/CoreSegmentQueryProcessor.h
 
     legacy/ArMemBase.h
 )

source/RobotAPI/libraries/armem/memory/CoreSegment.cpp

@@ -79,6 +79,17 @@ namespace armarx::armem
         return *it->second;
     }
 
+    ProviderSegment& CoreSegment::addProviderSegment(const ProviderSegmentPtr& providerSegment)
+    {
+        return addProviderSegment(std::make_unique<ProviderSegment>(*providerSegment));
+    }
+
+    ProviderSegment& CoreSegment::addProviderSegment(ProviderSegmentPtr&& providerSegment)
+    {
+        auto it = providerSegments.emplace(providerSegment->name, std::move(providerSegment)).first;
+        return *it->second;
+    }
+
 
     MemoryID CoreSegment::update(const InternalEntityUpdate& update)
     {

source/RobotAPI/libraries/armem/memory/CoreSegment.h

@@ -40,11 +40,14 @@ namespace armarx::armem
         const Entity& getEntity(const MemoryID& id) const override;
 
 
-        ProviderSegment& addProviderSegment(const std::string& name);
-
         virtual MemoryID update(const InternalEntityUpdate& update) override;
         using EntityStorage::update;
 
+        ProviderSegment& addProviderSegment(const std::string& name);
+        ProviderSegment& addProviderSegment(const ProviderSegmentPtr& providerSegment);
+        ProviderSegment& addProviderSegment(ProviderSegmentPtr&& providerSegment);
+
+
         void clear() override;
 
 

source/RobotAPI/libraries/armem/query/CoreSegmentQueryProcessor.cpp

+#include "CoreSegmentQueryProcessor.h"
+
+#include <regex>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include "../error/ArMemError.h"
+
+#include "../core/ice_conversions.h"
+
+
+namespace armarx::armem
+{
+
+    void CoreSegmentQueryProcessor::process(CoreSegment& result, const query::CoreSegmentQuery& query, const CoreSegment& coreSegment) const
+    {
+        if (auto q = dynamic_cast<const query::core::All*>(&query))
+        {
+            process(result, *q, coreSegment);
+        }
+        else if (auto q = dynamic_cast<const query::core::Single*>(&query))
+        {
+            process(result, *q, coreSegment);
+        }
+        else if (auto q = dynamic_cast<const query::core::Regex*>(&query))
+        {
+            process(result, *q, coreSegment);
+        }
+        else
+        {
+            throw armem::error::UnknownQueryType("core segment", query);
+        }
+    }
+
+
+    void CoreSegmentQueryProcessor::process(CoreSegment& result,
+        const query::core::All& query, const CoreSegment& coreSegment) const
+    {
+        for (const auto& [name, providerSegment] : coreSegment.providerSegments)
+        {
+            result.addProviderSegment(providerSegmentProcessor.process(query.providerSegmentQueries, *providerSegment));
+        }
+    }
+
+    void CoreSegmentQueryProcessor::process(CoreSegment& result,
+        const query::core::Single& query, const CoreSegment& coreSegment) const
+    {
+        try
+        {
+            const ProviderSegment& providerSegment = coreSegment.getProviderSegment(query.providerSegmentName);
+            result.addProviderSegment(providerSegmentProcessor.process(query.providerSegmentQueries, providerSegment));
+        }
+        catch (const error::MissingEntry&)
+        {
+            // Leave empty.
+        }
+    }
+
+    void CoreSegmentQueryProcessor::process(CoreSegment& result,
+            const query::core::Regex& query, const CoreSegment& coreSegment) const
+    {
+        std::regex regex(query.providerSegmentNameRegex);
+        for (const auto& [name, providerSegment] : coreSegment.providerSegments)
+        {
+            if (std::regex_search(providerSegment->name, regex))
+            {
+                result.addProviderSegment(providerSegmentProcessor.process(query.providerSegmentQueries, *providerSegment));
+            }
+        }
+    }
+
+
+    data::CoreSegment CoreSegmentQueryProcessor::processToIce(
+            const query::CoreSegmentQuery& query, const CoreSegment& coreSegment) const
+    {
+        return toIce<data::CoreSegment>(process(query, coreSegment));
+    }
+
+}

source/RobotAPI/libraries/armem/query/CoreSegmentQueryProcessor.h

+#pragma once
+
+#include <RobotAPI/interface/armem/query.h>
+
+#include "../memory/Memory.h"
+
+#include "ice_conversions.h"
+#include "BaseQueryProcessor.h"
+#include "ProviderSegmentQueryProcessor.h"
+
+
+
+namespace armarx::armem
+{
+
+    /**
+     * @brief Handles memory queries.
+     */
+    class CoreSegmentQueryProcessor : public BaseQueryProcessor<CoreSegment, query::CoreSegmentQuery>
+    {
+    public:
+
+        using BaseQueryProcessor::process;
+
+        void process(CoreSegment& result, const query::CoreSegmentQuery& query, const CoreSegment& coreSegment) const;
+
+        void process(CoreSegment& result, const query::core::All& query, const CoreSegment& coreSegment) const;
+        void process(CoreSegment& result, const query::core::Single& query, const CoreSegment& coreSegment) const;
+        void process(CoreSegment& result, const query::core::Regex& query, const CoreSegment& coreSegment) const;
+
+
+        data::CoreSegment processToIce(const query::CoreSegmentQuery& query, const CoreSegment& coreSegment) const;
+
+
+    private:
+
+        ProviderSegmentQueryProcessor providerSegmentProcessor;
+
+    };
+
+}