armem/dev => master

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