Reader.cpp

Go to the documentation of this file.

#include "Reader.h"
 
#include <string>
 
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/Duration.h>
 
#include <RobotAPI/libraries/armem/client/Query.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/selectors.h>
#include <RobotAPI/libraries/armem/client/util/SimpleReaderBase.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
 
#include <armarx/navigation/human/aron/Human.aron.generated.h>
#include <armarx/navigation/human/aron_conversions.h>
#include <armarx/navigation/human/types.h>
#include <armarx/navigation/memory/constants.h>
 
namespace armarx::navigation::memory::client::human
{
    Reader::~Reader() = default;
 
    armarx::armem::client::query::Builder
    Reader::buildHumansQuery(const Query& query) const
    {
        armarx::armem::client::query::Builder qb;
 
        // clang-format off
        qb
        .coreSegments().withName(properties().coreSegmentName)
        .providerSegments().withName(query.providerName)
        .entities().withName("humans")
        .snapshots().beforeOrAtTime(query.timestamp);
        // clang-format on
 
        return qb;
    }
 
    armarx::armem::client::query::Builder
    Reader::buildHumanGroupsQuery(const Query& query) const
    {
        armarx::armem::client::query::Builder qb;
 
        // clang-format off
        qb
        .coreSegments().withName(properties().coreSegmentName)
        .providerSegments().withName(query.providerName)
        .entities().withName("groups")
        .snapshots().beforeOrAtTime(query.timestamp);
        // clang-format on
 
        return qb;
    }
 
    std::string
    Reader::propertyPrefix() const
    {
        return "mem.nav.human.";
    }
 
    armarx::armem::client::util::SimpleReaderBase::Properties
    Reader::defaultProperties() const
    {
        return {.memoryName = memory::constants::NavigationMemoryName,
                .coreSegmentName = memory::constants::HumanCoreSegmentName};
    }
 
    namespace
    {
 
        navigation::human::Humans
        asHumans(const armem::wm::ProviderSegment& providerSegment,
                 const DateTime& timestamp,
                 const Duration& maxAge)
        {
            navigation::human::Humans humans;
 
            ARMARX_CHECK(not providerSegment.empty()) << "No entities";
            ARMARX_CHECK(providerSegment.size() == 1) << "There should be only one entity!";
 
            providerSegment.forEachEntity(
                [&humans, &timestamp, &maxAge](const armem::wm::Entity& entity)
                {
                    const auto& entitySnapshot = entity.getLatestSnapshot();
                    //ARMARX_CHECK(not entitySnapshot.empty()) << "No entity snapshot instances";
                    if (entitySnapshot.empty())
                    {
                        ARMARX_INFO << "No entity snapshot instances";
                        return;
                    }
 
                    entitySnapshot.forEachInstance(
                        [&](const armem::wm::EntityInstance& entityInstance)
                        {
                            const Duration dtToNow =
                                timestamp - entityInstance.metadata().referencedTime;
 
                            if (dtToNow < maxAge and dtToNow.isPositive())
                            {
                                const auto dto = navigation::human::arondto::Human::FromAron(
                                    entityInstance.data());
 
                                navigation::human::Human human;
                                fromAron(dto, human);
                                humans.push_back(human);
                            }
                        });
                });
 
            return humans;
        }
 
        navigation::human::HumanGroups
        asGroups(const armem::wm::ProviderSegment& providerSegment,
                 const DateTime& timestamp,
                 const Duration& maxAge)
        {
            navigation::human::HumanGroups humans;
 
            ARMARX_CHECK(not providerSegment.empty()) << "No entities";
            ARMARX_CHECK(providerSegment.size() == 1) << "There should be only one entity!";
 
            providerSegment.forEachEntity(
                [&humans, &timestamp, &maxAge](const armem::wm::Entity& entity)
                {
                    const auto& entitySnapshot = entity.getLatestSnapshot();
                    ARMARX_CHECK(not entitySnapshot.empty()) << "No entity snapshot instances";
 
                    entitySnapshot.forEachInstance(
                        [&](const armem::wm::EntityInstance& entityInstance)
                        {
                            const Duration dtToNow =
                                timestamp - entityInstance.metadata().referencedTime;
 
                            if (dtToNow < maxAge and dtToNow.isPositive())
                            {
                                const auto dto = navigation::human::arondto::HumanGroup::FromAron(
                                    entityInstance.data());
 
                                navigation::human::HumanGroup human;
                                fromAron(dto, human);
                                humans.push_back(human);
                            }
                        });
                });
 
            return humans;
        }
 
    } // namespace
 
    Reader::HumanGroupResult
    Reader::queryHumanGroups(const Query& query) const
    {
        const auto qb = buildHumansQuery(query);
 
        ARMARX_DEBUG << "[MappingDataReader] query ... ";
 
        const armem::client::QueryResult qResult = memoryReader().query(qb.buildQueryInput());
 
        ARMARX_DEBUG << "[MappingDataReader] result: " << qResult;
 
        if (not qResult.success)
        {
            ARMARX_WARNING << "Failed to query data from memory: " << qResult.errorMessage;
            return {.groups = {},
                    .status = HumanGroupResult::Status::Error,
                    .errorMessage = qResult.errorMessage};
        }
 
        const auto coreSegment = qResult.memory.getCoreSegment(properties().coreSegmentName);
 
        if (not coreSegment.hasProviderSegment(query.providerName))
        {
            ARMARX_DEBUG << "Provider segment " << QUOTED(query.providerName)  << " does not exist (yet).";
            return {.groups = {}, .status = HumanGroupResult::Status::NoData};
        }
 
        const armem::wm::ProviderSegment& providerSegment =
            coreSegment.getProviderSegment(query.providerName);
 
        if (providerSegment.empty())
        {
            ARMARX_DEBUG << "No entities.";
            return {.groups = {},
                    .status = HumanGroupResult::Status::NoData,
                    .errorMessage = "No entities"};
        }
 
        try
        {
            return HumanGroupResult{.groups =
                                        asGroups(providerSegment, query.timestamp, query.maxAge),
                                    .status = HumanGroupResult::Status::Success};
        }
        catch (...)
        {
            return HumanGroupResult{.groups = {},
                                    .status = HumanGroupResult::Status::Error,
                                    .errorMessage = GetHandledExceptionString()};
        }
    }
 
    Reader::HumanResult
    Reader::queryHumans(const Query& query) const
    {
        const auto qb = buildHumansQuery(query);
 
        ARMARX_DEBUG << "[MappingDataReader] query ... ";
 
        const armem::client::QueryResult qResult = memoryReader().query(qb.buildQueryInput());
 
        ARMARX_DEBUG << "[MappingDataReader] result: " << qResult;
 
        if (not qResult.success)
        {
            ARMARX_WARNING << "Failed to query data from memory: " << qResult.errorMessage;
            return {.humans = {},
                    .status = HumanResult::Status::Error,
                    .errorMessage = qResult.errorMessage};
        }
 
        const auto coreSegment = qResult.memory.getCoreSegment(properties().coreSegmentName);
 
        if (not coreSegment.hasProviderSegment(query.providerName))
        {
            ARMARX_DEBUG << "Provider segment " << QUOTED(query.providerName)  << " does not exist (yet).";
            return {.humans = {}, .status = HumanResult::Status::NoData};
        }
 
        const armem::wm::ProviderSegment& providerSegment =
            coreSegment.getProviderSegment(query.providerName);
 
        if (providerSegment.empty())
        {
            ARMARX_VERBOSE << "No entities.";
            return {.humans = {},
                    .status = HumanResult::Status::NoData,
                    .errorMessage = "No entities"};
        }
 
        try
        {
            return HumanResult{.humans = asHumans(providerSegment, query.timestamp, query.maxAge),
                               .status = HumanResult::Status::Success};
        }
        catch (...)
        {
            return HumanResult{.humans = {},
                               .status = HumanResult::Status::Error,
                               .errorMessage = GetHandledExceptionString()};
        }
    }
 
} // namespace armarx::navigation::memory::client::human