diff --git a/source/RobotAPI/interface/armem/query.ice b/source/RobotAPI/interface/armem/query.ice index ecd45446f13ce403493698e636f64aeb3f9d8001..bfe0764272dbee12270de46cfe7cf0b90f851bb0 100644 --- a/source/RobotAPI/interface/armem/query.ice +++ b/source/RobotAPI/interface/armem/query.ice @@ -75,15 +75,31 @@ module armarx }; /** - * @brief Get the last snapshot before the timestamp. + * @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/server/query_proc/base/EntityQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h index 9539618ac99a8933a11bd75d11e9e6b4103a2944..c0d154d95c0db573cabe312015be9b1af5dd2313 100644 --- a/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h +++ b/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h @@ -2,6 +2,7 @@ #include <RobotAPI/interface/armem/query.h> +#include "ArmarXCore/core/exceptions/LocalException.h" #include "BaseQueryProcessorBase.h" #include <ArmarXCore/core/logging/Logging.h> @@ -52,6 +53,10 @@ namespace armarx::armem::base::query_proc { 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); @@ -160,20 +165,50 @@ 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<int64>(query.maxEntries); + + const auto refIt = entity.history().lower_bound(referenceTimestamp); - if (referenceTimestamp.toMicroSeconds() < 0) + if (refIt == entity.history().end()) { - return; // TODO throw or warn? + ARMARX_WARNING << "No valid entities found."; + return; } - const auto it = entity.history().lower_bound(referenceTimestamp); + const auto nEntriesBefore = std::distance(entity.history().begin(), refIt); - if (it != entity.history().end()) + if (nEntriesBefore < maxEntries) + { + // TODO maybe warn that not enough elements exist, but the user will notice anyways + ; + } + + const int nEntries = std::min(nEntriesBefore, maxEntries); + + auto it = refIt; + for (int64 i = 0; i < nEntries; i++, --it) { addResultSnapshot(result, it); } @@ -184,6 +219,8 @@ namespace armarx::armem::base::query_proc 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(); @@ -198,10 +235,6 @@ namespace armarx::armem::base::query_proc 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);