Merge branch 'feature/armem-object-memory-attachments' into 'armem/dev'

Feature/armem object memory attachments

See merge request ArmarX/RobotAPI!145

scenarios/ArMemObjectMemory/config/ArticulatedObjectLocalizerExample.cfg

@@ -102,6 +102,14 @@ ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.ProviderName = Exam
 # ArmarX.ArticulatedObjectLocalizerExample.tpc.sub.MemoryListener = MemoryUpdates
 
 
+# ArmarX.ArticulatedObjectLocalizerExample.updateFrequency:  Memory update frequency (write).
+#  Attributes:
+#  - Default:            25
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.ArticulatedObjectLocalizerExample.updateFrequency = 25
+
+
 # ArmarX.CachePath:  Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
 #  Attributes:
 #  - Default:            mongo/.cache

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

@@ -57,6 +57,8 @@ namespace armarx::armem::server::obj
         articulatedObjectInstanceSegment.defineProperties(defs, prefix + "articulated.inst.");
         articulatedObjectClassSegment.defineProperties(defs, prefix + "articulated.cls.");
 
+        attachmentSegment.defineProperties(defs, prefix + "attachments.");
+
         return defs;
     }
 
@@ -69,7 +71,9 @@ namespace armarx::armem::server::obj
         articulatedObjectClassSegment(server::ComponentPluginUser::iceMemory,
                                       server::ComponentPluginUser::workingMemoryMutex),
         articulatedObjectInstanceSegment(server::ComponentPluginUser::iceMemory,
-                                         server::ComponentPluginUser::workingMemoryMutex)
+                                         server::ComponentPluginUser::workingMemoryMutex),
+        attachmentSegment(server::ComponentPluginUser::iceMemory,
+                          server::ComponentPluginUser::workingMemoryMutex)
     {
     }
 
@@ -120,6 +124,10 @@ namespace armarx::armem::server::obj
             articulatedObjectInstanceSegment.init();
         });
 
+        initSegmentWithCatch("attachment", [&]()
+        {
+            attachmentSegment.init();
+        });
 
     }
 
@@ -155,12 +163,9 @@ namespace armarx::armem::server::obj
             ArVizComponentPluginUser::getArvizClient()
         );
 
-
-        // {
-        //     std::lock_guard g(server::ComponentPluginUser::workingMemoryMutex);
-        //     // server::ComponentPluginUser::workingMemory.addCoreSegment("ArticulatedObjectInstance"); // TODO , arondto::::toInitialAronType());
-        //     server::ComponentPluginUser::workingMemory.addCoreSegment("ArticulatedObjectClass"); // TODO , arondto::::toInitialAronType());
-        // }
+        attachmentSegment.connect(
+            ArVizComponentPluginUser::getArvizClient()
+        );
 
         createRemoteGuiTab();
         RemoteGui_startRunningTask();

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

@@ -40,6 +40,7 @@
 #include <RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h>
 #include <RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.h>
 #include <RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.h>
+#include <RobotAPI/libraries/armem_objects/server/attachments/Segment.h>
 
 
 #define ICE_CURRENT_ARG const Ice::Current& = Ice::emptyCurrent
@@ -112,7 +113,9 @@ namespace armarx::armem::server::obj
 
         articulated_object_class::Segment articulatedObjectClassSegment;
         articulated_object_instance::Segment articulatedObjectInstanceSegment;
+        attachments::Segment attachmentSegment;
 
+        // associations::Segment associationsSegment;
 
         struct RemoteGuiTab : armarx::RemoteGui::Client::Tab
         {

source/RobotAPI/libraries/armem_objects/CMakeLists.txt

@@ -34,8 +34,14 @@ armarx_add_library(
         # server/articulated_object/SegmentAdapter.h
         server/articulated_object_instance/Visu.h
 
-        client/articulated_object/Reader.cpp
-        client/articulated_object/Writer.cpp
+        server/attachments/Segment.h
+
+        client/articulated_object/Reader.h
+        client/articulated_object/Writer.h
+        client/articulated_object/interfaces.h
+
+        client/attachment/Reader.h
+        client/attachment/Writer.h
 
     SOURCES
         aron_conversions.cpp
@@ -55,9 +61,13 @@ armarx_add_library(
         # server/articulated_object/SegmentAdapter.cpp
         server/articulated_object_instance/Visu.cpp
 
-        client/articulated_object/Reader.h
-        client/articulated_object/Writer.h
-        client/articulated_object/interfaces.h
+        server/attachments/Segment.cpp
+
+        client/articulated_object/Reader.cpp
+        client/articulated_object/Writer.cpp
+
+        client/attachment/Reader.cpp
+        client/attachment/Writer.cpp
 
 )
 
@@ -70,8 +80,8 @@ armarx_enable_aron_file_generation_for_target(
         aron/ObjectClass.xml
         aron/ObjectInstance.xml
 
-        # aron/Attachment.xml
-        aron/Constraint.xml
+        aron/Attachment.xml
+        # aron/Constraint.xml
 )
 
 add_library(${PROJECT_NAME}::armem_objects ALIAS armem_objects)

source/RobotAPI/libraries/armem_objects/aron/Attachment.xml

 <!--This class contains the data structure for ObjectPose -->
 <?xml version="1.0" encoding="UTF-8" ?>
 <AronTypeDefinition>
-    <CodeIncludes>
-        <Include include="<Eigen/Core>" />
+     <CodeIncludes>
+        <Include include="<RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>" />
     </CodeIncludes>
+     <AronIncludes>
+        <Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" />
+    </AronIncludes>
     <GenerateTypes>
 
-        <Object name="armarx::armem::arondto::Attachment">
+        <Object name="armarx::armem::arondto::attachment::AgentDescription">
 
-            <ObjectChild key="constraintType">
+            <!-- Memory link pointing to arondto::Robot or arondto::ArticulatedObject -->
+            <ObjectChild key="id">
+                <armarx::armem::arondto::MemoryID />
+            </ObjectChild>
+
+            <!-- one of the robot's frames -->
+            <ObjectChild key="frame">
+                <string/>
+            </ObjectChild>
+        </Object>
 
+        <!-- Fixed transformation between agent and object -->
+        <Object name="armarx::armem::arondto::attachment::ObjectAttachment">
+            <ObjectChild key="agent">
+                <armarx::armem::arondto::attachment::AgentDescription />
             </ObjectChild>
 
             <ObjectChild key="transformation">
                 <Pose/>
             </ObjectChild>
 
-            <ObjectChild key="objectName">
-                <string/>
+            <ObjectChild key="object">
+                <armarx::armem::arondto::MemoryID />
             </ObjectChild>
 
-            <ObjectChild key="objectNode">
-                <string/>
+            <ObjectChild key="active">
+                <Bool/>
+            </ObjectChild>
+
+            <ObjectChild key="timestamp">
+                <Time/>
+            </ObjectChild>
+        </Object>
+
+        <!-- Fixed transformation between agent and articulated object -->
+        <Object name="armarx::armem::arondto::attachment::ArticulatedObjectAttachment">
+
+            <ObjectChild key="agent">
+                <armarx::armem::arondto::attachment::AgentDescription />
+            </ObjectChild>
+
+            <ObjectChild key="transformation">
+                <Pose/>
+            </ObjectChild>
+
+            <ObjectChild key="object">
+                <armarx::armem::arondto::attachment::AgentDescription />
+            </ObjectChild>
+
+            <ObjectChild key="active">
+                <Bool/>
+            </ObjectChild>
+
+            <ObjectChild key="timestamp">
+                <Time/>
             </ObjectChild>
         </Object>
 

source/RobotAPI/libraries/armem_objects/aron_conversions.cpp

@@ -2,9 +2,8 @@
 
 #include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
 
-#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
-#include <RobotAPI/libraries/aron/common/aron_conversions/stl.h>
-#include <RobotAPI/libraries/aron/common/aron_conversions/armarx.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
 
 namespace armarx::armem
 {
@@ -30,6 +29,61 @@ namespace armarx::armem
         objpose::toAron(dto.pose, bo);
     }
 
+
+    /* Attachments */
+    void fromAron(const arondto::attachment::AgentDescription& dto, attachment::AgentDescription& bo)
+    {
+        fromAron(dto.id, bo.id);
+        aron::fromAron(dto.frame, bo.frame);
+    }
+
+    void toAron(arondto::attachment::AgentDescription& dto, const attachment::AgentDescription& bo)
+    {
+        toAron(dto.id, bo.id);
+        aron::toAron(dto.frame, bo.frame);
+    }
+
+
+    void fromAron(const arondto::attachment::ObjectAttachment& dto, attachment::ObjectAttachment& bo)
+    {
+        fromAron(dto.agent, bo.agent);
+        aron::fromAron(dto.transformation, bo.transformation);
+        fromAron(dto.object, bo.object);
+        aron::fromAron(dto.active, bo.active);
+        // TODO aron::fromAron(dto.timestamp, bo.timestamp);
+    }
+
+    void toAron(arondto::attachment::ObjectAttachment& dto, const attachment::ObjectAttachment& bo)
+    {
+        toAron(dto.agent, bo.agent);
+        aron::toAron(dto.transformation, bo.transformation);
+        toAron(dto.object, bo.object);
+        aron::toAron(dto.active, bo.active);
+        // TODO aron::toAron(dto.timestamp, bo.timestamp);
+    }
+
+
+    void fromAron(const arondto::attachment::ArticulatedObjectAttachment& dto, attachment::ArticulatedObjectAttachment& bo)
+    {
+        fromAron(dto.agent, bo.agent);
+        aron::fromAron(dto.transformation, bo.transformation);
+        fromAron(dto.object, bo.object);
+        aron::fromAron(dto.active, bo.active);
+        // TODO aron::fromAron(dto.timestamp, bo.timestamp);
+    }
+
+    void toAron(arondto::attachment::ArticulatedObjectAttachment& dto, const attachment::ArticulatedObjectAttachment& bo)
+    {
+        toAron(dto.agent, bo.agent);
+        aron::toAron(dto.transformation, bo.transformation);
+        toAron(dto.object, bo.object);
+        aron::toAron(dto.active, bo.active);
+        // TODO aron::toAron(dto.timestamp, bo.timestamp);
+    }
+
+
+
+
 }  // namespace armarx::armem
 
 armarx::armem::MemoryID

source/RobotAPI/libraries/armem_objects/aron_conversions.h

@@ -3,6 +3,8 @@
 #include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
 
 #include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
+
 #include <RobotAPI/libraries/armem_objects/types.h>
 
 namespace armarx::armem
@@ -13,6 +15,17 @@ namespace armarx::armem
     void fromAron(const arondto::ObjectInstance& dto, objpose::ObjectPose& bo);
     void toAron(arondto::ObjectInstance& dto, const objpose::ObjectPose& bo);
 
+
+    /* Attachments */
+    void fromAron(const arondto::attachment::AgentDescription& dto, attachment::AgentDescription& bo);
+    void toAron(arondto::attachment::AgentDescription& dto, const attachment::AgentDescription& bo);
+
+    void fromAron(const arondto::attachment::ObjectAttachment& dto, attachment::ObjectAttachment& bo);
+    void toAron(arondto::attachment::ObjectAttachment& dto, const attachment::ObjectAttachment& bo);
+
+    void fromAron(const arondto::attachment::ArticulatedObjectAttachment& dto, attachment::ArticulatedObjectAttachment& bo);
+    void toAron(arondto::attachment::ArticulatedObjectAttachment& dto, const attachment::ArticulatedObjectAttachment& bo);
+
 }  // namespace armarx::armem
 
 

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

+#include "Reader.h"
+
+#include <mutex>
+#include <optional>
+
+#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/workingmemory/CoreSegment.h"
+#include "RobotAPI/libraries/armem/util/util.h"
+#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
+#include "RobotAPI/libraries/armem_robot/aron_conversions.h"
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include "RobotAPI/libraries/aron/common/aron_conversions.h"
+
+namespace armarx::armem::attachment
+{
+
+    namespace
+    {
+
+        template<typename AronClass, typename ArmemClass>
+        auto getAttachments(const armarx::armem::wm::Memory& memory)
+        {
+            // using ArmemClass = decltype(fromAron(AronClass()));
+            using ArmemClassVector = std::vector<ArmemClass>;
+
+            ArmemClassVector attachments;
+
+            for (const auto&[_, coreSegment] : memory.coreSegments())
+            {
+                for (const auto& [providerName, providerSegment] : coreSegment.providerSegments())
+                {
+                    for (const auto& [name, entity] : providerSegment.entities())
+                    {
+                        if (entity.empty())
+                        {
+                            ARMARX_WARNING << "No entity found";
+                            continue;
+                        }
+
+                        const auto entitySnapshots = simox::alg::get_values(entity.history());
+                        const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
+
+                        try
+                        {
+                            AronClass aronAttachment;
+                            aronAttachment.fromAron(instance.data());
+
+                            ArmemClass attachment;
+                            fromAron(aronAttachment, attachment);
+
+                            if (attachment.active)
+                            {
+                                attachments.push_back(attachment);
+                            }
+
+                        }
+                        catch (const armarx::aron::error::AronException&)
+                        {
+                            continue;
+                        }
+                    }
+                }
+            }
+
+            return attachments;
+        }
+    }  // namespace
+
+    Reader::Reader(armem::ClientReaderComponentPluginUser& component) : component(component) {}
+
+    void Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+    {
+        ARMARX_DEBUG << "Reader: registerPropertyDefinitions";
+
+        const std::string prefix = propertyPrefix;
+
+        def->optional(properties.memoryName, prefix + "MemoryName");
+
+        def->optional(properties.coreAttachmentsSegmentName,
+                      prefix + "CoreSegment",
+                      "Name of the memory core segment to use for object attachments.");
+    }
+
+
+    void Reader::connect()
+    {
+        // Wait for the memory to become available and add it as dependency.
+        ARMARX_IMPORTANT << "Reader: Waiting for memory '" << properties.memoryName << "' ...";
+        auto result = component.useMemory(properties.memoryName);
+        if (not result.success)
+        {
+            ARMARX_ERROR << result.errorMessage;
+            return;
+        }
+
+        ARMARX_IMPORTANT << "Reader: Connected to memory '" << properties.memoryName;
+
+        memoryReader.setReadingMemory(result.proxy);
+    }
+
+
+    std::vector<ObjectAttachment> Reader::queryObjectAttachments(const armem::Time& timestamp) const
+    {
+        // Query all entities from all provider.
+        armem::client::query::Builder qb;
+
+        // clang-format off
+        qb
+        .coreSegments().withName(properties.coreAttachmentsSegmentName)
+        .providerSegments().all()
+        .entities().all()
+        .snapshots().beforeOrAtTime(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 {};
+        }
+
+        return getAttachments<::armarx::armem::arondto::attachment::ObjectAttachment, ::armarx::armem::attachment::ObjectAttachment>(qResult.memory);
+    }
+
+    std::vector<ObjectAttachment> Reader::queryObjectAttachments(const armem::Time& timestamp, const std::string& providerName) const
+    {
+        // Query all entities from provider.
+        armem::client::query::Builder qb;
+
+        // clang-format off
+        qb
+        .coreSegments().withName(properties.coreAttachmentsSegmentName)
+        .providerSegments().withName(providerName)
+        .entities().all()
+        .snapshots().beforeOrAtTime(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 {};
+        }
+
+        return getAttachments<::armarx::armem::arondto::attachment::ObjectAttachment, ::armarx::armem::attachment::ObjectAttachment>(qResult.memory);
+    }
+
+    std::vector<ArticulatedObjectAttachment> Reader::queryArticulatedObjectAttachments(const armem::Time& timestamp) const
+    {
+        // Query all entities from all provider.
+        armem::client::query::Builder qb;
+
+        // clang-format off
+        qb
+        .coreSegments().withName(properties.coreAttachmentsSegmentName)
+        .providerSegments().all()
+        .entities().all()
+        .snapshots().beforeOrAtTime(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 {};
+        }
+
+        return getAttachments<::armarx::armem::arondto::attachment::ArticulatedObjectAttachment, ::armarx::armem::attachment::ArticulatedObjectAttachment>(qResult.memory);
+    }
+
+    std::vector<ArticulatedObjectAttachment> Reader::queryArticulatedObjectAttachments(const armem::Time& timestamp, const std::string& providerName) const
+    {
+        // Query all entities from provider.
+        armem::client::query::Builder qb;
+
+        // clang-format off
+        qb
+        .coreSegments().withName(properties.coreAttachmentsSegmentName)
+        .providerSegments().withName(providerName)
+        .entities().all()
+        .snapshots().beforeOrAtTime(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 {};
+        }
+
+        return getAttachments<::armarx::armem::arondto::attachment::ArticulatedObjectAttachment, ::armarx::armem::attachment::ArticulatedObjectAttachment>(qResult.memory);
+    }
+
+
+
+}  // namespace armarx::armem::attachment
\ No newline at end of file

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

+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2021
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+#include <optional>
+
+#include "RobotAPI/libraries/armem/client.h"
+#include "RobotAPI/libraries/armem/client/Reader.h"
+#include "RobotAPI/libraries/armem_objects/types.h"
+
+namespace armarx::armem::attachment
+{
+    class Reader
+    {
+    public:
+        Reader(armem::ClientReaderComponentPluginUser& component);
+        virtual ~Reader() = default;
+
+        void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
+        void connect();
+
+        std::vector<ObjectAttachment> queryObjectAttachments(const armem::Time& timestamp) const;
+        std::vector<ObjectAttachment> queryObjectAttachments(const armem::Time& timestamp, const std::string& providerName) const;
+
+        std::vector<ArticulatedObjectAttachment> queryArticulatedObjectAttachments(const armem::Time& timestamp) const;
+        std::vector<ArticulatedObjectAttachment> queryArticulatedObjectAttachments(const armem::Time& timestamp, const std::string& providerName) const;
+
+    private:
+
+
+        struct Properties
+        {
+            std::string memoryName                  = "Object";
+            std::string coreAttachmentsSegmentName  = "Attachments";
+        } properties;
+
+        const std::string propertyPrefix = "mem.obj.attachment.";
+
+        armem::client::Reader memoryReader;
+        std::mutex memoryWriterMutex;
+
+        armem::ClientReaderComponentPluginUser& component;
+    };
+
+
+}  // namespace armarx::armem::attachment
\ No newline at end of file

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

+#include "Writer.h"
+
+#include <IceUtil/Time.h>
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <mutex>
+#include <optional>
+
+#include "ArmarXCore/core/logging/Logging.h"
+
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include "RobotAPI/libraries/armem_objects/aron_conversions.h"
+#include "RobotAPI/libraries/armem_robot/aron_conversions.h"
+#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
+
+
+namespace armarx::armem::attachment
+{
+    Writer::Writer(armem::ClientComponentPluginUser& component): component(component) {}
+
+    void Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+    {
+        ARMARX_DEBUG << "Writer: registerPropertyDefinitions";
+
+        const std::string prefix = propertyPrefix;
+
+        def->optional(properties.memoryName, prefix + "MemoryName");
+
+        def->optional(properties.coreAttachmentsSegmentName,
+                      prefix + "CoreSegment",
+                      "Name of the memory core segment to use for object attachments.");
+        def->optional(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 = component.useMemory(properties.memoryName);
+        if (not result.success)
+        {
+            ARMARX_ERROR << result.errorMessage;
+            return;
+        }
+
+        ARMARX_IMPORTANT << "Writer: Connected to memory '" << properties.memoryName;
+
+        memoryWriter.setWritingMemory(result.proxy);
+        memoryReader.setReadingMemory(result.proxy);
+    }
+
+
+    std::optional<armem::MemoryID> Writer::commit(const ObjectAttachment& attachment)
+    {
+        std::lock_guard g{memoryWriterMutex};
+
+        const auto result = memoryWriter.addSegment(properties.coreAttachmentsSegmentName, properties.providerName);
+
+        if (not result.success)
+        {
+            ARMARX_ERROR << "Creating core segment failed. Reason: " << result.errorMessage;
+            return std::nullopt;
+        }
+
+        const auto& timestamp = attachment.timestamp;
+
+        const auto providerId = armem::MemoryID(result.segmentID);
+        const auto entityID =
+            providerId
+            .withEntityName(attachment.object.entityName) // TODO check if meaningful
+            .withTimestamp(timestamp);
+
+        armem::EntityUpdate update;
+        update.entityID = entityID;
+
+        arondto::attachment::ObjectAttachment aronAttachment;
+        toAron(aronAttachment, attachment);
+
+        update.instancesData = {aronAttachment.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 std::nullopt;
+        }
+
+        return updateResult.snapshotID;
+
+    }
+
+    std::optional<armem::MemoryID> Writer::commit(const ArticulatedObjectAttachment& attachment)
+    {
+        std::lock_guard g{memoryWriterMutex};
+
+        const auto result = memoryWriter.addSegment(properties.coreAttachmentsSegmentName, properties.providerName);
+
+        if (not result.success)
+        {
+            ARMARX_ERROR << "Creating core segment failed. Reason: " << result.errorMessage;
+            return std::nullopt;
+        }
+
+        const auto& timestamp = attachment.timestamp;
+
+        const auto providerId = armem::MemoryID(result.segmentID);
+        const auto entityID =
+            providerId
+            .withEntityName(attachment.object.id.entityName) // TODO check if meaningful
+            .withTimestamp(timestamp);
+
+        armem::EntityUpdate update;
+        update.entityID = entityID;
+
+        arondto::attachment::ArticulatedObjectAttachment aronAttachment;
+        toAron(aronAttachment, attachment);
+
+        update.instancesData = {aronAttachment.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 std::nullopt;
+        }
+
+        return updateResult.snapshotID;
+    }
+
+
+
+}  // namespace armarx::armem::attachment
\ No newline at end of file

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

+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2021
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+
+#include "RobotAPI/libraries/armem/client/Reader.h"
+#include "RobotAPI/libraries/armem/client/Writer.h"
+#include "RobotAPI/libraries/armem/client.h"
+
+#include "RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h"
+
+#include "RobotAPI/libraries/armem_objects/types.h"
+
+namespace armarx::armem::attachment
+{
+
+    class Writer
+    {
+    public:
+        Writer(armem::ClientComponentPluginUser& component);
+        virtual ~Writer() = default;
+
+
+        void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
+        void connect();
+
+        std::optional<armem::MemoryID> commit(const ObjectAttachment& attachment);
+        std::optional<armem::MemoryID> commit(const ArticulatedObjectAttachment&);
+
+    private:
+
+        struct Properties
+        {
+            std::string memoryName                  = "Object";
+            std::string coreAttachmentsSegmentName  = "Attachments";
+            std::string providerName                = "AttachmentProvider";
+
+            bool allowClassCreation                 = false;
+        } properties;
+
+        const std::string propertyPrefix = "mem.obj.articulated.";
+
+        armem::client::Writer memoryWriter;
+        std::mutex memoryWriterMutex;
+
+        armem::client::Reader memoryReader;
+        std::mutex memoryReaderMutex;
+
+        armem::ClientComponentPluginUser& component;
+    };
+
+
+}  // namespace armarx::armem::attachment
\ No newline at end of file

source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp

+#include "Segment.h"
+
+#include <sstream>
+
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include "ArmarXCore/core/logging/Logging.h"
+
+#include "RobotAPI/libraries/armem/util/util.h"
+#include "RobotAPI/libraries/aron/common/aron_conversions.h"
+
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Visitor.h>
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+
+#include <RobotAPI/libraries/armem_objects/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
+
+
+namespace armarx::armem::server::obj::attachments
+{
+
+    Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter, std::mutex& memoryMutex) :
+        iceMemory(memoryToIceAdapter),
+        memoryMutex(memoryMutex)
+    {
+        Logging::setTag("Attachments");
+    }
+
+    Segment::~Segment() = default;
+
+    void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+    {
+        defs->optional(p.coreClassSegmentName, prefix + "CoreSegmentName", "Name of the object instance core segment.");
+        defs->optional(p.maxHistorySize, prefix + "MaxHistorySize", "Maximal size of object poses history (-1 for infinite).");
+    }
+
+    void Segment::init()
+    {
+        ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
+
+        coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreClassSegmentName, arondto::Robot::toInitialAronType());
+        coreSegment->setMaxHistorySize(p.maxHistorySize);
+    }
+
+    void Segment::connect(viz::Client arviz)
+    {
+        // this->visu = std::make_unique<Visu>(arviz, *this);
+    }
+
+    std::vector<armarx::armem::attachment::ObjectAttachment> Segment::getAttachments(const armem::Time& timestamp) const
+    {
+        std::vector<armarx::armem::attachment::ObjectAttachment> attachments;
+
+        for (const auto& [_, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreClassSegmentName))
+        {
+            for (const auto& [name, entity] :  provSeg.entities())
+            {
+                const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+                const auto aronAttachment = tryCast<armarx::armem::arondto::attachment::ObjectAttachment>(entityInstance);
+
+                if (not aronAttachment)
+                {
+                    ARMARX_WARNING << "Could not convert entity instance to 'ObjectAttachment'";
+                    continue;
+                }
+
+                ARMARX_DEBUG << "Key is " << armem::MemoryID(entity.id());
+
+                armarx::armem::attachment::ObjectAttachment attachment;
+                fromAron(*aronAttachment, attachment);
+
+                attachments.push_back(attachment);
+            }
+        }
+
+        return attachments;
+    }
+
+
+    // void Segment::RemoteGui::setup(const Segment& data)
+    // {
+    //     using namespace armarx::RemoteGui::Client;
+
+    //     maxHistorySize.setValue(std::max(1, int(data.p.maxHistorySize)));
+    //     maxHistorySize.setRange(1, 1e6);
+    //     infiniteHistory.setValue(data.p.maxHistorySize == -1);
+    //     discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
+
+    //     GridLayout grid;
+    //     int row = 0;
+    //     grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
+    //     row++;
+    //     grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
+    //     row++;
+    //     grid.add(Label("Discard Snapshots while Attached"), {row, 0}).add(discardSnapshotsWhileAttached, {row, 1});
+    //     row++;
+
+    //     group.setLabel("Data");
+    //     group.addChild(grid);
+    // }
+
+    // void Segment::RemoteGui::update(Segment& data)
+    // {
+    //     if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
+    //         || discardSnapshotsWhileAttached.hasValueChanged())
+    //     {
+    //         std::scoped_lock lock(data.memoryMutex);
+
+    //         if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
+    //         {
+    //             data.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
+    //             if (data.coreSegment)
+    //             {
+    //                 data.coreSegment->setMaxHistorySize(long(data.p.maxHistorySize));
+    //             }
+    //         }
+
+    //         data.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
+    //     }
+    // }
+
+}  // namespace armarx::armem::server::obj::attachments

source/RobotAPI/libraries/armem_objects/server/attachments/Segment.h

+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2021
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <string>
+#include <optional>
+#include <mutex>
+#include <unordered_map>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include "ArmarXCore/core/application/properties/PropertyDefinitionContainer.h"
+
+// #include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
+
+#include "RobotAPI/components/ArViz/Client/Client.h"
+
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include "RobotAPI/libraries/armem/core/Time.h"
+#include "RobotAPI/libraries/armem_objects/types.h"
+
+namespace armarx::armem
+{
+    namespace server
+    {
+        class MemoryToIceAdapter;
+    }
+
+    namespace wm
+    {
+        class CoreSegment;
+    }
+}  // namespace armarx::armem
+
+
+namespace armarx::armem::server::obj::attachments
+{
+    class Visu;
+
+
+    class Segment : public armarx::Logging
+    {
+    public:
+        Segment(server::MemoryToIceAdapter& iceMemory,
+                std::mutex& memoryMutex);
+
+        virtual ~Segment();
+
+        void connect(viz::Client arviz);
+
+        void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+
+        void init();
+
+        std::vector<armarx::armem::attachment::ObjectAttachment> getAttachments(const armem::Time& timestamp) const;
+
+
+    private:
+
+        server::MemoryToIceAdapter& iceMemory;
+        wm::CoreSegment* coreSegment = nullptr;
+        std::mutex& memoryMutex;
+
+        struct Properties
+        {
+            std::string coreClassSegmentName = "Attachments";
+            int64_t maxHistorySize = -1;
+        };
+        Properties p;
+
+        // std::unique_ptr<Visu> visu;
+
+    public:
+
+        // struct RemoteGui
+        // {
+        //     armarx::RemoteGui::Client::GroupBox group;
+
+        //     armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
+        //     armarx::RemoteGui::Client::CheckBox infiniteHistory;
+        //     armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
+
+        //     void setup(const Segment& data);
+        //     void update(Segment& data);
+        // };
+
+    };
+
+}  // namespace armarx::armem::server::obj::attachments

source/RobotAPI/libraries/armem_objects/types.h

@@ -21,9 +21,75 @@
 
 #pragma once
 
+#include "RobotAPI/libraries/armem/core/Time.h"
 #include <vector>
 
+#include <Eigen/Geometry>
+
 #include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+
+
+namespace armarx::armem::attachment
+{
+    using AgentID = armem::MemoryID;
+    using ObjectID = armem::MemoryID;
+
+    struct AgentDescription
+    {
+        /**
+         * @brief id either pointing to a arondto::Robot or arondto::ArticulatedObject
+         *
+         */
+        AgentID id;
+
+        std::string frame;
+    };
+
+    /**
+     * @brief ObjectAttachment describes a fixed transformation between an agent and an object.
+     *
+     *  The transformation is defined as follows:
+     *
+     *      agent.frame -> object root frame
+     *
+     */
+    struct ObjectAttachment
+    {
+        AgentDescription agent;
+
+        Eigen::Affine3f transformation;
+
+        ObjectID object;
+
+        armem::Time timestamp;
+
+        bool active;
+    };
+
+    /**
+     * @brief ArticulatedObjectAttachment describes a fixed transformation between an agent and an articulated object.
+     *
+     *  The transformation is defined as follows:
+     *
+     *      agent.frame -> object.frame
+     *
+     */
+    struct ArticulatedObjectAttachment
+    {
+        AgentDescription agent;
+
+        Eigen::Affine3f transformation;
+
+        AgentDescription object;
+
+        armem::Time timestamp;
+
+        bool active;
+
+    };
+
+}  // namespace armarx::armem::attachment
 
 namespace armarx::armem::articulated_object
 {

source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt

@@ -43,6 +43,7 @@ armarx_add_library(
 
 
         ./aron_conversions.h
+        ./utils.h
     SOURCES
         ./common/localization/TransformHelper.cpp
 
@@ -64,6 +65,8 @@ armarx_add_library(
         ./server/description/Segment.cpp
 
         ./aron_conversions.cpp
+        ./utils.cpp
+
 )
 
 

source/RobotAPI/libraries/armem_robot_state/utils.cpp

+#include "utils.h"
+
+namespace armarx::armem::robot_state
+{
+    armarx::armem::MemoryID makeMemoryID(const robot::RobotDescription& desc)
+    {
+        return MemoryID("RobotState/Description")
+               .withProviderSegmentName(desc.name)
+               .withEntityName("description");
+    }
+}
\ No newline at end of file

source/RobotAPI/libraries/armem_robot_state/utils.h

+#pragma once
+
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include "RobotAPI/libraries/armem_robot/types.h"
+
+namespace armarx::armem::robot_state
+{
+    armarx::armem::MemoryID makeMemoryID(const robot::RobotDescription& desc);
+}
+