From 01d8444b07cb6f4bdb6381d771762e0c367a0bc0 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Fri, 13 Sep 2024 00:36:38 +0200 Subject: [PATCH] BUGFIX: read ahead cache memory for rosbag2 was not freed if invoked via Dataset access API only --- .../mola_input_rosbag2/Rosbag2Dataset.h | 11 +++++---- mola_input_rosbag2/src/Rosbag2Dataset.cpp | 23 ++++++++++++++++--- 2 files changed, 27 insertions(+), 7 deletions(-) diff --git a/mola_input_rosbag2/include/mola_input_rosbag2/Rosbag2Dataset.h b/mola_input_rosbag2/include/mola_input_rosbag2/Rosbag2Dataset.h index a4b822c5..c674a2ca 100644 --- a/mola_input_rosbag2/include/mola_input_rosbag2/Rosbag2Dataset.h +++ b/mola_input_rosbag2/include/mola_input_rosbag2/Rosbag2Dataset.h @@ -144,10 +144,11 @@ class Rosbag2Dataset : public RawDataSourceBase, /** At initialization * */ - std::vector> read_ahead_; - size_t rosbag_next_idx_ = 0; - size_t rosbag_next_idx_write_ = 0; - std::set already_pub_sensor_labels_; + mutable std::vector> read_ahead_; + size_t rosbag_next_idx_ = 0; + size_t rosbag_next_idx_write_ = 0; + std::set already_pub_sensor_labels_; + mutable std::deque unload_queue_; //!< read_ahead_ indices // Methods and variables for the ROS->MRPT conversion // ------------------------------------------------------- @@ -200,6 +201,8 @@ class Rosbag2Dataset : public RawDataSourceBase, Obs catchExceptions(const std::function& f); + void autoUnloadOldEntries() const; + bool findOutSensorPose( mrpt::poses::CPose3D& des, const std::string& target_frame, const std::string& source_frame, diff --git a/mola_input_rosbag2/src/Rosbag2Dataset.cpp b/mola_input_rosbag2/src/Rosbag2Dataset.cpp index c625264f..99e0f92c 100644 --- a/mola_input_rosbag2/src/Rosbag2Dataset.cpp +++ b/mola_input_rosbag2/src/Rosbag2Dataset.cpp @@ -531,9 +531,6 @@ void Rosbag2Dataset::spinOnce() } } - // Free memory in read-ahead buffer: - read_ahead_.at(rosbag_next_idx_).reset(); - // Move on: rosbag_next_idx_++; } @@ -575,6 +572,8 @@ void Rosbag2Dataset::doReadAhead( for (size_t idx = startIdx; idx <= endIdx; idx++) { + unload_queue_.push_back(idx); // mark as recently accessed + if (read_ahead_.at(idx).has_value()) continue; // already read: // serialized data @@ -598,6 +597,9 @@ void Rosbag2Dataset::doReadAhead( } } + // and also, unload() very old observations. + autoUnloadOldEntries(); + MRPT_END } @@ -984,3 +986,18 @@ Rosbag2Dataset::Obs Rosbag2Dataset::catchExceptions( return {}; } } + +void Rosbag2Dataset::autoUnloadOldEntries() const +{ + const size_t MAX_UNLOAD_LEN = std::max(10, 2 * read_ahead_length_); + + // unload() very old observations. + while (unload_queue_.size() > MAX_UNLOAD_LEN) + { + const auto idx = unload_queue_.front(); + unload_queue_.erase(unload_queue_.begin()); + + // Free memory in read-ahead buffer: + read_ahead_.at(idx).reset(); + } +}