MemoryNameSystem.cpp

Go to the documentation of this file.

#include "MemoryNameSystem.h"
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/ManagedIceObject.h>
 
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
 
#include <SimoxUtility/algorithm/string/string_tools.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 (oldProxy->ice_getIdentity() != newProxy->ice_getIdentity())
                        {
                            // Replace old proxy by 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);
                }
            }
            // 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)
    {
        if (auto it = servers.find(memoryID.memoryName); it != servers.end())
        {
            return it->second;
        }
        else
        {
            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)
    {
        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);
    }
 
 
    Reader MemoryNameSystem::getReader(const MemoryID& memoryID)
    {
        auto server = resolveServer(memoryID);
        return Reader(server.reading, server.prediction);
    }
 
 
    Reader MemoryNameSystem::useReader(const MemoryID& memoryID)
    {
        auto server = useServer(memoryID);
        return Reader(server.reading, server.prediction);
    }
 
 
    Reader MemoryNameSystem::useReader(const MemoryID& memoryID, ManagedIceObject& component)
    {
        auto server = useServer(memoryID, component);
        return Reader(server.reading, server.prediction);
    }
 
 
    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<Reader> readerFactory(const mns::dto::MemoryServerInterfaces& server)
    {
        if (auto read = server.reading)
        {
            if (auto predict = server.prediction)
            {
                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, 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) << VAROUT(server.reading) << " | " << VAROUT(server.writing);
 
        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;
    }
 
}