Merge branch 'feature/armem-new-queries' into 'armem/dev'

new queries for snapshots: TimeApprox and BeforeTime

See merge request ArmarX/RobotAPI!147

source/RobotAPI/interface/armem/query.ice

@@ -49,6 +49,57 @@ module armarx
                         long first = 0;  ///< First index to get.
                         long last = -1;  ///< Last index to get (inclusive).
                     };
+
+
+                    /**
+                     * @brief Get snapshot(s) around timestamp.
+                     *
+                     * If there is a snapshot which exactly matches the query timestamp,
+                     * the behavior is as for the @see Single query. 
+                     * 
+                     * However, if there is no matching timestamp, the snapshots before 
+                     * and after the query timestamp will be returned.
+                     * 
+                     * If #eps is positive, the snapshots must be within the time
+                     * range [timestamp - eps, timestamp + eps]. 
+                     * Consequently, the query result can be empty.
+                     *
+                     * Hint: 
+                     *   This query can be quite useful when interpolating snapshots
+                     *   is possible.
+                     */
+                    class TimeApprox extends EntityQuery
+                    {
+                        long timestamp = -1;
+                        long eps = -1;
+                    };
+
+                    /**
+                     * @brief Get the last snapshot before or at the timestamp.
+                     * 
+                     * This query is kind of similar to latest() but with a reference timestamp.
+                     * 
+                     */
+                    class BeforeOrAtTime extends EntityQuery
+                    {
+                        long timestamp;
+                    }
+
+                    /**
+                     * @brief Get the last snapshot before the timestamp or a sequence of 
+                     *           n last elements before the timestamp depending on #maxEntries
+                     * 
+                     * Depending on #maxEntries, the behavior is as follows:
+                     *  - #maxEntries == 1 => last element before timestamp
+                     *  - #maxEntries > 1  => n last elements before timestamp
+                     *  - #maxEntries < 0  => all elements before timestamp
+                     */
+                    class BeforeTime extends EntityQuery
+                    {
+                        long timestamp;
+                        long maxEntries = 1;
+                    }
+
                 }
 
 

source/RobotAPI/libraries/armem/client/query/query_fns.h

@@ -134,4 +134,14 @@ namespace armarx::armem::client::query_fns
         };
     }
 
+    inline
+    std::function<void(query::SnapshotSelector&)>
+    atTimeApprox(Time time, Duration eps)
+    {
+        return [ = ](query::SnapshotSelector & selector)
+        {
+            selector.atTimeApprox(time, eps);
+        };
+    }
+
 }

source/RobotAPI/libraries/armem/client/query/selectors.cpp

@@ -42,6 +42,15 @@ namespace armarx::armem::client::query
         return *this;
     }
 
+    SnapshotSelector& SnapshotSelector::atTimeApprox(Time timestamp, Duration eps)
+    {
+        auto& q = _addQuery<dq::entity::TimeApprox>();
+        toIce(q.timestamp, timestamp);
+        toIce(q.eps, eps);
+        return *this;
+    }
+
+
     SnapshotSelector& SnapshotSelector::indexRange(long first, long last)
     {
         auto& q = _addQuery<dq::entity::IndexRange>();
@@ -51,6 +60,13 @@ namespace armarx::armem::client::query
     }
 
 
+    SnapshotSelector& SnapshotSelector::beforeTimestamp(Time timestamp)
+    {
+        auto& q = _addQuery<dq::entity::BeforeTime>();
+        toIce(q.timestamp, timestamp);
+        return *this;
+    }
+
 
     SnapshotSelector& EntitySelector::snapshots()
     {

source/RobotAPI/libraries/armem/client/query/selectors.h

@@ -26,6 +26,9 @@ namespace armarx::armem::client::query
 
         SnapshotSelector& latest();
         SnapshotSelector& atTime(Time timestamp);
+        SnapshotSelector& atTimeApprox(Time timestamp, Duration eps);
+        SnapshotSelector& beforeTimestamp(Time timestamp);
+
 
         SnapshotSelector& timeRange(Time min, Time max);
         SnapshotSelector& indexRange(long first, long last);

source/RobotAPI/libraries/armem/core/Time.h

@@ -7,8 +7,8 @@
 
 namespace armarx::armem
 {
-
     using Time = IceUtil::Time;
+    using Duration = IceUtil::Time;
 
     /**
      * @brief Returns `time` as e.g. "123456789.012 ms".

source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h

 #pragma once
 
-#include <RobotAPI/interface/armem/query.h>
+#include <cstdint>
+#include <iterator>
 
-#include "BaseQueryProcessorBase.h"
+#include <RobotAPI/interface/armem/query.h>
 
+#include <ArmarXCore/core/exceptions/LocalException.h>
 #include <ArmarXCore/core/logging/Logging.h>
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
 #include <RobotAPI/libraries/armem/core/error.h>
 #include <RobotAPI/libraries/armem/core/ice_conversions.h>
 
+#include "BaseQueryProcessorBase.h"
+
 
 namespace armarx::armem::base::query_proc
 {
@@ -48,6 +52,18 @@ namespace armarx::armem::base::query_proc
             {
                 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);
@@ -152,6 +168,115 @@ namespace armarx::armem::base::query_proc
             }
         }
 
+        void process(_EntityT& result,
+                     const armem::query::data::entity::BeforeOrAtTime& query,
+                     const _EntityT& entity) const
+        {
+            const auto referenceTimestamp = fromIce<Time>(query.timestamp);
+            ARMARX_CHECK(referenceTimestamp.toMicroSeconds() >= 0) << "Reference timestamp is negative!";
+
+            const auto it = entity.history().lower_bound(referenceTimestamp);
+
+            if (it != entity.history().end())
+            {
+                addResultSnapshot(result, it);
+            }
+        }
+
+        void process(_EntityT& result,
+                     const armem::query::data::entity::BeforeTime& query,
+                     const _EntityT& entity) const
+        {
+            const auto referenceTimestamp = fromIce<Time>(query.timestamp);
+            ARMARX_CHECK(referenceTimestamp.toMicroSeconds() >= 0) << "Reference timestamp is negative!";
+
+            const auto maxEntries = fromIce<std::int64_t>(query.maxEntries);
+
+            const auto refIt = entity.history().lower_bound(referenceTimestamp);
+
+            if (refIt == entity.history().end())
+            {
+                ARMARX_WARNING << "No valid entities found.";
+                return;
+            }
+
+            const auto nEntriesBefore = std::distance(entity.history().begin(), refIt);
+
+            const int nEntries = [&]()
+            {
+                // see query.ice
+                if (maxEntries > 0)
+                {
+                    return std::min(nEntriesBefore, maxEntries);
+                }
+
+                return nEntriesBefore; // all elements before timestamp
+            }
+            ();
+
+            auto it = refIt;
+            for (std::int64_t i = 0; i < nEntries; i++, --it)
+            {
+                addResultSnapshot(result, it);
+            }
+        }
+
+        void process(_EntityT& result,
+                     const armem::query::data::entity::TimeApprox& query,
+                     const _EntityT& entity) const
+        {
+            const auto referenceTimestamp = fromIce<Time>(query.timestamp);
+            ARMARX_CHECK(referenceTimestamp.toMicroSeconds() >= 0) << "Reference timestamp is negative!";
+
+            const auto referenceTimestampMicroSeconds = referenceTimestamp.toMicroSeconds();
+            const auto epsDuration = fromIce<Time>(query.eps).toMicroSeconds();
+
+            // elements have to be in range [t_ref - eps, t_ref + eps] if eps is positive
+            const auto isInRange = [&](const Time & t) -> bool
+            {
+                if (epsDuration <= 0)
+                {
+                    return true;
+                }
+
+                return std::abs(t.toMicroSeconds() - referenceTimestampMicroSeconds) <= epsDuration;
+            };
+
+
+            const auto beforeOrAt = entity.history().lower_bound(referenceTimestamp);
+            const auto after = entity.history().upper_bound(referenceTimestamp);
+
+            const bool isBeforeOrAtValid = beforeOrAt != entity.history().end();
+            const bool isAfterValid = isBeforeOrAtValid && after != entity.history().end();
+
+            // if 'before' is already invalid, there is nothing to be gained here.
+            if (not isBeforeOrAtValid)
+            {
+                return;
+            }
+
+            // only 'before' valid? or is 'before' perfect match?
+            if ((not isAfterValid) or (beforeOrAt->first == referenceTimestamp))
+            {
+                if (isInRange(beforeOrAt->first))
+                {
+                    addResultSnapshot(result, beforeOrAt);
+                }
+                return;
+            }
+            // -> now both are valid
+
+            // return both => user can interpolate
+            if (isInRange(beforeOrAt->first))
+            {
+                addResultSnapshot(result, beforeOrAt);
+            }
+            if (isInRange(after->first))
+            {
+                addResultSnapshot(result, after);
+            }
+        }
+
 
         static size_t negativeIndexSemantics(long index, size_t size)
         {