articulated object writer

scenarios/ArMemObjectMemory/config/ObjectMemory.cfg

@@ -174,6 +174,41 @@
 # ArmarX.ObjectMemory.mem.cls.CoreSegmentName = Class
 
 
+# ArmarX.ObjectMemory.mem.cls.Floor.EntityName:  Object class entity of the floor.
+#  Attributes:
+#  - Default:            Environment/floor-20x20
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.ObjectMemory.mem.cls.Floor.EntityName = Environment/floor-20x20
+
+
+# ArmarX.ObjectMemory.mem.cls.Floor.Height:  Height (z) of the floor plane. 
+# Set slightly below 0 to avoid z-fighting when drawing planes on the ground.
+#  Attributes:
+#  - Default:            true
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.cls.Floor.Height = true
+
+
+# ArmarX.ObjectMemory.mem.cls.Floor.LayerName:  Layer to draw the floor on.
+#  Attributes:
+#  - Default:            Floor
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.ObjectMemory.mem.cls.Floor.LayerName = Floor
+
+
+# ArmarX.ObjectMemory.mem.cls.Floor.Show:  Whether to show the floor.
+#  Attributes:
+#  - Default:            true
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.cls.Floor.Show = true
+
+
 # ArmarX.ObjectMemory.mem.cls.LoadFromObjectsPackage:  If true, load the objects from the objects package on startup.
 #  Attributes:
 #  - Default:            true

source/RobotAPI/libraries/armem_objects/CMakeLists.txt

@@ -34,6 +34,8 @@ armarx_add_library(
         # server/articulated_object/SegmentAdapter.h
         # server/articulated_object/Visu.h
 
+        client/articulated_object/Reader.cpp
+        client/articulated_object/Writer.cpp
 
     SOURCES
         aron_conversions.cpp
@@ -51,8 +53,15 @@ armarx_add_library(
         # server/articulated_object/Segment.cpp
         # server/articulated_object/SegmentAdapter.cpp
         # server/articulated_object/Visu.cpp
+
+        client/articulated_object/Reader.h
+        client/articulated_object/Writer.h
+        client/articulated_object/interfaces.h
+
 )
 
+
+
 armarx_enable_aron_file_generation_for_target(
     TARGET_NAME
         "${LIB_NAME}"

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

+#include "Writer.h"
+#include "RobotAPI/libraries/armem_objects/aron_conversions.h"
+
+#include <mutex>
+#include <RobotAPI/libraries/armem_objects/aron/Robot.aron.generated.h>
+
+
+namespace armarx::armem::articulated_object
+{
+
+    void Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+    {
+        ARMARX_DEBUG << "Writer: registerPropertyDefinitions";
+
+        MemoryConnector::registerPropertyDefinitions(def);
+
+        const std::string prefix = getPropertyPrefix();
+
+        def->optional(properties.coreSegmentName,
+                      prefix + "CoreSegment",
+                      "Name of the memory core segment to use.");
+
+        def->optional(properties.memoryName, prefix + "MemoryName");
+
+        def->required(properties.providerName, prefix + "ProviderName");
+    }
+
+    void Writer::connect()
+    {
+        // Wait for the memory to become available and add it as dependency.
+        ARMARX_IMPORTANT << "Writer: Waiting for memory '" << properties.memoryName << "' ...";
+        auto result = useMemory(properties.memoryName);
+        if (not result.success)
+        {
+            ARMARX_ERROR << result.errorMessage;
+            return;
+        }
+
+        ARMARX_IMPORTANT << "Writer: Connected to memory '" << properties.memoryName;
+
+        memoryWriter.setWritingMemory(result.proxy);
+    }
+
+    bool Writer::store(const ArticulatedObject& obj)
+    {
+        std::lock_guard g{memoryWriterMutex};
+
+        ARMARX_DEBUG << "Trying to create core segment + provider segment";
+
+        const auto result =
+            memoryWriter.addSegment(properties.memoryName, properties.providerName);
+
+        if (not result.success)
+        {
+            ARMARX_ERROR << result.errorMessage;
+
+            // TODO(fabian.reister): message
+            return false;
+        }
+
+        const auto& timestamp = obj.timestamp;
+
+        const auto providerId = armem::MemoryID(result.segmentID);
+        const auto entityID =
+            providerId
+            .withEntityName(obj.description.name)
+            .withTimestamp(timestamp);
+
+        armem::EntityUpdate update;
+        update.entityID = entityID;
+
+        arondto::Robot aronArticulatedObject;
+        toAron(aronArticulatedObject, obj);
+
+        update.instancesData = {aronArticulatedObject.toAron()};
+        update.timeCreated   = timestamp;
+
+        ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
+        armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
+
+        ARMARX_DEBUG << updateResult;
+
+        if (not updateResult.success)
+        {
+            ARMARX_ERROR << updateResult.errorMessage;
+        }
+
+        return updateResult.success;
+    }
+
+    const std::string& Writer::getPropertyPrefix() const
+    {
+        return propertyPrefix;
+    }
+
+} // namespace armarx::armem::articulated_object
\ No newline at end of file

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

 #pragma once
 
+#include <mutex>
+
+#include "RobotAPI/libraries/armem/client/MemoryConnector.h"
+#include "RobotAPI/libraries/armem/client/Writer.h"
+
 #include "interfaces.h"
 
 
 namespace armarx::armem::articulated_object
 {
 
-    class Writer: public WriterInterface
+    class Writer:
+        virtual public WriterInterface,
+        virtual public MemoryConnector
     {
     public:
-        virtual ~WriterInterface() = default;
+        virtual ~Writer() = default;
+
+        bool store(const ArticulatedObject& obj) override;
+
+
+        void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
+        void connect();
+
+        const std::string& getPropertyPrefix() const override;
+
+    private:
+
+        struct Properties
+        {
+            std::string memoryName       = "ObjectMemory";
+            std::string coreSegmentName  = "ArticulatedObjectInstance";
+            std::string providerName;
+        } properties;
+
+        const std::string propertyPrefix = "mem.obj.articulated.write.";
+
+        armem::client::Writer memoryWriter;
+        std::mutex memoryWriterMutex;
 
-        void store(const ArticulatedObject& obj) override;
     };
 
 

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

@@ -21,7 +21,7 @@ namespace armarx::armem::articulated_object
     public:
         virtual ~WriterInterface() = default;
 
-        virtual void store(const ArticulatedObject& obj) = 0;
+        virtual bool store(const ArticulatedObject& obj) = 0;
     };
 
 }  // namespace armarx::armem::articulated_object
\ No newline at end of file

source/RobotAPI/libraries/armem_objects/types.h

@@ -36,7 +36,7 @@ namespace armarx::armem
         RobotDescription description;
         RobotState config;
 
-        // IceUtil::Time timestamp;
+        IceUtil::Time timestamp;
     };
 
     namespace articulated_object