From fc97c2d629c725350a7ad471dcedfb25a0924a25 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 12 May 2021 08:26:38 +0200
Subject: [PATCH] new queries for snapshots: TimeApprox and BeforeTime
---
source/RobotAPI/interface/armem/query.ice | 35 ++++++++
.../libraries/armem/client/query/query_fns.h | 10 +++
.../armem/client/query/selectors.cpp | 16 ++++
.../libraries/armem/client/query/selectors.h | 3 +
source/RobotAPI/libraries/armem/core/Time.h | 2 +-
.../base/EntityQueryProcessorBase.h | 85 +++++++++++++++++++
6 files changed, 150 insertions(+), 1 deletion(-)
diff --git a/source/RobotAPI/interface/armem/query.ice b/source/RobotAPI/interface/armem/query.ice
index 3833b4444..ecd45446f 100644
--- a/source/RobotAPI/interface/armem/query.ice
+++ b/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;
+ }
}
diff --git a/source/RobotAPI/libraries/armem/client/query/query_fns.h b/source/RobotAPI/libraries/armem/client/query/query_fns.h
index 8dcb88991..40927ace4 100644
--- a/source/RobotAPI/libraries/armem/client/query/query_fns.h
+++ b/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);
+ };
+ }
+
}
diff --git a/source/RobotAPI/libraries/armem/client/query/selectors.cpp b/source/RobotAPI/libraries/armem/client/query/selectors.cpp
index f2bae7128..8b2aa11de 100644
--- a/source/RobotAPI/libraries/armem/client/query/selectors.cpp
+++ b/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()
{
diff --git a/source/RobotAPI/libraries/armem/client/query/selectors.h b/source/RobotAPI/libraries/armem/client/query/selectors.h
index 8bc2850b3..69fda84b5 100644
--- a/source/RobotAPI/libraries/armem/client/query/selectors.h
+++ b/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);
diff --git a/source/RobotAPI/libraries/armem/core/Time.h b/source/RobotAPI/libraries/armem/core/Time.h
index 328d1f59d..f76f7657d 100644
--- a/source/RobotAPI/libraries/armem/core/Time.h
+++ b/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".
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
index 447bd259a..9539618ac 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
+++ b/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)
{
--
GitLab