Writer.cpp

Go to the documentation of this file.

#include "Writer.h"
 
#include <mutex>
#include <optional>
 
#include <IceUtil/Time.h>
 
#include <SimoxUtility/algorithm/get_map_keys_values.h>
 
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/logging/Logging.h>
 
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/RobotDescription.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
 
namespace armarx::armem::attachment
{
    Writer::Writer(armem::client::MemoryNameSystem& memoryNameSystem) :
        memoryNameSystem(memoryNameSystem)
    {
    }
 
    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 << "' ...";
        try
        {
            memoryWriter = memoryNameSystem.useWriter(properties.memoryName);
            memoryReader = memoryNameSystem.useReader(properties.memoryName);
            ARMARX_IMPORTANT << "Writer: Connected to memory '" << properties.memoryName << "'";
        }
        catch (const armem::error::CouldNotResolveMemoryServer& e)
        {
            ARMARX_ERROR << e.what();
            return;
        }
    }
 
    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.referencedTime = 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.referencedTime = 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