diff --git a/source/RobotAPI/libraries/armem/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt
index 1becb6eb9017d691dc72a5372b78e87e4d76e589..a5274706c4f359b45f1e8d7fb925b66479865625 100644
--- a/source/RobotAPI/libraries/armem/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/CMakeLists.txt
@@ -37,6 +37,7 @@ set(LIB_FILES
core/base/detail/EntityContainerBase.cpp
core/base/detail/AronTyped.cpp
core/base/detail/iteration_mixins.cpp
+ core/base/detail/negative_index_semantics.cpp
# core/base/CoreSegmentBase.cpp
core/base/EntityBase.cpp
@@ -156,6 +157,7 @@ set(LIB_HEADERS
core/base/detail/EntityContainerBase.h
core/base/detail/AronTyped.h
core/base/detail/iteration_mixins.h
+ core/base/detail/negative_index_semantics.h
core/base/CoreSegmentBase.h
core/base/EntityBase.h
diff --git a/source/RobotAPI/libraries/armem/core/base/EntityBase.h b/source/RobotAPI/libraries/armem/core/base/EntityBase.h
index b3e3f7424657f3f6731e0fd35a01424ae65acdfc..f28f12b7be82e7c594c9306f6fa5e24b5decdd42 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntityBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/EntityBase.h
@@ -184,17 +184,6 @@ namespace armarx::armem::base
return getLatestItem().second;
}
- /**
- * @brief Return the lastest snapshot before or at time.
- * @param time The time.
- * @return The latest snapshot.
- * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
- * @throw `armem::error::MissingEntry` If no such snapshot
- */
- const EntitySnapshotT& getLatestSnapshotBeforeOrAt(const Time& time) const
- {
- return getLatestSnapshotBefore(time + Time::microSeconds(1));
- }
/**
* @brief Return the lastest snapshot before time.
@@ -231,37 +220,17 @@ namespace armarx::armem::base
}
/**
- * @brief Return all snapshots before (excluding) time.
- * @param time The time.
- * @return The latest snapshots.
- */
- std::vector<std::reference_wrapper<const EntitySnapshotT>>
- getSnapshotsBefore(const Time& time) const
- {
- std::vector<std::reference_wrapper<const EntitySnapshotT>> ret;
- for (const auto& [timestamp, snapshot] : this->_container)
- {
- if (timestamp >= time)
- {
- break;
- }
- ret.emplace_back(snapshot);
- }
- return ret;
- }
-
- /**
- * @brief Return all snapshots before or at time.
+ * @brief Return the lastest snapshot before or at time.
* @param time The time.
- * @return The latest snapshots.
+ * @return The latest snapshot.
+ * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
+ * @throw `armem::error::MissingEntry` If no such snapshot
*/
- std::vector<std::reference_wrapper<const EntitySnapshotT>>
- getSnapshotsBeforeOrAt(const Time& time) const
+ const EntitySnapshotT& getLatestSnapshotBeforeOrAt(const Time& time) const
{
- return getSnapshotsBefore(time + Time::microSeconds(1));
+ return getLatestSnapshotBefore(time + Time::microSeconds(1));
}
-
/**
* @brief Return first snapshot after or at time.
* @param time The time.
@@ -316,6 +285,94 @@ namespace armarx::armem::base
// forEachEntityInstance() is provided by ForEachEntityInstanceMixin.
+ /**
+ * @brief Return all snapshots before (excluding) time.
+ * @param time The time.
+ * @return The latest snapshots.
+ */
+ template <class FunctionT>
+ void forEachSnapshotBefore(const Time& time, FunctionT&& func) const
+ {
+ for (const auto& [timestamp, snapshot] : this->_container)
+ {
+ if (timestamp >= time)
+ {
+ break;
+ }
+ if (not func(snapshot))
+ {
+ break;
+ }
+ }
+ }
+
+ /**
+ * @brief Return all snapshots before or at time.
+ * @param time The time.
+ * @return The latest snapshots.
+ */
+ template <class FunctionT>
+ void forEachSnapshotBeforeOrAt(const Time& time, FunctionT&& func) const
+ {
+ getSnapshotsBefore(time + Time::microSeconds(1), func);
+ }
+
+
+ /**
+ * @brief Return all snapshots between, including, min and max.
+ * @param min The lowest time to include.
+ * @param min The highest time to include.
+ * @return The snapshots in [min, max].
+ */
+ template <class FunctionT>
+ void forEachSnapshotInTimeRange(const Time& min, const Time& max, FunctionT&& func) const
+ {
+ // Returns an iterator pointing to the first element that is not less than (i.e. greater or equal to) key.
+ auto begin = min.toMicroSeconds() > 0 ? this->_container.lower_bound(min) : this->_container.begin();
+ // Returns an iterator pointing to the first element that is *greater than* key.
+ auto end = max.toMicroSeconds() > 0 ? this->_container.upper_bound(max) : this->_container.end();
+
+ for (auto it = begin; it != end && it != this->_container.end(); ++it)
+ {
+ func(it->second);
+ }
+ }
+
+ /**
+ * @brief Return all snapshots from first to last index.
+ *
+ * Negative index are counted from the end, e.g.
+ * last == -1 results in getting all queries until the end.
+ *
+ * @param first The first index to include.
+ * @param first The last index to include.
+ * @return The snapshots in [first, last].
+ */
+ template <class FunctionT>
+ void forEachSnapshotInIndexRange(long first, long last, FunctionT&& func) const
+ {
+ if (this->empty())
+ {
+ return;
+ }
+
+ const size_t first_ = detail::negativeIndexSemantics(first, this->size());
+ const size_t last_ = detail::negativeIndexSemantics(last, this->size());
+
+ if (first_ <= last_)
+ {
+ auto it = this->_container.begin();
+ std::advance(it, first_);
+
+ size_t num = last_ - first_ + 1; // +1 to make last inclusive
+ for (size_t i = 0; i < num; ++i, ++it)
+ {
+ func(it->second);
+ }
+ }
+ }
+
+
[[deprecated("Direct container access is deprecated. Use forEach*() instead.")]]
inline const ContainerT& history() const
{
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/iteration_mixins.cpp b/source/RobotAPI/libraries/armem/core/base/detail/iteration_mixins.cpp
index 857ca6220294c2faea84e84953af250faa46973e..459cd4d5c7bd694bcb289496352d7e2cad4966cd 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/iteration_mixins.cpp
+++ b/source/RobotAPI/libraries/armem/core/base/detail/iteration_mixins.cpp
@@ -1,4 +1,5 @@
-#include "AronTyped.h"
+#include "iteration_mixins.h"
+
namespace armarx::armem::base::detail
{
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/negative_index_semantics.cpp b/source/RobotAPI/libraries/armem/core/base/detail/negative_index_semantics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a1875ff53545a3d2574cf9764a14d321946e5514
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/base/detail/negative_index_semantics.cpp
@@ -0,0 +1,17 @@
+#include "negative_index_semantics.h"
+
+#include <algorithm>
+
+
+size_t armarx::armem::base::detail::negativeIndexSemantics(long index, size_t size)
+{
+ const size_t max = size > 0 ? size - 1 : 0;
+ if (index >= 0)
+ {
+ return std::clamp<size_t>(static_cast<size_t>(index), 0, max);
+ }
+ else
+ {
+ return static_cast<size_t>(std::clamp<long>(static_cast<long>(size) + index, 0, static_cast<long>(max)));
+ }
+}
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/negative_index_semantics.h b/source/RobotAPI/libraries/armem/core/base/detail/negative_index_semantics.h
new file mode 100644
index 0000000000000000000000000000000000000000..4efe169276c83ff12c13400e6de162a8b4db5d8d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/base/detail/negative_index_semantics.h
@@ -0,0 +1,11 @@
+#pragma once
+
+#include <stddef.h>
+
+
+namespace armarx::armem::base::detail
+{
+
+ size_t negativeIndexSemantics(long index, size_t size);
+
+}
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 009683a32779d5e6728c1c86387ab0631c9193b2..644ec02282fa46f96e0d643d56094ad59b56eadf 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
@@ -148,25 +148,12 @@ namespace armarx::armem::base::query_proc
const armem::query::data::entity::IndexRange& query,
const _EntityT& entity) const
{
- if (entity.empty())
+ entity.forEachSnapshotInIndexRange(
+ query.first, query.last,
+ [this, &result](const EntitySnapshotT & snapshot)
{
- return;
- }
-
- size_t first = negativeIndexSemantics(query.first, entity.history().size());
- size_t last = negativeIndexSemantics(query.last, entity.history().size());
-
- if (first <= last)
- {
- auto it = entity.begin();
- std::advance(it, first);
-
- size_t num = last - first + 1; // +1 to make last inclusive
- for (size_t i = 0; i < num; ++i, ++it)
- {
- addResultSnapshot(result, it);
- }
- }
+ addResultSnapshot(result, snapshot);
+ });
}
void process(_EntityT& result,
@@ -176,16 +163,12 @@ namespace armarx::armem::base::query_proc
const armem::query::data::EntityQuery& query) const
{
(void) query;
-
- // Returns an iterator pointing to the first element that is not less than (i.e. greater or equal to) key.
- auto begin = min.toMicroSeconds() > 0 ? entity.history().lower_bound(min) : entity.history().begin();
- // Returns an iterator pointing to the first element that is *greater than* key.
- auto end = max.toMicroSeconds() > 0 ? entity.history().upper_bound(max) : entity.history().end();
-
- for (auto it = begin; it != end && it != entity.history().end(); ++it)
+ entity.forEachSnapshotInTimeRange(
+ min, max,
+ [this, &result](const EntitySnapshotT & snapshot)
{
- addResultSnapshot(result, it);
- }
+ addResultSnapshot(result, snapshot);
+ });
}
@@ -213,11 +196,18 @@ namespace armarx::armem::base::query_proc
const _EntityT& entity) const
{
const armem::Time referenceTimestamp = fromIce<Time>(query.timestamp);
- ARMARX_CHECK(referenceTimestamp.toMicroSeconds() >= 0) << "Reference timestamp is negative!";
+ ARMARX_CHECK(referenceTimestamp.toMicroSeconds() >= 0) << "Reference timestamp must be non-negative.";
+#if 0
try
{
- const auto& befores = entity.getSnapshotsBefore(referenceTimestamp);
+#endif
+ std::vector<const EntitySnapshotT*> befores;
+ entity.forEachSnapshotBefore(referenceTimestamp, [&befores](const EntitySnapshotT & s)
+ {
+ befores.push_back(&s);
+ return true;
+ });
size_t num = 0;
if (query.maxEntries < 0)
@@ -232,13 +222,15 @@ namespace armarx::armem::base::query_proc
for (size_t r = 0; r < num; ++r)
{
size_t i = befores.size() - 1 - r;
- addResultSnapshot(result, befores[i]);
+ addResultSnapshot(result, *befores[i]);
}
+#if 0
}
catch (const error::MissingEntry&)
{
// Leave empty.
}
+#endif
}
void process(_EntityT& result,
@@ -293,18 +285,6 @@ namespace armarx::armem::base::query_proc
}
}
- static size_t negativeIndexSemantics(long index, size_t size)
- {
- const size_t max = size > 0 ? size - 1 : 0;
- if (index >= 0)
- {
- return std::clamp<size_t>(static_cast<size_t>(index), 0, max);
- }
- else
- {
- return static_cast<size_t>(std::clamp<long>(static_cast<long>(size) + index, 0, static_cast<long>(max)));
- }
- }
protected:
virtual void addResultSnapshot(_EntityT& result, typename _EntityT::ContainerT::const_iterator it) const = 0;
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h
index 55c5e51126b49775db7317d6401aa695c4b5581c..66c2a8042b740ec72c6be3ca653f6ed72fc8c35b 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h
@@ -65,10 +65,11 @@ namespace armarx::armem::base::query_proc
const armem::query::data::memory::All& query,
const _MemoryT& memory) const
{
- for (const auto& [name, coreSegment] : memory.coreSegments())
+ memory.forEachCoreSegment([this, &result, &query](const CoreSegmentT & coreSegment)
{
result.addCoreSegment(coreSegmentProcessorProcess(query.coreSegmentQueries, coreSegment));
- }
+ return true;
+ });
}
void process(_MemoryT& result,
@@ -89,14 +90,15 @@ namespace armarx::armem::base::query_proc
const armem::query::data::memory::Regex& query,
const _MemoryT& memory) const
{
- std::regex regex(query.coreSegmentNameRegex);
- for (const auto& [name, coreSegment] : memory.coreSegments())
+ const std::regex regex(query.coreSegmentNameRegex);
+ memory.forEachCoreSegment([this, &result, &query, ®ex](const CoreSegmentT & coreSegment)
{
if (std::regex_search(coreSegment.name(), regex))
{
result.addCoreSegment(coreSegmentProcessorProcess(query.coreSegmentQueries, coreSegment));
}
- }
+ return true;
+ });
}
protected:
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/ProviderSegmentQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/ProviderSegmentQueryProcessorBase.h
index b3b7da7ffa9cc1d06aeadabef58c5aebba3974de..cb191d02edeaaa471290d5bf67475a5950572213 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/ProviderSegmentQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/ProviderSegmentQueryProcessorBase.h
@@ -57,10 +57,11 @@ namespace armarx::armem::base::query_proc
const armem::query::data::provider::All& query,
const _ProviderSegmentT& providerSegment) const
{
- for (const auto& [name, entity] : providerSegment.entities())
+ providerSegment.forEachEntity([this, &result, &query](const EntityT & entity)
{
result.addEntity(entityProcessorProcess(query.entityQueries, entity));
- }
+ return true;
+ });
}
void process(_ProviderSegmentT& result,
@@ -82,14 +83,15 @@ namespace armarx::armem::base::query_proc
const armem::query::data::provider::Regex& query,
const _ProviderSegmentT& providerSegment) const
{
- std::regex regex(query.entityNameRegex);
- for (const auto& [name, entity] : providerSegment.entities())
+ const std::regex regex(query.entityNameRegex);
+ providerSegment.forEachEntity([this, &result, &query, ®ex](const EntityT & entity)
{
if (std::regex_search(entity.name(), regex))
{
result.addEntity(entityProcessorProcess(query.entityQueries, entity));
}
- }
+ return true;
+ });
}
protected: