armem/dev => master

source/RobotAPI/libraries/armem_objects/client/attachment/Reader.cpp

@@ -9,15 +9,68 @@
 #include "RobotAPI/libraries/armem/core/Time.h"
 #include "RobotAPI/libraries/armem/client/query/Builder.h"
 #include "RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h"
+#include "RobotAPI/libraries/armem/util/util.h"
 #include "RobotAPI/libraries/armem_robot/robot_conversions.h"
 #include "RobotAPI/libraries/armem_robot/aron_conversions.h"
 #include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include "RobotAPI/libraries/aron/common/aron_conversions.h"
 
+namespace armarx::armem::attachment
+{
 
-namespace fs = ::std::filesystem;
+    namespace detail
+    {
 
-namespace armarx::armem::articulated_object
-{
+        template<typename AronClass, typename ArmemClass>
+        auto getAttachments(const armarx::armem::wm::Memory& memory)
+        {
+            // using ArmemClass = decltype(fromAron(AronClass()));
+            using ArmemClassVector = std::vector<ArmemClass>;
+
+            ArmemClassVector attachments;
+
+            for (const auto&[_, coreSegment] : memory.coreSegments())
+            {
+                for (const auto& [providerName, providerSegment] : coreSegment.providerSegments())
+                {
+                    for (const auto& [name, entity] : providerSegment.entities())
+                    {
+                        if (entity.empty())
+                        {
+                            ARMARX_WARNING << "No entity found";
+                            continue;
+                        }
+
+                        const auto entitySnapshots = simox::alg::get_values(entity.history());
+                        const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
+
+                        try
+                        {
+                            AronClass aronAttachment;
+                            aronAttachment.fromAron(instance.data());
+
+                            ArmemClass attachment;
+                            fromAron(aronAttachment, attachment);
+
+                            if (attachment.active)
+                            {
+                                attachments.push_back(attachment);
+                            }
+
+                        }
+                        catch (const armarx::aron::error::AronException&)
+                        {
+                            continue;
+                        }
+                    }
+                }
+            }
+
+            return attachments;
+        }
+    }  // namespace detail
 
     Reader::Reader(armem::ClientReaderComponentPluginUser& component) : component(component) {}
 
@@ -29,13 +82,9 @@ namespace armarx::armem::articulated_object
 
         def->optional(properties.memoryName, prefix + "MemoryName");
 
-        def->optional(properties.coreInstanceSegmentName,
-                      prefix + "CoreSegment",
-                      "Name of the memory core segment to use for object instances.");
-        def->optional(properties.coreClassSegmentName,
+        def->optional(properties.coreAttachmentsSegmentName,
                       prefix + "CoreSegment",
-                      "Name of the memory core segment to use for object classes.");
-        def->optional(properties.providerName, prefix + "ProviderName");
+                      "Name of the memory core segment to use for object attachments.");
     }
 
 
@@ -53,104 +102,20 @@ namespace armarx::armem::articulated_object
         ARMARX_IMPORTANT << "Reader: Connected to memory '" << properties.memoryName;
 
         memoryReader.setReadingMemory(result.proxy);
-
-        armem::MemoryID id = armem::MemoryID();
-        id.memoryName = properties.memoryName;
-        id.coreSegmentName = properties.coreClassSegmentName;
-        // listen to all provider segments!
-
-        memoryReader.subscribe(id, this, &Reader::updateKnownObjects);
     }
 
-    void Reader::updateKnownObject(const armem::MemoryID& snapshotId)
-    {
-        // const std::string& nameWithDataset = snapshotId.providerSegmentName;
-
-        // arondto::RobotDescription aronArticulatedObjectDescription;
-        // aronArticulatedObjectDescription.fromAron(snapshotId.);
 
-        // TODO(fabian.reister): implement
-    }
-
-    void Reader::updateKnownObjects(const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs)
+    std::vector<ObjectAttachment> Reader::queryObjectAttachments(const armem::Time& timestamp) const
     {
-        ARMARX_INFO << "New objects available!";
-
-        // // Query all entities from provider.
-        // armem::client::query::Builder qb;
-
-        // // clang-format off
-        // qb
-        // .coreSegments().withName(properties.coreClassSegmentName)
-        // .providerSegments().all() // TODO(fabian.reister): think about this: which authority is trustworthy?
-        // .entities().withName(name)
-        // .snapshots().atTime(timestamp);
-        // // clang-format on
-
-        // const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
-
-
-        // std::for_each(snapshotIDs.begin(), snapshotIDs.end(), [&](const auto & snapshotID)
-        // {
-        //     updateKnownObject(snapshotID);
-        // });
-    }
-
-
-    std::optional<ArticulatedObject> Reader::get(const std::string& name, const armem::Time& timestamp)
-    {
-        const auto description = queryDescription(name, timestamp);
-
-        if (not description)
-        {
-            ARMARX_WARNING << "Unknown object " << name;
-            return std::nullopt;
-        }
-
-        return get(*description, timestamp);
-    }
-
-
-    ArticulatedObject Reader::get(const ArticulatedObjectDescription& description,
-                                  const armem::Time& timestamp)
-    {
-        ArticulatedObject obj
-        {
-            .description = description,
-            .instance = "", // TODO(fabian.reister):
-            .config = {}, // will be populated by synchronize
-            .timestamp = timestamp
-        };
-
-        synchronize(obj, timestamp);
-
-        return obj;
-    }
-
-    void Reader::synchronize(ArticulatedObject& obj, const armem::Time& timestamp)
-    {
-        auto state = queryState(obj.description, timestamp);
-
-        if (not state) /* c++20 [[unlikely]] */
-        {
-            ARMARX_WARNING << "Could not synchronize object " << obj.description.name;
-            return;
-        }
-
-        obj.config = std::move(*state);
-    }
-
-    std::vector<robot::RobotDescription> Reader::queryDescriptions(const armem::Time& timestamp)
-    {
-        // Query all entities from provider.
+        // Query all entities from all provider.
         armem::client::query::Builder qb;
 
         // clang-format off
         qb
-        .coreSegments().withName(properties.coreClassSegmentName)
+        .coreSegments().withName(properties.coreAttachmentsSegmentName)
         .providerSegments().all()
         .entities().all()
-        .snapshots().latest(); // TODO beforeTime(timestamp);
+        .snapshots().beforeOrAtTime(timestamp);
         // clang-format on
 
         const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
@@ -162,20 +127,20 @@ namespace armarx::armem::articulated_object
             return {};
         }
 
-        return getRobotDescriptions(qResult.memory);
+        return detail::getAttachments<::armarx::armem::arondto::attachment::ObjectAttachment, ::armarx::armem::attachment::ObjectAttachment>(qResult.memory);
     }
 
-    std::optional<robot::RobotDescription> Reader::queryDescription(const std::string& name, const armem::Time& timestamp)
+    std::vector<ObjectAttachment> Reader::queryObjectAttachments(const armem::Time& timestamp, const std::string& providerName) const
     {
         // Query all entities from provider.
         armem::client::query::Builder qb;
 
         // clang-format off
         qb
-        .coreSegments().withName(properties.coreClassSegmentName)
-        .providerSegments().all() // TODO(fabian.reister): think about this: which authority is trustworthy?
-        .entities().withName(name)
-        .snapshots().atTime(timestamp);
+        .coreSegments().withName(properties.coreAttachmentsSegmentName)
+        .providerSegments().withName(providerName)
+        .entities().all()
+        .snapshots().beforeOrAtTime(timestamp);
         // clang-format on
 
         const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
@@ -184,25 +149,23 @@ namespace armarx::armem::articulated_object
 
         if (not qResult.success) /* c++20 [[unlikely]] */
         {
-            return std::nullopt;
+            return {};
         }
 
-        return getRobotDescription(qResult.memory);
+        return detail::getAttachments<::armarx::armem::arondto::attachment::ObjectAttachment, ::armarx::armem::attachment::ObjectAttachment>(qResult.memory);
     }
 
-    std::optional<robot::RobotState> Reader::queryState(const robot::RobotDescription& description, const armem::Time& timestamp)
+    std::vector<ArticulatedObjectAttachment> Reader::queryArticulatedObjectAttachments(const armem::Time& timestamp) const
     {
-        // TODO(fabian.reister): how to deal with multiple providers?
-
-        // Query all entities from provider.
+        // Query all entities from all provider.
         armem::client::query::Builder qb;
 
         // clang-format off
         qb
-        .coreSegments().withName(properties.coreInstanceSegmentName)
-        .providerSegments().withName(properties.providerName) // agent
-        .entities().withName(description.name)
-        .snapshots().atTime(timestamp);
+        .coreSegments().withName(properties.coreAttachmentsSegmentName)
+        .providerSegments().all()
+        .entities().all()
+        .snapshots().beforeOrAtTime(timestamp);
         // clang-format on
 
         const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
@@ -211,103 +174,37 @@ namespace armarx::armem::articulated_object
 
         if (not qResult.success) /* c++20 [[unlikely]] */
         {
-            return std::nullopt;
+            return {};
         }
 
-        return getRobotState(qResult.memory);
+        return detail::getAttachments<::armarx::armem::arondto::attachment::ArticulatedObjectAttachment, ::armarx::armem::attachment::ArticulatedObjectAttachment>(qResult.memory);
     }
 
-
-    std::optional<robot::RobotState> Reader::getRobotState(const armarx::armem::wm::Memory& memory) const
+    std::vector<ArticulatedObjectAttachment> Reader::queryArticulatedObjectAttachments(const armem::Time& timestamp, const std::string& providerName) const
     {
-        // clang-format off
-        const armem::wm::ProviderSegment& providerSegment = memory
-                .getCoreSegment(properties.coreInstanceSegmentName)
-                .getProviderSegment(properties.providerName);
-        // clang-format on
-        const auto entities = simox::alg::get_values(providerSegment.entities());
-
-        if (entities.empty())
-        {
-            ARMARX_WARNING << "No entity found";
-            return std::nullopt;
-        }
-
-        const auto entitySnapshots = simox::alg::get_values(entities.front().history());
-
-        if (entitySnapshots.empty())
-        {
-            ARMARX_WARNING << "No entity snapshots found";
-            return std::nullopt;
-        }
-
-        // TODO(fabian.reister): check if 0 available
-        const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
-
-        return robot::convertRobotState(instance);
-    }
-
-
+        // Query all entities from provider.
+        armem::client::query::Builder qb;
 
-    std::optional<robot::RobotDescription> Reader::getRobotDescription(const armarx::armem::wm::Memory& memory) const
-    {
         // clang-format off
-        const armem::wm::ProviderSegment& providerSegment = memory
-                .getCoreSegment(properties.coreClassSegmentName)
-                .getProviderSegment(properties.providerName); // TODO(fabian.reister): all
+        qb
+        .coreSegments().withName(properties.coreAttachmentsSegmentName)
+        .providerSegments().withName(providerName)
+        .entities().all()
+        .snapshots().beforeOrAtTime(timestamp);
         // clang-format on
-        const auto entities = simox::alg::get_values(providerSegment.entities());
 
-        if (entities.empty())
-        {
-            ARMARX_WARNING << "No entity found";
-            return std::nullopt;
-        }
+        const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
 
-        const auto entitySnapshots = simox::alg::get_values(entities.front().history());
+        ARMARX_DEBUG << "Lookup result in reader: " << qResult;
 
-        if (entitySnapshots.empty())
+        if (not qResult.success) /* c++20 [[unlikely]] */
         {
-            ARMARX_WARNING << "No entity snapshots found";
-            return std::nullopt;
+            return {};
         }
 
-        // TODO(fabian.reister): check if 0 available
-        const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
-
-        return robot::convertRobotDescription(instance);
+        return detail::getAttachments<::armarx::armem::arondto::attachment::ArticulatedObjectAttachment, ::armarx::armem::attachment::ArticulatedObjectAttachment>(qResult.memory);
     }
 
-    std::vector<robot::RobotDescription> Reader::getRobotDescriptions(const armarx::armem::wm::Memory& memory) const
-    {
-        std::vector<robot::RobotDescription> descriptions;
-
-        const armem::wm::CoreSegment& coreSegment = memory.getCoreSegment(properties.coreClassSegmentName);
-
-        for (const auto& [providerName, providerSegment] : coreSegment.providerSegments())
-        {
-            for (const auto& [name, entity] : providerSegment.entities())
-            {
-                if (entity.empty())
-                {
-                    ARMARX_WARNING << "No entity found";
-                    continue;
-                }
-
-                const auto entitySnapshots = simox::alg::get_values(entity.history());
-                const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
-
-                const auto robotDescription = robot::convertRobotDescription(instance);
-
-                if (robotDescription)
-                {
-                    descriptions.push_back(*robotDescription);
-                }
-            }
-        }
-
-        return descriptions;
-    }
 
 
-} // namespace armarx::armem::articulated_object
\ No newline at end of file
+}  // namespace armarx::armem::attachment
\ No newline at end of file