Entity.cpp

Go to the documentation of this file.

// Header
#include "Entity.h"
#include <memory>
 
// SImox
#include <SimoxUtility/algorithm/string.h>
 
// ArmarX
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/TimeUtil.h>
 
#include "RobotAPI/libraries/armem/server/ltm/persistence/MemoryPersistenceStrategy.h"
#include <RobotAPI/libraries/armem/core/base/detail/negative_index_semantics.h>
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
 
#include "processors/filter/frequencyFilter/FrequencyFilter.h"
 
namespace armarx::armem::server::ltm
{
    Entity::Entity(const std::string& exportName,
                   const armem::MemoryID& memoryId /* UNESCAPED */,
                   const std::shared_ptr<Processors>& filters,
                   const std::shared_ptr<persistence::MemoryPersistenceStrategy>& persistenceStrategy) :
        EntityBase(exportName, memoryId, filters), persistenceStrategy_(persistenceStrategy) {}
 
    bool
    Entity::forEachSnapshot(std::function<void(EntitySnapshot&)> func) const
    {
        std::lock_guard l(ltm_mutex);
 
        ARMARX_TRACE;
 
        ARMARX_DEBUG << "For each entity snapshot (entity id=" << id().cleanID().str() << ")";
 
        // Some weird sideeffects are happening here. Prob. some references are mixed up somewhere
        id().clearTimestamp();
        // persistenceStrategy_->setMemoryID(id());
 
        for (auto& entitySnapshotKey : persistenceStrategy_->getContainerKeys(id()))
        {
            ARMARX_DEBUG << "Found entity snapshot=" << entitySnapshotKey;
 
            std::shared_ptr<persistence::MemoryPersistenceStrategy> entitySnapshotPersistenceStrategy(persistenceStrategy_);
 
            EntitySnapshot ltmEntitySnapshot(
                getExportName(),
                id().withTimestamp(timeFromStringMicroSeconds(entitySnapshotKey)),
                processors,
                entitySnapshotPersistenceStrategy);
 
 
            func(ltmEntitySnapshot);
        }
 
        return true;
    }
 
    bool
    Entity::forEachSnapshotInIndexRange(long first,
                                        long last,
                                        std::function<void(EntitySnapshot&)> func) const
    {
        std::lock_guard l(ltm_mutex);
 
        ARMARX_TRACE;
 
        // ARMARX_WARNING << "PLEASE NOTE THAT QUERYING THE LTM INDEX WISE MAY BE BUGGY BECAUSE THE FILESYSTEM ITERATOR IS UNSORTED!";
 
        if (first < 0 or last < 0)
        {
            // We need to know what the size of the memory is... May be slow
            unsigned long size = 0;
            auto countFunc = [&](EntitySnapshot& ltmEntitySnapshot) { size++; };
            forEachSnapshot(std::move(countFunc));
 
            first = armarx::armem::base::detail::negativeIndexSemantics(first, size);
            last = armarx::armem::base::detail::negativeIndexSemantics(last, size);
        }
 
        long checked = 0;
        auto runInIndexRangeFunc = [&](EntitySnapshot& ltmEntitySnapshot)
        {
            if (checked >= first && checked <= last)
            {
                func(ltmEntitySnapshot);
            }
            checked++;
        };
 
        return forEachSnapshot(std::move(runInIndexRangeFunc));
    }
 
    bool
    Entity::forEachSnapshotInTimeRange(const Time& min,
                                       const Time& max,
                                       std::function<void(EntitySnapshot&)> func) const
    {
        std::lock_guard l(ltm_mutex);
 
        ARMARX_TRACE;
 
        auto runInTimeRange = [&](EntitySnapshot& ltmEntitySnapshot)
        {
            Time ts = ltmEntitySnapshot.id().timestamp;
            if (ts >= min && ts <= max)
            {
                func(ltmEntitySnapshot);
            }
        };
 
        return forEachSnapshot(std::move(runInTimeRange));
    }
 
    bool
    Entity::forEachSnapshotBeforeOrAt(const Time& time,
                                      std::function<void(EntitySnapshot&)> func) const
    {
        std::lock_guard l(ltm_mutex);
 
        ARMARX_TRACE;
 
        auto runBeforeOrAtFunc = [&](EntitySnapshot& ltmEntitySnapshot)
        {
            Time ts = ltmEntitySnapshot.id().timestamp;
            if (ts <= time)
            {
                func(ltmEntitySnapshot);
            }
        };
 
        return forEachSnapshot(std::move(runBeforeOrAtFunc));
    }
 
    bool
    Entity::forEachSnapshotBefore(const Time& time, std::function<void(EntitySnapshot&)> func) const
    {
        std::lock_guard l(ltm_mutex);
 
        ARMARX_TRACE;
 
        auto runBeforeFunc = [&](EntitySnapshot& ltmEntitySnapshot)
        {
            Time ts = ltmEntitySnapshot.id().timestamp;
            if (ts < time)
            {
                func(ltmEntitySnapshot);
            }
        };
 
        return forEachSnapshot(std::move(runBeforeFunc));
    }
 
    bool
    Entity::hasSnapshot(const Time& snapshotTime) const
    {
        std::lock_guard l(ltm_mutex);
 
        ARMARX_TRACE;
 
        // Just in case
        id().clearTimestamp();
 
        bool foundSnapshot = persistenceStrategy_->containsContainer(id(), std::to_string(snapshotTime.toMicroSecondsSinceEpoch()));
 
        return foundSnapshot;
    }
 
    std::shared_ptr<EntitySnapshot>
    Entity::findSnapshot(const Time& snapshotTime) const
    {
        std::lock_guard l(ltm_mutex);
 
        ARMARX_TRACE;
 
        if (!hasSnapshot(snapshotTime))
        {
            return nullptr;
        }
 
        // // Make a copy, otherwise we would also alter the entity id
        MemoryID entitySnapshotId = id().withTimestamp(snapshotTime);
        
        return std::make_shared<EntitySnapshot>(getExportName(), entitySnapshotId, processors, persistenceStrategy_);
    }
 
    std::shared_ptr<EntitySnapshot>
    Entity::findLatestSnapshot() const
    {
        std::lock_guard l(ltm_mutex);
 
        ARMARX_TRACE;
 
        Time bestMatch = Time::Invalid();
        auto runLatestFunc = [&](EntitySnapshot& ltmEntitySnapshot)
        {
            Time ts = ltmEntitySnapshot.id().timestamp;
            if (ts > bestMatch)
            {
                bestMatch = ts;
            }
        };
 
        forEachSnapshot(std::move(runLatestFunc));
 
        if (bestMatch == Time::Invalid())
        {
            return nullptr;
        }
 
        // Just in case
        id().clearTimestamp();
        
        MemoryID entitySnapshotId = id().withTimestamp(bestMatch);
       
        return std::make_shared<EntitySnapshot>(getExportName(),
                                                entitySnapshotId,
                                                processors,
                                                persistenceStrategy_);
    }
 
    std::shared_ptr<EntitySnapshot>
    Entity::findLatestSnapshotBefore(const Time& time) const
    {
        std::lock_guard l(ltm_mutex);
 
        ARMARX_TRACE;
 
        Time bestMatch = Time::Invalid();
        auto findLatestSnapshotBeforeFunc = [&](EntitySnapshot& e)
        {
            auto ts = e.id().timestamp;
            if (ts < time && ts > bestMatch)
            {
                bestMatch = ts;
            }
        };
 
        forEachSnapshot(std::move(findLatestSnapshotBeforeFunc));
 
        if (bestMatch == Time::Invalid())
        {
            return nullptr;
        }
        
        // Just in case
        id().clearTimestamp();
 
        MemoryID entitySnapshotId = id().withTimestamp(bestMatch);
 
        return std::make_shared<EntitySnapshot>(getExportName(),
                                                entitySnapshotId,
                                                processors,
                                                persistenceStrategy_);
    }
 
    std::shared_ptr<EntitySnapshot>
    Entity::findLatestSnapshotBeforeOrAt(const Time& time) const
    {
        std::lock_guard l(ltm_mutex);
 
        ARMARX_TRACE;
 
        Time bestMatch = Time::Invalid();
        auto findLatestSnapshotBeforeOrAtFunc = [&](EntitySnapshot& e)
        {
            auto ts = e.id().timestamp;
            if (ts <= time && ts > bestMatch)
            {
                bestMatch = ts;
            }
        };
 
        forEachSnapshot(std::move(findLatestSnapshotBeforeOrAtFunc));
 
        if (bestMatch == Time::Invalid())
        {
            return nullptr;
        }      
 
        
        MemoryID entitySnapshotId = id().withTimestamp(bestMatch);
        
        return std::make_shared<EntitySnapshot>(getExportName(),
                                                entitySnapshotId,
                                                processors,
                                                persistenceStrategy_);
    }
 
    std::shared_ptr<EntitySnapshot>
    Entity::findFirstSnapshotAfter(const Time& time) const
    {
        std::lock_guard l(ltm_mutex);
 
        ARMARX_TRACE;
 
        Time bestMatch{Duration::MicroSeconds(std::numeric_limits<long>::max())};
        auto findFirstSnapshotAfterFunc = [&](EntitySnapshot& e)
        {
            auto ts = e.id().timestamp;
            if (ts > time && ts < bestMatch)
            {
                bestMatch = ts;
            }
        };
 
        forEachSnapshot(std::move(findFirstSnapshotAfterFunc));
 
        if (bestMatch == Time(Duration::MicroSeconds(std::numeric_limits<long>::max())))
        {
            return nullptr;
        }
 
        
        MemoryID entitySnapshotId = id().withTimestamp(bestMatch);
        
        return std::make_shared<EntitySnapshot>(getExportName(),
                                                id().withTimestamp(bestMatch),
                                                processors,
                                                persistenceStrategy_);
    }
 
    std::shared_ptr<EntitySnapshot>
    Entity::findFirstSnapshotAfterOrAt(const Time& time) const
    {
        std::lock_guard l(ltm_mutex);
 
        ARMARX_TRACE;
 
        Time bestMatch{Duration::MicroSeconds(std::numeric_limits<long>::max())};
        auto findFirstSnapshotAfterOrAtFunc = [&](EntitySnapshot& e)
        {
            auto ts = e.id().timestamp;
            if (ts >= time && ts < bestMatch)
            {
                bestMatch = ts;
            }
        };
 
        forEachSnapshot(std::move(findFirstSnapshotAfterOrAtFunc));
 
        if (bestMatch == Time(Duration::MicroSeconds(std::numeric_limits<long>::max())))
        {
            return nullptr;
        }
        
        MemoryID entitySnapshotId = id().withTimestamp(bestMatch);
        
        return std::make_shared<EntitySnapshot>(getExportName(),
                                                entitySnapshotId,
                                                processors,
                                                persistenceStrategy_);
    }
 
    void
    Entity::_loadAllReferences(armem::wm::Entity& wmEntity)
    {
        std::lock_guard l(ltm_mutex);
 
        ARMARX_TRACE;
 
        wmEntity.id() = id().getEntityID().cleanID();
 
        forEachSnapshot(
            [&wmEntity](auto& ltmEntitySnapshot)
            {
                if (!wmEntity.hasSnapshot(
                        ltmEntitySnapshot.id()
                            .timestamp)) // we only load the references if the snapshot is not existant
                {
                    armem::wm::EntitySnapshot s;
                    ltmEntitySnapshot.loadAllReferences(s);
                    wmEntity.addSnapshot(s);
                }
            });
    }
 
    void
    Entity::_loadLatestNReferences(int n, armem::wm::Entity& wmEntity)
    {
        std::lock_guard l(ltm_mutex);
 
        ARMARX_TRACE;
 
        wmEntity.id() = id().getEntityID().cleanID();
        int count = 0;
        ARMARX_DEBUG << "Entity::Load latestNReference";
        ARMARX_DEBUG << "EntityID=" << id().cleanID().str();
 
        //this is a little bit ugly, TODO: find an easier way to count the snapshots
        forEachSnapshot([&count](auto& entitySnapshot) { count++; });
 
        int current = 0;
 
        ARMARX_DEBUG << "Counted =" << count;
 
        forEachSnapshot(
            [&wmEntity, &n, &current, &count](auto& ltmEntitySnapshot)
            {
                if (!wmEntity.hasSnapshot(
                        ltmEntitySnapshot.id()
                            .timestamp)) // we only load the references if the snapshot is not existant
                {
                    if (current >= (count - n))
                    {
                        armem::wm::EntitySnapshot wmEntitySnapshot;
                        ltmEntitySnapshot.loadAllReferences(wmEntitySnapshot);
                        ltmEntitySnapshot.resolve(wmEntitySnapshot);
                        wmEntity.addSnapshot(wmEntitySnapshot);
                    }
                    else
                    {
                        ARMARX_DEBUG << "Skipping snapshot with timestamp " << ltmEntitySnapshot.id().timestamp;
                    }
                    current++;
                }
            });
    }
 
    void
    Entity::_resolve(armem::wm::Entity& wmEntity)
    {
        std::lock_guard l(ltm_mutex);
 
        ARMARX_TRACE;
 
        ARMARX_DEBUG << "Resolve entity id=" << id().cleanID().str();
 
        wmEntity.forEachSnapshot(
            [&](auto& wmEntitySnapshot)
            {
                // Just in case
                id().clearTimestamp();
                
                std::shared_ptr<persistence::MemoryPersistenceStrategy> entitySnapshotPersistenceStrategy(persistenceStrategy_);
 
                
                EntitySnapshot ltmEntitySnapshot(getExportName(),
                                 id().withTimestamp(wmEntitySnapshot.id().timestamp),
                                 processors,
                                 entitySnapshotPersistenceStrategy);
                ltmEntitySnapshot.resolve(wmEntitySnapshot);
            });
    }
 
    void
    Entity::_store(const armem::wm::Entity& wmEntity, bool simulatedVersion)
    {
        std::lock_guard l(ltm_mutex);
 
        ARMARX_TRACE;
 
        if (id().entityName.empty())
        {
            ARMARX_WARNING
                << "During storage of segment '" << wmEntity.id().str()
                << "' I noticed that the corresponding LTM has no id set. "
                << "I set the id of the LTM to the same name, however this should not happen!";
            id().entityName = wmEntity.id().entityName;
        }
 
        ARMARX_DEBUG << "Directly store entity";
 
 
        /*if (!connected())
        {
            ARMARX_WARNING << "LTM ENTITY NOT CONNECTED ALTHOUGH ENABLED " << id().str();
            return;
        }*/
 
        //writeForeignKeyToPreviousDocument();
 
        wmEntity.forEachSnapshot(
            [&](const auto& wmEntitySnapshot)
            {
                // Just in case
                id().clearTimestamp();
                
                std::shared_ptr<persistence::MemoryPersistenceStrategy> entitySnapshotPersistenceStrategy(persistenceStrategy_);
                
                EntitySnapshot ltmEntitySnapshot(getExportName(),
                                 id().withTimestamp(wmEntitySnapshot.id().timestamp),
                                 processors,
                                 entitySnapshotPersistenceStrategy);
 
                // check if snapshot already exists
                if (hasSnapshot(wmEntitySnapshot.id().timestamp))
                {
                    ARMARX_DEBUG << "Ignoring to put an EntitiySnapshot into the LTM because "
                                   "the timestamp already existed (we assume snapshots are "
                                   "const and do not change outside the ltm).";
                    return;
                }
 
                for (auto& filters : processors->snapFilters)
                {
                    bool accepted = filters->accept(wmEntitySnapshot, simulatedVersion);
 
                    if (!accepted)
                    {
                        ARMARX_DEBUG << "Ignoring to put an EntitySnapshot into the LTM because it "
                                        "got filtered.";
                        return;
                    }
                    else
                    {
                        //ARMARX_INFO << "Storing EntitySnapshot";
                    }
                }
 
                ltmEntitySnapshot.store(wmEntitySnapshot);
                statistics.recordedSnapshots++;
            });
    }
} // namespace armarx::armem::server::ltm