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::_implForEachSnapshot(std::function<void(EntitySnapshot&)> func) const
    {
        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::_implForEachSnapshotInIndexRange(long first,
                                             long last,
                                             std::function<void(EntitySnapshot&)> func) const
    {
        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++; };
            _implForEachSnapshot(std::move(countFunc));  // Direct call to _impl, no locking!
 
            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 _implForEachSnapshot(std::move(runInIndexRangeFunc));  // Direct call to _impl!
    }
 
    bool
    Entity::_implForEachSnapshotInTimeRange(const Time& min,
                                            const Time& max,
                                            std::function<void(EntitySnapshot&)> func) const
    {
        ARMARX_TRACE;
 
        auto runInTimeRange = [&](EntitySnapshot& ltmEntitySnapshot)
        {
            Time ts = ltmEntitySnapshot.id().timestamp;
            if (ts >= min && ts <= max)
            {
                func(ltmEntitySnapshot);
            }
        };
 
        return _implForEachSnapshot(std::move(runInTimeRange));
    }
 
    bool
    Entity::_implForEachSnapshotBeforeOrAt(const Time& time,
                                           std::function<void(EntitySnapshot&)> func) const
    {
        ARMARX_TRACE;
 
        auto runBeforeOrAtFunc = [&](EntitySnapshot& ltmEntitySnapshot)
        {
            Time ts = ltmEntitySnapshot.id().timestamp;
            if (ts <= time)
            {
                func(ltmEntitySnapshot);
            }
        };
 
        return _implForEachSnapshot(std::move(runBeforeOrAtFunc));
    }
 
    bool
    Entity::_implForEachSnapshotBefore(const Time& time, std::function<void(EntitySnapshot&)> func) const
    {
        ARMARX_TRACE;
 
        auto runBeforeFunc = [&](EntitySnapshot& ltmEntitySnapshot)
        {
            Time ts = ltmEntitySnapshot.id().timestamp;
            if (ts < time)
            {
                func(ltmEntitySnapshot);
            }
        };
 
        return _implForEachSnapshot(std::move(runBeforeFunc));
    }
 
    bool
    Entity::_implForEachSnapshotBeforeReverse(const Time& time, std::function<bool(EntitySnapshot&)> func) const
    {
        ARMARX_TRACE;
 
        // Get all snapshot keys and sort them in reverse order (newest first)
        id().clearTimestamp();
        auto keys = persistenceStrategy_->getContainerKeys(id());
 
        // Parse and sort timestamps in descending order
        std::vector<Time> timestamps;
        timestamps.reserve(keys.size());
        for (const auto& key : keys)
        {
            Time ts = timeFromStringMicroSeconds(key);
            if (ts < time)
            {
                timestamps.push_back(ts);
            }
        }
        std::sort(timestamps.begin(), timestamps.end(), std::greater<Time>());
 
        // Iterate in reverse order (newest first), with early termination
        for (const Time& ts : timestamps)
        {
            std::shared_ptr<persistence::MemoryPersistenceStrategy> entitySnapshotPersistenceStrategy(persistenceStrategy_);
 
            EntitySnapshot ltmEntitySnapshot(
                getExportName(),
                id().withTimestamp(ts),
                processors,
                entitySnapshotPersistenceStrategy);
 
            if (!func(ltmEntitySnapshot))
            {
                break;  // Early termination
            }
        }
 
        return true;
    }
 
    bool
    Entity::_implHasSnapshot(const Time& snapshotTime) const
    {
        ARMARX_TRACE;
 
        // Just in case
        id().clearTimestamp();
 
        bool foundSnapshot = persistenceStrategy_->containsContainer(id(), std::to_string(snapshotTime.toMicroSecondsSinceEpoch()));
 
        return foundSnapshot;
    }
 
    std::shared_ptr<EntitySnapshot>
    Entity::_implFindSnapshot(const Time& snapshotTime) const
    {
        ARMARX_TRACE;
 
        if (!_implHasSnapshot(snapshotTime))  // Call _impl version since we're inside _impl method
        {
            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::_implFindLatestSnapshot() const
    {
        ARMARX_TRACE;
 
        Time bestMatch = Time::Invalid();
        auto runLatestFunc = [&](EntitySnapshot& ltmEntitySnapshot)
        {
            Time ts = ltmEntitySnapshot.id().timestamp;
            if (ts > bestMatch)
            {
                bestMatch = ts;
            }
        };
 
        _implForEachSnapshot(std::move(runLatestFunc));  // Call _impl version
 
        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::_implFindLatestSnapshotBefore(const Time& time) const
    {
        ARMARX_TRACE;
 
        Time bestMatch = Time::Invalid();
        auto findLatestSnapshotBeforeFunc = [&](EntitySnapshot& e)
        {
            auto ts = e.id().timestamp;
            if (ts < time && ts > bestMatch)
            {
                bestMatch = ts;
            }
        };
 
        _implForEachSnapshot(std::move(findLatestSnapshotBeforeFunc));  // Call _impl version
 
        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::_implFindLatestSnapshotBeforeOrAt(const Time& time) const
    {
        ARMARX_TRACE;
 
        Time bestMatch = Time::Invalid();
        auto findLatestSnapshotBeforeOrAtFunc = [&](EntitySnapshot& e)
        {
            auto ts = e.id().timestamp;
            if (ts <= time && ts > bestMatch)
            {
                bestMatch = ts;
            }
        };
 
        _implForEachSnapshot(std::move(findLatestSnapshotBeforeOrAtFunc));  // Call _impl version
 
        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::_implFindFirstSnapshotAfter(const Time& time) const
    {
        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;
            }
        };
 
        _implForEachSnapshot(std::move(findFirstSnapshotAfterFunc));  // Call _impl version
 
        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::_implFindFirstSnapshotAfterOrAt(const Time& time) const
    {
        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;
            }
        };
 
        _implForEachSnapshot(std::move(findFirstSnapshotAfterOrAtFunc));  // Call _impl version
 
        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)
    {
        ARMARX_TRACE;
 
        wmEntity.id() = id().getEntityID().cleanID();
 
        _implForEachSnapshot(  // Call _impl version
            [&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)
    {
        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
        _implForEachSnapshot([&count](auto& entitySnapshot) { count++; });  // Call _impl version
 
        int current = 0;
 
        ARMARX_DEBUG << "Counted =" << count;
 
        _implForEachSnapshot(  // Call _impl version
            [&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)
    {
        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)
    {
        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 (_implHasSnapshot(wmEntitySnapshot.id().timestamp))  // Call _impl version
                {
                    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;
                }
 
                // Note: Filter evaluation is now done in Memory::_preFilterMemory() before
                // snapshots are enqueued for async storage. This avoids duplicate filter
                // evaluation and issues with stateful filters (e.g., FrequencyFilter updating
                // its lastTimesPerEntity state twice for the same snapshot).
 
                ltmEntitySnapshot.store(wmEntitySnapshot);
                statistics.recordedSnapshots++;
            });
    }
} // namespace armarx::armem::server::ltm