From 4e33d31b3d75e23f87ecc20e24401f07e6a41d21 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Tue, 12 Mar 2024 22:10:45 +0000 Subject: [PATCH 1/7] Remove unused parameter --- tf2/include/tf2/time_cache.h | 4 ---- 1 file changed, 4 deletions(-) diff --git a/tf2/include/tf2/time_cache.h b/tf2/include/tf2/time_cache.h index ad22e0611..36c4b31e1 100644 --- a/tf2/include/tf2/time_cache.h +++ b/tf2/include/tf2/time_cache.h @@ -104,10 +104,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; From 321bd225afb5c81f7396fe68de4e00db0b2d7bb0 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Tue, 12 Mar 2024 22:13:59 +0000 Subject: [PATCH 2/7] 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(); } ``` --- tf2/src/cache.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tf2/src/cache.cpp b/tf2/src/cache.cpp index 9d53a2ee9..ee1027095 100644 --- a/tf2/src/cache.cpp +++ b/tf2/src/cache.cpp @@ -310,7 +310,7 @@ TimePoint TimeCache::getOldestTimestamp() 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(); From 12f2d94c3f6771209665619a896830b6fb397fee Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Tue, 12 Mar 2024 22:19:02 +0000 Subject: [PATCH 3/7] 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(); } ``` --- tf2/src/cache.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tf2/src/cache.cpp b/tf2/src/cache.cpp index ee1027095..cd0974cb4 100644 --- a/tf2/src/cache.cpp +++ b/tf2/src/cache.cpp @@ -247,7 +247,7 @@ 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_) { From 5bc4c8568778ba996cbc4de16294f5b28908b9e2 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Tue, 12 Mar 2024 22:22:47 +0000 Subject: [PATCH 4/7] 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 --- tf2/src/cache.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/tf2/src/cache.cpp b/tf2/src/cache.cpp index cd0974cb4..e3e2e0624 100644 --- a/tf2/src/cache.cpp +++ b/tf2/src/cache.cpp @@ -249,10 +249,9 @@ bool TimeCache::insertData(const TransformStorage & new_data) { 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()) { From 05e84974b65697cfe486ae242d12908551926672 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Tue, 12 Mar 2024 22:24:30 +0000 Subject: [PATCH 5/7] 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` --- tf2/src/cache.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/tf2/src/cache.cpp b/tf2/src/cache.cpp index e3e2e0624..85cb11208 100644 --- a/tf2/src/cache.cpp +++ b/tf2/src/cache.cpp @@ -254,12 +254,11 @@ bool TimeCache::insertData(const TransformStorage & new_data) return false; } - while (storage_it != storage_.end()) { - if (storage_it->stamp_ <= new_data.stamp_) { - break; - } - storage_it++; - } + auto storage_it = 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); From a3755da0e64a14c2130ca5750f90c3b20ae7b9fd Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Tue, 12 Mar 2024 22:28:58 +0000 Subject: [PATCH 6/7] Remove to better express what this pointer is represetngin --- tf2/src/cache.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/tf2/src/cache.cpp b/tf2/src/cache.cpp index 85cb11208..c23321bef 100644 --- a/tf2/src/cache.cpp +++ b/tf2/src/cache.cpp @@ -254,14 +254,15 @@ bool TimeCache::insertData(const TransformStorage & new_data) return false; } - auto storage_it = std::find_if( + // 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(); From 4559ea89bfa49c0cae59999bfab6b7baacf9bd31 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Tue, 12 Mar 2024 22:29:51 +0000 Subject: [PATCH 7/7] 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/src/cache.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/tf2/src/cache.cpp b/tf2/src/cache.cpp index c23321bef..f9ba3b2f8 100644 --- a/tf2/src/cache.cpp +++ b/tf2/src/cache.cpp @@ -311,8 +311,9 @@ void TimeCache::pruneList() { 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