new queries for snapshots: TimeApprox and BeforeTime

source/RobotAPI/interface/armem/query.ice

@@ -49,6 +49,41 @@ 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 the timestamp.
+                     * 
+                     * This query is kind of similar to latest() but with a reference timestamp.
+                     * 
+                     */
+                    class BeforeTime extends EntityQuery
+                    {
+                        long timestamp;
+                    }
                 }
 
 

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

@@ -48,6 +48,14 @@ 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::BeforeTime*>(&query))
+            {
+                process(result, *q, entity);
+            }
             else
             {
                 throw armem::error::UnknownQueryType("entity snapshot", query);
@@ -152,6 +160,83 @@ namespace armarx::armem::base::query_proc
             }
         }
 
+        void process(_EntityT& result,
+                     const armem::query::data::entity::BeforeTime& query,
+                     const _EntityT& entity) const
+        {
+            const auto referenceTimestamp = fromIce<Time>(query.timestamp);
+
+            if (referenceTimestamp.toMicroSeconds() < 0)
+            {
+                return; // TODO throw or warn?
+            }
+
+            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::TimeApprox& query,
+                     const _EntityT& entity) const
+        {
+            const auto referenceTimestamp = fromIce<Time>(query.timestamp);
+            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;
+            };
+
+            if (referenceTimestamp.toMicroSeconds() < 0)
+            {
+                return; // TODO throw or warn?
+            }
+
+            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)
         {