Add methods to remove entities from Node

When an entity is disposed, make sure to also remove it from the Node.
This resolves an issue where invalid entities may be used by other classes or users.
For example, a disposed Subscription being accessed by the executor.

Fixes #105

Signed-off-by: Jacob Perron <jacob@openrobotics.org>

Author: jacobperron

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

@@ -131,6 +131,7 @@ public final Class<MessageDefinition> getResponseType() {
   public final void dispose() {
     Node node = this.nodeReference.get();
     if (node != null) {
+      node.removeClient(this);
       nativeDispose(node.getHandle(), this.handle);
       this.handle = 0;
     }

rcljava/src/main/java/org/ros2/rcljava/node/Node.java

@@ -122,6 +122,55 @@ <T extends ServiceDefinition> Client<T> createClient(
   <T extends ServiceDefinition> Client<T> createClient(final Class<T> serviceType,
       final String serviceName) throws NoSuchFieldException, IllegalAccessException;
 
+  /**
+   * Remove a Subscription created by this Node.
+   *
+   * Calling this method effectively invalidates the passed @{link Subscription}.
+   * If the subscription was not created by this Node, then nothing happens.
+   *
+   * @param subscription The object to remove from this node.
+   * @return true if the subscription was removed, false if the subscription was already
+   *   removed or was never created by this Node.
+   */
+  boolean removeSubscription(final Subscription subscription);
+
+  /**
+   * Remove a Publisher created by this Node.
+   *
+   * Calling this method effectively invalidates the passed @{link Publisher}.
+   * If the publisher was not created by this Node, then nothing happens.
+   *
+   * @param publisher The object to remove from this node.
+   * @return true if the publisher was removed, false if the publisher was already
+   *   removed or was never created by this Node.
+   */
+  boolean removePublisher(final Publisher publisher);
+
+  /**
+   * Remove a Service created by this Node.
+   *
+   * Calling this method effectively invalidates the passed @{link Service}.
+   * If the service was not created by this Node, then nothing happens.
+   *
+   * @param service The object to remove from this node.
+   * @return true if the service was removed, false if the service was already
+   *   removed or was never created by this Node.
+   */
+  boolean removeService(final Service service);
+
+  /**
+   * Remove a Client created by this Node.
+   *
+   * Calling this method effectively invalidates the passed @{link Client}.
+   * If the client was not created by this Node, then nothing happens.
+   *
+   * @param client The object to remove from this node.
+   * @return true if the client was removed, false if the client was already
+   *   removed or was never created by this Node.
+   */
+  boolean removeClient(final Client client);
+
+
   WallTimer createWallTimer(final long period, final TimeUnit unit, final Callback callback);
 
   String getName();

rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java

@@ -221,6 +221,20 @@ public final <T extends MessageDefinition> Subscription<T> createSubscription(
     return this.<T>createSubscription(messageType, topic, callback, QoSProfile.DEFAULT);
   }
 
+  /**
+   * {@inheritDoc}
+   */
+  public boolean removeSubscription(final Subscription subscription) {
+    return this.subscriptions.remove(subscription);
+  }
+
+  /**
+   * {@inheritDoc}
+   */
+  public boolean removePublisher(final Publisher publisher) {
+    return this.publishers.remove(publisher);
+  }
+
   /**
    * {@inheritDoc}
    */
@@ -300,6 +314,20 @@ public <T extends ServiceDefinition> Client<T> createClient(final Class<T> servi
   private static native <T extends ServiceDefinition> long nativeCreateClientHandle(
       long handle, Class<T> cls, String serviceName, long qosProfileHandle);
 
+  /**
+   * {@inheritDoc}
+   */
+  public boolean removeService(final Service service) {
+    return this.services.remove(service);
+  }
+
+  /**
+   * {@inheritDoc}
+   */
+  public boolean removeClient(final Client client) {
+    return this.clients.remove(client);
+  }
+
   /**
    * {@inheritDoc}
    */

rcljava/src/main/java/org/ros2/rcljava/publisher/PublisherImpl.java

@@ -117,6 +117,7 @@ public final WeakReference<Node> getNodeReference() {
   public final void dispose() {
     Node node = this.nodeReference.get();
     if (node != null) {
+      node.removePublisher(this);
       nativeDispose(node.getHandle(), this.handle);
       this.handle = 0;
     }

rcljava/src/main/java/org/ros2/rcljava/service/ServiceImpl.java

@@ -85,6 +85,7 @@ public final Class<MessageDefinition> getResponseType() {
   public final void dispose() {
     Node node = this.nodeReference.get();
     if (node != null) {
+      node.removeService(this);
       nativeDispose(node.getHandle(), this.handle);
       this.handle = 0;
     }

rcljava/src/main/java/org/ros2/rcljava/subscription/SubscriptionImpl.java

@@ -129,6 +129,7 @@ public final WeakReference<Node> getNodeReference() {
   public final void dispose() {
     Node node = this.nodeReference.get();
     if (node != null) {
+      node.removeSubscription(this);
       nativeDispose(node.getHandle(), this.handle);
       this.handle = 0;
     }

rcljava/src/test/java/org/ros2/rcljava/subscription/SubscriptionTest.java

@@ -26,7 +26,7 @@
 
 public class SubscriptionTest {
   @Test
-  public final void testCreate() {
+  public final void testCreateAndDispose() {
     RCLJava.rclJavaInit();
     Node node = RCLJava.createNode("test_node");
     Subscription<std_msgs.msg.String> subscription = node.<std_msgs.msg.String>createSubscription(
@@ -36,6 +36,14 @@ public void accept(final std_msgs.msg.String msg) {}
     assertEquals(node.getHandle(), subscription.getNodeReference().get().getHandle());
     assertNotEquals(0, subscription.getNodeReference().get().getHandle());
     assertNotEquals(0, subscription.getHandle());
+    assertEquals(1, node.getSubscriptions().size());
+
+    // We expect that calling dispose should result in a zero handle
+    // and the reference is dropped from the Node
+    subscription.dispose();
+    assertEquals(0, subscription.getHandle());
+    assertEquals(0, node.getSubscriptions().size());
+
     RCLJava.shutdown();
   }
 }