query beforeOrAt: tests + fix

source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h

@@ -169,20 +169,6 @@ 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);
-            }
-        }
 
         inline auto lastElementBeforeOrAt(const auto& history, const auto timestamp) const
         {
@@ -239,6 +225,22 @@ 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 beforeOrAt = lastElementBeforeOrAt(entity.history(), referenceTimestamp);
+
+            if (beforeOrAt != entity.history().end())
+            {
+                addResultSnapshot(result, beforeOrAt);
+            }
+        }
+
+
         void process(_EntityT& result,
                      const armem::query::data::entity::BeforeTime& query,
                      const _EntityT& entity) const

source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp

@@ -337,6 +337,52 @@ BOOST_AUTO_TEST_CASE(test_entity_BeforeTime_2)
 }
 
 
+
+/* BeforeOrAtTime */
+
+BOOST_AUTO_TEST_CASE(test_entity_BeforeOrAtTime_before)
+{
+    BOOST_REQUIRE_EQUAL(entity.size(), 5);
+    addResults(query::entity::BeforeOrAtTime{ 3500 });
+    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));
+    }
+}
+
+BOOST_AUTO_TEST_CASE(test_entity_BeforeOrAtTime_at)
+{
+    BOOST_REQUIRE_EQUAL(entity.size(), 5);
+    addResults(query::entity::BeforeOrAtTime{ 3000 });
+    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));
+    }
+}
+
+BOOST_AUTO_TEST_CASE(test_entity_BeforeOrAtTime_lookup_past)
+{
+    BOOST_REQUIRE_EQUAL(entity.size(), 5);
+    addResults(query::entity::BeforeOrAtTime{ 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(times.empty());
+    }
+}
+
 /* TimeApprox */
 
 /**