EntityBase.h

Go to the documentation of this file.

#pragma once
 
#include <map>
#include <string>
 
#include <SimoxUtility/algorithm/get_map_keys_values.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
 
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/Time.h>
 
#include "EntitySnapshotBase.h"
#include "detail/MemoryContainerBase.h"
#include "detail/iteration_mixins.h"
#include "detail/lookup_mixins.h"
#include "detail/negative_index_semantics.h"
 
namespace armarx::armem::base
{
 
    /**
     * @brief An entity over a period of time.
     *
     * An entity should be a physical thing or abstract concept existing
     * (and potentially evolving) over time.
     *
     * Examples are:
     * - objects (the green box)
     * - agents (robot, human)
     * - locations (frige, sink)
     * - grasp affordances (general, or for a specific object)
     * - images
     * - point clouds
     * - other sensory values
     *
     * At each point in time (`EntitySnapshot`), the entity can have a
     * (potentially variable) number of instances (`EntityInstance`),
     * each containing a single `AronData` object of a specific `AronType`.
     */
    template <class _EntitySnapshotT, class _Derived>
    class EntityBase :
        public detail::MemoryContainerBase<std::map<Time, _EntitySnapshotT>, _Derived>,
        public detail::ForEachEntityInstanceMixin<_Derived>,
        public detail::GetFindInstanceMixin<_Derived>,
        public detail::GetLatestInstanceMixin<_Derived>,
        public detail::GetLatestSnapshotMixin<_Derived>
    {
        using Base = detail::MemoryContainerBase<std::map<Time, _EntitySnapshotT>, _Derived>;
 
    public:
        using typename Base::ContainerT;
        using typename Base::DerivedT;
 
        using EntitySnapshotT = _EntitySnapshotT;
        using EntityInstanceT = typename EntitySnapshotT::EntityInstanceT;
 
        using ChildT = EntitySnapshotT;
 
        struct UpdateResult
        {
            armarx::armem::UpdateType entityUpdateType;
            MemoryID id;
            std::vector<EntitySnapshotT> removedSnapshots;
 
            UpdateResult() = default;
        };
 
    public:
        EntityBase()
        {
        }
 
        explicit EntityBase(const std::string& name, const MemoryID& parentID = {}) :
            EntityBase(parentID.withEntityName(name))
        {
        }
 
        explicit EntityBase(const MemoryID& id) : Base(id)
        {
        }
 
        EntityBase(const EntityBase& other) = default;
        EntityBase(EntityBase&& other) = default;
        EntityBase& operator=(const EntityBase& other) = default;
        EntityBase& operator=(EntityBase&& other) = default;
 
        // READING
 
        // Get key
        inline std::string&
        name()
        {
            return this->id().entityName;
        }
 
        inline const std::string&
        name() const
        {
            return this->id().entityName;
        }
 
        /// Indicate whether the entity has any snapshots.
        bool
        hasSnapshots() const
        {
            return not this->empty();
        }
 
        // Has child with key
        /// Indicate whether a snapshot at the given time exists.
        bool
        hasSnapshot(const Time& time) const
        {
            return this->findSnapshot(time) != nullptr;
        }
 
        // Has child with MemoryID
        /// Indicate whether a snapshot with the given ID exists.
        bool
        hasSnapshot(const MemoryID& snapshotID) const
        {
            return this->findSnapshot(snapshotID) != nullptr;
        }
 
        // Find child via key
        EntitySnapshotT*
        findSnapshot(const Time& timestamp)
        {
            return detail::findChildByKey(timestamp, this->_container);
        }
 
        const EntitySnapshotT*
        findSnapshot(const Time& timestamp) const
        {
            return detail::findChildByKey(timestamp, this->_container);
        }
 
        // Get child via key
        /**
         * @brief Get a snapshot.
         * @param time The time.
         * @return The snapshot, if it exists.
         *
         * @throws `armem::error::MissingEntry` If there is no such entry.
         */
        EntitySnapshotT&
        getSnapshot(const Time& time)
        {
            return detail::getChildByKey(time,
                                         this->_container,
                                         *this,
                                         [](const Time& time)
                                         { return toDateTimeMilliSeconds(time); });
        }
 
        const EntitySnapshotT&
        getSnapshot(const Time& time) const
        {
            return detail::getChildByKey(time,
                                         this->_container,
                                         *this,
                                         [](const Time& time)
                                         { return toDateTimeMilliSeconds(time); });
        }
 
        // Find child via MemoryID
        EntitySnapshotT*
        findSnapshot(const MemoryID& snapshotID)
        {
            detail::checkHasTimestamp(snapshotID);
            return this->findSnapshot(snapshotID.timestamp);
        }
 
        const EntitySnapshotT*
        findSnapshot(const MemoryID& snapshotID) const
        {
            detail::checkHasTimestamp(snapshotID);
            return this->findSnapshot(snapshotID.timestamp);
        }
 
        // Get child via MemoryID
        EntitySnapshotT&
        getSnapshot(const MemoryID& snapshotID)
        {
            detail::checkHasTimestamp(snapshotID);
            return this->getSnapshot(snapshotID.timestamp);
        }
 
        const EntitySnapshotT&
        getSnapshot(const MemoryID& snapshotID) const
        {
            detail::checkHasTimestamp(snapshotID);
            return this->getSnapshot(snapshotID.timestamp);
        }
 
        // get/findInstance are provided by GetFindInstanceMixin
 
 
        // More getter/finder for snapshots
 
        /**
         * @brief Get the latest timestamp.
         * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
         */
        Time
        getLatestTimestamp() const
        {
            return this->getLatestSnapshot().time();
        }
 
        /**
         * @brief Get the oldest timestamp.
         * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
         */
        Time
        getFirstTimestamp() const
        {
            return this->getFirstSnapshot().time();
        }
 
        /**
         * @brief Return the snapshot with the most recent timestamp.
         * @return The latest snapshot or nullptr if the entity is empty.
         */
        EntitySnapshotT*
        findLatestSnapshot()
        {
            return this->empty() ? nullptr : &this->_container.rbegin()->second;
        }
 
        const EntitySnapshotT*
        findLatestSnapshot() const
        {
            return this->empty() ? nullptr : &this->_container.rbegin()->second;
        }
 
        /**
         * @brief Return the snapshot with the least recent timestamp.
         * @return The first snapshot or nullptr if the entity is empty.
         */
        EntitySnapshotT*
        findFirstSnapshot()
        {
            return this->empty() ? nullptr : &this->_container.begin()->second;
        }
 
        const EntitySnapshotT*
        findFirstSnapshot() const
        {
            return this->empty() ? nullptr : &this->_container.begin()->second;
        }
 
        /**
         * @brief Return the snapshot with the least recent timestamp.
         * @return The first snapshot.
         * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
         */
        EntitySnapshotT&
        getFirstSnapshot()
        {
            return const_cast<EntitySnapshotT&>(
                const_cast<const EntityBase*>(this)->getFirstSnapshot());
        }
 
        const EntitySnapshotT&
        getFirstSnapshot() const
        {
            if (const EntitySnapshotT* snapshot = this->findFirstSnapshot())
            {
                return *snapshot;
            }
            else
            {
                throw armem::error::EntityHistoryEmpty(name(), "when getting the first snapshot.");
            }
        }
 
        /**
         * @brief Return the lastest snapshot before time.
         * @param time The time.
         * @return The latest snapshot < time or nullptr if none was found.
         */
        const EntitySnapshotT*
        findLatestSnapshotBefore(const Time& time) const
        {
            if (this->empty())
            {
                return nullptr;
            }
 
            // We want the rightmost element < time
            // lower_bound() gives us the the leftmost >= time, which is probably the right neighbour
            typename ContainerT::const_iterator greaterEq = this->_container.lower_bound(time);
            // Special cases:
            // refIt = begin() => no element < time
            if (greaterEq == this->_container.begin())
            {
                return nullptr;
            }
 
            // end(): No element >= time, we can still have one < time (rbegin()) => std::prev(end())
            // empty has been checked above
 
            // std::prev of end() is ok
            typename ContainerT::const_iterator less = std::prev(greaterEq);
 
            // we return the element before if possible
            return &less->second;
        }
 
        /**
         * @brief Return the latest snapshot before or at time.
         * @param time The time.
         * @return The latest snapshot <= time or nullptr if none was found.
         */
        const EntitySnapshotT*
        findLatestSnapshotBeforeOrAt(const Time& time) const
        {
            return findLatestSnapshotBefore(time + Duration::MicroSeconds(1));
        }
 
        /**
         * @brief Return first snapshot after or at time.
         * @param time The time.
         * @return The first snapshot >= time or nullptr if none was found.
         */
        const EntitySnapshotT*
        findFirstSnapshotAfterOrAt(const Time& time) const
        {
            // We want the leftmost element >= time.
            // That's lower bound.
            typename ContainerT::const_iterator greaterEq = this->_container.lower_bound(time);
 
            if (greaterEq == this->_container.end())
            {
                return nullptr;
            }
            return &greaterEq->second;
        }
 
        /**
         * @brief Return first snapshot after time.
         * @param time The time.
         * @return The first snapshot >= time or nullptr if none was found.
         */
        const EntitySnapshotT*
        findFirstSnapshotAfter(const Time& time) const
        {
            return findFirstSnapshotAfter(time - Duration::MicroSeconds(1));
        }
 
        auto*
        findLatestInstance(int instanceIndex = 0)
        {
            auto* snapshot = this->findLatestSnapshot();
            return snapshot ? snapshot->findInstance(instanceIndex) : nullptr;
        }
 
        const auto*
        findLatestInstance(int instanceIndex = 0) const
        {
            auto* snapshot = this->findLatestSnapshot();
            return snapshot ? snapshot->findInstance(instanceIndex) : nullptr;
        }
 
 
#if 0 // Do not offer this yet.
        auto* findLatestInstanceData(int instanceIndex = 0)
        {
            auto* instance = this->findLatestInstance(instanceIndex);
            return instance ? &instance->data() : nullptr;
        }
        const auto* findLatestInstanceData(int instanceIndex = 0) const
        {
            auto* instance = this->findLatestInstance(instanceIndex);
            return instance ? &instance->data() : nullptr;
        }
#endif
 
 
        // ITERATION
 
        /**
         * @param func Function like: bool process(EntitySnapshotT& snapshot)
         */
        template <class SnapshotFunctionT>
        bool
        forEachSnapshot(SnapshotFunctionT&& func)
        {
            return this->forEachChild(func);
        }
 
        /**
         * @param func Function like void process(const EntitySnapshotT& snapshot)
         */
        template <class SnapshotFunctionT>
        bool
        forEachSnapshot(SnapshotFunctionT&& func) const
        {
            return this->forEachChild(func);
        }
 
        /**
         * @param func Function like: bool process(EntitySnapshotT& snapshot)
         */
        template <class SnapshotFunctionT>
        bool
        forEachSnapshotIn(const MemoryID& id, SnapshotFunctionT&& func)
        {
            return this->forEachChild(func);
        }
 
        /**
         * @param func Function like void process(const EntitySnapshotT& snapshot)
         */
        template <class SnapshotFunctionT>
        bool
        forEachSnapshotIn(const MemoryID& id, SnapshotFunctionT&& func) const
        {
            if (id.hasTimestamp())
            {
                if (const auto* child = findSnapshot(id.timestamp))
                {
                    child->forEachChild(func);
                }
            }
            return this->forEachChild(func);
        }
 
        // forEachInstance() is provided by ForEachEntityInstanceMixin.
 
 
        /**
         * @brief Return all snapshots before (excluding) time.
         * @param time The time.
         * @return The latest snapshots.
         */
        template <class FunctionT>
        void
        forEachSnapshotBefore(const Time& time, FunctionT&& func) const
        {
            for (const auto& [timestamp, snapshot] : this->_container)
            {
                if (timestamp >= time)
                {
                    break;
                }
                if (not call(func, snapshot))
                {
                    break;
                }
            }
        }
 
        /**
         * @brief Return all snapshots before or at time.
         * @param time The time.
         * @return The latest snapshots.
         */
        template <class FunctionT>
        void
        forEachSnapshotBeforeOrAt(const Time& time, FunctionT&& func) const
        {
            getSnapshotsBefore(time + Duration::MicroSeconds(1), func);
        }
 
        /**
         * @brief Return all snapshots between, including, min and max.
         * @param min The lowest time to include.
         * @param min The highest time to include.
         * @return The snapshots in [min, max].
         */
        template <class FunctionT>
        void
        forEachSnapshotInTimeRange(const Time& min, const Time& max, FunctionT&& func) const
        {
            // Returns an iterator pointing to the first element that is not less than (i.e. greater or equal to) key.
            auto begin = min.toMicroSecondsSinceEpoch() > 0 ? this->_container.lower_bound(min)
                                                            : this->_container.begin();
            // Returns an iterator pointing to the first element that is *greater than* key.
            auto end = max.toMicroSecondsSinceEpoch() > 0 ? this->_container.upper_bound(max)
                                                          : this->_container.end();
 
            for (auto it = begin; it != end && it != this->_container.end(); ++it)
            {
                if (not call(func, it->second))
                {
                    break;
                }
            }
        }
 
        /**
         * @brief Return all snapshots from first to last index.
         *
         * Negative index are counted from the end, e.g.
         * last == -1 results in getting all queries until the end.
         *
         * @param first The first index to include.
         * @param first The last index to include.
         * @return The snapshots in [first, last].
         */
        template <class FunctionT>
        void
        forEachSnapshotInIndexRange(long first, long last, FunctionT&& func) const
        {
            if (this->empty())
            {
                return;
            }
 
            const size_t first_ = detail::negativeIndexSemantics(first, this->size());
            const size_t last_ = detail::negativeIndexSemantics(last, this->size());
 
            if (first_ <= last_)
            {
                auto it = this->_container.begin();
                std::advance(it, first_);
 
                size_t num = last_ - first_ + 1; // +1 to make last inclusive
                for (size_t i = 0; i < num; ++i, ++it)
                {
                    if (not call(func, it->second))
                    {
                        break;
                    }
                }
            }
        }
 
        /**
         * @param func Function like void process(EntityInstanceT& instance)>
         */
        template <class InstanceFunctionT>
        bool
        forEachInstanceIn(const MemoryID& id, InstanceFunctionT&& func)
        {
            return detail::forEachInstanceIn(id,
                                             func,
                                             *this,
                                             id.hasTimestamp(),
                                             id.hasTimestamp() ? this->findSnapshot(id.timestamp)
                                                               : nullptr);
        }
 
        /**
         * @param func Function like void process(EntityInstanceT& instance)>
         */
        template <class InstanceFunctionT>
        bool
        forEachInstanceIn(const MemoryID& id, InstanceFunctionT&& func) const
        {
            return detail::forEachInstanceIn(id,
                                             func,
                                             *this,
                                             id.hasTimestamp(),
                                             id.hasTimestamp() ? this->findSnapshot(id.timestamp)
                                                               : nullptr);
        }
 
        // Get child keys
        /// @brief Get all timestamps in the history.
        std::vector<Time>
        getTimestamps() const
        {
            return simox::alg::get_keys(this->_container);
        }
 
        // MODIFICATION
 
        /**
         * @brief Add the given update to this entity's history.
         * @param update The update.
         * @return The snapshot ID of the update.
         */
        UpdateResult
        update(const EntityUpdate& update)
        {
            this->_checkContainerName(update.entityID.entityName, this->name());
            UpdateResult ret;
 
            EntitySnapshotT* snapshot;
 
            auto it = this->_container.find(update.referencedTime);
            if (it == this->_container.end())
            {
                // Insert into history.
                snapshot = &addSnapshot(update.referencedTime);
                // ret.removedSnapshots = this->truncate();
                ret.entityUpdateType = UpdateType::InsertedNew;
            }
            else
            {
                snapshot = &it->second;
                ret.entityUpdateType = UpdateType::UpdatedExisting;
            }
            // Update entry.
            snapshot->update(update);
            ret.id = snapshot->id();
 
            return ret;
        }
 
        template <class OtherDerivedT>
        void
        append(const OtherDerivedT& other)
        {
            other.forEachSnapshot(
                [this](const auto& snapshot)
                {
                    auto it = this->_container.find(snapshot.time());
                    if (it == this->_container.end())
                    {
                        EntitySnapshotT copy{snapshot};
                        copy.id() = this->id().withTimestamp(
                            snapshot.time()); // update id (e.g. memory name) if necessary
                        this->_container.emplace(snapshot.time(), copy);
                    }
                    // else: snapshot already exists
                    // We assume that a snapshot does not change, so ignore
                    return true;
                });
        }
 
        /// Add a snapshot at the given time.
        EntitySnapshotT&
        addSnapshot(const Time& timestamp)
        {
            return this->addSnapshot(timestamp, EntitySnapshotT(timestamp));
        }
 
        /// Copy and insert a snapshot
        EntitySnapshotT&
        addSnapshot(const EntitySnapshotT& snapshot)
        {
            return this->addSnapshot(snapshot.time(), EntitySnapshotT(snapshot));
        }
 
        /// Move and insert a snapshot
        EntitySnapshotT&
        addSnapshot(EntitySnapshotT&& snapshot)
        {
            Time timestamp = snapshot.time(); // Copy before move.
            return this->addSnapshot(timestamp, std::move(snapshot));
        }
 
        /// Insert a snapshot in-place.
        template <class... Args>
        EntitySnapshotT&
        addSnapshot(const Time& timestamp, Args... args)
        {
            auto it = this->_container.emplace_hint(this->_container.end(), timestamp, args...);
            it->second.id() = this->id().withTimestamp(timestamp);
            return it->second;
        }
 
        // MISC
 
        bool
        equalsDeep(const DerivedT& other) const
        {
            //std::cout << "Entity::equalsDeep" << std::endl;
            if (this->size() != other.size())
            {
                return false;
            }
            for (const auto& [key, snapshot] : this->_container)
            {
                if (not other.hasSnapshot(key))
                {
                    return false;
                }
                if (not snapshot.equalsDeep(other.getSnapshot(key)))
                {
                    return false;
                }
            }
            return true;
        }
 
        std::string
        getKeyString() const
        {
            return this->id().entityName;
        }
 
        static std::string
        getLevelName()
        {
            return "entity";
        }
    };
 
} // namespace armarx::armem::base