Add get publishers by node (osrf#34)

Signed-off-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>

Author: ivanpauno

rcljava/include/org_ros2_rcljava_node_NodeImpl.h

@@ -137,6 +137,15 @@ JNIEXPORT void
 JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetSubscriptionsInfo(
   JNIEnv *, jclass, jlong, jstring, jobject);
 
+/*
+ * Class:     org_ros2_rcljava_node_NodeImpl
+ * Method:    nativeGetPublisherNamesAndTypesByNode
+ * Signature: (JLjava/lang/String;Ljava/lang/String;Ljava/util/Collection;)V
+ */
+JNIEXPORT void
+JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetPublisherNamesAndTypesByNode(
+  JNIEnv *, jclass, jlong, jstring, jstring, jobject);
+
 #ifdef __cplusplus
 }
 #endif

rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp

@@ -487,3 +487,41 @@ Java_org_ros2_rcljava_node_NodeImpl_nativeGetSubscriptionsInfo(
   get_endpoint_info_common(
     env, handle, jtopic_name, jsubscriptions_info, rcl_get_subscriptions_info_by_topic);
 }
+
+JNIEXPORT void JNICALL
+Java_org_ros2_rcljava_node_NodeImpl_nativeGetPublisherNamesAndTypesByNode(
+  JNIEnv * env, jclass, jlong handle, jstring jname, jstring jnamespace, jobject jnames_and_types)
+{
+  rcl_node_t * node = reinterpret_cast<rcl_node_t *>(handle);
+  if (!node) {
+    rcljava_throw_exception(env, "java/lang/IllegalArgumentException", "node handle is NULL");
+    return;
+  }
+
+  const char * name = env->GetStringUTFChars(jname, NULL);
+  auto release_jname = rcpputils::make_scope_exit(
+    [jname, name, env]() {env->ReleaseStringUTFChars(jname, name);});
+  const char * namespace_ = env->GetStringUTFChars(jnamespace, NULL);
+  auto release_jnamespace = rcpputils::make_scope_exit(
+    [jnamespace, namespace_, env]() {env->ReleaseStringUTFChars(jnamespace, namespace_);});
+
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  rcl_names_and_types_t publisher_names_and_types = rcl_get_zero_initialized_names_and_types();
+  auto fini_names_and_types = rcpputils::make_scope_exit(
+    [pnames_and_types = &publisher_names_and_types, env]() {
+      rcl_ret_t ret = rcl_names_and_types_fini(pnames_and_types);
+      if (!env->ExceptionCheck() && RCL_RET_OK != ret) {
+        rcljava_throw_rclexception(env, ret, "failed to fini publisher names and types structure");
+      }
+    });
+
+  rcl_ret_t ret = rcl_get_publisher_names_and_types_by_node(
+    node,
+    &allocator,
+    false,
+    name,
+    namespace_,
+    &publisher_names_and_types);
+  RCLJAVA_COMMON_THROW_FROM_RCL(env, ret, "failed to get publisher names and types");
+  fill_jnames_and_types(env, publisher_names_and_types, jnames_and_types);
+}

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

@@ -598,4 +598,15 @@ <T extends ServiceDefinition> Client<T> createClient(final Class<T> serviceType,
    *    passed topic.
    */
   Collection<EndpointInfo> getSubscriptionsInfo(final String topicName);
+
+  /**
+   * Return the publisher names and types that were created from the node specified by the given
+   * node name and namespace.
+   * See @{link graph#NameAndTypes} for more information about the returned value.
+   *
+   * @param nodeName name of the node we want to know its publishers.
+   * @param nodeNamespace namespace of the node we want to know its publishers.
+   * @return the detected publisher names and types.
+   */
+  Collection<NameAndTypes> getPublisherNamesAndTypesByNode(String nodeName, String nodeNamespace);
 }

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

@@ -800,4 +800,15 @@ public final Collection<EndpointInfo> getSubscriptionsInfo(final String topicNam
 
   private native static final void nativeGetSubscriptionsInfo(
     final long handle, final String topicName, ArrayList<EndpointInfo> endpointInfo);
+
+  public final Collection<NameAndTypes> getPublisherNamesAndTypesByNode(
+    String nodeName, String nodeNamespace)
+  {
+    Collection<NameAndTypes> namesAndTypes = new ArrayList();
+    nativeGetPublisherNamesAndTypesByNode(this.handle, nodeName, nodeNamespace, namesAndTypes);
+    return namesAndTypes;
+  }
+
+  private static native final Collection<NameAndTypes> nativeGetPublisherNamesAndTypesByNode(
+    long handle, String nodeName, String nodeNamespace, Collection<NameAndTypes> namesAndTypes);
 }

rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java

@@ -42,6 +42,7 @@
 import org.ros2.rcljava.client.Client;
 import org.ros2.rcljava.concurrent.RCLFuture;
 import org.ros2.rcljava.consumers.Consumer;
+import org.ros2.rcljava.consumers.BiConsumer;
 import org.ros2.rcljava.consumers.TriConsumer;
 import org.ros2.rcljava.executors.Executor;
 import org.ros2.rcljava.executors.MultiThreadedExecutor;
@@ -1186,4 +1187,89 @@ public void accept(final Collection<EndpointInfo> info) {
     subscription.dispose();
     subscription2.dispose();
   }
+
+  @Test
+  public final void testGetPublisherNamesAndTypesByNode() throws Exception {
+    final Node remoteNode = RCLJava.createNode("test_get_publisher_names_and_types_remote_node");
+    Publisher<rcljava.msg.UInt32> publisher1 = node.<rcljava.msg.UInt32>createPublisher(
+      rcljava.msg.UInt32.class, "test_get_publisher_names_and_types_one");
+    Publisher<rcljava.msg.UInt32> publisher2 = node.<rcljava.msg.UInt32>createPublisher(
+      rcljava.msg.UInt32.class, "test_get_publisher_names_and_types_two");
+    Publisher<rcljava.msg.UInt32> publisher3 = remoteNode.<rcljava.msg.UInt32>createPublisher(
+      rcljava.msg.UInt32.class, "test_get_publisher_names_and_types_two");
+    Publisher<rcljava.msg.UInt32> publisher4 = remoteNode.<rcljava.msg.UInt32>createPublisher(
+      rcljava.msg.UInt32.class, "test_get_publisher_names_and_types_three");
+    Subscription<rcljava.msg.Empty> subscription = node.<rcljava.msg.Empty>createSubscription(
+      rcljava.msg.Empty.class, "test_get_topic_names_and_types_this_should_not_appear",
+      new Consumer<rcljava.msg.Empty>() {
+        public void accept(final rcljava.msg.Empty msg) {}
+      });
+
+    BiConsumer<Collection<NameAndTypes>, Collection<NameAndTypes>> validateNameAndTypes =
+    new BiConsumer<Collection<NameAndTypes>, Collection<NameAndTypes>>() {
+      public void accept(final Collection<NameAndTypes> local, Collection<NameAndTypes> remote) {
+        // TODO(ivanpauno): Using assertj may help a lot here https://assertj.github.io/doc/.
+        assertEquals(local.size(), 2);
+        assertTrue(
+          "topic 'test_get_publisher_names_and_types_one' was not discovered for local node",
+          local.contains(
+            new NameAndTypes(
+              "/test_get_publisher_names_and_types_one",
+              new ArrayList(Arrays.asList("rcljava/msg/UInt32")))));
+        assertTrue(
+          "topic 'test_get_publisher_names_and_types_two' was not discovered for local node",
+          local.contains(
+            new NameAndTypes(
+              "/test_get_publisher_names_and_types_two",
+              new ArrayList(Arrays.asList("rcljava/msg/UInt32")))));
+
+        assertEquals(remote.size(), 2);
+        assertTrue(
+          "topic 'test_get_publisher_names_and_types_two' was not discovered for remote node",
+          remote.contains(
+            new NameAndTypes(
+              "/test_get_publisher_names_and_types_two",
+              new ArrayList(Arrays.asList("rcljava/msg/UInt32")))));
+        assertTrue(
+          "topic 'test_get_publisher_names_and_types_three' was not discovered for remote node",
+          remote.contains(
+            new NameAndTypes(
+              "/test_get_publisher_names_and_types_three",
+              new ArrayList(Arrays.asList("rcljava/msg/UInt32")))));
+      }
+    };
+
+    long start = System.currentTimeMillis();
+    boolean ok = false;
+    Collection<NameAndTypes> local = null;
+    Collection<NameAndTypes> remote = null;
+    do {
+      local = this.node.getPublisherNamesAndTypesByNode("test_node", "/");
+      remote = this.node.getPublisherNamesAndTypesByNode(
+        "test_get_publisher_names_and_types_remote_node", "/");
+      try {
+        validateNameAndTypes.accept(local, remote);
+        ok = true;
+      } catch (AssertionError err) {
+        // ignore here, it's going to be validated again at the end.
+      }
+      // TODO(ivanpauno): We could wait for the graph guard condition to be triggered if that
+      // would be available.
+      try {
+        TimeUnit.MILLISECONDS.sleep(100);
+      } catch (InterruptedException err) {
+        // ignore
+      }
+    } while (!ok && System.currentTimeMillis() < start + 1000);
+    assertNotNull(local);
+    assertNotNull(remote);
+    validateNameAndTypes.accept(local, remote);
+
+    publisher1.dispose();
+    publisher2.dispose();
+    publisher3.dispose();
+    publisher4.dispose();
+    subscription.dispose();
+    remoteNode.dispose();
+  }
 }