EntityQueryProcessorBase.h

Go to the documentation of this file.

#pragma once
 
#include <cstdint>
#include <iterator>
#include <limits>
 
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
#include <ArmarXCore/core/time/ice_conversions.h>
 
#include <RobotAPI/interface/armem/query.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/ice_conversions.h>
 
#include "BaseQueryProcessorBase.h"
 
namespace armarx::armem::server::query_proc::base::detail
{
    void checkReferenceTimestampNonNegative(const Time& timestamp);
}
 
namespace armarx::armem::server::query_proc::base
{
 
    template <class _EntityT, class _ResultEntityT>
    class EntityQueryProcessorBase :
        public BaseQueryProcessorBase<_EntityT, _ResultEntityT, armem::query::data::EntityQuery>
    {
    protected:
        using Base =
            BaseQueryProcessorBase<_EntityT, _ResultEntityT, armem::query::data::EntityQuery>;
 
    public:
        using EntityT = _EntityT;
        using EntitySnapshotT = typename EntityT::EntitySnapshotT;
 
        using ResultEntityT = _ResultEntityT;
        using ResultSnapshotT = typename ResultEntityT::EntitySnapshotT;
 
 
    public:
        using Base::process;
 
        virtual void
        process(ResultEntityT& result,
                const armem::query::data::EntityQuery& query,
                const EntityT& entity) const override
        {
            if (auto q = dynamic_cast<const armem::query::data::entity::All*>(&query))
            {
                process(result, *q, entity);
            }
            else if (auto q = dynamic_cast<const armem::query::data::entity::Single*>(&query))
            {
                process(result, *q, entity);
            }
            else if (auto q = dynamic_cast<const armem::query::data::entity::TimeRange*>(&query))
            {
                process(result, *q, entity);
            }
            else if (auto q = dynamic_cast<const armem::query::data::entity::IndexRange*>(&query))
            {
                process(result, *q, entity);
            }
            else if (auto q = dynamic_cast<const armem::query::data::entity::TimeApprox*>(&query))
            {
                process(result, *q, entity);
            }
            else if (auto q =
                         dynamic_cast<const armem::query::data::entity::BeforeOrAtTime*>(&query))
            {
                process(result, *q, entity);
            }
            else if (auto q = dynamic_cast<const armem::query::data::entity::BeforeTime*>(&query))
            {
                process(result, *q, entity);
            }
            else
            {
                throw armem::error::UnknownQueryType("entity snapshot", query);
            }
        }
 
        virtual void
        process(ResultEntityT& result,
                const armem::query::data::entity::All& query,
                const EntityT& entity) const
        {
            (void)query;
            // Copy this entitiy and its contents.
 
            entity.forEachSnapshot([this, &result](const EntitySnapshotT& snapshot)
                                   { this->addResultSnapshot(result, snapshot); });
        }
 
        virtual void
        process(ResultEntityT& result,
                const armem::query::data::entity::Single& query,
                const EntityT& entity) const
        {
            if (query.timestamp.timeSinceEpoch.microSeconds < 0)
            {
                if (auto snapshot = entity.findLatestSnapshot())
                {
                    this->addResultSnapshot(result, *snapshot);
                }
            }
            else
            {
                const Time time = armarx::fromIce<Time>(query.timestamp);
                if (auto snapshot = entity.findSnapshot(time))
                {
                    this->addResultSnapshot(result, *snapshot);
                }
                else
                {
                    // Leave empty.
#if 0
                    std::stringstream ss;
                    ss << "Failed to retrieve snapshot with timestamp "
                       << armem::toDateTimeMilliSeconds(time)
                       << " from entity " << entity.id() << ".\n"
                       << "Entity has timestamps: ";
                    for (const Time& t : entity.getTimestamps())
                    {
                        ss << "\n- " << armem::toDateTimeMilliSeconds(t);
                    }
                    ARMARX_IMPORTANT << ss.str();
#endif
                }
            }
        }
 
        virtual void
        process(ResultEntityT& result,
                const armem::query::data::entity::TimeRange& query,
                const EntityT& entity) const
        {
            if (query.minTimestamp.timeSinceEpoch.microSeconds <=
                    query.maxTimestamp.timeSinceEpoch.microSeconds ||
                query.minTimestamp.timeSinceEpoch.microSeconds < 0 ||
                query.maxTimestamp.timeSinceEpoch.microSeconds < 0)
            {
                const Time min = armarx::fromIce<Time>(query.minTimestamp);
                const Time max = armarx::fromIce<Time>(query.maxTimestamp);
                process(result, min, max, entity, query);
            }
        }
 
        virtual void
        process(ResultEntityT& result,
                const armem::query::data::entity::IndexRange& query,
                const EntityT& entity) const
        {
            entity.forEachSnapshotInIndexRange(query.first,
                                               query.last,
                                               [this, &result](const EntitySnapshotT& snapshot)
                                               { this->addResultSnapshot(result, snapshot); });
        }
 
        virtual void
        process(ResultEntityT& result,
                const Time& min,
                const Time& max,
                const EntityT& entity,
                const armem::query::data::EntityQuery& query) const
        {
            (void)query;
            entity.forEachSnapshotInTimeRange(min,
                                              max,
                                              [this, &result](const EntitySnapshotT& snapshot)
                                              { this->addResultSnapshot(result, snapshot); });
        }
 
        virtual void
        process(ResultEntityT& result,
                const armem::query::data::entity::BeforeOrAtTime& query,
                const EntityT& entity) const
        {
            const Time referenceTimestamp = armarx::fromIce<Time>(query.timestamp);
            base::detail::checkReferenceTimestampNonNegative(referenceTimestamp);
 
            if (auto beforeOrAt = entity.findLatestSnapshotBeforeOrAt(referenceTimestamp))
            {
                this->addResultSnapshot(result, *beforeOrAt);
            }
        }
 
        virtual void
        process(ResultEntityT& result,
                const armem::query::data::entity::BeforeTime& query,
                const EntityT& entity) const
        {
            const Time referenceTimestamp = armarx::fromIce<Time>(query.timestamp);
            base::detail::checkReferenceTimestampNonNegative(referenceTimestamp);
 
            // Collect snapshots in reverse order (newest first) with early termination
            std::vector<const EntitySnapshotT*> snapshots;
            const size_t maxEntries = (query.maxEntries < 0)
                                          ? std::numeric_limits<size_t>::max()
                                          : static_cast<size_t>(query.maxEntries);
 
            entity.forEachSnapshotBeforeReverse(
                referenceTimestamp,
                [&snapshots, maxEntries](const EntitySnapshotT& s)
                {
                    snapshots.push_back(&s);
                    return snapshots.size() < maxEntries; // Stop when we have enough
                });
 
            // Add in chronological order (reverse of what we collected)
            for (auto it = snapshots.rbegin(); it != snapshots.rend(); ++it)
            {
                this->addResultSnapshot(result, **it);
            }
        }
 
        virtual void
        process(ResultEntityT& result,
                const armem::query::data::entity::TimeApprox& query,
                const EntityT& entity) const
        {
            const Time referenceTimestamp = armarx::fromIce<Time>(query.timestamp);
            base::detail::checkReferenceTimestampNonNegative(referenceTimestamp);
 
            // elements have to be in range [t_ref - eps, t_ref + eps] if eps is positive
            const auto isInRange = [&](const Time& t) -> bool
            {
                return query.eps.microSeconds <= 0 or
                       std::abs((t - referenceTimestamp).toMicroSeconds()) <=
                           query.eps.microSeconds;
            };
 
            // last element before or at timestamp
            if (auto beforeOrAt = entity.findLatestSnapshotBeforeOrAt(referenceTimestamp))
            {
                const auto timestampOfMatchBefore = beforeOrAt->id().timestamp;
                const auto isPerfectMatch = timestampOfMatchBefore == referenceTimestamp;
                if (isInRange(timestampOfMatchBefore))
                {
                    this->addResultSnapshot(result, *beforeOrAt);
                }
 
                // earsly stop, not necessary to also get the next since the match is perfect
                if (isPerfectMatch)
                {
                    return;
                }
 
                // first element after or at timestamp (or at because of fewer checks, we can assure that there is not element at)
                const auto after = entity.findFirstSnapshotAfterOrAt(referenceTimestamp);
                if (after)
                {
                    const auto timestampOfMatchAfter = after->id().timestamp;
                    if (isInRange(timestampOfMatchAfter))
                    {
                        this->addResultSnapshot(result, *after);
                    }
                }
            }
        }
 
 
    protected:
        virtual void addResultSnapshot(ResultEntityT& result,
                                       const EntitySnapshotT& snapshot) const = 0;
    };
} // namespace armarx::armem::server::query_proc::base