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.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 <RobotAPI/libraries/armem/util/util.h>
 
#include <armarx/navigation/algorithms/Room.h>
#include <armarx/navigation/algorithms/aron/Room.aron.generated.h>
#include <armarx/navigation/algorithms/aron_conversions.h>
#include <armarx/navigation/memory/constants.h>
 
namespace armarx::navigation::memory::client::rooms
{
    Reader::~Reader() = default;
 
    armarx::armem::client::query::Builder
    Reader::buildQuery(const Query& query) const
    {
        armarx::armem::client::query::Builder qb;
 
        auto& coreSegmentSelector =
            qb.coreSegments().withName(memory::constants::RoomsCoreSegmentName);
 
        armem::client::query::ProviderSegmentSelector& providerSegmentSelector =
            [&]() -> armem::client::query::ProviderSegmentSelector&
        {
            if (query.providerName)
            {
                return coreSegmentSelector.providerSegments().withName(*query.providerName);
            }
            return coreSegmentSelector.providerSegments().all();
        }();
 
        armem::client::query::EntitySelector& entitiySelector =
            [&]() -> armem::client::query::EntitySelector&
        {
            if (query.name)
            {
                return providerSegmentSelector.entities().withName(*query.name);
            }
 
            return providerSegmentSelector.entities().all();
        }();
 
        entitiySelector.snapshots().beforeOrAtTime(query.timestamp);
 
        return qb;
    }
 
    std::string
    Reader::propertyPrefix() const
    {
        return "mem.nav.rooms.";
    }
 
    armarx::armem::client::util::SimpleReaderBase::Properties
    Reader::defaultProperties() const
    {
        return {.memoryName = memory::constants::NavigationMemoryName,
                .coreSegmentName = memory::constants::RoomsCoreSegmentName};
    }
 
    inline armarx::navigation::algorithms::Room
    asRoom(const armem::wm::ProviderSegment& providerSegment)
    {
        ARMARX_CHECK(not providerSegment.empty()) << "No entities";
        ARMARX_CHECK_EQUAL(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);
                return false;
            });
        ARMARX_CHECK_NOT_NULL(entityInstance);
 
        const auto aronDto =
            armem::tryCast<armarx::navigation::algorithms::arondto::Room>(*entityInstance);
        ARMARX_CHECK(aronDto) << "Failed casting to Room";
        const armarx::navigation::algorithms::arondto::Room& dto = *aronDto;
 
        navigation::algorithms::Room room;
        fromAron(dto, room);
 
        return room;
    }
 
    Reader::Result
    Reader::query(const Query& query) const
    {
        const auto qb = buildQuery(query);
 
        ARMARX_DEBUG << "[rooms::Reader] query ... ";
 
        const armem::client::QueryResult qResult = memoryReader().query(qb.buildQueryInput());
 
        ARMARX_DEBUG << "[rooms::Reader] result: " << qResult;
 
        if (not qResult.success)
        {
            ARMARX_WARNING << "Failed to query data from memory: " << qResult.errorMessage;
            return {
                .rooms = {}, .status = Result::Status::Error, .errorMessage = qResult.errorMessage};
        }
 
 
        Reader::Result result;
        //  result.rooms
 
        qResult.memory.forEachInstance(
            [&](const armem::wm::EntityInstance& instance)
            {
                const auto dto = algorithms::arondto::Room::FromAron(instance.data());
 
                navigation::algorithms::Room room;
                fromAron(dto, room);
                result.rooms.push_back(room);
            });
 
        if (result.rooms.empty())
        {
            ARMARX_WARNING << "No entities.";
            return {.rooms = {}, .status = Result::Status::NoData};
        }
 
        result.status = Result::Status::Success;
        return result;
 
 
        // const auto coreSegment = qResult.memory.getCoreSegment(properties().coreSegmentName);
 
        // if (not coreSegment.hasProviderSegment(query.providerName))
        // {
        //     ARMARX_WARNING << "Provider segment `" << query.providerName
        //                    << "` does not exist (yet).";
        //     return {.rooms = {}, .status = Result::Status::NoData};
        // }
 
        // const armem::wm::ProviderSegment& providerSegment =
        //     coreSegment.getProviderSegment(query.providerName);
 
        // if (providerSegment.empty())
        // {
        //     ARMARX_WARNING << "No entities.";
        //     return {.room = std::nullopt,
        //             .status = Result::Status::NoData,
        //             .errorMessage = "No entities"};
        // }
 
        // try
        // {
        //     return Result{.room = asRoom(providerSegment), .status = Result::Status::Success};
        // }
        // catch (...)
        // {
        //     return Result{.status = Result::Status::Error,
        //                   .errorMessage = GetHandledExceptionString()};
        // }
    }
 
} // namespace armarx::navigation::memory::client::rooms