warning and exceptions; new queries

source/RobotAPI/interface/armem/query.ice

@@ -75,15 +75,31 @@ module armarx
                     };
 
                     /**
-                     * @brief Get the last snapshot before the timestamp.
+                     * @brief Get the last snapshot before or at the timestamp.
                      * 
                      * This query is kind of similar to latest() but with a reference timestamp.
                      * 
                      */
+                    class BeforeOrAtTime extends EntityQuery
+                    {
+                        long timestamp;
+                    }
+
+                    /**
+                     * @brief Get the last snapshot before the timestamp or a sequence of 
+                     *           n last elements before the timestamp depending on #maxEntries
+                     * 
+                     * Depending on #maxEntries, the behavior is as follows:
+                     *  - #maxEntries == 1 => last element before timestamp
+                     *  - #maxEntries > 1  => n last elements before timestamp
+                     *  - #maxEntries < 0  => all elements before timestamp
+                     */
                     class BeforeTime extends EntityQuery
                     {
                         long timestamp;
+                        long maxEntries = 1;
                     }
+
                 }
 
 

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

@@ -2,6 +2,7 @@
 
 #include <RobotAPI/interface/armem/query.h>
 
+#include "ArmarXCore/core/exceptions/LocalException.h"
 #include "BaseQueryProcessorBase.h"
 
 #include <ArmarXCore/core/logging/Logging.h>
@@ -52,6 +53,10 @@ namespace armarx::armem::base::query_proc
             {
                 process(result, *q, entity);
             }
+            else if (auto q = dynamic_cast<const armem::query::data::entity::BeforeOrAtTime*>(&query))
+            {
+                process(result, *q, entity);
+            }
             else if (auto q = dynamic_cast<const armem::query::data::entity::BeforeTime*>(&query))
             {
                 process(result, *q, entity);
@@ -160,20 +165,50 @@ 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);
+            }
+        }
+
         void process(_EntityT& result,
                      const armem::query::data::entity::BeforeTime& query,
                      const _EntityT& entity) const
         {
             const auto referenceTimestamp = fromIce<Time>(query.timestamp);
+            ARMARX_CHECK(referenceTimestamp.toMicroSeconds() >= 0) << "Reference timestamp is negative!";
+
+            const auto maxEntries = fromIce<int64>(query.maxEntries);
+
+            const auto refIt = entity.history().lower_bound(referenceTimestamp);
 
-            if (referenceTimestamp.toMicroSeconds() < 0)
+            if (refIt == entity.history().end())
             {
-                return; // TODO throw or warn?
+                ARMARX_WARNING << "No valid entities found.";
+                return;
             }
 
-            const auto it = entity.history().lower_bound(referenceTimestamp);
+            const auto nEntriesBefore = std::distance(entity.history().begin(), refIt);
 
-            if (it != entity.history().end())
+            if (nEntriesBefore < maxEntries)
+            {
+                // TODO maybe warn that not enough elements exist, but the user will notice anyways
+                ;
+            }
+
+            const int nEntries = std::min(nEntriesBefore, maxEntries);
+
+            auto it = refIt;
+            for (int64 i = 0; i < nEntries; i++, --it)
             {
                 addResultSnapshot(result, it);
             }
@@ -184,6 +219,8 @@ namespace armarx::armem::base::query_proc
                      const _EntityT& entity) const
         {
             const auto referenceTimestamp = fromIce<Time>(query.timestamp);
+            ARMARX_CHECK(referenceTimestamp.toMicroSeconds() >= 0) << "Reference timestamp is negative!";
+
             const auto referenceTimestampMicroSeconds = referenceTimestamp.toMicroSeconds();
             const auto epsDuration = fromIce<Time>(query.eps).toMicroSeconds();
 
@@ -198,10 +235,6 @@ namespace armarx::armem::base::query_proc
                 return std::abs(t.toMicroSeconds() - referenceTimestampMicroSeconds) <= epsDuration;
             };
 
-            if (referenceTimestamp.toMicroSeconds() < 0)
-            {
-                return; // TODO throw or warn?
-            }
 
             const auto beforeOrAt = entity.history().lower_bound(referenceTimestamp);
             const auto after = entity.history().upper_bound(referenceTimestamp);