Reader.cpp

Go to the documentation of this file.

#include "Reader.h"
 
#include <algorithm>
#include <cstddef>
#include <map>
#include <mutex>
#include <optional>
#include <set>
#include <sstream>
#include <string>
#include <utility>
#include <vector>
 
#include <unistd.h> // For getpid()
 
#include <Ice/Current.h>
#include <Ice/Exception.h>
#include <Ice/LocalException.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.tpp>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/interface/core/time.h>
 
#include "RobotAPI/libraries/armem/client/MemoryNameSystem.h"
#include "RobotAPI/libraries/armem/client/Query.h"
#include "RobotAPI/libraries/armem/client/query/selectors.h"
#include "RobotAPI/libraries/armem/core/Prediction.h"
#include "RobotAPI/libraries/armem/core/error/ArMemError.h"
#include "RobotAPI/libraries/armem/core/query/DataMode.h"
#include "RobotAPI/libraries/aron/core/Path.h"
#include "RobotAPI/libraries/aron/core/aron_conversions.h"
#include "RobotAPI/libraries/aron/core/data/variant/Variant.h"
#include "RobotAPI/libraries/aron/core/data/variant/container/Dict.h"
#include "RobotAPI/libraries/aron/core/type/variant/Variant.h"
#include "RobotAPI/libraries/aron/core/type/variant/container/Object.h"
#include <RobotAPI/interface/armem/prediction.h>
#include <RobotAPI/interface/armem/query.h>
#include <RobotAPI/interface/armem/server/PredictingMemoryInterface.h>
#include <RobotAPI/interface/armem/server/ReadingLongTermMemoryInterface.h>
#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
#include <RobotAPI/interface/armem/server/RecordingMemoryInterface.h>
#include <RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>
#include <RobotAPI/libraries/armem/aron/MemoryLink.aron.generated.h>
#include <RobotAPI/libraries/armem/core/MemoryID_operators.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/util/util.h>
#include <RobotAPI/libraries/aron/common/util/object_finders.h>
#include <RobotAPI/libraries/aron/core/data/visitor/RecursiveVisitor.h>
 
#include "query/Builder.h"
#include "query/query_fns.h"
 
namespace armarx::armem::client
{
 
    Reader::Reader(server::ReadingMemoryInterfacePrx readingMemory,
                   server::PredictingMemoryInterfacePrx predictingMemory,
                   server::ReadingLongTermMemoryInterfacePrx readingLtmMemory) :
        readingPrx(readingMemory), predictionPrx(predictingMemory), readingLtmPrx(readingLtmMemory)
    {
        // Automatically add PID to Ice context for client tracking
        Ice::Context ctx;
        ctx["clientPid"] = std::to_string(getpid());
 
        if (readingMemory)
        {
            this->readingPrx = readingMemory->ice_context(ctx);
        }
        if (predictingMemory)
        {
            this->predictionPrx = predictingMemory->ice_context(ctx);
        }
        if (readingLtmMemory)
        {
            this->readingLtmPrx = readingLtmMemory->ice_context(ctx);
        }
    }
 
    QueryResult
    Reader::queryLTM(const QueryInput& input, bool storeIntoWM) const
    {
        return QueryResult::fromIce(queryLTM(input.toIce(), storeIntoWM));
    }
 
    armem::query::data::Result
    Reader::queryLTM(const armem::query::data::Input& input, bool storeIntoWM) const
    {
        armem::query::data::Result result;
        if (!readingPrx)
        {
            throw error::ProxyNotSet("ReadingMemoryInterfacePrx",
                                     "Reading interface proxy must be set to perform a query.");
        }
 
        try
        {
            result = readingLtmPrx->queryLTM(input, storeIntoWM);
        }
        catch (const ::Ice::ConnectionRefusedException& e)
        {
            // the proxy is invalid. we must perform a reconnect
            ARMARX_INFO << "Trying to reconnect ...";
        }
        catch (const Ice::LocalException& e)
        {
            std::stringstream sstream;
            sstream << "Memory query failed.\nReason: " << e.what();
            result.errorMessage = sstream.str();
 
            ARMARX_VERBOSE << result.errorMessage;
        }
        return result;
    }
 
    QueryResult
    Reader::query(const QueryInput& input) const
    {
        return QueryResult::fromIce(query(input.toIce()));
    }
 
    armem::query::data::Result
    Reader::query(const armem::query::data::Input& input) const
    {
        armem::query::data::Result result;
        if (!readingPrx)
        {
            throw error::ProxyNotSet("ReadingMemoryInterfacePrx",
                                     "Reading interface proxy must be set to perform a query.");
        }
 
        try
        {
            result = readingPrx->query(input);
        }
        catch (const ::Ice::ConnectionRefusedException& e)
        {
            // the proxy is invalid. we must perform a reconnect
            ARMARX_INFO << "Proxy is invalid. Must reconnect.";
        }
        catch (const Ice::LocalException& e)
        {
            std::stringstream sstream;
            sstream << "Memory query failed.\nReason: " << e.what();
            result.errorMessage = sstream.str();
 
            ARMARX_VERBOSE << result.errorMessage;
        }
        return result;
    }
 
    QueryResult
    Reader::query(armem::query::data::MemoryQueryPtr query, armem::query::DataMode dataMode) const
    {
        return this->query(armem::query::data::MemoryQuerySeq{query}, dataMode);
    }
 
    QueryResult
    Reader::query(const armem::query::data::MemoryQuerySeq& queries,
                  armem::query::DataMode dataMode) const
    {
        QueryInput input;
        input.memoryQueries = queries;
        input.dataMode = dataMode;
        return this->query(input);
    }
 
    QueryResult
    Reader::query(const QueryBuilder& queryBuilder) const
    {
        return this->query(queryBuilder.buildQueryInput());
    }
 
    QueryResult
    Reader::query(const QueryInput& input,
                  armem::client::MemoryNameSystem& mns,
                  int recursionDepth) const
    {
        return QueryResult::fromIce(query(input.toIce(), mns, recursionDepth));
    }
 
    armem::query::data::Result
    Reader::query(const armem::query::data::Input& input,
                  armem::client::MemoryNameSystem& mns,
                  int recursionDepth) const
    {
        if (!readingPrx)
        {
            throw error::ProxyNotSet("ReadingMemoryInterfacePrx",
                                     "Reading interface proxy must be set to perform a query.");
        }
 
        armem::query::data::Result result;
        try
        {
            result = readingPrx->query(input);
            QueryResult bObj = QueryResult::fromIce(result);
            bObj.memory.forEachInstance(
                [&bObj, &mns, recursionDepth](armem::wm::EntityInstance& instance)
                { resolveMemoryLinks(instance, bObj.memory, mns, recursionDepth); });
            result = bObj.toIce();
        }
        catch (const Ice::LocalException& e)
        {
            std::stringstream sstream;
            sstream << "Memory query failed.\nReason: " << e.what();
            result.errorMessage = sstream.str();
        }
        catch (const exceptions::local::ExpressionException& e)
        {
            std::stringstream sstream;
            sstream << "Encountered malformed MemoryLink: " << e.what();
            result.errorMessage = sstream.str();
        }
 
        return result;
    }
 
    QueryResult
    Reader::query(armem::query::data::MemoryQueryPtr query, // NOLINT
                  armem::client::MemoryNameSystem& mns,
                  int recursionDepth) const
    {
        return this->query(armem::query::data::MemoryQuerySeq{query}, mns, recursionDepth);
    }
 
    QueryResult
    Reader::query(const armem::query::data::MemoryQuerySeq& queries,
                  armem::client::MemoryNameSystem& mns,
                  int recursionDepth) const
    {
        QueryInput input;
        input.memoryQueries = queries;
        return this->query(input, mns, recursionDepth);
    }
 
    QueryResult
    Reader::query(const QueryBuilder& queryBuilder,
                  armem::client::MemoryNameSystem& mns,
                  int recursionDepth) const
    {
        return this->query(queryBuilder.buildQueryInput(), mns, recursionDepth);
    }
 
    /**
     * Get the MemoryID and data required to fill in the given MemoryLink.
     *
     * Returns nothing if the data could not be retrieved or its type does not
     * match the MemoryLink's template type.
     *
     * @param linkData the data object of the MemoryLink
     * @param linkType the type object of the MemoryLink
     * @return a pair of memoryID and data to fill in a MemoryLink, or nothing if not available
     */
    std::optional<std::pair<MemoryID, aron::data::VariantPtr>>
    findDataForLink(const aron::data::VariantPtr& linkData,
                    const aron::type::VariantPtr& linkType,
                    armem::client::MemoryNameSystem& mns)
    {
        static const aron::Path memoryLinkIDPath{{"memoryID"}};
        static const aron::Path memoryLinkDataPath{{"data"}};
        armem::arondto::MemoryID dto;
        auto memoryIDDict =
            aron::data::Dict::DynamicCastAndCheck(linkData->navigateAbsolute(memoryLinkIDPath));
        dto.fromAron(memoryIDDict);
        auto memoryID = aron::fromAron<armem::MemoryID>(dto);
        if (!memoryID.isWellDefined() || !memoryID.hasEntityName())
        {
            ARMARX_INFO << "Encountered unresolvable MemoryID '" << memoryID.str()
                        << "', ignoring.";
            return {};
        }
 
        auto nextMemory = resolveID(mns, memoryID);
        if (!nextMemory)
        {
            ARMARX_WARNING << "Could not retrieve data for " << memoryID.str() << ", skipping...";
            return {};
        }
        auto nextDataAndType = extractDataAndType(nextMemory.value(), memoryID);
        if (!nextDataAndType)
        {
            ARMARX_WARNING << "Data or type for " << memoryID.str()
                           << " not available in memory containing that MemoryID.";
            return {};
        }
        auto [nextData, nextType] = nextDataAndType.value();
        auto linkTemplate =
            aron::type::Object::DynamicCastAndCheck(linkType)->getTemplateInstantiations().at(0);
        std::string nextObjectName = nextType->getObjectNameWithTemplateInstantiations();
        if (nextType->getObjectName() != linkTemplate)
        {
            ARMARX_WARNING << "Linked object " << memoryID.str() << " is of the type "
                           << nextType->getObjectName() << ", but the link requires the type "
                           << linkTemplate;
            return {};
        }
        return {{memoryID,
                 // This is currently the only method to prefix the path to the data correctly.
                 aron::data::Variant::FromAronDTO(
                     nextData->toAronDTO(),
                     linkData->getPath().withElement(memoryLinkDataPath.toString()))}};
    }
 
    void
    Reader::resolveMemoryLinks(armem::wm::EntityInstance& instance,
                               armem::wm::Memory& input,
                               armem::client::MemoryNameSystem& mns,
                               int recursionDepth)
    {
        static const aron::Path memoryLinkDataPath{{"data"}};
        // MemoryID is used for the template instantiation as a placeholder -
        // you could use any type here.
        static const std::string linkType =
            arondto::MemoryLink<arondto::MemoryID>::ToAronType()->getFullName();
 
        std::set<armem::MemoryID> seenIDs{instance.id()};
        std::vector<aron::Path> currentPaths{{{""}}};
        std::vector<aron::Path> nextPaths;
        int depth = 0;
 
        auto dataAndType = extractDataAndType(input, instance.id());
        if (!dataAndType)
        {
            ARMARX_INFO << "Instance '" << instance.id().str() << "' does not have a defined type.";
            return;
        }
        auto [instanceData, instanceType] = dataAndType.value();
        while (!currentPaths.empty() && (recursionDepth == -1 || depth < recursionDepth))
        {
            for (const auto& path : currentPaths)
            {
                aron::SubObjectFinder finder(linkType);
                if (path.getFirstElement().empty())
                {
                    armarx::aron::data::visitRecursive(finder, instanceData, instanceType);
                }
                else
                {
                    armarx::aron::data::visitRecursive(finder,
                                                       instanceData->navigateAbsolute(path),
                                                       instanceType->navigateAbsolute(path));
                }
 
                for (auto& [linkPathStr, linkDataAndType] : finder.getFoundObjects()) // NOLINT
                {
                    auto [linkData, linkType] = linkDataAndType;
                    auto result = findDataForLink(linkData, linkType, mns);
                    if (result)
                    {
                        auto [memoryID, dataToAppend] = result.value();
                        if (seenIDs.find(memoryID) != seenIDs.end())
                        {
                            continue;
                        }
                        seenIDs.insert(memoryID);
 
                        aron::data::Dict::DynamicCastAndCheck(linkData)->setElement(
                            memoryLinkDataPath.toString(), dataToAppend);
                        nextPaths.push_back(
                            linkData->getPath().withElement(memoryLinkDataPath.toString()));
                    }
                }
            }
            currentPaths = std::move(nextPaths);
            nextPaths.clear();
            ++depth;
        }
    }
 
    QueryResult
    Reader::queryMemoryIDs(const std::vector<MemoryID>& ids, armem::query::DataMode dataMode) const
    {
        using namespace client::query_fns;
 
        query::Builder qb(dataMode);
        for (const MemoryID& id : ids)
        {
            query::EntitySelector& entity =
                qb.coreSegments(withID(id)).providerSegments(withID(id)).entities(withID(id));
 
            if (id.hasTimestamp())
            {
                entity.snapshots(withID(id));
            }
            else
            {
                entity.snapshots(latest());
            }
        }
        return query(qb);
    }
 
    std::optional<wm::EntitySnapshot>
    Reader::getLatestSnapshotOf(const std::vector<MemoryID>& _snapshotIDs) const
    {
        std::vector<MemoryID> snapshotIDs = _snapshotIDs;
 
        client::QueryResult result = this->queryMemoryIDs(snapshotIDs);
        if (result.success)
        {
            std::sort(snapshotIDs.begin(), snapshotIDs.end(), compareTimestampDecreasing);
            for (const MemoryID& snapshotID : snapshotIDs)
            {
                try
                {
                    wm::EntitySnapshot& snapshot = result.memory.getSnapshot(snapshotID);
                    return snapshot;
                }
                catch (const armem::error::ArMemError&)
                {
                }
            }
            return std::nullopt;
        }
        else
        {
            ARMARX_INFO << "Error querying " << snapshotIDs.size() << " STT snapshots:\n"
                        << result.errorMessage;
            return std::nullopt;
        }
    }
 
    QueryResult
    Reader::getLatestSnapshotsIn(const MemoryID& id, armem::query::DataMode dataMode) const
    {
        using namespace client::query_fns;
        if (!id.isWellDefined())
        {
            throw armem::error::InvalidMemoryID(id, "ID must be well defined, but was not.");
        }
 
        query::Builder qb(dataMode);
        query::CoreSegmentSelector& core =
            id.hasCoreSegmentName() ? qb.coreSegments(withID(id)) : qb.coreSegments(all());
        query::ProviderSegmentSelector& prov = id.hasProviderSegmentName()
                                                   ? core.providerSegments(withID(id))
                                                   : core.providerSegments(all());
        query::EntitySelector& entity =
            id.hasEntityName() ? prov.entities(withID(id)) : prov.entities(all());
        entity.snapshots(latest());
 
        return query(qb);
    }
 
    std::optional<wm::EntitySnapshot>
    Reader::getLatestSnapshotIn(const MemoryID& id, armem::query::DataMode dataMode) const
    {
        client::QueryResult result = getLatestSnapshotsIn(id, dataMode);
        if (result.success)
        {
            std::optional<wm::EntitySnapshot> latest = std::nullopt;
            result.memory.forEachEntity(
                [&latest](const wm::Entity& entity)
                {
                    if (not entity.empty())
                    {
                        const wm::EntitySnapshot& snapshot = entity.getLatestSnapshot();
                        if (not latest.has_value() or latest->time() < snapshot.time())
                        {
                            latest = snapshot;
                        }
                    }
                    return true;
                });
            return latest;
        }
        else
        {
            ARMARX_INFO << "Error querying latest snapshot in " << id;
            return std::nullopt;
        }
    }
 
    QueryResult
    Reader::getAllLatestSnapshots(armem::query::DataMode dataMode) const
    {
        using namespace client::query_fns;
 
        query::Builder qb(dataMode);
        qb.coreSegments(all()).providerSegments(all()).entities(all()).snapshots(latest());
 
        return this->query(qb);
    }
 
    QueryResult
    Reader::getAll(armem::query::DataMode dataMode) const
    {
        using namespace client::query_fns;
 
        query::Builder qb(dataMode);
        qb.coreSegments(all()).providerSegments(all()).entities(all()).snapshots(all());
 
        return this->query(qb);
    }
 
    server::dto::DirectlyStoreResult
    Reader::directlyStore(const server::dto::DirectlyStoreInput& input) const
    {
        server::RecordingMemoryInterfacePrx storingMemoryPrx =
            server::RecordingMemoryInterfacePrx::checkedCast(readingPrx);
        if (storingMemoryPrx)
        {
            return storingMemoryPrx->directlyStore(input);
        }
        else
        {
            ARMARX_WARNING << "Could not store a query into the LTM. It seems like the Memory does "
                              "not implement the StoringMemoryInterface.";
            return {};
        }
    }
 
    void
    Reader::startRecording() const
    {
        server::RecordingMemoryInterfacePrx storingMemoryPrx =
            server::RecordingMemoryInterfacePrx::checkedCast(readingPrx);
        if (storingMemoryPrx)
        {
            server::dto::StartRecordInput i;
            i.executionTime = armarx::core::time::dto::DateTime();
            i.startTime = armarx::core::time::dto::DateTime();
            i.recordingID = "";
            i.configuration.clear();
 
            // Use async call to avoid blocking the GUI thread.
            // We call begin_startRecord() but don't call end_startRecord() because we don't need
            // to wait for the result or know if it succeeded - this is a fire-and-forget operation.
            // The remote server will handle startRecord() asynchronously and we return immediately.
            storingMemoryPrx->begin_startRecord(i);
            return;
        }
        else
        {
            ARMARX_WARNING << "Could not store a query into the LTM. It seems like the Memory does "
                              "not implement the StoringMemoryInterface.";
        }
    }
 
    void
    Reader::stopRecording() const
    {
        server::RecordingMemoryInterfacePrx storingMemoryPrx =
            server::RecordingMemoryInterfacePrx::checkedCast(readingPrx);
        if (storingMemoryPrx)
        {
            // Use async call to avoid blocking the GUI thread.
            // We call begin_stopRecord() but don't call end_stopRecord() because we don't need
            // to wait for the result or know if it succeeded - this is a fire-and-forget operation.
            // The remote server will handle stopRecord() asynchronously and we return immediately.
            storingMemoryPrx->begin_stopRecord();
            return;
        }
        else
        {
            ARMARX_WARNING << "Could not store a query into the LTM. It seems like the Memory does "
                              "not implement the StoringMemoryInterface.";
        }
    }
 
    void
    Reader::setReadingMemory(server::ReadingMemoryInterfacePrx readingMemory)
    {
        this->readingPrx = readingMemory;
    }
 
    void
    Reader::setPredictingMemory(server::PredictingMemoryInterfacePrx predictingMemory)
    {
        this->predictionPrx = predictingMemory;
    }
 
    std::vector<PredictionResult>
    Reader::predict(const std::vector<PredictionRequest>& requests) const
    {
        if (!predictionPrx)
        {
            throw error::ProxyNotSet(
                "PredictingMemoryInterfacePrx",
                "Prediction interface proxy must be set to request a prediction.");
        }
 
        armem::prediction::data::PredictionRequestSeq iceRequests;
        for (const auto& request : requests)
        {
            iceRequests.push_back(request.toIce());
        }
 
        armem::prediction::data::PredictionResultSeq results;
        try
        {
            results = predictionPrx->predict(iceRequests);
        }
        catch (const Ice::LocalException& e)
        {
            armem::prediction::data::PredictionResult failure;
            failure.success = false;
            std::stringstream sstream;
            sstream << "Prediction request failed. Reason: " << e.what();
            failure.errorMessage = sstream.str();
 
            for (size_t i = 0; i < requests.size(); ++i)
            {
                results.push_back(failure);
            }
        }
 
        std::vector<PredictionResult> boResults;
        for (const auto& result : results)
        {
            boResults.push_back(PredictionResult::fromIce(result));
        }
 
        return boResults;
    }
 
    std::map<MemoryID, std::vector<PredictionEngine>>
    Reader::getAvailablePredictionEngines() const
    {
        if (!predictionPrx)
        {
            throw error::ProxyNotSet(
                "PredictingMemoryInterfacePrx",
                "Prediction interface proxy must be set to request a prediction.");
        }
 
        armem::prediction::data::EngineSupportMap engines;
        try
        {
            engines = predictionPrx->getAvailableEngines();
        }
        catch (const Ice::LocalException& e)
        {
            // Just leave engines empty in this case.
        }
 
        std::map<MemoryID, std::vector<PredictionEngine>> boMap;
        armarx::fromIce(engines, boMap);
 
        return boMap;
    }
} // namespace armarx::armem::client