Articulated objects are now stored in memory (writer is working)

source/RobotAPI/components/armem/client/ArticulatedObjectExampleMemoryWriterClient/ArticulatedObjectExampleMemoryWriterClient.cpp

 
 
 #include "ArticulatedObjectExampleMemoryWriterClient.h"
-#include "ArmarXCore/core/PackagePath.h"
-#include "ArmarXCore/core/system/ArmarXDataPath.h"
-#include "RobotAPI/libraries/armem/core/Time.h"
 
-#include <Eigen/src/Geometry/Transform.h>
+#include <memory>
+
+#include <Eigen/Geometry>
+
 #include <IceUtil/Time.h>
+
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/XML/RobotIO.h>
 #include <VirtualRobot/VirtualRobot.h>
-#include <memory>
 
+#include <ArmarXCore/core/PackagePath.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 #include <ArmarXCore/core/time/CycleUtil.h>
 
@@ -17,19 +21,18 @@
 #include <RobotAPI/libraries/armem/client/query/query_fns.h>
 #include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
 #include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
 
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/XML/RobotIO.h>
 
 namespace armarx::articulated_object
 {
     ArticulatedObjectExampleMemoryWriterClient::ArticulatedObjectExampleMemoryWriterClient() :
-        articulatedObjectWriter(new ::armarx::armem::articulated_object::Writer(*this)){}
+        articulatedObjectWriter(new ::armarx::armem::articulated_object::Writer(*this)) {}
 
     armarx::PropertyDefinitionsPtr ArticulatedObjectExampleMemoryWriterClient::createPropertyDefinitions()
     {
         armarx::PropertyDefinitionsPtr defs =
-                new ComponentPropertyDefinitions(getConfigIdentifier());
+            new ComponentPropertyDefinitions(getConfigIdentifier());
 
         defs->topic(debugObserver);
 
@@ -55,31 +58,29 @@ namespace armarx::articulated_object
         task->start();
     }
 
-    void ArticulatedObjectExampleMemoryWriterClient::onDisconnectComponent() { task->stop(); }
+    void ArticulatedObjectExampleMemoryWriterClient::onDisconnectComponent()
+    {
+        task->stop();
+    }
 
     void ArticulatedObjectExampleMemoryWriterClient::onExitComponent() {}
 
     VirtualRobot::RobotPtr createDishwasher()
     {
         const std::string xml =
-                "Environment/mobile-kitchen/dishwasher-only/dishwasher.xml";
+            "./ArmarXObjects/Environment/mobile-kitchen/dishwasher-only/dishwasher.xml";
         const std::string name = "dishwasher";
 
-        const PackagePath packagePath(armarx::ArmarXDataPath::getProject({"ArmarXObjects"}, xml), xml);
-
-        auto robot = VirtualRobot::RobotIO::loadRobot(packagePath.toSystemPath(), VirtualRobot::RobotIO::eStructure);
-
-        return robot;
+        return VirtualRobot::RobotIO::loadRobot(ArmarXDataPath::resolvePath(xml), VirtualRobot::RobotIO::eStructure);
     }
 
     armem::articulated_object::ArticulatedObject convert(const VirtualRobot::Robot& obj, const armem::Time& timestamp)
     {
-        ARMARX_INFO << "filename is " << obj.getFilename();
-
-        
+        ARMARX_INFO << "Filename is " << obj.getFilename();
 
-        return 
-        armem::articulated_object::ArticulatedObject{
+        return
+            armem::articulated_object::ArticulatedObject
+        {
             .description = {
                 .name = obj.getName(),
                 .xml = PackagePath(armarx::ArmarXDataPath::getProject({"ArmarXObjects"}, obj.getFilename()), obj.getFilename())
@@ -106,7 +107,6 @@ namespace armarx::articulated_object
 
         while (not task->isStopped())
         {
-
             ARMARX_INFO << "Reporting articulated objects";
 
             const IceUtil::Time now = TimeUtil::GetTime();

source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp

@@ -23,6 +23,7 @@
 #include "ObjectMemory.h"
 
 #include <RobotAPI/interface/core/articulated_object/topic.h>
+#include <mutex>
 
 
 namespace armarx::armem::server::obj
@@ -120,8 +121,16 @@ namespace armarx::armem::server::obj
             ArVizComponentPluginUser::arviz
         );
 
+
+        {
+            std::lock_guard g(server::ComponentPluginUser::workingMemoryMutex);
+            server::ComponentPluginUser::workingMemory.addCoreSegment("ArticulatedObjectInstance"); // TODO , arondto::::toInitialAronType());
+            server::ComponentPluginUser::workingMemory.addCoreSegment("ArticulatedObjectClass"); // TODO , arondto::::toInitialAronType());
+        }
+
         createRemoteGuiTab();
         RemoteGui_startRunningTask();
+
     }
 
     void ObjectMemory::onDisconnectComponent()

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

+#include "Reader.h"
+
+#include <mutex>
+
+#include "ArmarXCore/core/logging/Logging.h"
+#include "RobotAPI/libraries/armem/core/Time.h"
+#include "RobotAPI/libraries/armem/client/query/Builder.h"
+#include "RobotAPI/libraries/armem_objects/aron_conversions.h"
+#include <RobotAPI/libraries/armem_objects/aron/Robot.aron.generated.h>
+#include <optional>
+
+namespace armarx::armem::articulated_object
+{
+
+    Reader::Reader(ManagedIceObject& component) : MemoryConnector(component) {}
+
+
+    ArticulatedObject Reader::get(const ArticulatedObjectDescription& description,
+                                  const armem::Time& timestamp)
+    {
+        ArticulatedObject obj
+        {
+            .description = description,
+            .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::optional<RobotState> Reader::queryState(const RobotDescription& description, const armem::Time& timestamp)
+    {
+        // TODO(fabian.reister): how to deal with multiple providers?
+
+        // Query all entities from provider.
+        armem::client::query::Builder qb;
+
+        // clang-format off
+        qb
+        .coreSegments().withName(properties.memoryName)
+        .providerSegments().withName(properties.providerName) // agent
+        .entities().withName(description.name)
+        .snapshots().atTime(timestamp);
+        // clang-format on
+
+        const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+        ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+        if (not qResult.success) /* c++20 [[unlikely]] */
+        {
+            return std::nullopt;
+        }
+
+        return getRobotState(qResult.memory);
+    }
+
+    std::optional<RobotState> convert(const armem::wm::EntityInstance& instance)
+    {
+        arondto::RobotState aronRobotState;
+        try
+        {
+            aronRobotState.fromAron(instance.data());
+        }
+        catch (...)
+        {
+            return std::nullopt;
+        }
+
+        RobotState robotState;
+        fromAron(aronRobotState, robotState);
+
+        return robotState;
+    }
+
+    std::optional<RobotState> Reader::getRobotState(const armarx::armem::wm::Memory& memory) const
+    {
+        // clang-format off
+        const armem::wm::ProviderSegment& providerSegment = memory
+                .getCoreSegment(properties.memoryName)
+                .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 convert(instance);
+    }
+
+
+
+} // namespace armarx::armem::articulated_object
\ No newline at end of file

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

 #pragma once
 
-#include "interfaces.h"
+#include <mutex>
+#include <optional>
+
+#include "RobotAPI/libraries/armem/client/MemoryConnector.h"
+#include "RobotAPI/libraries/armem/client/Reader.h"
 
+#include "interfaces.h"
 
 namespace armarx::armem::articulated_object
 {
@@ -10,10 +15,31 @@ namespace armarx::armem::articulated_object
         virtual public ::armarx::armem::MemoryConnector
     {
     public:
+        Reader(ManagedIceObject& component);
         virtual ~Reader() = default;
 
         void synchronize(ArticulatedObject& obj, const armem::Time& timestamp) override;
         ArticulatedObject get(const ArticulatedObjectDescription& description, const armem::Time& timestamp) override;
+
+        std::optional<RobotState> queryState(const RobotDescription& description, const armem::Time& timestamp);
+
+    protected:
+        std::optional<RobotState> getRobotState(const armarx::armem::wm::Memory& memory) const;
+
+    private:
+
+        struct Properties
+        {
+            std::string memoryName       = "ObjectMemory";
+            std::string coreSegmentName  = "ArticulatedObjectInstance";
+            std::string providerName;
+        } properties;
+
+        const std::string propertyPrefix = "mem.obj.articulated.read.";
+
+        armem::client::Reader memoryReader;
+        std::mutex memoryWriterMutex;
+
     };
 
 

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

 #include "Writer.h"
+
 #include "RobotAPI/libraries/armem_objects/aron_conversions.h"
 
 #include <mutex>
@@ -49,13 +50,11 @@ namespace armarx::armem::articulated_object
         ARMARX_DEBUG << "Trying to create core segment + provider segment";
 
         const auto result =
-            memoryWriter.addSegment(properties.memoryName, properties.providerName);
+            memoryWriter.addSegment(properties.coreSegmentName, properties.providerName);
 
         if (not result.success)
         {
-            ARMARX_ERROR << result.errorMessage;
-
-            // TODO(fabian.reister): message
+            ARMARX_ERROR << "Creating core segment failed. Reason: " << result.errorMessage;
             return false;
         }