MemoryNameSystem.cpp

Go to the documentation of this file.

#include "MemoryNameSystem.h"
 
#include <map>
#include <optional>
#include <sstream>
#include <string>
#include <vector>
 
#include <Ice/Exception.h>
#include <Ice/LocalException.h>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
 
#include <ArmarXCore/core/ManagedIceObject.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/util/StringHelpers.h>
 
#include "RobotAPI/libraries/armem/client/Query.h"
#include "RobotAPI/libraries/armem/client/util/MemoryListener.h"
#include "RobotAPI/libraries/armem/core/MemoryID.h"
#include "RobotAPI/libraries/armem/core/error/ArMemError.h"
#include "RobotAPI/libraries/armem/core/error/mns.h"
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
 
namespace armarx::armem::client
{
 
    MemoryNameSystem::MemoryNameSystem()
    {
    }
 
    MemoryNameSystem::MemoryNameSystem(mns::MemoryNameSystemInterfacePrx mns,
                                       ManagedIceObject* component) :
        util::MemoryListener(component), mns(mns), component(component)
    {
    }
 
    void
    MemoryNameSystem::initialize(mns::MemoryNameSystemInterfacePrx mns, ManagedIceObject* component)
    {
        this->mns = mns;
        setComponent(component);
    }
 
    void
    MemoryNameSystem::update()
    {
        ARMARX_CHECK_NOT_NULL(mns);
 
        mns::dto::GetAllRegisteredServersResult result;
        try
        {
            result = mns->getAllRegisteredServers();
        }
        catch (const Ice::NotRegisteredException& e)
        {
            throw error::MemoryNameSystemQueryFailed(e.what());
        }
 
        if (result.success)
        {
            for (const auto& [name, server] : result.servers)
            {
                auto [it, inserted] = servers.try_emplace(name, server);
                if (inserted)
                {
                    // OK
                }
                else
                {
                    // Compare ice identities, update if it changed.
                    auto foo = [](auto& oldProxy, const auto& newProxy)
                    {
                        if (not newProxy)
                        {
                            ARMARX_WARNING << "New proxy is invalid";
                            return;
                        }
 
                        try
                        {
                            newProxy->ice_ping();
                        }
                        catch (const Ice::Exception& e)
                        {
                            // The new proxy must be reachable
                            throw error::MemoryNameSystemQueryFailed(e.what());
                        }
 
                        if (not oldProxy)
                        {
                            ARMARX_WARNING << "Old proxy is invalid";
                            oldProxy = newProxy;
                            return;
                        }
 
                        try
                        {
                            oldProxy->ice_ping();
 
                            if (oldProxy->ice_getIdentity() != newProxy->ice_getIdentity())
                            {
                                // Replace old proxy by new one. This is the case if the identity changed.
                                oldProxy = newProxy;
                            }
                        }
                        catch (const Ice::Exception&)
                        {
                            // if there is any exception, replace the old proxy by the new one
                            oldProxy = newProxy;
                        }
                    };
                    foo(it->second.reading, server.reading);
                    foo(it->second.writing, server.writing);
                    foo(it->second.prediction, server.prediction);
                    foo(it->second.actions, server.actions);
                    foo(it->second.configuration, server.configuration);
                    foo(it->second.loading, server.loading);
                    foo(it->second.readingLtm, server.readingLtm);
                }
            }
            // Remove all entries which are not there anymore.
            for (auto it = servers.begin(); it != servers.end();)
            {
                if (result.servers.count(it->first) == 0)
                {
                    it = servers.erase(it);
                }
                else
                {
                    ++it;
                }
            }
        }
        else
        {
            throw error::MemoryNameSystemQueryFailed(result.errorMessage);
        }
    }
 
    mns::dto::MemoryServerInterfaces
    MemoryNameSystem::resolveServer(const MemoryID& memoryID)
    {
        update();
        if (auto it = servers.find(memoryID.memoryName); it != servers.end())
        {
            return it->second;
        }
        else
        {
            throw error::CouldNotResolveMemoryServer(memoryID);
        }
    }
 
    mns::dto::MemoryServerInterfaces
    MemoryNameSystem::waitForServer(const MemoryID& memoryID)
    {
        update();
        if (memoryID.memoryName.empty())
        {
            ARMARX_WARNING << "waitForServer called with EMPTY memory name! "
                           << "This will block forever. MemoryID: " << memoryID;
        }
        if (auto it = servers.find(memoryID.memoryName); it != servers.end())
        {
            return it->second;
        }
        else
        {
            mns::dto::WaitForServerInput input;
            input.name = memoryID.memoryName;
 
            ARMARX_CHECK_NOT_NULL(mns);
            ARMARX_INFO << "Waiting for memory server for " << memoryID << " ...";
            mns::dto::WaitForServerResult result = mns->waitForServer(input);
            if (result.success)
            {
                ARMARX_INFO << "Resolved memory for " << memoryID << ".";
                return result.server;
            }
            else
            {
                throw error::CouldNotResolveMemoryServer(memoryID, result.errorMessage);
            }
        }
    }
 
    mns::dto::MemoryServerInterfaces
    MemoryNameSystem::useServer(const MemoryID& memoryID)
    {
        ARMARX_CHECK_NOT_NULL(component)
            << "Owning component not set when using a memory server. \n"
            << "When calling `armem::mns::MemoryNameSystem::useServer()`, the owning component "
               "which should "
            << "receive the dependency to the memory server must be set beforehand. \n\n"
            << "Use `armem::mns::MemoryNameSystem::setComponent()` or pass the component on "
               "construction "
            << "before calling useServer().";
        return useServer(memoryID, *component);
    }
 
    mns::dto::MemoryServerInterfaces
    MemoryNameSystem::useServer(const MemoryID& memoryID, ManagedIceObject& component)
    {
        mns::dto::MemoryServerInterfaces server = waitForServer(memoryID);
        // Add dependency.
 
        component.usingProxy(server.reading->ice_getIdentity().name);
        return server;
    }
 
    mns::dto::MemoryServerInterfaces
    MemoryNameSystem::useServer(const std::string& memoryName)
    {
        return useServer(MemoryID().withMemoryName(memoryName));
    }
 
    mns::dto::MemoryServerInterfaces
    MemoryNameSystem::useServer(const std::string& memoryName, ManagedIceObject& component)
    {
        return useServer(MemoryID().withMemoryName(memoryName), component);
    }
 
    Configurator
    MemoryNameSystem::getConfigurator(const MemoryID& memoryID)
    {
        auto server = resolveServer(memoryID);
        return Configurator(server.configuration);
    }
 
    Configurator
    MemoryNameSystem::useConfigurator(const MemoryID& memoryID)
    {
        auto server = useServer(memoryID);
        return Configurator(server.configuration);
    }
 
    Configurator
    MemoryNameSystem::useConfigurator(const MemoryID& memoryID, ManagedIceObject& component)
    {
        auto server = useServer(memoryID, component);
        return Configurator(server.configuration);
    }
 
    Configurator
    MemoryNameSystem::useConfigurator(const std::string& memoryName)
    {
        return useConfigurator(MemoryID().withMemoryName(memoryName));
    }
 
    Configurator
    MemoryNameSystem::useConfigurator(const std::string& memoryName, ManagedIceObject& component)
    {
        return useConfigurator(MemoryID().withMemoryName(memoryName), component);
    }
 
    Loader
    MemoryNameSystem::getLoader(const MemoryID& memoryID)
    {
        auto server = resolveServer(memoryID);
        return Loader(server.loading);
    }
 
    Loader
    MemoryNameSystem::useLoader(const MemoryID& memoryID)
    {
        auto server = useServer(memoryID);
        return Loader(server.loading);
    }
 
    Loader
    MemoryNameSystem::useLoader(const MemoryID& memoryID, ManagedIceObject& component)
    {
        auto server = useServer(memoryID, component);
        return Loader(server.loading);
    }
 
    Loader
    MemoryNameSystem::useLoader(const std::string& memoryName)
    {
        return useLoader(MemoryID().withMemoryName(memoryName));
    }
 
    Loader
    MemoryNameSystem::useLoader(const std::string& memoryName, ManagedIceObject& component)
    {
        return useLoader(MemoryID().withMemoryName(memoryName), component);
    }
 
    Reader
    MemoryNameSystem::getReader(const MemoryID& memoryID)
    {
        auto server = resolveServer(memoryID);
        return Reader(server.reading, server.prediction, server.readingLtm);
    }
 
    Reader
    MemoryNameSystem::useReader(const MemoryID& memoryID)
    {
        auto server = useServer(memoryID);
        return Reader(server.reading, server.prediction, server.readingLtm);
    }
 
    Reader
    MemoryNameSystem::useReader(const MemoryID& memoryID, ManagedIceObject& component)
    {
        auto server = useServer(memoryID, component);
        return Reader(server.reading, server.prediction, server.readingLtm);
    }
 
    Reader
    MemoryNameSystem::useReader(const std::string& memoryName)
    {
        return useReader(MemoryID().withMemoryName(memoryName));
    }
 
    Reader
    MemoryNameSystem::useReader(const std::string& memoryName, ManagedIceObject& component)
    {
        return useReader(MemoryID().withMemoryName(memoryName), component);
    }
 
    template <class ClientT>
    std::map<std::string, ClientT>
    MemoryNameSystem::_getAllClients(ClientFactory<ClientT>&& factory) const
    {
        std::map<std::string, ClientT> result;
        for (const auto& [name, server] : servers)
        {
            if (std::optional<ClientT> client = factory(server))
            {
                result[name] = client.value();
            }
        }
        return result;
    }
 
    std::optional<Configurator>
    configuratorFactory(const mns::dto::MemoryServerInterfaces& server)
    {
        if (auto config = server.configuration)
        {
            return Configurator(config);
        }
 
        return std::nullopt;
    }
 
    std::optional<Loader>
    loaderFactory(const mns::dto::MemoryServerInterfaces& server)
    {
        if (auto load = server.loading)
        {
            return Loader(load);
        }
 
        return std::nullopt;
    }
 
    std::optional<Reader>
    readerFactory(const mns::dto::MemoryServerInterfaces& server)
    {
        if (auto read = server.reading)
        {
            if (auto predict = server.prediction)
            {
                if (auto readLtm = server.readingLtm)
                {
                    return Reader(read, predict, readLtm);
                }
 
                return Reader(read, predict);
            }
            else
            {
                return Reader(read);
            }
        }
        return std::nullopt;
    }
 
    std::optional<Writer>
    writerFactory(const mns::dto::MemoryServerInterfaces& server)
    {
        if (auto write = server.writing)
        {
            return Writer(write);
        }
        return std::nullopt;
    }
 
    std::map<std::string, Loader>
    MemoryNameSystem::getAllLoaders(bool update)
    {
        if (update)
        {
            this->update();
        }
 
        return _getAllClients<Loader>(loaderFactory);
    }
 
    std::map<std::string, Loader>
    MemoryNameSystem::getAllLoaders() const
    {
        return _getAllClients<Loader>(loaderFactory);
    }
 
    std::map<std::string, Configurator>
    MemoryNameSystem::getAllConfigurators(bool update)
    {
        if (update)
        {
            this->update();
        }
 
        return _getAllClients<Configurator>(configuratorFactory);
    }
 
    std::map<std::string, Configurator>
    MemoryNameSystem::getAllConfigurators() const
    {
        return _getAllClients<Configurator>(configuratorFactory);
    }
 
    std::map<std::string, Reader>
    MemoryNameSystem::getAllReaders(bool update)
    {
        if (update)
        {
            this->update();
        }
 
        return _getAllClients<Reader>(readerFactory);
    }
 
    std::map<std::string, Reader>
    MemoryNameSystem::getAllReaders() const
    {
        return _getAllClients<Reader>(readerFactory);
    }
 
    Writer
    MemoryNameSystem::getWriter(const MemoryID& memoryID)
    {
        return Writer(resolveServer(memoryID).writing);
    }
 
    Writer
    MemoryNameSystem::useWriter(const MemoryID& memoryID)
    {
        return Writer(useServer(memoryID).writing);
    }
 
    Writer
    MemoryNameSystem::useWriter(const MemoryID& memoryID, ManagedIceObject& component)
    {
        return Writer(useServer(memoryID, component).writing);
    }
 
    Writer
    MemoryNameSystem::useWriter(const std::string& memoryName)
    {
        return useWriter(MemoryID().withMemoryName(memoryName));
    }
 
    Writer
    MemoryNameSystem::useWriter(const std::string& memoryName, ManagedIceObject& component)
    {
        return useWriter(MemoryID().withMemoryName(memoryName), component);
    }
 
    std::map<std::string, Writer>
    MemoryNameSystem::getAllWriters(bool update)
    {
        if (update)
        {
            this->update();
        }
        return _getAllClients<Writer>(writerFactory);
    }
 
    std::map<std::string, Writer>
    MemoryNameSystem::getAllWriters() const
    {
        return _getAllClients<Writer>(writerFactory);
    }
 
    std::optional<wm::EntityInstance>
    MemoryNameSystem::resolveEntityInstance(const MemoryID& id)
    {
        auto result = resolveEntityInstances({id});
        if (result.size() > 0)
        {
            return result.begin()->second;
        }
        else
        {
            return std::nullopt;
        }
    }
 
    std::map<MemoryID, wm::EntityInstance>
    MemoryNameSystem::resolveEntityInstances(const std::vector<MemoryID>& ids)
    {
        std::stringstream errors;
        int errorCounter = 0;
 
        std::map<std::string, std::vector<MemoryID>> idsPerMemory;
        for (const auto& id : ids)
        {
            idsPerMemory[id.memoryName].push_back(id);
        }
 
        std::map<MemoryID, wm::EntityInstance> result;
        for (const auto& [memoryName, ids] : idsPerMemory)
        {
            Reader reader = getReader(MemoryID().withMemoryName(memoryName));
            QueryResult queryResult = reader.queryMemoryIDs(ids);
            if (queryResult.success)
            {
                for (const MemoryID& id : ids)
                {
                    try
                    {
                        if (id.hasInstanceIndex())
                        {
                            result[id] = queryResult.memory.getInstance(id);
                        }
                        else if (id.hasTimestamp())
                        {
                            result[id] = queryResult.memory.getSnapshot(id).getInstance(0);
                        }
                        else if (id.hasEntityName())
                        {
                            result[id] =
                                queryResult.memory.getEntity(id).getLatestSnapshot().getInstance(0);
                        }
                        else
                        {
                            std::stringstream ss;
                            ss << "MemoryNameSystem::" << __FUNCTION__
                               << "requires IDs to be entity, snapshot or instance IDs,"
                               << "but ID has no entity name.";
                            throw error::InvalidMemoryID(id, ss.str());
                        }
                    }
                    catch (const error::ArMemError& e)
                    {
                        errors << "\n#" << ++errorCounter << "\n"
                               << "Failed to retrieve " << id << " from query result: \n"
                               << e.what();
                    }
                }
            }
            else
            {
                errors << "\n# " << ++errorCounter << "\n"
                       << "Failed to query '" << memoryName << "': \n"
                       << queryResult.errorMessage;
            }
        }
 
        if (errors.str().size() > 0)
        {
            ARMARX_INFO << "MemoryNameSystem::" << __FUNCTION__
                        << ": The following errors may affect your result: "
                        << "\n\n"
                        << errors.str() << "\n\n"
                        << "When querying entity instances: \n- "
                        << simox::alg::join(simox::alg::multi_to_string(ids), "\n- ");
        }
 
        return result;
    }
 
    void
    MemoryNameSystem::registerServer(const MemoryID& memoryID,
                                     mns::dto::MemoryServerInterfaces server)
    {
        mns::dto::RegisterServerInput input;
        input.name = memoryID.memoryName;
        input.server = server;
        ARMARX_CHECK(server.reading or server.writing or server.configuration or server.loading or
                     server.readingLtm)
            << VAROUT(server.reading) << " | " << VAROUT(server.writing) << " | "
            << VAROUT(server.configuration) << " | " << VAROUT(server.loading) << " | "
            << VAROUT(server.readingLtm);
 
        ARMARX_CHECK_NOT_NULL(mns);
        mns::dto::RegisterServerResult result = mns->registerServer(input);
        if (!result.success)
        {
            throw error::ServerRegistrationOrRemovalFailed(
                "register", memoryID, result.errorMessage);
        }
    }
 
    void
    MemoryNameSystem::removeServer(const MemoryID& memoryID)
    {
        mns::dto::RemoveServerInput input;
        input.name = memoryID.memoryName;
 
        ARMARX_CHECK_NOT_NULL(mns);
        mns::dto::RemoveServerResult result = mns->removeServer(input);
        if (!result.success)
        {
            throw error::ServerRegistrationOrRemovalFailed("remove", memoryID, result.errorMessage);
        }
    }
 
    mns::MemoryNameSystemInterfacePrx
    MemoryNameSystem::getMemoryNameSystem() const
    {
        return mns;
    }
 
    void
    MemoryNameSystem::getMemoryNameSystem(mns::MemoryNameSystemInterfacePrx mns)
    {
        this->mns = mns;
    }
 
    void
    MemoryNameSystem::setComponent(ManagedIceObject* component)
    {
        util::MemoryListener::setComponent(component);
        this->component = component;
    }
 
} // namespace armarx::armem::client