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 5633dc1fe9e50290b1a655e9c1bc89b5ab1be21f..57cf3dc4ac8979bd466475745d7102dd49658a27 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
@@ -184,6 +184,61 @@ namespace armarx::armem::base::query_proc
}
}
+ inline auto lastElementBeforeOrAt(const auto& history, const auto timestamp) const
+ {
+ // first element equal or greater
+ typename std::map<Time, EntitySnapshotT>::const_iterator refItFwd = history.upper_bound(timestamp);
+
+ // last element less than
+ typename std::map<Time, EntitySnapshotT>::const_iterator refItFwdLt = std::prev(refItFwd);
+
+ // last element not greater than => if this is invalid, we have no suitable elements
+ typename std::map<Time, EntitySnapshotT>::const_reverse_iterator refIt(refItFwd);
+ if (refIt == history.rend())
+ {
+ return history.end();
+ }
+
+ // now either refItFwd is a perfect match ...
+ if (refItFwd->first == timestamp)
+ {
+ return refItFwd;
+ }
+
+ // ... or we return the element before if possible
+ if (refItFwdLt != history.begin())
+ {
+ return refItFwdLt;
+ }
+
+ return history.end();
+ }
+
+ inline auto lastElementBefore(const auto& history, const auto& timestamp) const
+ {
+ // first element equal or greater
+ typename std::map<Time, EntitySnapshotT>::const_iterator refItFwd = history.upper_bound(timestamp);
+
+ // last element less than
+ typename std::map<Time, EntitySnapshotT>::const_iterator refItFwdLt = std::prev(refItFwd);
+
+ // last element not greater than => if this is invalid, we have no suitable elements
+ typename std::map<Time, EntitySnapshotT>::const_reverse_iterator refIt(refItFwd);
+ if (refIt == history.rend())
+ {
+ return history.end();
+ }
+
+ // we return the element before if possible
+ if (refItFwdLt != history.begin())
+ {
+ return refItFwdLt;
+ }
+
+ return history.end();
+ }
+
+
void process(_EntityT& result,
const armem::query::data::entity::BeforeTime& query,
const _EntityT& entity) const
@@ -193,20 +248,14 @@ namespace armarx::armem::base::query_proc
const auto maxEntries = fromIce<std::int64_t>(query.maxEntries);
- // first element equal or greater
- typename std::map<Time, EntitySnapshotT>::const_iterator refItFwd = entity.history().upper_bound(referenceTimestamp);
- // last element not greater than
- typename std::map<Time, EntitySnapshotT>::const_iterator refItFwdLt = std::prev(refItFwd);
-
- // last element not greater than
- typename std::map<Time, EntitySnapshotT>::const_reverse_iterator refIt(refItFwd);
- if (refIt == entity.history().rend())
+ const auto itBefore = lastElementBefore(entity.history(), referenceTimestamp);
+ if (itBefore == entity.history().end())
{
- ARMARX_WARNING << "No valid entities found.";
+ ARMARX_WARNING << "No suitable element found";
return;
}
- const auto nEntriesBefore = std::distance(entity.history().begin(), refItFwdLt);
+ const auto nEntriesBefore = std::distance(entity.history().begin(), itBefore);
const int nEntries = [&]()
{
@@ -220,7 +269,7 @@ namespace armarx::armem::base::query_proc
}
();
- auto it = refItFwdLt;
+ auto it = itBefore;
for (std::int64_t i = 0; i < nEntries; i++, --it)
{
addResultSnapshot(result, it);
@@ -231,6 +280,7 @@ namespace armarx::armem::base::query_proc
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!";
@@ -248,24 +298,34 @@ namespace armarx::armem::base::query_proc
return std::abs(t.toMicroSeconds() - referenceTimestampMicroSeconds) <= epsDuration;
};
+ // first element before or at timestamp
+ const auto beforeOrAt = lastElementBeforeOrAt(entity.history(), referenceTimestamp);
+ const bool isBeforeOrAtValid = beforeOrAt != entity.history().end();
+ const auto isPerfectMatch = isBeforeOrAtValid && beforeOrAt->first == referenceTimestamp;
- const auto beforeOrAt = entity.history().lower_bound(referenceTimestamp);
+ // first element greater
const auto after = entity.history().upper_bound(referenceTimestamp);
+ const bool isAfterValid = after != entity.history().end();
- 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)
+ // if both are invalid, there is nothing to be gained here.
+ if ((not isBeforeOrAtValid) and (not isAfterValid))
{
const armem::Time dt = referenceTimestamp - entity.getLatestTimestamp();
ARMARX_WARNING << "Lookup " << dt.toMilliSeconds() << " ms into the future.";
return;
+ }
+ // -> now one or both are valid ...
+
+ // is 'before' a perfect match?
+ if(isPerfectMatch)
+ {
+ addResultSnapshot(result, beforeOrAt);
+ return;
}
- // only 'before' valid? or is 'before' perfect match?
- if ((not isAfterValid) or (beforeOrAt->first == referenceTimestamp))
+ // only 'before' valid?
+ if (not isAfterValid)
{
if (isInRange(beforeOrAt->first))
{
@@ -273,26 +333,29 @@ namespace armarx::armem::base::query_proc
}
return;
}
+
+ // only 'after' valid?
+ if (not isBeforeOrAtValid)
+ {
+ if (isInRange(after->first))
+ {
+ addResultSnapshot(result, after);
+ }
+ return;
+ }
// -> now both are valid
- // return both => user can interpolate
+ // return both (if in range) => user can interpolate
if (isInRange(beforeOrAt->first))
{
addResultSnapshot(result, beforeOrAt);
}
- else
- {
- ARMARX_WARNING << "No valid entity before timestamp";
- }
if (isInRange(after->first))
{
addResultSnapshot(result, after);
}
- else
- {
- ARMARX_WARNING << "No valid entity after timestamp";
- }
+
}
diff --git a/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp
index 15e23b3d942d1e6363ed7ff2d3ab63f140757ff9..4a14aa7fce29f5262f3d4eeb7080f2d6986c27f2 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp
@@ -20,6 +20,7 @@
* GNU General Public License
*/
+#include <RobotAPI/interface/armem/query.h>
#define BOOST_TEST_MODULE RobotAPI::ArmarXLibraries::armem
#define ARMARX_BOOST_TEST
@@ -294,7 +295,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_to_end)
}
-/* beforeTime */
+/* BeforeTime */
BOOST_AUTO_TEST_CASE(test_entity_BeforeTime_1)
{
@@ -312,8 +313,6 @@ BOOST_AUTO_TEST_CASE(test_entity_BeforeTime_1)
}
-/* beforeTime */
-
BOOST_AUTO_TEST_CASE(test_entity_BeforeTime_2)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
@@ -336,6 +335,110 @@ BOOST_AUTO_TEST_CASE(test_entity_BeforeTime_2)
}
+/* TimeApprox */
+
+BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_no_limit)
+{
+ BOOST_REQUIRE_EQUAL(entity.size(), 5);
+ addResults(query::entity::TimeApprox{ 3500, -1});
+ BOOST_REQUIRE_EQUAL(results.size(), 2);
+
+ for (const auto& result : results)
+ {
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ BOOST_REQUIRE_EQUAL(times.size(), 2);
+
+ std::vector<armem::Time> expected
+ {
+ armem::Time::microSeconds(3000), armem::Time::microSeconds(4000)
+ };
+
+ BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
+ }
+
+}
+
+
+BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_600)
+{
+ BOOST_REQUIRE_EQUAL(entity.size(), 5);
+ addResults(query::entity::TimeApprox{ 3500, 600});
+ BOOST_REQUIRE_EQUAL(results.size(), 2);
+
+ for (const auto& result : results)
+ {
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ BOOST_REQUIRE_EQUAL(times.size(), 2);
+
+ std::vector<armem::Time> expected
+ {
+ armem::Time::microSeconds(3000), armem::Time::microSeconds(4000)
+ };
+
+ BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
+ }
+
+}
+
+
+BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_too_small)
+{
+ BOOST_REQUIRE_EQUAL(entity.size(), 5);
+ addResults(query::entity::TimeApprox{ 3500, 100});
+ BOOST_REQUIRE_EQUAL(results.size(), 2);
+
+ for (const auto& result : results)
+ {
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ BOOST_REQUIRE_EQUAL(times.size(), 0);
+ }
+
+}
+
+
+BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_only_next)
+{
+ BOOST_REQUIRE_EQUAL(entity.size(), 5);
+ addResults(query::entity::TimeApprox{ 3700, 400});
+ BOOST_REQUIRE_EQUAL(results.size(), 2);
+
+ for (const auto& result : results)
+ {
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ BOOST_REQUIRE_EQUAL(times.size(), 1);
+
+ std::vector<armem::Time> expected
+ {
+ armem::Time::microSeconds(4000)
+ };
+
+ BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
+ }
+
+}
+
+BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_only_previous)
+{
+ BOOST_REQUIRE_EQUAL(entity.size(), 5);
+ addResults(query::entity::TimeApprox{ 3300, 400});
+ BOOST_REQUIRE_EQUAL(results.size(), 2);
+
+ for (const auto& result : results)
+ {
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ BOOST_REQUIRE_EQUAL(times.size(), 1);
+
+ std::vector<armem::Time> expected
+ {
+ armem::Time::microSeconds(3000)
+ };
+
+ BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
+ }
+
+}
+
+
BOOST_AUTO_TEST_CASE(test_negative_index_semantics)
{