armem/dev => master

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

@@ -6,14 +6,13 @@
 #include "ArmarXCore/core/logging/Logging.h"
 #include <ArmarXCore/core/PackagePath.h>
 
-#include "RobotAPI/libraries/armem/core/Time.h"
 #include "RobotAPI/libraries/armem/client/query/Builder.h"
+#include "RobotAPI/libraries/armem/core/Time.h"
 #include "RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h"
-#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
 #include "RobotAPI/libraries/armem_robot/aron_conversions.h"
+#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
 #include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
 
-
 namespace fs = ::std::filesystem;
 
 namespace armarx::armem::articulated_object
@@ -38,7 +37,6 @@ namespace armarx::armem::articulated_object
         def->optional(properties.providerName, prefix + "ProviderName");
     }
 
-
     void Reader::connect()
     {
         // Wait for the memory to become available and add it as dependency.
@@ -55,7 +53,7 @@ namespace armarx::armem::articulated_object
         memoryReader.setReadingMemory(result.proxy);
 
         armem::MemoryID id = armem::MemoryID();
-        id.memoryName = properties.memoryName;
+        id.memoryName      = properties.memoryName;
         id.coreSegmentName = properties.coreClassSegmentName;
         // listen to all provider segments!
 
@@ -72,7 +70,8 @@ namespace armarx::armem::articulated_object
         // TODO(fabian.reister): implement
     }
 
-    void Reader::updateKnownObjects(const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs)
+    void Reader::updateKnownObjects(const armem::MemoryID& subscriptionID,
+                                    const std::vector<armem::MemoryID>& snapshotIDs)
     {
         ARMARX_INFO << "New objects available!";
 
@@ -89,15 +88,14 @@ namespace armarx::armem::articulated_object
 
         // 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)
+    std::optional<ArticulatedObject> Reader::get(const std::string& name,
+            const armem::Time& timestamp)
     {
         const auto description = queryDescription(name, timestamp);
 
@@ -110,17 +108,13 @@ namespace armarx::armem::articulated_object
         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
-        };
+        ArticulatedObject obj{.description = description,
+                              .instance    = "", // TODO(fabian.reister):
+                              .config      = {}, // will be populated by synchronize
+                              .timestamp   = timestamp};
 
         synchronize(obj, timestamp);
 
@@ -165,7 +159,8 @@ namespace armarx::armem::articulated_object
         return getRobotDescriptions(qResult.memory);
     }
 
-    std::optional<robot::RobotDescription> Reader::queryDescription(const std::string& name, const armem::Time& timestamp)
+    std::optional<robot::RobotDescription> Reader::queryDescription(const std::string& name,
+            const armem::Time& timestamp)
     {
         // Query all entities from provider.
         armem::client::query::Builder qb;
@@ -174,8 +169,8 @@ namespace armarx::armem::articulated_object
         qb
         .coreSegments().withName(properties.coreClassSegmentName)
         .providerSegments().all() // TODO(fabian.reister): think about this: which authority is trustworthy?
-        .entities().withName(name)
-        .snapshots().atTime(timestamp);
+        .entities().all() // withName(name)
+        .snapshots().latest();
         // clang-format on
 
         const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
@@ -184,13 +179,15 @@ namespace armarx::armem::articulated_object
 
         if (not qResult.success) /* c++20 [[unlikely]] */
         {
+            ARMARX_WARNING << qResult.errorMessage;
             return std::nullopt;
         }
 
         return getRobotDescription(qResult.memory);
     }
 
-    std::optional<robot::RobotState> Reader::queryState(const robot::RobotDescription& description, const armem::Time& timestamp)
+    std::optional<robot::RobotState> Reader::queryState(const robot::RobotDescription& description,
+            const armem::Time& timestamp)
     {
         // TODO(fabian.reister): how to deal with multiple providers?
 
@@ -200,9 +197,9 @@ namespace armarx::armem::articulated_object
         // clang-format off
         qb
         .coreSegments().withName(properties.coreInstanceSegmentName)
-        .providerSegments().withName(properties.providerName) // agent
+        .providerSegments().all() // withName(properties.providerName) // agent
         .entities().withName(description.name)
-        .snapshots().atTime(timestamp);
+        .snapshots().latest();
         // clang-format on
 
         const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
@@ -217,76 +214,91 @@ namespace armarx::armem::articulated_object
         return getRobotState(qResult.memory);
     }
 
-
-    std::optional<robot::RobotState> Reader::getRobotState(const armarx::armem::wm::Memory& memory) const
+    std::optional<robot::RobotState>
+    Reader::getRobotState(const armarx::armem::wm::Memory& memory) const
     {
         // clang-format off
-        const armem::wm::ProviderSegment& providerSegment = memory
-                .getCoreSegment(properties.coreInstanceSegmentName)
-                .getProviderSegment(properties.providerName);
+        const armem::wm::CoreSegment& coreSegment = memory
+                .getCoreSegment(properties.coreInstanceSegmentName);
         // clang-format on
-        const auto entities = simox::alg::get_values(providerSegment.entities());
 
-        if (entities.empty())
+        for (const auto &[_, providerSegment] : coreSegment.providerSegments())
         {
-            ARMARX_WARNING << "No entity found";
-            return std::nullopt;
-        }
 
-        const auto entitySnapshots = simox::alg::get_values(entities.front().history());
+            const auto entities = simox::alg::get_values(providerSegment.entities());
 
-        if (entitySnapshots.empty())
-        {
-            ARMARX_WARNING << "No entity snapshots found";
-            return std::nullopt;
-        }
+            if (entities.empty())
+            {
+                ARMARX_WARNING << "No entity found";
+                continue;
+            }
 
-        // TODO(fabian.reister): check if 0 available
-        const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
+            const auto entitySnapshots = simox::alg::get_values(entities.front().history());
 
-        return robot::convertRobotState(instance);
-    }
+            if (entitySnapshots.empty())
+            {
+                ARMARX_WARNING << "No entity snapshots found";
+                continue;
+            }
 
+            // TODO(fabian.reister): check if 0 available
+            const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
 
+            return robot::convertRobotState(instance);
+        }
+
+        return std::nullopt;
+    }
 
-    std::optional<robot::RobotDescription> Reader::getRobotDescription(const armarx::armem::wm::Memory& memory) const
+    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
+        const armem::wm::CoreSegment& coreSegment = memory
+                .getCoreSegment(properties.coreClassSegmentName);
+        // .getProviderSegment(properties.providerName); // TODO(fabian.reister): all
         // clang-format on
-        const auto entities = simox::alg::get_values(providerSegment.entities());
 
-        if (entities.empty())
+        for (const auto &[_, providerSegment] : coreSegment.providerSegments())
         {
-            ARMARX_WARNING << "No entity found";
-            return std::nullopt;
-        }
+            const auto entities = simox::alg::get_values(providerSegment.entities());
+
+            if (entities.empty())
+            {
+                ARMARX_WARNING << "No entity found";
+                continue;
+                // return std::nullopt;
+            }
 
-        const auto entitySnapshots = simox::alg::get_values(entities.front().history());
+            const auto entitySnapshots = simox::alg::get_values(entities.front().history());
 
-        if (entitySnapshots.empty())
-        {
-            ARMARX_WARNING << "No entity snapshots found";
-            return std::nullopt;
-        }
+            if (entitySnapshots.empty())
+            {
+                ARMARX_WARNING << "No entity snapshots found";
+                continue;
+                // return std::nullopt;
+            }
+
+            // TODO(fabian.reister): check if 0 available
+            const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
 
-        // TODO(fabian.reister): check if 0 available
-        const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
+            return robot::convertRobotDescription(instance);
+        }
 
-        return robot::convertRobotDescription(instance);
+        return std::nullopt;
     }
 
-    std::vector<robot::RobotDescription> Reader::getRobotDescriptions(const armarx::armem::wm::Memory& memory) const
+    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);
+        const armem::wm::CoreSegment& coreSegment =
+            memory.getCoreSegment(properties.coreClassSegmentName);
 
-        for (const auto& [providerName, providerSegment] : coreSegment.providerSegments())
+        for (const auto &[providerName, providerSegment] : coreSegment.providerSegments())
         {
-            for (const auto& [name, entity] : providerSegment.entities())
+            for (const auto &[name, entity] : providerSegment.entities())
             {
                 if (entity.empty())
                 {
@@ -309,5 +321,4 @@ namespace armarx::armem::articulated_object
         return descriptions;
     }
 
-
 } // namespace armarx::armem::articulated_object
\ No newline at end of file