Reader.cpp

Go to the documentation of this file.

#include "Reader.h"
 
#include <optional>
#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/Clock.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/util/CPPUtility/trace.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/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/aron_conversions.h>
#include <armarx/navigation/memory/constants.h>
 
namespace armarx::navigation::memory::client::costmap
{
    Reader::~Reader() = default;
 
    armarx::armem::client::query::Builder
    Reader::buildQuery(const Query& query) const
    {
        armarx::armem::client::query::Builder qb;
 
        // clang-format off
        auto& sel = qb
        .coreSegments().withName(properties().coreSegmentName)
        .providerSegments().withName(query.providerName)
        .entities();
        if (query.name.empty()){
            sel = sel.all();
        }else{
            sel = sel.withName(query.name);
        }
        sel.snapshots().beforeOrAtTime(query.timestamp);
        // clang-format on
 
        return qb;
    }
 
    std::string
    Reader::propertyPrefix() const
    {
        return "mem.nav.costmap.";
    }
 
    armarx::armem::client::util::SimpleReaderBase::Properties
    Reader::defaultProperties() const
    {
        return {.memoryName = memory::constants::NavigationMemoryName,
                .coreSegmentName = memory::constants::CostmapCoreSegmentName};
    }
 
    namespace
    {
        algorithms::Costmap
        asCostmap(const armem::wm::ProviderSegment& providerSegment)
        {
            ARMARX_CHECK_NOT_EMPTY(providerSegment) << "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);
                    ARMARX_DEBUG << VAROUT(entitySnapshot.time());
                    return false;
                });
            ARMARX_CHECK_NOT_NULL(entityInstance);
            return algorithms::fromAron(*entityInstance);
        }
    } // namespace
 
    Reader::Result
    Reader::query(const Query& query) const
    {
        const auto qb = buildQuery(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 {.costmap = std::nullopt,
                    .status = Result::Status::Error,
                    .errorMessage = qResult.errorMessage};
        }
 
        const auto coreSegment = qResult.memory.getCoreSegment(properties().coreSegmentName);
 
        if (not coreSegment.hasProviderSegment(query.providerName))
        {
            ARMARX_VERBOSE << "Provider segment `" << query.providerName
                           << "` does not exist (yet).";
            return {.costmap = std::nullopt, .status = Result::Status::NoData};
        }
 
        const armem::wm::ProviderSegment& providerSegment =
            coreSegment.getProviderSegment(query.providerName);
 
        if (providerSegment.empty())
        {
            ARMARX_VERBOSE << "No entities.";
            return {.costmap = std::nullopt,
                    .status = Result::Status::NoData,
                    .errorMessage = "No entities"};
        }
 
        try
        {
            return Result{.costmap = asCostmap(providerSegment), .status = Result::Status::Success};
        }
        catch (...)
        {
            return Result{.status = Result::Status::Error,
                          .errorMessage = GetHandledExceptionString()};
        }
    }
 
    navigation::algorithms::Costmap
    Reader::waitForCostmap(std::string& providerName, std::string& name, int delayMs) const
    {
        ARMARX_TRACE;
        while (true)
        {
            const auto timestamp = armarx::Clock::Now();
 
            const navigation::memory::client::costmap::Reader::Query costmapQuery{
                .providerName = "distance_to_obstacle_costmap_provider",
                .name = "distance_to_obstacles",
                .timestamp = timestamp};
 
            const Result costmapResult = query(costmapQuery);
 
            // if costmap is not available yet, wait
            if (costmapResult.status != Result::Success)
            {
                ARMARX_INFO << deactivateSpam(10.) << "Waiting for costmap";
                Clock::WaitFor(Duration::MilliSeconds(delayMs));
                continue;
            }
 
            ARMARX_VERBOSE << "Received costmap \"" << providerName << "/" << name << "\"";
 
            ARMARX_CHECK_EQUAL(costmapResult.status, Result::Success);
 
            ARMARX_TRACE;
            ARMARX_CHECK(costmapResult.costmap.has_value());
            return costmapResult.costmap.value();
        };
    }
 
} // namespace armarx::navigation::memory::client::costmap