GraspCandidateReader.cpp

Go to the documentation of this file.

#include "GraspCandidateReader.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>
#include <RobotAPI/libraries/armem/core/wm/visitor/FunctionalVisitor.h>
#include <RobotAPI/libraries/armem/util/util.h>
 
namespace armarx::armem
{
    GraspCandidateReader::GraspCandidateReader()
    {
    }
 
    void
    GraspCandidateReader::connect(armem::client::MemoryNameSystem& memoryNameSystem, bool use)
    {
        // Wait for the memory to become available and add it as dependency.
        ARMARX_IMPORTANT << "GraspCandidateReader: Waiting for memory '" << properties.memoryName
                         << "' ...";
        try
        {
            memoryReader = use ? memoryNameSystem.useReader(properties.memoryName)
                               : memoryNameSystem.getReader(MemoryID(properties.memoryName));
            ARMARX_IMPORTANT << "GraspCandidateReader: Connected to memory '"
                             << properties.memoryName;
        }
        catch (const armem::error::CouldNotResolveMemoryServer& e)
        {
            ARMARX_ERROR << e.what();
            return;
        }
    }
 
    armarx::grasping::GraspCandidate
    asGraspCandidate(const armem::wm::EntityInstance& instance)
    {
        armarx::grasping::GraspCandidate candidate;
 
        grasping::arondto::GraspCandidate aronTransform;
 
        aronTransform.fromAron(instance.data());
 
        fromAron(aronTransform, candidate);
 
        return candidate;
    }
 
    armarx::grasping::BimanualGraspCandidate
    asBimanualGraspCandidate(const armem::wm::EntityInstance& instance)
    {
        armarx::grasping::BimanualGraspCandidate candidate;
 
        grasping::arondto::BimanualGraspCandidate aronTransform;
        aronTransform.fromAron(instance.data());
 
        fromAron(aronTransform, candidate);
 
        return candidate;
    }
 
    grasping::GraspCandidatePtr
    GraspCandidateReader::queryGraspCandidateInstanceByID(const armem::MemoryID& id) const
    {
        auto dict = queryGraspCandidateInstancesByID({id});
        if (auto it = dict.find(id.str()); it != dict.end())
        {
            return it->second;
        }
        else
        {
            return nullptr;
        }
    }
 
    grasping::GraspCandidateDict
    GraspCandidateReader::queryGraspCandidateInstancesByID(const std::vector<MemoryID>& ids) const
    {
        armem::client::query::Builder qb;
 
        ARMARX_DEBUG << "Query for memory name: " << properties.memoryName;
 
        qb.multipleEntitySnapshots(ids);
 
        const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
 
        if (!qResult.success)
        {
            // todo catch in provider
            throw armem::error::QueryFailed(properties.memoryName, qResult.errorMessage);
        }
 
        armarx::grasping::GraspCandidateDict candidates;
        for (const MemoryID& id : ids)
        {
            if (const armem::wm::EntityInstance* instance = qResult.memory.findInstance(id))
            {
                candidates[id.str()] = new grasping::GraspCandidate(asGraspCandidate(*instance));
            }
        }
 
        return candidates;
    }
 
    grasping::BimanualGraspCandidatePtr
    GraspCandidateReader::queryBimanualGraspCandidateInstanceByID(const armem::MemoryID& id) const
    {
        armem::client::query::Builder qb;
 
        ARMARX_DEBUG << "Query for memory name: " << properties.memoryName;
 
        qb.singleEntitySnapshot(id.getEntitySnapshotID());
 
        const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
 
        if (!qResult.success)
        {
            // todo catch in provider
            throw armem::error::QueryFailed(properties.memoryName, qResult.errorMessage);
        }
 
 
        armarx::grasping::BimanualGraspCandidatePtr candidate;
 
        armem::wm::FunctionalVisitor visitor;
        visitor.instanceConstFn = [id, &candidate](armem::wm::EntityInstance const& instance)
        {
            if (instance.id() == id)
            {
                candidate =
                    new grasping::BimanualGraspCandidate(asBimanualGraspCandidate(instance));
            }
            return true;
        };
 
        visitor.applyTo(qResult.memory);
 
        return candidate;
    }
 
    grasping::GraspCandidateDict
    GraspCandidateReader::queryLatestGraspCandidateEntity(const std::string& provider,
                                                          const std::string& entity) const
    {
        armem::client::query::Builder qb;
 
        ARMARX_DEBUG << "Query for memory name: " << properties.memoryName;
 
        qb.coreSegments()
            .withName(properties.graspCandidateMemoryName)
            .providerSegments()
            .withName(provider)
            .entities()
            .withName(entity)
            .snapshots()
            .latest();
 
        const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
 
        return getGraspCandidatesFromResultSet(qResult);
    }
 
    std::map<std::string, grasping::BimanualGraspCandidatePtr>
    GraspCandidateReader::queryLatestBimanualGraspCandidateEntity(const std::string& provider,
                                                                  const std::string& entity) const
    {
        armem::client::query::Builder qb;
 
        ARMARX_DEBUG << "Query for memory name: " << properties.memoryName;
 
        qb.coreSegments()
            .withName(properties.bimanualGraspCandidateMemoryName)
            .providerSegments()
            .withName(provider)
            .entities()
            .withName(entity)
            .snapshots()
            .latest();
 
        const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
 
        return getBimanualGraspCandidatesFromResultSet(qResult);
    }
 
    grasping::GraspCandidateDict
    GraspCandidateReader::queryLatestGraspCandidates(const std::string& provider) const
    {
 
        armem::client::query::Builder qb;
 
        ARMARX_DEBUG << "Query for memory name: " << properties.memoryName;
 
 
        if (!provider.empty())
        {
            qb.coreSegments()
                .withName(properties.graspCandidateMemoryName)
                .providerSegments()
                .withName(provider)
                .entities()
                .all()
                .snapshots()
                .latest();
        }
        else
        {
            qb.coreSegments()
                .withName(properties.graspCandidateMemoryName)
                .providerSegments()
                .all()
                .entities()
                .all()
                .snapshots()
                .latest();
        }
 
        const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
 
        return getGraspCandidatesFromResultSet(qResult);
    }
 
    std::map<std::string, grasping::BimanualGraspCandidatePtr>
    GraspCandidateReader::queryLatestBimanualGraspCandidates(const std::string& provider) const
    {
        armem::client::query::Builder qb;
 
        ARMARX_DEBUG << "Query for memory name: " << properties.memoryName;
 
 
        if (!provider.empty())
        {
            qb.coreSegments()
                .withName(properties.bimanualGraspCandidateMemoryName)
                .providerSegments()
                .withName(provider)
                .entities()
                .all()
                .snapshots()
                .latest();
        }
        else
        {
            qb.coreSegments()
                .withName(properties.bimanualGraspCandidateMemoryName)
                .providerSegments()
                .all()
                .entities()
                .all()
                .snapshots()
                .latest();
        }
 
        const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
 
        return getBimanualGraspCandidatesFromResultSet(qResult);
    }
 
    void
    GraspCandidateReader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
    {
        ARMARX_DEBUG << "GraspCandidateReader: registerPropertyDefinitions";
 
        const std::string prefix = propertyPrefix;
 
        def->optional(properties.graspCandidateMemoryName,
                      prefix + "GraspCandidateMemoryName",
                      "Name of the grasping memory core segment to use.");
 
        def->optional(properties.memoryName, prefix + "MemoryName");
    }
 
    grasping::GraspCandidateDict
    GraspCandidateReader::getGraspCandidatesFromResultSet(
        const armem::client::QueryResult& qResult) const
    {
        if (!qResult.success)
        {
            throw armem::error::QueryFailed(properties.memoryName, qResult.errorMessage);
        }
 
 
        std::map<std::string, armarx::grasping::GraspCandidatePtr> candidates;
 
        armem::wm::FunctionalVisitor visitor;
        visitor.instanceConstFn = [&candidates](armem::wm::EntityInstance const& instance)
        {
            candidates[instance.id().str()] =
                new grasping::GraspCandidate(asGraspCandidate(instance));
            return true;
        };
 
        visitor.applyTo(qResult.memory);
 
 
        return candidates;
    }
 
    std::map<std::string, grasping::BimanualGraspCandidatePtr>
    GraspCandidateReader::getBimanualGraspCandidatesFromResultSet(
        const armem::client::QueryResult& qResult) const
    {
        if (!qResult.success)
        {
            throw armem::error::QueryFailed(properties.memoryName, qResult.errorMessage);
        }
 
 
        std::map<std::string, armarx::grasping::BimanualGraspCandidatePtr> candidates;
 
        armem::wm::FunctionalVisitor visitor;
        visitor.instanceConstFn = [&candidates](armem::wm::EntityInstance const& instance)
        {
            candidates[instance.id().str()] =
                new grasping::BimanualGraspCandidate(asBimanualGraspCandidate(instance));
            return true;
        };
 
        visitor.applyTo(qResult.memory);
 
 
        return candidates;
    }
 
    grasping::GraspCandidateDict
    GraspCandidateReader::queryGraspCandidatesNewerThan(const std::string& provider,
                                                        const DateTime& timestamp) const
    {
        armem::client::query::Builder qb;
 
        ARMARX_DEBUG << "Query for memory name: " << properties.memoryName;
 
 
        if (!provider.empty())
        {
            qb.coreSegments()
                .withName(properties.graspCandidateMemoryName)
                .providerSegments()
                .withName(provider)
                .entities()
                .all()
                .snapshots()
                .timeRange(timestamp, armarx::core::time::DateTime::Now());
        }
        else
        {
            qb.coreSegments()
                .withName(properties.graspCandidateMemoryName)
                .providerSegments()
                .all()
                .entities()
                .all()
                .snapshots()
                .timeRange(timestamp, armarx::core::time::DateTime::Now());
        }
 
        const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
        return getGraspCandidatesFromResultSet(qResult);
    }
 
 
} // namespace armarx::armem