KnownGraspCandidateReader.cpp

Go to the documentation of this file.

#include "KnownGraspCandidateReader.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/armem_robot_state/aron/Robot.aron.generated.h>
// #include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
// #include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
 
namespace armarx::armem::grasping::known_grasps
{
    void
    Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
    {
        ARMARX_DEBUG << "Reader: 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
    Reader::connect(armem::client::MemoryNameSystem& memoryNameSystem)
    {
        // Wait for the memory to become available and add it as dependency.
        ARMARX_IMPORTANT << "Reader: Waiting for memory '" << properties.memoryName << "' ...";
        try
        {
            memoryReader = memoryNameSystem.useReader(properties.memoryName);
            ARMARX_IMPORTANT << "Reader: Connected to memory '" << properties.memoryName << "'";
        }
        catch (const armem::error::CouldNotResolveMemoryServer& e)
        {
            ARMARX_ERROR << e.what();
            return;
        }
    }
 
    std::optional<armem::grasping::arondto::KnownGraspInfo>
    Reader::queryKnownGraspInfo(const armem::wm::Memory& memory, const armem::Time&)
    {
        // clang-format off
        const armem::wm::CoreSegment& s = memory
                .getCoreSegment(properties.coreSegmentName);
        // clang-format on
 
        const armem::wm::EntityInstance* instance = nullptr;
        s.forEachInstance([&instance](const wm::EntityInstance& i) { instance = &i; });
        if (instance == nullptr)
        {
            ARMARX_VERBOSE << "No entity snapshots found";
            return std::nullopt;
        }
        return armem::grasping::arondto::KnownGraspInfo::FromAron(instance->data());
    }
 
    std::optional<armarx::armem::grasping::arondto::KnownGraspInfo>
    Reader::queryKnownGraspInfoByEntityName(const std::string& entityName,
                                            const armem::Time& timestamp)
    {
        // Query all entities from all provider.
        armem::client::query::Builder qb;
 
        // clang-format off
        qb
        .coreSegments().withName(properties.coreSegmentName)
        .providerSegments().all()
        .entities().withName(entityName) // first, we search for the input entity, which may be dataset/class/index
        .snapshots().beforeOrAtTime(timestamp);
        // clang-format on
 
        const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
 
        ARMARX_VERBOSE << "Lookup result in reader: " << qResult;
 
        if (not qResult.success) /* c++20 [[unlikely]] */
        {
            return std::nullopt;
        }
 
        auto ret = queryKnownGraspInfo(qResult.memory, timestamp);
 
        if (not ret)
        {
            // No grasp info was found. Try if we find a grasp without the object entity index
            auto split = simox::alg::split(entityName, "/");
            if (split.size() > 2) // there is more than just dataset/class
            {
                ARMARX_INFO << "No grasp found for object entity " << entityName
                            << ". Search for grasp without the index";
                return queryKnownGraspInfoByEntityName(split[0] + "/" + split[1], timestamp);
            }
        }
 
        return ret;
    }
 
} // namespace armarx::armem::grasping::known_grasps