Add method to prune pending requests in client (#82)

Signed-off-by: Ivan Santiago Paunovic <[email protected]>

Author: ivanpauno

rcljava/src/main/java/org/ros2/rcljava/client/Client.java

@@ -38,6 +38,20 @@ <U extends MessageDefinition, V extends MessageDefinition> ResponseFuture<V> asy
 
   <V extends MessageDefinition> boolean removePendingRequest(ResponseFuture<V> future);
 
+  /**
+   * Remove old pending requests that this client has done.
+   * Future responses to removed requests will be ignored.
+   */
+  long prunePendingRequests();
+
+  /**
+   * Remove old pending requests that where done before the specified time point.
+   * Future responses to removed requests will be ignored.
+   *
+   * @param nanoTime requests done before this timepoint will be removed.
+   */
+  long prunePendingRequestsOlderThan(long nanoTime);
+
   /**
    * Check if the service server is available.
    *

rcljava/src/main/java/org/ros2/rcljava/client/ClientImpl.java

@@ -22,6 +22,7 @@
 import java.lang.Long;
 import java.util.AbstractMap;
 import java.util.HashMap;
+import java.util.Iterator;
 import java.util.Map;
 import java.util.concurrent.Future;
 import java.util.concurrent.TimeUnit;
@@ -52,7 +53,25 @@ public class ClientImpl<T extends ServiceDefinition> implements Client<T> {
   private final WeakReference<Node> nodeReference;
   private long handle;
   private final String serviceName;
-  private Map<Long, Map.Entry<Consumer, ResponseFuture>> pendingRequests;
+
+  private class PendingRequest
+  {
+    public Consumer callback;
+    public ResponseFuture future;
+    public long requestTimestamp;
+
+    public PendingRequest(
+      final Consumer callback,
+      final ResponseFuture future,
+      final long requestTimestamp)
+    {
+      this.callback = callback;
+      this.future = future;
+      this.requestTimestamp = requestTimestamp;
+    }
+  }
+
+  private Map<Long, PendingRequest> pendingRequests;
 
   private final ServiceDefinition serviceDefinition;
 
@@ -66,7 +85,7 @@ public ClientImpl(
     this.handle = handle;
     this.serviceName = serviceName;
     this.serviceDefinition = serviceDefinition;
-    this.pendingRequests = new HashMap<Long, Map.Entry<Consumer, ResponseFuture>>();
+    this.pendingRequests = new HashMap<Long, PendingRequest>();
   }
 
   public ServiceDefinition getServiceDefinition() {
@@ -88,8 +107,7 @@ public void accept(Future<V> input) {}
           request.getDestructorInstance(), request);
       ResponseFuture<V> future = new ResponseFuture<V>(sequenceNumber);
 
-      Map.Entry<Consumer, ResponseFuture> entry =
-          new AbstractMap.SimpleEntry<Consumer, ResponseFuture>(callback, future);
+      PendingRequest entry = new PendingRequest(callback, future, System.nanoTime());
       pendingRequests.put(sequenceNumber, entry);
       return future;
     }
@@ -98,20 +116,44 @@ public void accept(Future<V> input) {}
   public final <V extends MessageDefinition> boolean
   removePendingRequest(ResponseFuture<V> future) {
     synchronized (pendingRequests) {
-      Map.Entry<Consumer, ResponseFuture> entry = pendingRequests.remove(
+      PendingRequest entry = pendingRequests.remove(
         future.getRequestSequenceNumber());
       return entry != null;
     }
   }
 
+  public final long
+  prunePendingRequests() {
+    synchronized (pendingRequests) {
+      long size = pendingRequests.size();
+      pendingRequests.clear();
+      return size;
+    }
+  }
+
+  public final long
+  prunePendingRequestsOlderThan(long nanoTime) {
+    synchronized (pendingRequests) {
+      Iterator<Map.Entry<Long, PendingRequest>> iter = pendingRequests.entrySet().iterator();
+      long removed = 0;
+      while(iter.hasNext()) {
+        if(iter.next().getValue().requestTimestamp < nanoTime) {
+          iter.remove();
+          ++removed;
+        }
+      }
+      return removed;
+    }
+  }
+
   public final <U extends MessageDefinition> void handleResponse(
       final RMWRequestId header, final U response) {
     synchronized (pendingRequests) {
       long sequenceNumber = header.sequenceNumber;
-      Map.Entry<Consumer, ResponseFuture> entry = pendingRequests.remove(sequenceNumber);
+      PendingRequest entry = pendingRequests.remove(sequenceNumber);
       if (entry != null) {
-        Consumer<Future> callback = entry.getKey();
-        ResponseFuture<U> future = entry.getValue();
+        Consumer<Future> callback = entry.callback;
+        ResponseFuture<U> future = entry.future;
         future.set(response);
         callback.accept(future);
         return;

rcljava/src/test/java/org/ros2/rcljava/client/ClientTest.java

@@ -37,6 +37,7 @@
 
 import org.ros2.rcljava.RCLJava;
 import org.ros2.rcljava.concurrent.RCLFuture;
+import org.ros2.rcljava.consumers.Consumer;
 import org.ros2.rcljava.consumers.TriConsumer;
 import org.ros2.rcljava.executors.Executor;
 import org.ros2.rcljava.node.Node;
@@ -157,4 +158,36 @@ public final void testRemovePendingRequest() throws Exception {
     assertTrue(client.removePendingRequest(responseFuture));
     assertFalse(client.removePendingRequest(responseFuture));
   }
+
+  @Test
+  public final void testPrunePendingRequestsOlderThan() throws Exception {
+    RCLFuture<rcljava.srv.AddTwoInts_Response> consumerFuture =
+        new RCLFuture<rcljava.srv.AddTwoInts_Response>();
+
+    TestClientConsumer clientConsumer = new TestClientConsumer(consumerFuture);
+
+    Service<rcljava.srv.AddTwoInts> service = node.<rcljava.srv.AddTwoInts>createService(
+        rcljava.srv.AddTwoInts.class, "add_two_ints", clientConsumer);
+
+    rcljava.srv.AddTwoInts_Request request = new rcljava.srv.AddTwoInts_Request();
+    request.setA(2);
+    request.setB(3);
+
+    Client<rcljava.srv.AddTwoInts> client =
+        node.<rcljava.srv.AddTwoInts>createClient(
+          rcljava.srv.AddTwoInts.class, "add_two_ints");
+
+    assertTrue(client.waitForService(Duration.ofSeconds(10)));
+
+    long oldNanoTime = System.nanoTime();
+    ResponseFuture<rcljava.srv.AddTwoInts_Response> responseFuture = client.asyncSendRequest(
+        request,
+        new Consumer<Future<rcljava.srv.AddTwoInts_Response>>() {
+            public final void accept(final Future<rcljava.srv.AddTwoInts_Response> futureResponse) {}
+        });
+
+    assertEquals(0, client.prunePendingRequestsOlderThan(oldNanoTime));
+    assertEquals(1, client.prunePendingRequestsOlderThan(System.nanoTime()));
+    assertEquals(0, client.prunePendingRequestsOlderThan(System.nanoTime()));
+  }
 }