Fix constantly increasing memory in std::list (backport #636)

tf2/CMakeLists.txt

@@ -105,6 +105,11 @@ if(BUILD_TESTING)
   if(TARGET test_time)
     target_link_libraries(test_time tf2)
   endif()
+
+  ament_add_gtest(test_storage test/test_storage.cpp)
+  if(TARGET test_storage)
+    target_link_libraries(test_storage tf2)
+  endif()
 endif()
 
 ament_export_dependencies(console_bridge geometry_msgs rcutils rosidl_runtime_cpp)

tf2/include/tf2/transform_storage.h

@@ -67,6 +67,22 @@ class TransformStorage
     return *this;
   }
 
+  TF2_PUBLIC
+  bool operator==(const TransformStorage & rhs) const
+  {
+    return (this->rotation_ == rhs.rotation_) &&
+           (this->translation_ == rhs.translation_) &&
+           (this->stamp_ == rhs.stamp_) &&
+           (this->frame_id_ == rhs.frame_id_) &&
+           (this->child_frame_id_ == rhs.child_frame_id_);
+  }
+
+  TF2_PUBLIC
+  bool operator!=(const TransformStorage & rhs) const
+  {
+    return !(*this == rhs);
+  }
+
   tf2::Quaternion rotation_;
   tf2::Vector3 translation_;
   TimePoint stamp_;

tf2/src/cache.cpp

@@ -28,6 +28,7 @@
 
 /** \author Tully Foote */
 
+#include <algorithm>
 #include <cassert>
 #include <sstream>
 #include <string>
@@ -260,7 +261,10 @@ bool TimeCache::insertData(const TransformStorage & new_data)
     }
     storage_it++;
   }
-  storage_.insert(storage_it, new_data);
+  // Insert elements only if not already present
+  if (std::find(storage_.begin(), storage_.end(), new_data) == storage_.end()) {
+    storage_.insert(storage_it, new_data);
+  }
 
   pruneList();
   return true;

tf2/test/cache_unittest.cpp

@@ -112,6 +112,27 @@ TEST(TimeCache, RepeatabilityReverseInsertOrder)
   }
 }
 
+TEST(TimeCache, RepeatedElements)
+{
+  constexpr uint64_t runs = 100;
+
+  tf2::TimeCache cache;
+  EXPECT_EQ(cache.getListLength(), 0);
+
+  tf2::TransformStorage stor;
+  setIdentity(stor);
+  stor.frame_id_ = tf2::CompactFrameID(0);
+  stor.stamp_ = tf2::TimePoint(std::chrono::nanoseconds(0));
+
+  // Attempt to insert the same element 100 times
+  for (uint64_t i = 1; i < runs; ++i) {
+    cache.insertData(stor);
+  }
+
+  // Even after 100 insertions, there should be one unique element in the internal list
+  EXPECT_EQ(cache.getListLength(), 1);
+}
+
 TEST(TimeCache, ZeroAtFront)
 {
   uint64_t runs = 100;

tf2/test/test_storage.cpp

@@ -0,0 +1,130 @@
+// Copyright (c) 2024 Dexory. All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the Willow Garage nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+#include <gtest/gtest.h>
+
+#include "tf2/LinearMath/Quaternion.h"
+#include "tf2/LinearMath/Vector3.h"
+#include "tf2/time.h"
+#include "tf2/transform_storage.h"
+
+class TransformStorageTest : public ::testing::Test
+{
+protected:
+  tf2::TransformStorage createTransformStorage()
+  {
+    const tf2::CompactFrameID frame_id(0);
+    const tf2::CompactFrameID child_frame_id(1);
+    const tf2::TimePoint stamp(tf2::TimePointZero);
+    const tf2::Quaternion rotation(0.0, 0.0, 0.0, 1.0);
+    const tf2::Vector3 translation(0.0, 0.0, 0.0);
+    return tf2::TransformStorage(stamp, rotation, translation, frame_id, child_frame_id);
+  }
+};
+
+TEST_F(TransformStorageTest, EqualityOperator) {
+  // Create a dummy storage, set to identity
+  tf2::TransformStorage transformStorage1 = createTransformStorage();
+
+  // tf2::Quaternion rotation_;
+  {
+    tf2::TransformStorage transformStorage2 = createTransformStorage();
+    ASSERT_TRUE(transformStorage1 == transformStorage2);
+    transformStorage2.rotation_.setValue(1.0, 0.0, 0.0, 0.0);
+    ASSERT_FALSE(transformStorage1 == transformStorage2);
+  }
+  // tf2::Vector3 translation_;
+  {
+    tf2::TransformStorage transformStorage2 = createTransformStorage();
+    ASSERT_TRUE(transformStorage1 == transformStorage2);
+    transformStorage2.translation_.setValue(1.0, 0.0, 0.0);
+    ASSERT_FALSE(transformStorage1 == transformStorage2);
+  }
+  // TimePoint stamp_;
+  {
+    tf2::TransformStorage transformStorage2 = createTransformStorage();
+    ASSERT_TRUE(transformStorage1 == transformStorage2);
+    transformStorage2.stamp_ = tf2::TimePoint(tf2::durationFromSec(1.0));
+    ASSERT_FALSE(transformStorage1 == transformStorage2);
+  }
+  // CompactFrameID frame_id_;
+  {
+    tf2::TransformStorage transformStorage2 = createTransformStorage();
+    ASSERT_TRUE(transformStorage1 == transformStorage2);
+    transformStorage2.frame_id_ = 55;
+    ASSERT_FALSE(transformStorage1 == transformStorage2);
+  }
+  // CompactFrameID child_frame_id_;
+  {
+    tf2::TransformStorage transformStorage2 = createTransformStorage();
+    ASSERT_TRUE(transformStorage1 == transformStorage2);
+    transformStorage2.translation_.setValue(1.0, 0.0, 0.0);
+    ASSERT_FALSE(transformStorage1 == transformStorage2);
+  }
+}
+
+TEST_F(TransformStorageTest, InequalityOperator) {
+  // Create a dummy storage, set to identity
+  tf2::TransformStorage transformStorage1 = createTransformStorage();
+
+  // tf2::Quaternion rotation_;
+  {
+    tf2::TransformStorage transformStorage2 = createTransformStorage();
+    ASSERT_TRUE(transformStorage1 == transformStorage2);
+    transformStorage2.rotation_.setValue(1.0, 0.0, 0.0, 0.0);
+    ASSERT_TRUE(transformStorage1 != transformStorage2);
+  }
+  // tf2::Vector3 translation_;
+  {
+    tf2::TransformStorage transformStorage2 = createTransformStorage();
+    ASSERT_TRUE(transformStorage1 == transformStorage2);
+    transformStorage2.translation_.setValue(1.0, 0.0, 0.0);
+    ASSERT_TRUE(transformStorage1 != transformStorage2);
+  }
+  // TimePoint stamp_;
+  {
+    tf2::TransformStorage transformStorage2 = createTransformStorage();
+    ASSERT_TRUE(transformStorage1 == transformStorage2);
+    transformStorage2.stamp_ = tf2::TimePoint(tf2::durationFromSec(1.0));
+    ASSERT_TRUE(transformStorage1 != transformStorage2);
+  }
+  // CompactFrameID frame_id_;
+  {
+    tf2::TransformStorage transformStorage2 = createTransformStorage();
+    ASSERT_TRUE(transformStorage1 == transformStorage2);
+    transformStorage2.frame_id_ = 55;
+    ASSERT_TRUE(transformStorage1 != transformStorage2);
+  }
+  // CompactFrameID child_frame_id_;
+  {
+    tf2::TransformStorage transformStorage2 = createTransformStorage();
+    ASSERT_TRUE(transformStorage1 == transformStorage2);
+    transformStorage2.translation_.setValue(1.0, 0.0, 0.0);
+    ASSERT_TRUE(transformStorage1 != transformStorage2);
+  }
+}