diff --git a/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg b/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg
index 6e9a251dfa0299e26cdf1216eb8663695714ed77..88ad0e549f399971ccb3aecafa7d32ef7df4be42 100644
--- a/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg
+++ b/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
diff --git a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
index 6e11e66194887d0304f34b9bd2cbaf58da4732fc..f222e9c2092d984d2faeda9cac2ec64858f324e0 100644
--- a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
+++ b/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}"
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..b445f3ae6d780866cecbb44b8bd9a1a06f06fd17 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
@@ -0,0 +1,96 @@
+#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
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
index 9afe430b8e7241b09f21e2745f93bb9e56f2e33c..fdcac2eaf2aa1020079c2173df47d9925767592a 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
@@ -1,17 +1,45 @@
#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;
};
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/interfaces.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/interfaces.h
index 9ffb994aee690080a8254d11ab8f5d4f7eba14d5..56ade407493d602bc6d64692f656a22ab606e708 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/interfaces.h
+++ b/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
diff --git a/source/RobotAPI/libraries/armem_objects/types.h b/source/RobotAPI/libraries/armem_objects/types.h
index c772840db4e270470bce8efa2535af5c81e92094..0db042762ef9e95da2508be38da71688a722a49f 100644
--- a/source/RobotAPI/libraries/armem_objects/types.h
+++ b/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