beforeTime query: fix + test

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);

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);