diff --git a/include/message_filters/cache.hpp b/include/message_filters/cache.hpp index e7ec325..0056217 100644 --- a/include/message_filters/cache.hpp +++ b/include/message_filters/cache.hpp @@ -209,6 +209,12 @@ class Cache : public SimpleFilter namespace mt = message_filters::message_traits; std::lock_guard lock(cache_lock_); + if (cache_.size() == 0) + { + // early return for empty cache; Logic below is only valid for caches with at least one element. + return {}; + } + // Find the starting index. (Find the first index after [or at] the start of the interval) int start_index = static_cast(cache_.size()) - 1; while (start_index > 0 && diff --git a/test/msg_cache_unittest.cpp b/test/msg_cache_unittest.cpp index 25dcbf3..1e98a0d 100644 --- a/test/msg_cache_unittest.cpp +++ b/test/msg_cache_unittest.cpp @@ -78,7 +78,12 @@ void fillCacheEasy(message_filters::Cache & cache, unsigned int start, unsi TEST(Cache, emptySurroundingInterval) { message_filters::Cache cache(10); - const std::vector > interval_data = cache.getSurroundingInterval(rclcpp::Time(5, 0), rclcpp::Time(9, 0)); + const std::vector> interval_data = cache.getSurroundingInterval( + rclcpp::Time( + 5, + 0), rclcpp::Time( + 9, 0)); + EXPECT_EQ(interval_data.size(), (unsigned int) 0); // empty cache shall return empty interval }