ObjectWriter.cpp

Go to the documentation of this file.

#include "ObjectWriter.h"
 
#include <mutex>
#include <optional>
 
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/logging/Logging.h>
 
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/util/util.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::obj::instance
{
    Writer::Writer()
    {
    }
 
    void
    Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
    {
        ARMARX_DEBUG << "Writer: registerPropertyDefinitions";
 
        const std::string prefix = propertyPrefix;
 
        def->optional(properties.memoryName, prefix + "MemoryName");
 
        def->optional(properties.coreSegmentName,
                      prefix + "CoreSegment",
                      "Name of the memory core segment to use for object instances.");
    }
 
    void
    Writer::connect(armem::client::MemoryNameSystem& memoryNameSystem)
    {
        // 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);
            ARMARX_IMPORTANT << "Writer: Connected to memory '" << properties.memoryName << "'";
        }
        catch (const armem::error::CouldNotResolveMemoryServer& e)
        {
            ARMARX_ERROR << e.what();
            return;
        }
    }
 
    bool
    Writer::commitObject(const armem::arondto::ObjectInstance& inst,
                         const std::string& provider,
                         const armem::Time& t)
    {
        armem::Commit c;
        auto& e = c.add();
 
        e.entityID.memoryName = properties.memoryName;
        e.entityID.coreSegmentName = properties.coreSegmentName;
        e.entityID.providerSegmentName = provider;
        e.entityID.entityName = inst.pose.objectID.dataset + "/" + inst.pose.objectID.className +
                                "/" + inst.pose.objectID.instanceName;
        e.referencedTime = t;
        e.sentTime = armem::Time::Now();
        e.instancesData = {inst.toAron()};
 
        e.confidence = inst.pose.confidence;
 
        auto res = memoryWriter.commit(c);
 
        if (!res.allSuccess())
        {
            ARMARX_ERROR << "Failed to commit an ObjectInstance to memory: "
                         << res.allErrorMessages();
            return false;
        }
        return true;
    }
 
} // namespace armarx::armem::obj::instance