diff --git a/source/RobotAPI/interface/armem/query.ice b/source/RobotAPI/interface/armem/query.ice
index 3833b4444736bc1f78fdf9c34d1c5d6ea4acfc8c..bfe0764272dbee12270de46cfe7cf0b90f851bb0 100644
--- a/source/RobotAPI/interface/armem/query.ice
+++ b/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;
+ }
+
}
diff --git a/source/RobotAPI/libraries/armem/client/query/query_fns.h b/source/RobotAPI/libraries/armem/client/query/query_fns.h
index 8dcb88991450b95583c7228b2349fdfdf05cba95..40927ace4f6af3602b426689e82439c09507b666 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 f2bae712818b566ef51444aafb84af6031d338d0..8b2aa11dea44ad42cad732b76c65c87eb1cc7ad2 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 8bc2850b390b394a5d2b5fc07753bb846f598416..69fda84b54cbfc3c0c0395e23d33ebfebd7d9d1b 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 328d1f59d793cbcc9a7d0febc0951775d200013a..f76f7657dda6e14a9c35bb4ad5edf58471680144 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 447bd259a2c24a3ca80c91c59a518daed56c18f1..9b85847791e21aee0031e40b551b2fceff1e9547 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
@@ -1,15 +1,19 @@
#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)
{