Merge pull request #70 from MOLAorg/fix/rosbag2-memory-free

BUGFIX: read ahead cache memory for rosbag2 was not freed
MOLAorg · Sep 12, 2024 · 8cd83ee · 8cd83ee
2 parents 01bb91b + 01d8444
commit 8cd83ee
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 7 deletions.
diff --git 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<std::optional<DatasetEntry>> read_ahead_;
-    size_t                                   rosbag_next_idx_       = 0;
-    size_t                                   rosbag_next_idx_write_ = 0;
-    std::set<std::string>                    already_pub_sensor_labels_;
+    mutable std::vector<std::optional<DatasetEntry>> read_ahead_;
+    size_t                                           rosbag_next_idx_       = 0;
+    size_t                                           rosbag_next_idx_write_ = 0;
+    std::set<std::string>                            already_pub_sensor_labels_;
+    mutable std::deque<size_t> 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<Obs()>& 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
@@ -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<size_t>(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();
+    }
+}