From 3ce50510eefabc929d623fca78441c668523f9a3 Mon Sep 17 00:00:00 2001
From: Sunggoo Kim <sung.goo.kim@samsung.com>
Date: Fri, 11 Sep 2020 10:01:48 +0900
Subject: [PATCH 1/2] clear node/context handle on RclJava.cleanup()

---
 rcljava/src/main/java/org/ros2/rcljava/RCLJava.java | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java b/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java
index ef58b7ae..044e03f7 100644
--- a/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java
+++ b/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java
@@ -102,9 +102,12 @@ private static void cleanup() {
 
       node.dispose();
     }
+    nodes.clear();
+
     for (Context context : contexts) {
       context.dispose();
     }
+    contexts.clear();
   }
 
   static {

From bbff9b137a0d62b6eea98b9f11f1344dc80b5b03 Mon Sep 17 00:00:00 2001
From: Sunggoo Kim <sung.goo.kim@samsung.com>
Date: Thu, 17 Sep 2020 15:24:34 +0900
Subject: [PATCH 2/2] clear publishers, subscriptions, clients, services and
 timers on Node.dispose()

---
 .../main/java/org/ros2/rcljava/RCLJava.java   | 20 -------------------
 .../java/org/ros2/rcljava/node/NodeImpl.java  | 17 ++++++++++++++++
 2 files changed, 17 insertions(+), 20 deletions(-)

diff --git a/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java b/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java
index 044e03f7..75e2148d 100644
--- a/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java
+++ b/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java
@@ -80,26 +80,6 @@ private RCLJava() {}
 
   private static void cleanup() {
     for (Node node : nodes) {
-      for (Subscription subscription : node.getSubscriptions()) {
-        subscription.dispose();
-      }
-
-      for (Publisher publisher : node.getPublishers()) {
-        publisher.dispose();
-      }
-
-      for (Timer timer : node.getTimers()) {
-        timer.dispose();
-      }
-
-      for (Service service : node.getServices()) {
-        service.dispose();
-      }
-
-      for (Client client : node.getClients()) {
-        client.dispose();
-      }
-
       node.dispose();
     }
     nodes.clear();
diff --git a/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java
index 68018fa8..9fdc1c27 100644
--- a/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java
+++ b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java
@@ -24,6 +24,7 @@
 import org.ros2.rcljava.consumers.TriConsumer;
 import org.ros2.rcljava.contexts.Context;
 import org.ros2.rcljava.qos.QoSProfile;
+import org.ros2.rcljava.interfaces.Disposable;
 import org.ros2.rcljava.interfaces.MessageDefinition;
 import org.ros2.rcljava.interfaces.ServiceDefinition;
 import org.ros2.rcljava.parameters.ParameterType;
@@ -343,10 +344,26 @@ public final Collection<Client> getClients() {
    */
   private static native void nativeDispose(long handle);
 
+  private <T extends Disposable> void cleanupDisposables(Collection<T> disposables) {
+    for (Disposable disposable : disposables) {
+      disposable.dispose();
+    }
+    disposables.clear();
+  }
+
+  private void cleanup() {
+    cleanupDisposables(subscriptions);
+    cleanupDisposables(publishers);
+    cleanupDisposables(timers);
+    cleanupDisposables(services);
+    cleanupDisposables(clients);
+  }
+
   /**
    * {@inheritDoc}
    */
   public final void dispose() {
+    cleanup();
     nativeDispose(this.handle);
     this.handle = 0;
   }