ObjectReader.cpp

Go to the documentation of this file.

#include "ObjectReader.h"
 
#include <optional>
 
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
 
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.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/FamiliarObjectInstance.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
#include <RobotAPI/libraries/armem_objects/constants.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
 
namespace armarx::armem::obj::familiar_object_instance
{
    void
    Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
    {
        const std::string prefix = propertyPrefix;
 
        def->optional(p.memoryName, prefix + "MemoryName");
    }
 
    void
    Reader::connect(armem::client::MemoryNameSystem& memoryNameSystem)
    {
        ARMARX_IMPORTANT << "Reader: Waiting for memory '" << objects::constants::MemoryName
                         << "' ...";
        try
        {
            memoryReader = memoryNameSystem.useReader(objects::constants::MemoryName);
            ARMARX_IMPORTANT << "Reader: Connected to memory '" << objects::constants::MemoryName
                             << "'";
        }
        catch (const armem::error::CouldNotResolveMemoryServer& e)
        {
            ARMARX_ERROR << e.what();
            return;
        }
    }
 
    inline std::optional<arondto::FamiliarObjectInstance>
    convert(const wm::EntityInstance& instance)
    {
        arondto::FamiliarObjectInstance aronBo;
        try
        {
            aronBo.fromAron(instance.data());
        }
        catch (...)
        {
            ARMARX_WARNING << "Conversion to ObjectPose failed!";
            return std::nullopt;
        }
 
        return aronBo;
    }
 
    std::optional<std::map<std::string, arondto::FamiliarObjectInstance>>
    Reader::queryLatestFamiliarObjectInstance(const ObjectID& instanceId,
                                              const std::optional<std::string>& providerName) const
    {
 
        ARMARX_TRACE;
 
        armem::client::query::Builder qb;
        {
            auto& cs = qb.coreSegments().withName(
                objects::constants::CoreFamiliarObjectInstanceSegmentName);
 
            client::query::ProviderSegmentSelector* providerSegments = nullptr;
            if (providerName.has_value())
            {
                providerSegments = &cs.providerSegments().withName(providerName.value());
            }
            else
            {
                providerSegments = &cs.providerSegments().all();
            }
 
            providerSegments->entities().withName(instanceId.str()).snapshots().latest();
        }
 
 
        const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
 
        ARMARX_DEBUG << "Lookup result in reader: " << qResult;
 
        if (not qResult.success) /* c++20 [[unlikely]] */
        {
            ARMARX_WARNING << qResult.errorMessage;
            return std::nullopt;
        }
 
 
        std::map<std::string, arondto::FamiliarObjectInstance> result;
 
        qResult.memory.forEachProviderSegment(
            [&result](const wm::ProviderSegment& ps)
            {
                auto& thisResult = result[ps.name()];
 
                ARMARX_CHECK_EQUAL(ps.getEntityNames().size(), 1)
                    << "Query returned more than one entity";
 
                ps.forEachInstance(
                    [&thisResult](const wm::EntityInstance& e)
                    {
                        if (auto converted = convert(e))
                        {
                            thisResult = converted.value();
                        }
                    });
            });
 
        return result;
    }
 
    std::map<std::string, std::vector<arondto::FamiliarObjectInstance>>
    Reader::queryAllLatestFamiliarObjectInstances(
        const std::optional<std::string>& providerName) const
    {
        ARMARX_TRACE;
 
        armem::client::query::Builder qb;
        {
            auto& cs = qb.coreSegments().withName(
                objects::constants::CoreFamiliarObjectInstanceSegmentName);
 
            client::query::ProviderSegmentSelector* providerSegments = nullptr;
            if (providerName.has_value())
            {
                providerSegments = &cs.providerSegments().withName(providerName.value());
            }
            else
            {
                providerSegments = &cs.providerSegments().all();
            }
 
            providerSegments->entities().all().snapshots().latest();
        }
 
        const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
 
        ARMARX_DEBUG << "Lookup result in reader: " << qResult;
 
        if (not qResult.success) /* c++20 [[unlikely]] */
        {
            ARMARX_WARNING << qResult.errorMessage;
            return {};
        }
 
        std::map<std::string, std::vector<arondto::FamiliarObjectInstance>> result;
 
        qResult.memory.forEachProviderSegment(
            [&result](const wm::ProviderSegment& ps)
            {
                auto& thisResult = result[ps.name()];
 
                ps.forEachInstance(
                    [&thisResult](const wm::EntityInstance& e)
                    {
                        if (auto converted = convert(e))
                        {
                            thisResult.push_back(converted.value());
                        }
                    });
            });
 
        return result;
    }
 
    std::map<std::string, std::vector<arondto::FamiliarObjectInstance>>
    Reader::queryLatestFamiliarObjectInstancesFromClass(
        const ObjectID& classId,
        const std::optional<std::string>& providerName) const
    {
        ARMARX_TRACE;
 
        armem::client::query::Builder qb;
        {
            auto& cs = qb.coreSegments().withName(
                objects::constants::CoreFamiliarObjectInstanceSegmentName);
 
            client::query::ProviderSegmentSelector* providerSegments = nullptr;
            if (providerName.has_value())
            {
                providerSegments = &cs.providerSegments().withName(providerName.value());
            }
            else
            {
                providerSegments = &cs.providerSegments().all();
            }
 
            providerSegments->entities()
                .withNamesStartingWith(classId.getClassID().str())
                .snapshots()
                .latest();
        }
 
        const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
 
        ARMARX_DEBUG << "Lookup result in reader: " << qResult;
 
        if (not qResult.success) /* c++20 [[unlikely]] */
        {
            ARMARX_WARNING << qResult.errorMessage;
            return {};
        }
 
        std::map<std::string, std::vector<arondto::FamiliarObjectInstance>> result;
 
        qResult.memory.forEachProviderSegment(
            [&result](const wm::ProviderSegment& ps)
            {
                auto& thisResult = result[ps.name()];
 
                ps.forEachInstance(
                    [&thisResult](const wm::EntityInstance& e)
                    {
                        if (auto converted = convert(e))
                        {
                            thisResult.push_back(converted.value());
                        }
                    });
            });
 
 
        return result;
    }
 
} // namespace armarx::armem::obj::familiar_object_instance