Reader.cpp

Go to the documentation of this file.

#include "Reader.h"
 
#include <mutex>
#include <optional>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/PackagePath.h>
 
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/util/util.h>
// #include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
// #include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
// #include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.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::attachment
{
 
    namespace
    {
 
        template <typename AronClass, typename ArmemClass>
        auto getAttachments(const armarx::armem::wm::Memory& memory)
        {
            // using ArmemClass = decltype(fromAron(AronClass()));
            using ArmemClassVector = std::vector<ArmemClass>;
 
            ArmemClassVector attachments;
            memory.forEachEntity([&attachments](const wm::Entity & entity)
            {
                if (entity.empty())
                {
                    ARMARX_WARNING << "No entity snapshot found";
                    return true;
                }
 
                const armem::wm::EntityInstance& instance = entity.getFirstSnapshot().getInstance(0);
                try
                {
                    AronClass aronAttachment;
                    aronAttachment.fromAron(instance.data());
 
                    ArmemClass attachment;
                    fromAron(aronAttachment, attachment);
 
                    if (attachment.active)
                    {
                        attachments.push_back(attachment);
                    }
 
                }
                catch (const armarx::aron::error::AronException&)
                {
                    return true;
                }
                return true;
            });
 
            return attachments;
        }
    }  // namespace
 
    Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
        memoryNameSystem(memoryNameSystem)
    {}
 
    void Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
    {
        ARMARX_DEBUG << "Reader: 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.");
    }
 
 
    void Reader::connect()
    {
        // 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::vector<ObjectAttachment> Reader::queryObjectAttachments(const armem::Time& timestamp) const
    {
        // Query all entities from all provider.
        armem::client::query::Builder qb;
 
        // clang-format off
        qb
        .coreSegments().withName(properties.coreAttachmentsSegmentName)
        .providerSegments().all()
        .entities().all()
        .snapshots().beforeOrAtTime(timestamp);
        // clang-format on
 
        const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
 
        ARMARX_DEBUG << "Lookup result in reader: " << qResult;
 
        if (not qResult.success) /* c++20 [[unlikely]] */
        {
            return {};
        }
 
        return getAttachments <
               ::armarx::armem::arondto::attachment::ObjectAttachment,
               ::armarx::armem::attachment::ObjectAttachment > (qResult.memory);
    }
 
    std::vector<ObjectAttachment> Reader::queryObjectAttachments(const armem::Time& timestamp, const std::string& providerName) const
    {
        // Query all entities from provider.
        armem::client::query::Builder qb;
 
        // clang-format off
        qb
        .coreSegments().withName(properties.coreAttachmentsSegmentName)
        .providerSegments().withName(providerName)
        .entities().all()
        .snapshots().beforeOrAtTime(timestamp);
        // clang-format on
 
        const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
 
        ARMARX_DEBUG << "Lookup result in reader: " << qResult;
 
        if (not qResult.success) /* c++20 [[unlikely]] */
        {
            return {};
        }
 
        return getAttachments<::armarx::armem::arondto::attachment::ObjectAttachment, ::armarx::armem::attachment::ObjectAttachment>(qResult.memory);
    }
 
    std::vector<ArticulatedObjectAttachment> Reader::queryArticulatedObjectAttachments(const armem::Time& timestamp) const
    {
        // Query all entities from all provider.
        armem::client::query::Builder qb;
 
        // clang-format off
        qb
        .coreSegments().withName(properties.coreAttachmentsSegmentName)
        .providerSegments().all()
        .entities().all()
        .snapshots().beforeOrAtTime(timestamp);
        // clang-format on
 
        const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
 
        ARMARX_DEBUG << "Lookup result in reader: " << qResult;
 
        if (not qResult.success) /* c++20 [[unlikely]] */
        {
            return {};
        }
 
        return getAttachments<::armarx::armem::arondto::attachment::ArticulatedObjectAttachment, ::armarx::armem::attachment::ArticulatedObjectAttachment>(qResult.memory);
    }
 
    std::vector<ArticulatedObjectAttachment> Reader::queryArticulatedObjectAttachments(const armem::Time& timestamp, const std::string& providerName) const
    {
        // Query all entities from provider.
        armem::client::query::Builder qb;
 
        // clang-format off
        qb
        .coreSegments().withName(properties.coreAttachmentsSegmentName)
        .providerSegments().withName(providerName)
        .entities().all()
        .snapshots().beforeOrAtTime(timestamp);
        // clang-format on
 
        const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
 
        ARMARX_DEBUG << "Lookup result in reader: " << qResult;
 
        if (not qResult.success) /* c++20 [[unlikely]] */
        {
            return {};
        }
 
        return getAttachments<::armarx::armem::arondto::attachment::ArticulatedObjectAttachment, ::armarx::armem::attachment::ArticulatedObjectAttachment>(qResult.memory);
    }
 
 
 
}  // namespace armarx::armem::attachment