MemoryToIceAdapter.cpp

Go to the documentation of this file.

#include "MemoryToIceAdapter.h"
 
#include <thread>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/ice_conversions.h>
 
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/ice_conversions.h>
#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
#include <RobotAPI/libraries/armem/core/wm/memory_conversions.h>
#include <RobotAPI/libraries/armem/server/wm/ice_conversions.h>
#include <RobotAPI/libraries/aron/core/Exception.h>
 
#include "query_proc/ltm/disk/ltm.h"
#include "query_proc/wm/wm.h"
 
namespace armarx::armem::server
{
 
    MemoryToIceAdapter::MemoryToIceAdapter(wm::Memory* workingMemory,
                                           server::ltm::Memory* longtermMemory) :
        workingMemory(workingMemory), longtermMemory(longtermMemory)
    {
    }
 
    void
    MemoryToIceAdapter::setMemoryListener(client::MemoryListenerInterfacePrx memoryListener)
    {
        this->memoryListenerTopic = memoryListener;
    }
 
    // WRITING
    data::AddSegmentResult
    MemoryToIceAdapter::addSegment(const data::AddSegmentInput& input, bool addCoreSegments)
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(workingMemory);
 
        data::AddSegmentResult output;
 
        server::wm::CoreSegment* coreSegment = nullptr;
        try
        {
            coreSegment = &workingMemory->getCoreSegment(input.coreSegmentName);
        }
        catch (const armem::error::MissingEntry& e)
        {
            if (addCoreSegments)
            {
                coreSegment = &workingMemory->addCoreSegment(input.coreSegmentName);
            }
            else
            {
                output.success = false;
                output.errorMessage = e.what();
                return output;
            }
        }
        ARMARX_CHECK_NOT_NULL(coreSegment);
 
        if (input.providerSegmentName.size() > 0)
        {
            coreSegment->doLocked(
                [&coreSegment, &input]()
                {
                    try
                    {
                        coreSegment->addProviderSegment(input.providerSegmentName);
                    }
                    catch (const armem::error::ContainerEntryAlreadyExists&)
                    {
                        // This is ok.
                        if (input.clearWhenExists)
                        {
                            server::wm::ProviderSegment& provider =
                                coreSegment->getProviderSegment(input.providerSegmentName);
                            provider.clear();
                        }
                    }
                });
        }
 
        armem::MemoryID segmentID;
        segmentID.memoryName = workingMemory->name();
        segmentID.coreSegmentName = input.coreSegmentName;
        segmentID.providerSegmentName = input.providerSegmentName;
 
        output.success = true;
        output.segmentID = segmentID.str();
        return output;
    }
 
    data::AddSegmentsResult
    MemoryToIceAdapter::addSegments(const data::AddSegmentsInput& input, bool addCoreSegments)
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(workingMemory);
 
        data::AddSegmentsResult output;
        for (const auto& i : input)
        {
            output.push_back(addSegment(i, addCoreSegments));
        }
        return output;
    }
 
    data::CommitResult
    MemoryToIceAdapter::commit(const data::Commit& commitIce, Time timeArrived)
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(workingMemory);
        auto handleException = [](const std::string& what)
        {
            data::CommitResult result;
            data::EntityUpdateResult& r = result.results.emplace_back();
            r.success = false;
            r.errorMessage = what;
            return result;
        };
 
        armem::Commit commit;
        try
        {
            ::armarx::armem::fromIce(commitIce, commit, timeArrived);
        }
        catch (const aron::error::AronNotValidException& e)
        {
            throw;
            return handleException(e.what());
        }
        catch (const Ice::Exception& e)
        {
            throw;
            return handleException(e.what());
        }
 
        armem::CommitResult result = this->commit(commit);
        data::CommitResult resultIce;
        toIce(resultIce, result);
 
        return resultIce;
    }
 
    data::CommitResult
    MemoryToIceAdapter::commit(const data::Commit& commitIce)
    {
        ARMARX_TRACE;
        return commit(commitIce, armem::Time::Now());
    }
 
    armem::CommitResult
    MemoryToIceAdapter::commit(const armem::Commit& commit)
    {
        ARMARX_TRACE;
        return this->_commit(commit, false);
    }
 
    data::CommitResult
    MemoryToIceAdapter::commitLocking(const data::Commit& commitIce, Time timeArrived)
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(workingMemory);
        auto handleException = [](const std::string& what)
        {
            data::CommitResult result;
            data::EntityUpdateResult& r = result.results.emplace_back();
            r.success = false;
            r.errorMessage = what;
            return result;
        };
 
        armem::Commit commit;
        try
        {
            ::armarx::armem::fromIce(commitIce, commit, timeArrived);
        }
        catch (const aron::error::AronNotValidException& e)
        {
            throw;
            return handleException(e.what());
        }
        catch (const Ice::Exception& e)
        {
            throw;
            return handleException(e.what());
        }
 
        armem::CommitResult result = this->commitLocking(commit);
        data::CommitResult resultIce;
        toIce(resultIce, result);
 
        return resultIce;
    }
 
    data::CommitResult
    MemoryToIceAdapter::commitLocking(const data::Commit& commitIce)
    {
        ARMARX_TRACE;
        return commitLocking(commitIce, armem::Time::Now());
    }
 
    armem::CommitResult
    MemoryToIceAdapter::commitLocking(const armem::Commit& commit)
    {
        ARMARX_TRACE;
        return this->_commit(commit, true);
    }
 
    armem::CommitResult
    MemoryToIceAdapter::_commit(const armem::Commit& commit, bool locking)
    {
        ARMARX_TRACE;
        std::vector<data::MemoryID> updatedIDs;
        const bool publishUpdates = bool(memoryListenerTopic);
 
        CommitResult commitResult;
        for (const EntityUpdate& update : commit.updates)
        {
            EntityUpdateResult& result = commitResult.results.emplace_back();
            try
            {
                auto updateResult =
                    locking ? workingMemory->updateLocking(update) : workingMemory->update(update);
 
                result.success = true;
                result.snapshotID = updateResult.id;
                result.arrivedTime = update.arrivedTime;
 
                for (const auto& snapshot : updateResult.removedSnapshots)
                {
                    ARMARX_DEBUG << "The id " << snapshot.id() << " was removed from wm";
                }
 
 
                // Consollidate to ltm(s)
                if (longtermMemory->isRecording())
                {
                    // convert removedSnapshots to Memory
                    armem::wm::Memory m(longtermMemory->name());
                    armem::wm::toMemory(m, *workingMemory, updateResult.removedSnapshots);
                    //this removes information about segments, because new memory (wm) is used
 
                    // store memory
                    longtermMemory->store(m);
                }
 
                if (publishUpdates)
                {
                    data::MemoryID& id = updatedIDs.emplace_back();
                    toIce(id, result.snapshotID);
                }
            }
            catch (const error::ArMemError& e)
            {
                result.success = false;
                result.errorMessage = e.what();
            }
            catch (const aron::error::AronException& e)
            {
                result.success = false;
                result.errorMessage = e.what();
            }
            catch (const Ice::Exception& e)
            {
                result.success = false;
                result.errorMessage = e.what();
            }
            catch (...)
            {
                ARMARX_INFO << "Error during LTM consollidation";
            }
        }
 
        if (publishUpdates)
        {
            memoryListenerTopic->memoryUpdated(updatedIDs);
        }
 
        return commitResult;
    }
 
    // READING
    armem::query::data::Result
    MemoryToIceAdapter::query(const armem::query::data::Input& input)
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(workingMemory);
        ARMARX_CHECK_NOT_NULL(longtermMemory);
 
        // Core segment processors will aquire the core segment locks.
        query_proc::wm_server::MemoryQueryProcessor wmServerProcessor(
            armem::query::boolToDataMode(input.withData));
        armem::wm::Memory wmResult = wmServerProcessor.process(input, *workingMemory);
 
        armem::query::data::Result result;
 
        /*query_proc::ltm_server::MemoryQueryProcessor ltmProcessor;
        armem::wm::Memory ltmResult = ltmProcessor.process(input, *longtermMemory);
 
        if (not ltmResult.empty())
        {
 
            // convert memory ==> meaning resolving references
            // upon query, the LTM only returns a structure of the data (memory without data)
            longtermMemory->resolve(ltmResult);
 
            // append result to return memory and sanity check
            wmResult.append(ltmResult);
            ARMARX_CHECK(not wmResult.empty());
 
            // Ist das wirklich notwendig?
            // query again to limit output size (TODO: Skip if querytype is all)
            //auto queryInput = armem::client::QueryInput::fromIce(input);
            //query_proc::wm::MemoryQueryProcessor wm2wmProcessor(armem::query::boolToDataMode(input.withData));
            //wmResult = wm2wmProcessor.process(queryInput.toIce(), wmResult);
            //if (wmResult.empty())
            //{
            //    ARMARX_ERROR << "A merged and postprocessed Memory has no data although at least the LTM result contains data. This indicates that something is wrong.";
            //}
 
            if (longtermMemory->isRecording())
            {
                // TODO: also move results of ltm to wm
                //this->commit(toCommit(ltm_converted));
 
                // mark removed entries of wm in viewer
                // TODO
            }
        }*/
 
        result.memory = armarx::toIce<data::MemoryPtr>(wmResult);
 
        result.success = true;
        if (result.memory->coreSegments.size() == 0)
        {
            ARMARX_DEBUG << "No data in memory found after query.";
        }
 
        return result;
    }
 
    client::QueryResult
    MemoryToIceAdapter::query(const client::QueryInput& input)
    {
        ARMARX_TRACE;
        return client::QueryResult::fromIce(query(input.toIce()));
    }
 
    armem::structure::data::GetServerStructureResult
    MemoryToIceAdapter::getServerStructure()
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(workingMemory);
        ARMARX_CHECK_NOT_NULL(longtermMemory);
 
        armem::structure::data::GetServerStructureResult ret;
        ret.success = true;
 
        wm::Memory structure;
        structure.id() = workingMemory->id();
 
        // Get all info from the WM
        client::QueryBuilder builder(armem::query::DataMode::NoData);
        builder.all();
        auto query_result = this->query(builder.buildQueryInput());
        if (query_result.success)
        {
            structure.append(query_result.memory);
        }
 
        // Get all info from the LTM
        structure.append(longtermMemory->loadAllReferences());
 
        ret.serverStructure = armarx::toIce<data::MemoryPtr>(structure);
 
        return ret;
    }
 
    // LTM LOADING FROM LTM
 
    // LTM STORING AND RECORDING
    dto::DirectlyStoreResult
    MemoryToIceAdapter::directlyStore(const dto::DirectlyStoreInput& directlStoreInput)
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(longtermMemory);
 
        dto::DirectlyStoreResult output;
        output.success = true;
 
        armem::wm::Memory m = armarx::fromIce<armem::wm::Memory>(directlStoreInput.memory);
        longtermMemory->directlyStore(m);
 
        return output;
    }
 
    dto::StartRecordResult
    MemoryToIceAdapter::startRecord(const dto::StartRecordInput& startRecordInput)
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(longtermMemory);
        ARMARX_IMPORTANT << "Enabling the recording of memory " << longtermMemory->id().str();
        longtermMemory->startRecording();
 
        dto::StartRecordResult ret;
        ret.success = true;
 
        return ret;
    }
 
    dto::StopRecordResult
    MemoryToIceAdapter::stopRecord()
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(longtermMemory);
        ARMARX_IMPORTANT << "Disabling the recording of memory " << longtermMemory->id().str();
 
        //put calling stopRecording into a seperate thread and detach to make sure GUI does not freeze
        auto ltm = longtermMemory;
        std::thread stopRecordingThread([&ltm](){
            ltm->stopRecording();
        });
        std::thread savingDataUpdateThread([&ltm](){
            ltm->bufferFinished();
        });
        stopRecordingThread.detach();
        savingDataUpdateThread.detach();
 
        ARMARX_IMPORTANT
            << "Stopped all LTM recordings, please wait with stopping the component until "
               "all files are written";
 
        dto::StopRecordResult ret;
        ret.success = true;
 
        return ret;
    }
 
    dto::RecordStatusResult
    MemoryToIceAdapter::getRecordStatus()
    {
        dto::RecordStatusResult ret;
        ret.success = true;
 
        long savedSnapshots;
        long totalSnapshots;
        longtermMemory->forEachCoreSegment(
            [&savedSnapshots, &totalSnapshots](const auto& c)
            {
                c.forEachProviderSegment(
                    [&savedSnapshots, &totalSnapshots](const auto& p)
                    {
                        p.forEachEntity(
                            [&savedSnapshots, &totalSnapshots](const auto& e)
                            {
                                savedSnapshots += e.getStatistics().recordedSnapshots;
 
                                e.forEachSnapshot([&totalSnapshots](const auto&)
                                                  { totalSnapshots++; });
                            });
                    });
            });
 
        ret.status.savedSnapshots = savedSnapshots;
        ret.status.totalSnapshots = totalSnapshots;
 
        return ret;
    }
 
    // PREDICTION
    prediction::data::PredictionResultSeq
    MemoryToIceAdapter::predict(prediction::data::PredictionRequestSeq requests)
    {
        auto res = workingMemory->dispatchPredictions(
            armarx::fromIce<std::vector<PredictionRequest>>(requests));
        return armarx::toIce<prediction::data::PredictionResultSeq>(res);
    }
 
    prediction::data::EngineSupportMap
    MemoryToIceAdapter::getAvailableEngines()
    {
        prediction::data::EngineSupportMap result;
        armarx::toIce(result, workingMemory->getAllPredictionEngines());
 
        // Uncomment once LTM also supports prediction engines.
 
        /*prediction::data::EngineSupportMap ltmMap;
        armarx::toIce(ltmMap, longtermMemory->getAllPredictionEngines());
        for (const auto& [memoryID, engines] : ltmMap)
        {
            auto entryIter = result.find(memoryID);
            if (entryIter == result.end())
            {
                result.emplace(memoryID, engines);
            }
            else
            {
                // Merge LTM-supported engines with WM-supported engines, removing duplicates
                std::set<prediction::data::PredictionEngine> engineSet;
                engineSet.insert(entryIter->second.begin(), entryIter->second.end());
                engineSet.insert(engines.begin(), engines.end());
                entryIter->second.assign(engineSet.begin(), engineSet.end());
            }
        }*/
 
        return result;
    }
 
} // namespace armarx::armem::server