Reader.cpp

Go to the documentation of this file.

#include "Reader.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_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