GraspCandidateWriter.cpp

Go to the documentation of this file.

#include "GraspCandidateWriter.h"
 
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
 
#include <RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.aron.generated.h>
#include <RobotAPI/libraries/GraspingUtility/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/error/mns.h>
 
namespace armarx::armem
{
 
    void
    GraspCandidateWriter::connect(armem::client::MemoryNameSystem& memoryNameSystem)
    {
        // Wait for the memory to become available and add it as dependency.
        ARMARX_IMPORTANT << "GraspCandidateWriter: Waiting for memory '" << properties.memoryName
                         << "' ...";
        try
        {
            memoryWriter = memoryNameSystem.useWriter(properties.memoryName);
            ARMARX_IMPORTANT << "GraspCandidateWriter: Connected to memory '"
                             << properties.memoryName << "'";
        }
        catch (const armem::error::CouldNotResolveMemoryServer& e)
        {
            ARMARX_ERROR << e.what();
            return;
        }
    }
 
    void
    GraspCandidateWriter::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
    {
        ARMARX_DEBUG << "GraspCandidateWriter: registerPropertyDefinitions";
 
        const std::string prefix = propertyPrefix;
 
        def->optional(properties.graspMemoryName,
                      prefix + "GraspMemoryName",
                      "Name of the grasping memory core segment to use.");
        def->optional(properties.bimanualGraspMemoryName,
                      prefix + "BimanualGraspMemoryName",
                      "Name of the bimanual grasping memory core segment to use.");
 
        def->optional(properties.memoryName, prefix + "MemoryName");
    }
 
    bool
    GraspCandidateWriter::commitGraspCandidate(const armarx::grasping::GraspCandidate& candidate,
                                               const armem::Time& timestamp,
                                               const std::string& provider)
    {
        armarx::grasping::arondto::GraspCandidate aronGraspCandidate;
        std::string objectName = "UnknownObject";
        if (candidate.sourceInfo)
        {
            objectName = candidate.sourceInfo->referenceObjectName;
        }
 
 
        toAron(aronGraspCandidate, candidate);
        auto dict = aronGraspCandidate.toAron();
 
        return commitToMemory({dict}, provider, objectName, timestamp, properties.graspMemoryName);
    }
 
    bool
    GraspCandidateWriter::commitBimanualGraspCandidate(
        const armarx::grasping::BimanualGraspCandidate& candidate,
        const armem::Time& timestamp,
        const std::string& provider)
    {
        armarx::grasping::arondto::BimanualGraspCandidate aronGraspCandidate;
 
        std::string objectName = "UnknownObject";
        if (candidate.sourceInfo)
        {
            objectName = candidate.sourceInfo->referenceObjectName;
        }
 
        toAron(aronGraspCandidate, candidate);
 
        auto dict = aronGraspCandidate.toAron();
 
        return commitToMemory(
            {dict}, provider, objectName, timestamp, properties.bimanualGraspMemoryName);
    }
 
    bool
    GraspCandidateWriter::commitGraspCandidateSeq(
        const armarx::grasping::GraspCandidateSeq& candidates,
        const armem::Time& timestamp,
        const std::string& provider)
    {
 
        bool success = true;
 
        std::map<std::string, std::vector<armarx::aron::data::DictPtr>> updates = {};
        armarx::grasping::arondto::GraspCandidate aronGraspCandidate;
 
        for (const auto& candidate : candidates)
        {
            std::string objectName = "UnknownObject";
            if (candidate->sourceInfo)
            {
                objectName = candidate->sourceInfo->referenceObjectName;
            }
            toAron(aronGraspCandidate, *candidate);
 
            updates[objectName].push_back(aronGraspCandidate.toAron());
        }
 
        for (const auto& [key, dict] : updates)
        {
 
            if (!commitToMemory(dict, provider, key, timestamp, properties.graspMemoryName))
            {
                success = false;
            }
        }
        return success;
    }
 
    bool
    GraspCandidateWriter::commitBimanualGraspCandidateSeq(
        const armarx::grasping::BimanualGraspCandidateSeq& candidates,
        const armem::Time& timestamp,
        const std::string& provider)
    {
        bool success = true;
 
        std::map<std::string, std::vector<armarx::aron::data::DictPtr>> updates = {};
        armarx::grasping::arondto::BimanualGraspCandidate aronGraspCandidate;
 
        for (const armarx::grasping::BimanualGraspCandidatePtr& candidate : candidates)
        {
            std::string objectName = "UnknownObject";
            if (candidate->sourceInfo)
            {
                objectName = candidate->sourceInfo->referenceObjectName;
            }
            toAron(aronGraspCandidate, *candidate);
            updates[objectName].push_back(aronGraspCandidate.toAron());
        }
 
        for (const auto& [key, dict] : updates)
        {
 
            if (!commitToMemory(dict, provider, key, timestamp, properties.bimanualGraspMemoryName))
            {
                success = false;
            }
        }
        return success;
    }
 
    bool
    GraspCandidateWriter::commitToMemory(const std::vector<armarx::aron::data::DictPtr>& instances,
                                         const std::string& providerName,
                                         const std::string& entityName,
                                         const armem::Time& timestamp,
                                         const std::string& coreMemoryName)
    {
        std::lock_guard g{memoryWriterMutex};
 
        const auto result = memoryWriter.addSegment(coreMemoryName, providerName);
 
        if (not result.success)
        {
            ARMARX_ERROR << result.errorMessage;
 
            // TODO(fabian.reister): message
            return false;
        }
 
        const auto providerId = armem::MemoryID(result.segmentID);
        const auto entityID = providerId.withEntityName(entityName).withTimestamp(timestamp);
 
        armem::EntityUpdate update;
        update.entityID = entityID;
        update.instancesData = instances;
        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 updateResult.success;
    }
} // namespace armarx::armem