diff --git a/tf2/include/tf2/time_cache.h b/tf2/include/tf2/time_cache.h index 36c4b31e1..b99e0e5eb 100644 --- a/tf2/include/tf2/time_cache.h +++ b/tf2/include/tf2/time_cache.h @@ -97,16 +97,14 @@ using TimeCacheInterfacePtr = std::shared_ptr; /// default value of 10 seconds storage constexpr tf2::Duration TIMECACHE_DEFAULT_MAX_STORAGE_TIME = std::chrono::seconds(10); -/** \brief A class to keep a sorted linked list in time +/** \brief A class to keep a sorted linked list in time (newest first, oldest + * last). * This builds and maintains a list of timestamped * data. And provides lookup functions to get * data out as a function of time. */ class TimeCache : public TimeCacheInterface { public: - /// 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; TF2_PUBLIC explicit TimeCache(tf2::Duration max_storage_time = TIMECACHE_DEFAULT_MAX_STORAGE_TIME); @@ -134,6 +132,13 @@ class TimeCache : public TimeCacheInterface TF2_PUBLIC virtual TimePoint getOldestTimestamp(); +protected: + // (Internal) Return a reference to the internal list of tf2 frames, which + // are sorted in timestamp order. + // Any items with the same timestamp will be in reverse order of insertion. + TF2_PUBLIC + const std::list & getAllItems() const; + private: typedef std::list L_TransformStorage; L_TransformStorage storage_; diff --git a/tf2/src/cache.cpp b/tf2/src/cache.cpp index f9ba3b2f8..cbbc71ef3 100644 --- a/tf2/src/cache.cpp +++ b/tf2/src/cache.cpp @@ -30,6 +30,7 @@ #include #include +#include #include #include #include @@ -307,6 +308,11 @@ TimePoint TimeCache::getOldestTimestamp() return storage_.back().stamp_; } +const std::list & TimeCache::getAllItems() const +{ + return storage_; +} + void TimeCache::pruneList() { const TimePoint latest_time = getLatestTimestamp(); diff --git a/tf2/test/cache_unittest.cpp b/tf2/test/cache_unittest.cpp index a2a024f70..e00aa0c10 100644 --- a/tf2/test/cache_unittest.cpp +++ b/tf2/test/cache_unittest.cpp @@ -31,6 +31,7 @@ #include #include #include +#include #include #include #include @@ -67,6 +68,133 @@ void setIdentity(tf2::TransformStorage & stor) stor.rotation_.setValue(0.0, 0.0, 0.0, 1.0); } +class TimeCacheInternal : public tf2::TimeCache +{ +public: + using tf2::TimeCache::TimeCache; + using tf2::TimeCache::getAllItems; +}; + +// Shorthand for making incomplete but unique transforms. +tf2::TransformStorage makeItem(uint32_t nanosec, uint32_t frame_id) +{ + tf2::TransformStorage stor{}; + stor.frame_id_ = frame_id; + stor.stamp_ = tf2::TimePoint(std::chrono::nanoseconds(nanosec)); + // Initialize remaining elements. + stor.child_frame_id_ = 0; + setIdentity(stor); + return stor; +} + +std::string toMakeItemString(const tf2::TransformStorage & item) +{ + std::stringstream out; + std::chrono::duration nanosec = std::chrono::duration_cast( + item.stamp_.time_since_epoch()); + out << "makeItem(" << nanosec.count() << ", " << item.frame_id_ << ")"; + return out.str(); +} + +// Reformat list for ease of debugging pursuant to this test point. +std::string listToMakeItemStrings(const std::list & storage) +{ + std::stringstream out; + out << "{\n"; + for (auto & item : storage) { + out << " " << toMakeItemString(item) << ",\n"; + } + out << "}"; + return out.str(); +} + +// Tests the behavior of sorting and pruning relevant to the implementation for +// performance concerns. Certain details may change as the implementation +// evolves. +TEST(TimeCache, GetAllItems) +{ + tf2::Duration max_storage_time(std::chrono::nanoseconds(10)); + TimeCacheInternal cache(max_storage_time); + + const auto item_a = makeItem(0, 0); + const auto item_b = makeItem(10, 1); + const auto item_c = makeItem(5, 2); + const auto item_d = makeItem(3, 3); + // Same timestamp, different id. + const auto item_e = makeItem(8, 4); + const auto item_f = makeItem(8, 5); + const auto item_g = makeItem(8, 6); + + // Insert in order. + cache.insertData(item_a); + cache.insertData(item_b); + cache.insertData(item_c); + cache.insertData(item_d); + cache.insertData(item_e); + cache.insertData(item_f); + cache.insertData(item_g); + + // Note that the difference between the oldest and newest timestamp is exactly equal + // to the max storage duration. + EXPECT_EQ( + cache.getLatestTimestamp() - cache.getOldestTimestamp(), + max_storage_time); + + // Expect that storage is descending. + const std::list & storage_expected{ + item_b, + // Same timestamps have effectively reversed insertion order. + item_g, + item_f, + item_e, + // Remaining are in descending order. + item_c, + item_d, + item_a}; + + const std::list storage = cache.getAllItems(); + EXPECT_EQ(storage, storage_expected) + << "storage: " << listToMakeItemStrings(storage) << "\n" + << "storage_expected: " << listToMakeItemStrings(storage_expected) << "\n"; + + // Insert repeated, in reverse. Nothing should change. + cache.insertData(item_g); + cache.insertData(item_f); + cache.insertData(item_e); + cache.insertData(item_d); + cache.insertData(item_c); + cache.insertData(item_b); + cache.insertData(item_a); + + const std::list storage_repeat = cache.getAllItems(); + EXPECT_EQ(storage_repeat, storage_expected) + << "storage_repeat: " << listToMakeItemStrings(storage_repeat) << "\n" + << "storage_expected: " << listToMakeItemStrings(storage_expected) << "\n"; + + // Insert newer data, and expect stale data to be pruned, even if newly inserted. + const auto item_h = makeItem(15, 7); + const auto item_i = makeItem(0, 8); // This will be dropped. + const auto item_j = makeItem(5, 9); + + cache.insertData(item_h); + cache.insertData(item_i); + cache.insertData(item_j); + + const std::list & storage_expected_new{ + item_h, + item_b, + item_g, + item_f, + item_e, + item_j, + item_c}; + // item_a, item_d, and item_i are pruned. + const std::list storage_new = cache.getAllItems(); + EXPECT_EQ(storage_new, storage_expected_new) + << "storage_new: " << listToMakeItemStrings(storage_new) << "\n" + << "storage_expected_new: " << listToMakeItemStrings(storage_expected_new) << "\n"; +} + TEST(TimeCache, Repeatability) { unsigned int runs = 100; @@ -82,6 +210,7 @@ TEST(TimeCache, Repeatability) cache.insertData(stor); } + EXPECT_EQ(cache.getListLength(), runs - 1); for (uint64_t i = 1; i < runs; i++) { cache.getData(tf2::TimePoint(std::chrono::nanoseconds(i)), stor); @@ -105,6 +234,7 @@ TEST(TimeCache, RepeatabilityReverseInsertOrder) cache.insertData(stor); } + EXPECT_EQ(cache.getListLength(), runs); for (uint64_t i = 1; i < runs; i++) { cache.getData(tf2::TimePoint(std::chrono::nanoseconds(i)), stor); EXPECT_EQ(stor.frame_id_, i); @@ -330,8 +460,11 @@ TEST(TimeCache, DuplicateEntries) stor.stamp_ = tf2::TimePoint(std::chrono::nanoseconds(1)); cache.insertData(stor); + EXPECT_EQ(cache.getListLength(), 1); cache.insertData(stor); + // Exact repeated element, should not grow in length. + EXPECT_EQ(cache.getListLength(), 1); cache.getData(tf2::TimePoint(std::chrono::nanoseconds(1)), stor);