[cache_unittest] Add direct implementation testing on ordering, pruning (backport #678)

tf2/include/tf2/time_cache.h

@@ -97,7 +97,8 @@ using TimeCacheInterfacePtr = std::shared_ptr<TimeCacheInterface>;
 /// 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. */
@@ -111,6 +112,7 @@ class TimeCache : public TimeCacheInterface
   /// 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);
 
@@ -138,6 +140,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<TransformStorage> & getAllItems() const;
+
 private:
   typedef std::list<TransformStorage> L_TransformStorage;
   L_TransformStorage storage_;

tf2/src/cache.cpp

@@ -30,6 +30,7 @@
 
 #include <algorithm>
 #include <cassert>
+#include <list>
 #include <sstream>
 #include <string>
 #include <utility>
@@ -308,6 +309,11 @@ TimePoint TimeCache::getOldestTimestamp()
   return storage_.back().stamp_;
 }
 
+const std::list<TransformStorage> & TimeCache::getAllItems() const
+{
+  return storage_;
+}
+
 void TimeCache::pruneList()
 {
   TimePoint latest_time = storage_.begin()->stamp_;

tf2/test/cache_unittest.cpp

@@ -31,6 +31,7 @@
 #include <chrono>
 #include <cmath>
 #include <stdexcept>
+#include <sstream>
 #include <string>
 #include <utility>
 #include <vector>
@@ -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<std::chrono::nanoseconds>(
+    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<tf2::TransformStorage> & 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<tf2::TransformStorage> & 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<tf2::TransformStorage> 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<tf2::TransformStorage> 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<tf2::TransformStorage> & 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<tf2::TransformStorage> 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);