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<T>::front() is(gcclib):
```cpp
reference
front() _GLIBCXX_NOEXCEPT
{ return *begin(); }
```

* Same argument as 321bd22

```cpp

TimePoint TimeCache::getLatestTimestamp()
{
  // empty list case
  // ...
  return storage_.front().stamp_;
}
```

and std::list<T>::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

@@ -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;

tf2/src/cache.cpp

@@ -247,23 +247,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();
@@ -310,10 +309,11 @@ 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();
-  }
+  storage_.remove_if(
+    [&](const auto & transform) {
+      return transform.stamp_ < latest_time - max_storage_time_;
+    });
 }
 }  // namespace tf2