From 0f226b069b8f71c90880bdc91f6edef80442d67d Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Fri, 14 May 2021 15:10:01 +0200
Subject: [PATCH] beforeTime query: fix + test
---
.../base/EntityQueryProcessorBase.h | 15 ++++---
.../libraries/armem/test/ArMemQueryTest.cpp | 43 +++++++++++++++++++
2 files changed, 53 insertions(+), 5 deletions(-)
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 dcc1fbf78..5633dc1fe 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
@@ -193,15 +193,20 @@ namespace armarx::armem::base::query_proc
const auto maxEntries = fromIce<std::int64_t>(query.maxEntries);
- const auto refIt = entity.history().lower_bound(referenceTimestamp);
-
- if (refIt == entity.history().end())
+ // 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())
{
ARMARX_WARNING << "No valid entities found.";
return;
}
- const auto nEntriesBefore = std::distance(entity.history().begin(), refIt);
+ const auto nEntriesBefore = std::distance(entity.history().begin(), refItFwdLt);
const int nEntries = [&]()
{
@@ -215,7 +220,7 @@ namespace armarx::armem::base::query_proc
}
();
- auto it = refIt;
+ auto it = refItFwdLt;
for (std::int64_t i = 0; i < nEntries; i++, --it)
{
addResultSnapshot(result, it);
diff --git a/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp
index 42d357b00..15e23b3d9 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp
@@ -294,6 +294,49 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_to_end)
}
+/* beforeTime */
+
+BOOST_AUTO_TEST_CASE(test_entity_BeforeTime_1)
+{
+ BOOST_REQUIRE_EQUAL(entity.size(), 5);
+ addResults(query::entity::BeforeTime{ 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(), 1);
+
+ BOOST_REQUIRE_EQUAL(times.front(), armem::Time::microSeconds(3000));
+ }
+
+}
+
+/* beforeTime */
+
+BOOST_AUTO_TEST_CASE(test_entity_BeforeTime_2)
+{
+ BOOST_REQUIRE_EQUAL(entity.size(), 5);
+ addResults(query::entity::BeforeTime{ 3500, 2});
+ 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(2000), armem::Time::microSeconds(3000)
+ };
+
+ BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
+ }
+
+}
+
+
+
BOOST_AUTO_TEST_CASE(test_negative_index_semantics)
{
BOOST_CHECK_EQUAL(EntityQueryProcessor::negativeIndexSemantics(0, 0), 0);
--
GitLab