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 "RobotAPI/libraries/armem/client/query/Builder.h"
#include <RobotAPI/libraries/armem/client/Query.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
 
#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/core/aron/Trajectory.aron.generated.h>
#include <armarx/navigation/core/aron_conversions.h>
#include <armarx/navigation/human/types.h>
#include <armarx/navigation/memory/constants.h>
 
namespace armarx::navigation::memory::client::stack_result
{
    armarx::navigation::memory::client::stack_result::Reader::LocalPlannerResult
    Reader::queryLocalPlannerResult(const Query& query)
    {
        const auto qb = buildLocalPlannerResultQuery(query);
        const auto& providerName = query.clientID;
 
        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 {.trajectory = {},
                    .status = LocalPlannerResult::Status::Error,
                    .errorMessage = qResult.errorMessage};
        }
 
        const auto coreSegment = qResult.memory.getCoreSegment(properties().coreSegmentName);
 
        if (not coreSegment.hasProviderSegment(providerName))
        {
            ARMARX_DEBUG << "Provider segment `" << providerName << "` does not exist (yet).";
            return {.trajectory = {}, .status = LocalPlannerResult::Status::NoData};
        }
 
        const armem::wm::ProviderSegment& providerSegment =
            coreSegment.getProviderSegment(providerName);
 
        if (providerSegment.empty())
        {
            ARMARX_DEBUG << "No entities.";
            return {.trajectory = {},
                    .status = LocalPlannerResult::Status::NoData,
                    .errorMessage = "No entities"};
        }
 
        try
        {
            return LocalPlannerResult{.trajectory = asLocalPlannerResult(providerSegment),
                                      .status = LocalPlannerResult::Status::Success};
        }
        catch (...)
        {
            return LocalPlannerResult{.trajectory = {},
                                      .status = LocalPlannerResult::Status::Error,
                                      .errorMessage = GetHandledExceptionString()};
        }
    }
 
    std::string
    Reader::propertyPrefix() const
    {
        return "mem.nav.stack_result.";
    }
 
    Reader::Properties
    Reader::defaultProperties() const
    {
        return Properties{.memoryName = memory::constants::NavigationMemoryName,
                          .coreSegmentName = memory::constants::LocalPlannerResultCoreSegment};
    }
 
    armarx::navigation::core::LocalTrajectory
    Reader::asLocalPlannerResult(const armem::wm::ProviderSegment& providerSegment)
    {
        navigation::human::HumanGroups humans;
 
        ARMARX_CHECK(not providerSegment.empty()) << "No entities";
        ARMARX_CHECK(providerSegment.size() == 1) << "There should be only one entity!";
 
        const armem::wm::EntityInstance* entityInstance = nullptr;
 
        providerSegment.forEachEntity(
            [&entityInstance](const armem::wm::Entity& entity)
            {
                const auto& entitySnapshot = entity.getLatestSnapshot();
                ARMARX_CHECK(not entitySnapshot.empty()) << "No entity snapshot instances";
 
                entityInstance = &entitySnapshot.getInstance(0);
            });
 
        ARMARX_CHECK_NOT_NULL(entityInstance);
 
        const auto dto =
            navigation::core::arondto::LocalTrajectory::FromAron(entityInstance->data());
 
        armarx::navigation::core::LocalTrajectory localTrajectory;
        fromAron(dto, localTrajectory);
 
        return localTrajectory;
    }
 
    armarx::armem::client::query::Builder
    Reader::buildLocalPlannerResultQuery(const Query& query) const
    {
        armarx::armem::client::query::Builder qb;
 
        // clang-format off
        qb
        .coreSegments().withName(properties().coreSegmentName)
        .providerSegments().withName(query.clientID)
        .entities().withName("trajectory")
        .snapshots().beforeOrAtTime(query.timestamp);
        // clang-format on
 
        return qb;
    }
 
} // namespace armarx::navigation::memory::client::stack_result