Merge branch 'master' of git.h2t.iar.kit.edu:sw/armarx/robot-api

source/RobotAPI/libraries/armem_objects/client/class/ClassReader.cpp

@@ -36,10 +36,10 @@ namespace armarx::armem::obj::clazz
     {
         armem::client::query::Builder builder;
         auto& entities = builder.coreSegments()
-                            .withName(properties().coreSegmentName)
-                            .providerSegments()
-                            .all()
-                            .entities();
+                             .withName(properties().coreSegmentName)
+                             .providerSegments()
+                             .all()
+                             .entities();
 
         for (const ObjectID& objectID : objectIDs)
         {
@@ -67,6 +67,40 @@ namespace armarx::armem::obj::clazz
         return objectClasses;
     }
 
+    std::map<ObjectID, armem::clazz::ObjectClass>
+    ClassReader::getAllObjectClasses()
+    {
+        armem::client::query::Builder builder;
+        
+        // clang-format off
+        builder
+            .coreSegments().withName(properties().coreSegmentName)
+            .providerSegments().all()
+            .entities().all()
+            .snapshots().latest();
+        // clang-format on
+
+        const armem::client::QueryResult result = memoryReader().query(builder);
+        if (not result.success)
+        {
+            throw armem::error::QueryFailed(result.errorMessage);
+        }
+
+        std::map<ObjectID, armem::clazz::ObjectClass> objectClasses;
+
+        result.memory.forEachInstance(
+            [&objectClasses](const armem::wm::EntityInstance& instance) -> bool
+            {
+                const ObjectID classID = ObjectID::FromString(instance.id().entityName);
+                auto aron = instance.dataAs<armarx::armem::arondto::ObjectClass>();
+                armarx::armem::clazz::fromAron(aron, objectClasses[classID]);
+
+                return true;
+            });
+
+        return objectClasses;
+    }
+
     std::string
     ClassReader::propertyPrefix() const
     {

source/RobotAPI/libraries/armem_objects/client/class/ClassReader.h

@@ -46,6 +46,8 @@ namespace armarx::armem::obj::clazz
         std::map<armarx::ObjectID, armem::clazz::ObjectClass>
         getObjectClasses(const std::vector<armarx::ObjectID>& objectIDs);
 
+        std::map<armarx::ObjectID, armem::clazz::ObjectClass> getAllObjectClasses();
+
     protected:
         std::string propertyPrefix() const final;
         Properties defaultProperties() const final;

source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp

@@ -10,6 +10,7 @@
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 #include <ArmarXCore/core/logging/Logging.h>
 #include <ArmarXCore/core/time/Clock.h>
+#include <ArmarXCore/core/time/Duration.h>
 
 #include "RobotAPI/libraries/armem_robot_state/client/common/constants.h"
 #include "RobotAPI/libraries/armem_robot_state/common/localization/types.h"
@@ -49,11 +50,14 @@ namespace armarx::armem::robot_state
     {
         transformReader.connect(memoryNameSystem);
 
+        armarx::Clock::WaitFor(armarx::Duration::MilliSeconds(200));
+
         // Wait for the memory to become available and add it as dependency.
         ARMARX_INFO << "RobotReader: Waiting for memory '" << constants::memoryName << "' ...";
         try
         {
-            memoryReader = memoryNameSystem.useReader(constants::memoryName);
+            // memoryReader = memoryNameSystem.useReader(constants::memoryName);
+            memoryReader = transformReader.getMemoryReader();
             ARMARX_INFO << "RobotReader: Connected to memory '" << constants::memoryName << "'";
         }
         catch (const armem::error::CouldNotResolveMemoryServer& e)

source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h

@@ -58,6 +58,8 @@ namespace armarx::armem::robot_state::client::localization
 
         void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def) override;
 
+        armem::client::Reader getMemoryReader(){return memoryReader;}
+
     private:
         armem::client::Reader memoryReader;
         mutable std::mutex memoryReaderMutex;