Fixed image queue leak

Author: TomaszTB

ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/StandAloneRealsenseProcess.java

@@ -85,7 +85,7 @@ public StandAloneRealsenseProcess(ROS2Node ros2Node,
       d455PublishThread.addTopic(PerceptionAPI.D455_DEPTH_IMAGE, RealSenseImageSensor.DEPTH_IMAGE_KEY);
       loopOnDemand(d455PublishThread, realsensePublishDemandNode);
 
-      BlockingQueue<RawImage> rawImageCollection = new LinkedBlockingQueue<>();
+      BlockingQueue<RawImage> rawImageCollection = new LinkedBlockingQueue<>(10);
       d455Sensor.registerImageCollector(rawImageCollection, RealSenseImageSensor.DEPTH_IMAGE_KEY);
       rapidHeightMapThread = new RapidHeightMapThread(ros2Helper.getROS2Node(),
                                                       syncedRobot,

ihmc-perception/src/main/java/us/ihmc/sensors/ImageSensor.java

@@ -7,10 +7,10 @@
 import us.ihmc.perception.RawImage;
 
 import java.util.ArrayList;
-import java.util.Collection;
 import java.util.HashMap;
 import java.util.List;
 import java.util.Map;
+import java.util.concurrent.BlockingQueue;
 
 public abstract class ImageSensor implements AutoCloseable
 {
@@ -22,7 +22,7 @@ public abstract class ImageSensor implements AutoCloseable
 
    private final RepeatingTaskThread grabThread;
    private final Object grabNotification = new Object();
-   private final Map<Integer, List<Collection<RawImage>>> imageCollectors = new HashMap<>();
+   private final Map<Integer, List<BlockingQueue<RawImage>>> imageQueues = new HashMap<>();
 
    public ImageSensor(String sensorName)
    {
@@ -105,24 +105,25 @@ public ReferenceFrame getSensorFrame()
    public abstract ReferenceFrame[] getImageFrames();
 
    /**
-    * Register an image collector for images of a particular key.
+    * Register an image queue for images of a particular key.
     * <p>
-    * Every image grabbed by the sensor will be added to the passed in collection.
+    * Every image grabbed by the sensor will be added to the passed in queue.
     * The code accessing the collection must call {@link RawImage#release()} on each image,
     * once it's done using the image.
     * <p>
-    * Although any class that implements {@link Collection} can be passed in, the implementation of
-    * the {@link Collection#add(Object)} method should not take a significant amount of time.
+    * If the queue becomes full (i.e. when {@code BlockingQueue#remainingCapacity() == 0})
+    * the oldest image will be removed so the new image can be added.
+    * Ensure a reasonable queue capacity is set to prevent memory leaks.
     *
-    * @param imageCollector Collection into which the images will be added.
+    * @param imageQueue Blocking queue into which the images will be added.
     * @param imageKey The key for images to be collected.
     */
-   public void registerImageCollector(Collection<RawImage> imageCollector, int imageKey)
+   public void registerImageCollector(BlockingQueue<RawImage> imageQueue, int imageKey)
    {
-      if (imageCollectors.containsKey(imageKey))
-         imageCollectors.get(imageKey).add(imageCollector);
+      if (imageQueues.containsKey(imageKey))
+         imageQueues.get(imageKey).add(imageQueue);
       else
-         imageCollectors.put(imageKey, new ArrayList<>(List.of(imageCollector)));
+         imageQueues.put(imageKey, new ArrayList<>(List.of(imageQueue)));
    }
 
    public String getSensorName()
@@ -189,8 +190,16 @@ private void grabAndNotify()
 
       for (int imageKey : getImageKeys())
       {
-         if (imageCollectors.containsKey(imageKey))
-            imageCollectors.get(imageKey).forEach(collector -> collector.add(getImage(imageKey)));
+         if (imageQueues.containsKey(imageKey))
+         {
+            imageQueues.get(imageKey).forEach(queue ->
+            {
+               if (queue.remainingCapacity() == 0)
+                  queue.remove().release();
+
+               queue.add(getImage(imageKey));
+            });
+         }
       }
    }
 }