Reader.cpp

Go to the documentation of this file.

#include "Reader.h"
 
#include <map>
#include <ostream>
#include <sstream>
#include <string>
#include <vector>
 
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/algorithm/string/fuzzy_multi_match.hpp>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Query.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/util/SimpleReaderBase.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/error/mns.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem_locations/aron/Location.aron.generated.h>
#include <RobotAPI/libraries/aron/common/aron_conversions/framed.h>
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include <armarx/navigation/core/Graph.h>
#include <armarx/navigation/core/aron/Graph.aron.generated.h>
#include <armarx/navigation/core/location.h>
#include <armarx/navigation/graph/constants.h>
#include <armarx/navigation/location/constants.h>
 
namespace armarx::navigation::memory::client::graph
{
    std::string
    Reader::propertyPrefix() const
    {
        return "mem.nav.graph.";
    }
 
    Reader::Properties
    Reader::defaultProperties() const
    {
        return Properties{.memoryName = navigation::location::coreSegmentID.memoryName,
                          .coreSegmentName = navigation::location::coreSegmentID.coreSegmentName};
    }
 
    std::map<std::string, core::Location>
    Reader::locations()
    {
        std::map<std::string, core::Location> locations;
 
        const auto locs = queryLocations();
        for (const auto& [id, location] : locs)
        {
            const std::string locationName = id.providerSegmentName + "/" + id.entityName;
            ARMARX_DEBUG << "Location: " << locationName;
 
            armarx::FramedPose fp;
            armarx::fromAron(location.framedPose, fp);
 
            core::Location& loc = locations[locationName];
            loc.name = locationName;
            loc.framedPose = fp;
        }
 
        return locations;
    }
 
    std::vector<armarx::navigation::core::Graph>
    Reader::graphs()
    {
        ARMARX_TRACE;
 
        std::map<armem::MemoryID, armarx::navigation::core::Graph> graphs;
 
        const auto graphSegment = allGraphs();
        graphSegment.forEachEntity(
            [&](const armem::wm::Entity& entity)
            {
                navigation::core::Graph& graph = graphs[entity.id()];
                if (const armem::wm::EntityInstance* instance = entity.findLatestInstance())
                {
                    navigation::core::arondto::Graph aron;
                    aron.fromAron(instance->data());
                    fromAron(aron, graph);
                }
                // else: empty layer
            });
 
 
        const auto locSegment = allLocations();
 
        for (auto& [_, graph] : graphs)
        {
            armarx::navigation::core::resolveLocations(graph, locSegment);
        }
 
        return simox::get_values(graphs);
    }
 
    armem::wm::CoreSegment
    Reader::allLocations()
    {
        ARMARX_TRACE;
        // Query all entities from provider.
        armem::client::query::Builder qb;
 
        // clang-format off
        qb
        .coreSegments().withName(navigation::location::coreSegmentID.coreSegmentName)
        .providerSegments().all()
        .entities().all()
        .snapshots().latest();
        // clang-format on
 
        const armem::client::QueryResult qResult =
            memoryReaderLocations.query(qb.buildQueryInput());
        ARMARX_DEBUG << "Lookup result in reader: " << qResult;
 
        if (not qResult.success)
        {
            ARMARX_WARNING << "Failure ...";
            return {};
        }
 
        const auto& locSegment =
            qResult.memory.getCoreSegment(navigation::location::coreSegmentID.coreSegmentName);
        return locSegment;
    }
 
    armem::wm::CoreSegment
    Reader::allGraphs()
    {
        ARMARX_TRACE;
 
        // Query all entities from provider.
        armem::client::query::Builder qb;
 
        // clang-format off
        qb
        .coreSegments().withName(navigation::graph::coreSegmentID.coreSegmentName)
        .providerSegments().all()
        .entities().all()
        .snapshots().latest();
        // clang-format on
 
        const armem::client::QueryResult qResult = memoryReaderGraphs.query(qb.buildQueryInput());
        ARMARX_DEBUG << "Lookup result in reader: " << qResult;
 
        if (not qResult.success)
        {
            ARMARX_WARNING << "Failure ...";
            return {};
        }
 
        const auto& graphSegment =
            qResult.memory.getCoreSegment(navigation::graph::coreSegmentID.coreSegmentName);
        return graphSegment;
    }
 
    std::map<armem::MemoryID, location::arondto::Location>
    Reader::queryLocations()
    {
        // Query all entities from provider.
        armem::client::query::Builder qb;
 
        // clang-format off
        qb
        .coreSegments().withName(navigation::location::coreSegmentID.coreSegmentName)
        .providerSegments().all()
        .entities().all()
        .snapshots().latest();
        // clang-format on
 
        const armem::client::QueryResult qResult =
            memoryReaderLocations.query(qb.buildQueryInput());
        ARMARX_DEBUG << "Lookup result in reader: " << qResult;
 
        if (not qResult.success)
        {
            return {};
        }
 
        std::map<armem::MemoryID, location::arondto::Location> locations;
 
        const auto& locSegment =
            qResult.memory.getCoreSegment(navigation::location::coreSegmentID.coreSegmentName);
        locSegment.forEachEntity(
            [&](const armem::wm::Entity& entity)
            {
                if (const armem::wm::EntityInstance* instance = entity.findLatestInstance())
                {
                    locations[entity.id()].fromAron(instance->data());
                }
            });
 
        ARMARX_VERBOSE << "Retrieved " << locations.size() << " locations.";
        return locations;
    }
 
    core::Location
    Reader::resolveLocationId(const std::string& locationId)
    {
        const auto locs = locations();
 
        ARMARX_CHECK(locs.count(locationId) > 0) << "Location `" << locationId << "` unknown!";
 
        return locs.at(locationId);
    }
 
    ResolveLocationNameResult
    Reader::resolveLocationName(const std::string& locationName,
                                double minMatchRatio,
                                std::ostream* log)
    {
        const std::map<armem::MemoryID, location::arondto::Location> locations = queryLocations();
        return ResolveLocationName(locations, locationName, minMatchRatio, log);
    }
 
    ResolveLocationNameResult
    Reader::ResolveLocationName(
        const std::map<armem::MemoryID, location::arondto::Location>& locations,
        const std::string& locationName,
        double minMatchRatio,
        std::ostream* log)
    {
        std::map<armarx::armem::MemoryID, std::vector<std::string>> locationIdsToWords;
        for (const auto& [id, location] : locations)
        {
            if (location.names.has_value())
            {
                locationIdsToWords[id] = location.names->recognized;
            }
            locationIdsToWords[id].push_back(id.entityName);
            // TODO: Split entityName by "/" and add components?
        }
 
        ResolveLocationNameResult resolved;
 
        if (locationIdsToWords.empty())
        {
            std::stringstream msg;
            msg << "Found no locations in core segment "
                << armarx::navigation::location::coreSegmentID << " the name '" << locationName
                << "' could be matched against.";
 
            resolved.errorMessage = msg.str();
            return resolved;
        }
 
        auto match = log ? simox::alg::fuzzy_multi_match(locationName, locationIdsToWords, *log)
                         : simox::alg::fuzzy_multi_match(locationName, locationIdsToWords);
        ARMARX_CHECK(match.has_value());
 
        if (match->ratio < minMatchRatio)
        {
            std::stringstream msg;
            msg << "Match ratio " << match->ratio << " of best match '" << *match->word
                << "' (location ID " << match->option
                << ") is below the requested minimum match ratio (" << minMatchRatio << ").";
 
            resolved.errorMessage = msg.str();
            return resolved;
        }
 
        resolved.resolved = ResolvedLocationName{
            .locationId = *match->option,
            .locationName = *match->word,
            .matchRatio = match->ratio,
        };
 
        return resolved;
    }
 
    void
    Reader::connect(armarx::armem::client::MemoryNameSystem& mns)
    {
        // for the sake of completion ...
        // armem::client::util::SimpleReaderBase::connect();
 
        // Wait for the memory to become available and add it as dependency.
        ARMARX_INFO << "Reader: Waiting for memory '" << navigation::graph::coreSegmentID.memoryName
                    << "' ...";
 
        try
        {
            ARMARX_INFO << "graphs ...";
            memoryReaderGraphs = mns.useReader(
                armem::MemoryID().withMemoryName(navigation::graph::coreSegmentID.memoryName));
 
            ARMARX_INFO << "locations ...";
            memoryReaderLocations = mns.useReader(
                armem::MemoryID().withMemoryName(navigation::location::coreSegmentID.memoryName));
 
            ARMARX_INFO << "Reader: Connected to memory '"
                        << navigation::graph::coreSegmentID.memoryName << "'";
        }
        catch (const armem::error::CouldNotResolveMemoryServer& e)
        {
            ARMARX_ERROR << e.what();
            return;
        }
    }
 
} // namespace armarx::navigation::memory::client::graph