From b512f81d5491f27b4be91dfb5d51a8db7333d873 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Fri, 22 Mar 2024 09:51:40 +0100 Subject: [PATCH 1/3] Nacho/minor fixes tf2 cache (#658) * Remove unused parameter * Make use of API function to improve redability ```cpp TimePoint TimeCache::getLatestTimestamp() { return storage_.front().stamp_; } ``` And std::list::front() is(gcclib): ```cpp reference front() _GLIBCXX_NOEXCEPT { return *begin(); } ``` * Same argument as 321bd225afb5c ```cpp TimePoint TimeCache::getLatestTimestamp() { // empty list case // ... return storage_.front().stamp_; } ``` and std::list::front(): ```cpp reference front() _GLIBCXX_NOEXCEPT { return *begin(); } ``` * Improve readbility by relying on STL functions By now reading to this block I can tell that we are preventing to inserting a new element in the list, that has a timestamp that is actually older than the max_storage_time_ we allow for * Remove hardcoded algorithmg for STL one The intent of the code is now more clear, instead of relying on raw loops, we "find if" there is any element in the list that has a stamp older than the incoming one. With this we find the position in the list where we should insert the current timestamp: `storage_it` * Remove to better express what this pointer is represetngin * Replace raw loop for STL algorithm Remove if any element is older thant the max_storage_time_ allowed, relative to the latest(sooner) time seems clear npw --- tf2/include/tf2/time_cache.h | 4 ---- tf2/src/cache.cpp | 32 ++++++++++++++++---------------- 2 files changed, 16 insertions(+), 20 deletions(-) diff --git a/tf2/include/tf2/time_cache.h b/tf2/include/tf2/time_cache.h index 3472a5d9d..562a3e3b9 100644 --- a/tf2/include/tf2/time_cache.h +++ b/tf2/include/tf2/time_cache.h @@ -105,10 +105,6 @@ constexpr tf2::Duration TIMECACHE_DEFAULT_MAX_STORAGE_TIME = std::chrono::second class TimeCache : public TimeCacheInterface { public: - /// Number of nano-seconds to not interpolate below. - TF2_PUBLIC - static const int MIN_INTERPOLATION_DISTANCE = 5; - /// Maximum length of linked list, to make sure not to be able to use unlimited memory. TF2_PUBLIC static const unsigned int MAX_LENGTH_LINKED_LIST = 1000000; diff --git a/tf2/src/cache.cpp b/tf2/src/cache.cpp index 41fa9b3a4..cbbc71ef3 100644 --- a/tf2/src/cache.cpp +++ b/tf2/src/cache.cpp @@ -248,23 +248,22 @@ CompactFrameID TimeCache::getParent( bool TimeCache::insertData(const TransformStorage & new_data) { - L_TransformStorage::iterator storage_it = storage_.begin(); + const TimePoint latest_time = getLatestTimestamp(); - if (storage_it != storage_.end()) { - if (storage_it->stamp_ > new_data.stamp_ + max_storage_time_) { - return false; - } + // Avoid inserting data in the past that already exceeds the max_storage_time_ + if (!storage_.empty() && new_data.stamp_ < latest_time - max_storage_time_) { + return false; } - while (storage_it != storage_.end()) { - if (storage_it->stamp_ <= new_data.stamp_) { - break; - } - storage_it++; - } + // Find the oldest element in the list before the incoming stamp. + auto last_transform_pos = std::find_if( + storage_.begin(), storage_.end(), [&](const auto & transfrom) { + return transfrom.stamp_ <= new_data.stamp_; + }); + // Insert elements only if not already present if (std::find(storage_.begin(), storage_.end(), new_data) == storage_.end()) { - storage_.insert(storage_it, new_data); + storage_.insert(last_transform_pos, new_data); } pruneList(); @@ -316,10 +315,11 @@ const std::list & TimeCache::getAllItems() const void TimeCache::pruneList() { - TimePoint latest_time = storage_.begin()->stamp_; + const TimePoint latest_time = getLatestTimestamp(); - while (!storage_.empty() && storage_.back().stamp_ + max_storage_time_ < latest_time) { - storage_.pop_back(); - } + storage_.remove_if( + [&](const auto & transform) { + return transform.stamp_ < latest_time - max_storage_time_; + }); } } // namespace tf2 From d1143f5ae2c36229b8aa25dfe87de83f449bdc00 Mon Sep 17 00:00:00 2001 From: Eric Cousineau Date: Wed, 22 May 2024 17:04:24 -0400 Subject: [PATCH 2/3] [TimeCache] Improve performance for insertData() and pruneList() (#680) Signed-off-by: Eric Cousineau Co-authored-by: Chris Lalancette --- tf2/src/cache.cpp | 46 ++++++++++++++++++++++++++++++++++------------ 1 file changed, 34 insertions(+), 12 deletions(-) diff --git a/tf2/src/cache.cpp b/tf2/src/cache.cpp index cbbc71ef3..607fc1523 100644 --- a/tf2/src/cache.cpp +++ b/tf2/src/cache.cpp @@ -248,25 +248,48 @@ CompactFrameID TimeCache::getParent( bool TimeCache::insertData(const TransformStorage & new_data) { + // In order to minimize the number of times we iterate over this data, we: + // (1) Prune all old data first, regardless if new_data is added, + // (2) We use find_if to scan from newest to oldest, and stop at the first + // point where the timestamp is equal or older to new_data's. + // (3) From this point, we scan with more expensive full equality checks to + // ensure we do not reinsert the same exact data. + // (4) If we the data is not duplicated, then we simply insert new_data at + // the point found in (2). const TimePoint latest_time = getLatestTimestamp(); + // (1) Always prune data. + pruneList(); + // Avoid inserting data in the past that already exceeds the max_storage_time_ if (!storage_.empty() && new_data.stamp_ < latest_time - max_storage_time_) { return false; } - // Find the oldest element in the list before the incoming stamp. - auto last_transform_pos = std::find_if( - storage_.begin(), storage_.end(), [&](const auto & transfrom) { - return transfrom.stamp_ <= new_data.stamp_; + // (2) Find the oldest element in the list before the incoming stamp. + auto insertion_pos = std::find_if( + storage_.begin(), storage_.end(), [&](const auto & transform) { + return transform.stamp_ <= new_data.stamp_; }); - // Insert elements only if not already present - if (std::find(storage_.begin(), storage_.end(), new_data) == storage_.end()) { - storage_.insert(last_transform_pos, new_data); + bool should_insert = true; + + // (3) Search along all data with same timestamp (sorted), and see if we have + // an exact duplicate. + auto maybe_same_pos = insertion_pos; + while (maybe_same_pos != storage_.end() && maybe_same_pos->stamp_ == new_data.stamp_) { + if (*maybe_same_pos == new_data) { + should_insert = false; + break; + } + maybe_same_pos++; + } + + // (4) Insert elements only if not already present + if (should_insert) { + storage_.insert(insertion_pos, new_data); } - pruneList(); return true; } @@ -317,9 +340,8 @@ void TimeCache::pruneList() { const TimePoint latest_time = getLatestTimestamp(); - storage_.remove_if( - [&](const auto & transform) { - return transform.stamp_ < latest_time - max_storage_time_; - }); + while (!storage_.empty() && storage_.back().stamp_ + max_storage_time_ < latest_time) { + storage_.pop_back(); + } } } // namespace tf2 From 13431ac6d9b37e4cb5ea66ac17e716ae2280436a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 29 May 2024 10:53:40 +0200 Subject: [PATCH 3/3] Don't break ABI MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- tf2/include/tf2/time_cache.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/tf2/include/tf2/time_cache.h b/tf2/include/tf2/time_cache.h index 562a3e3b9..3472a5d9d 100644 --- a/tf2/include/tf2/time_cache.h +++ b/tf2/include/tf2/time_cache.h @@ -105,6 +105,10 @@ constexpr tf2::Duration TIMECACHE_DEFAULT_MAX_STORAGE_TIME = std::chrono::second class TimeCache : public TimeCacheInterface { public: + /// Number of nano-seconds to not interpolate below. + TF2_PUBLIC + static const int MIN_INTERPOLATION_DISTANCE = 5; + /// Maximum length of linked list, to make sure not to be able to use unlimited memory. TF2_PUBLIC static const unsigned int MAX_LENGTH_LINKED_LIST = 1000000;