Merge branch 'armem/dev'

Conflicts:
	source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp

source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp

@@ -19,6 +19,7 @@
 #include <ArmarXCore/core/time/CycleUtil.h>
 #include <ArmarXCore/core/logging/Logging.h>
 #include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
 
 #include <RobotAPI/libraries/armem/client/query/Builder.h>
 #include <RobotAPI/libraries/armem/client/query/query_fns.h>
@@ -39,6 +40,9 @@ namespace armarx::robot_state
 
         defs->topic(debugObserver);
 
+        defs->optional(p.robotName, "robotName");
+        defs->optional(p.updateFrequency, "updateFrequency");
+
         virtualRobotReader.registerPropertyDefinitions(defs);
 
         return defs;
@@ -55,7 +59,9 @@ namespace armarx::robot_state
     {
         virtualRobotReader.connect();
 
-        task = new RunningTask<VirtualRobotReaderExampleClient>(this, &VirtualRobotReaderExampleClient::run);
+        ARMARX_IMPORTANT << "Running virtual robot synchronization example.";
+
+        task = new PeriodicTask<VirtualRobotReaderExampleClient>(this, &VirtualRobotReaderExampleClient::run, 1000 / p.updateFrequency);
         task->start();
     }
 
@@ -68,44 +74,32 @@ namespace armarx::robot_state
 
     void VirtualRobotReaderExampleClient::run()
     {
-        ARMARX_IMPORTANT << "Running virtual robot synchronization example.";
-
-        std::shared_ptr<VirtualRobot::Robot> virtualRobot{nullptr};
 
-        CycleUtil cycle(IceUtil::Time::milliSeconds(100));
-        IceUtil::Time start = TimeUtil::GetTime();
+        // initialize if needed
+        if (virtualRobot == nullptr)
+        {
+            TIMING_START(getRobot);
 
-        CycleUtil c(100);
+            virtualRobot = virtualRobotReader.getRobot(p.robotName, IceUtil::Time::now());
 
-        while (not task->isStopped())
-        {
-            // initialize
-            if(virtualRobot == nullptr)
+            if (virtualRobot == nullptr)
             {
-                TIMING_START(getRobot);
-
-                virtualRobot = virtualRobotReader.getRobot("Armar6", IceUtil::Time::now());
-
-                if(virtualRobot == nullptr)
-                {
-                    ARMARX_WARNING << "Could not create virtual robot.";
-                    c.waitForCycleDuration();
-                    continue;
-                }
-                // only print timing once the robot is loadable & loaded
-                TIMING_END_STREAM(getRobot, ARMARX_INFO);
+                ARMARX_WARNING << deactivateSpam(1) << "Could not create virtual robot.";
+                return;
             }
+            // only print timing once the robot is loadable & loaded
+            TIMING_END_STREAM(getRobot, ARMARX_INFO);
+        }
 
-            ARMARX_INFO << "Synchronizing robot";
+        ARMARX_INFO << deactivateSpam(10) << "Synchronizing robot";
 
-            const IceUtil::Time now = TimeUtil::GetTime();
+        const IceUtil::Time now = TimeUtil::GetTime();
+
+        TIMING_START(synchronizeRobot);
+        virtualRobotReader.synchronizeRobot(*virtualRobot, now);
+        TIMING_END_STREAM(synchronizeRobot, ARMARX_INFO);
 
-            TIMING_START(synchronizeRobot);
-            virtualRobotReader.synchronizeRobot(*virtualRobot, now);
-            TIMING_END_STREAM(synchronizeRobot, ARMARX_INFO);
 
-            c.waitForCycleDuration();
-        }
     }
 
 }  // namespace armarx::robot_state

source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h

@@ -75,7 +75,13 @@ namespace armarx::robot_state
 
     private:
 
-        armarx::RunningTask<VirtualRobotReaderExampleClient>::pointer_type task;
+        struct Properties
+        {
+            std::string robotName{"Armar6"};
+            float updateFrequency{10.F};
+        } p;
+
+        armarx::PeriodicTask<VirtualRobotReaderExampleClient>::pointer_type task;
 
         armarx::DebugObserverInterfacePrx debugObserver;
 
@@ -83,6 +89,8 @@ namespace armarx::robot_state
 
         armem::robot_state::VirtualRobotReader virtualRobotReader;
 
+        std::shared_ptr<VirtualRobot::Robot> virtualRobot{nullptr};
+
     };
 
 }  // namespace armarx::robot_state

source/RobotAPI/libraries/armem/client/query/query_fns.h

@@ -156,7 +156,7 @@ namespace armarx::armem::client::query_fns
 
     inline
     std::function<void(query::SnapshotSelector&)>
-    beforeTime(Time time, long nElements)
+    beforeTime(Time time, long nElements = 1)
     {
         return [ = ](query::SnapshotSelector & selector)
         {

source/RobotAPI/libraries/armem/client/query/selectors.h

@@ -28,7 +28,7 @@ namespace armarx::armem::client::query
         SnapshotSelector& atTime(Time timestamp);
         SnapshotSelector& atTimeApprox(Time timestamp, Duration eps);
 
-        SnapshotSelector& beforeTime(Time timestamp, long maxEntries);
+        SnapshotSelector& beforeTime(Time timestamp, long maxEntries = 1);
         SnapshotSelector& beforeOrAtTime(Time timestamp);
 
         SnapshotSelector& timeRange(Time min, Time max);

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,15 +248,14 @@ 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())
+            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(), refIt);
+            const auto nEntriesBefore = std::distance(entity.history().begin(), itBefore);
 
             const int nEntries = [&]()
             {
@@ -215,7 +269,7 @@ namespace armarx::armem::base::query_proc
             }
             ();
 
-            auto it = refIt;
+            auto it = itBefore;
             for (std::int64_t i = 0; i < nEntries; i++, --it)
             {
                 addResultSnapshot(result, it);
@@ -226,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!";
 
@@ -243,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))
                 {
@@ -268,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";
-            }
+            
         }
 
 

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

@@ -20,6 +20,9 @@
  *             GNU General Public License
  */
 
+#include "ArmarXCore/core/exceptions/LocalException.h"
+#include <RobotAPI/interface/armem/query.h>
+#include <boost/test/tools/old/interface.hpp>
 #define BOOST_TEST_MODULE RobotAPI::ArmarXLibraries::armem
 
 #define ARMARX_BOOST_TEST
@@ -294,6 +297,262 @@ 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));
+    }
+
+}
+
+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());
+    }
+
+}
+
+
+/* TimeApprox */
+
+/**
+ * @brief Lookup between elements. No range specified.
+ *
+ * Desired behavior: Return elements before and after timestamp.
+ */
+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());
+    }
+
+}
+
+/**
+ * @brief Lookup between elements. Range is OK.
+ *
+ * Desired behavior: Return elements before and after timestamp.
+ */
+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());
+    }
+
+}
+
+/**
+ * @brief Lookup between elements. Range is too small.
+ *
+ * Desired behavior: Return empty list.
+ */
+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);
+    }
+
+}
+
+/**
+ * @brief Lookup between elements. Only next element is in range.
+ *
+ * Desired behavior: Return only element after query timestamp.
+ */
+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());
+    }
+}
+
+/**
+ * @brief Lookup between elements. Only previous element is in range.
+ *
+ * Desired behavior: Return only element before query timestamp.
+ */
+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());
+    }
+}
+
+/**
+ * @brief Lookup with perfect match.
+ *
+ * Desired behavior: Return only element matching timestamp exactly.
+ */
+BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_perfect_match)
+{
+    BOOST_REQUIRE_EQUAL(entity.size(), 5);
+    addResults(query::entity::TimeApprox{ 3000, -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);
+
+        std::vector<armem::Time> expected
+        {
+            armem::Time::microSeconds(3000)
+        };
+
+        BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
+    }
+}
+
+/**
+ * @brief Invalid lookup into the past.
+ *
+ * Desired behavior: Return empty list.
+ */
+BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_past)
+{
+    BOOST_REQUIRE_EQUAL(entity.size(), 5);
+    addResults(query::entity::TimeApprox{ 1, 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());
+    }
+}
+
+/**
+ * @brief Invalid lookup into the future.
+ *
+ * Desired behavior: Return empty list.
+ */
+BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_future)
+{
+    BOOST_REQUIRE_EQUAL(entity.size(), 5);
+    addResults(query::entity::TimeApprox{ 10'000, 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());
+    }
+}
+
+/**
+ * @brief Lookup into the future, but still considered valid as time range is not set.
+ *
+ * Desired behavior: Return most recent element in history.
+ */
+BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_future_valid)
+{
+    BOOST_REQUIRE_EQUAL(entity.size(), 5);
+    addResults(query::entity::TimeApprox{ 10'000, -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);
+
+        std::vector<armem::Time> expected
+        {
+            armem::Time::microSeconds(5'000)
+        };
+
+        BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
+    }
+}
+
+BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_invalid_timestamp)
+{
+    BOOST_REQUIRE_THROW(addResults(query::entity::TimeApprox{ -1, 1}), ::armarx::LocalException);
+}
+
+
+
 BOOST_AUTO_TEST_CASE(test_negative_index_semantics)
 {
     BOOST_CHECK_EQUAL(EntityQueryProcessor::negativeIndexSemantics(0, 0), 0);

source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp

@@ -172,7 +172,7 @@ namespace armarx::armem::robot_state
         .coreSegments().withName(properties.proprioceptionCoreSegment)
         .providerSegments().withName(description.name) // agent
         .entities().all() // TODO
-        .snapshots().atTime(timestamp);
+        .snapshots().beforeTime(timestamp);
         // clang-format on
 
         const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
@@ -189,7 +189,7 @@ namespace armarx::armem::robot_state
 
     std::optional<robot::RobotState::Pose> RobotReader::queryGlobalPose(const robot::RobotDescription& description, const armem::Time& timestamp) const
     {
-        const auto result = transformReader.getGlobalPose(description.name, "root", timestamp.toMicroSeconds());
+        const auto result = transformReader.getGlobalPose(description.name, "root", timestamp);
         if (not result)
         {
             return std::nullopt;

source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h

@@ -43,7 +43,6 @@ namespace armarx::armem::robot_state
         VirtualRobotReader(armem::ClientReaderComponentPluginUser& component);
         virtual ~VirtualRobotReader() = default;
 
-        // TODO(fabian.reister): register property defs
         void connect();
         void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def);
 

source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp

@@ -21,8 +21,8 @@
  */
 
 #include "TransformReader.h"
-#include "RobotAPI/libraries/armem_robot_state/common/localization/types.h"
 #include "RobotAPI/libraries/armem/core/Time.h"
+#include "RobotAPI/libraries/armem_robot_state/common/localization/types.h"
 
 #include <algorithm>
 #include <iterator>