Add get topic names and type method to Node class (#32)

* Add NameAndTypes class
* Add getTopicNamesAndTypes method to Node

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

Author: ivanpauno

rcljava/CMakeLists.txt

@@ -141,6 +141,7 @@ set(${PROJECT_NAME}_sources
   "src/main/java/org/ros2/rcljava/events/EventStatus.java"
   "src/main/java/org/ros2/rcljava/events/PublisherEventStatus.java"
   "src/main/java/org/ros2/rcljava/events/SubscriptionEventStatus.java"
+  "src/main/java/org/ros2/rcljava/graph/NameAndTypes.java"
   "src/main/java/org/ros2/rcljava/publisher/statuses/LivelinessLost.java"
   "src/main/java/org/ros2/rcljava/publisher/statuses/OfferedDeadlineMissed.java"
   "src/main/java/org/ros2/rcljava/publisher/statuses/OfferedQosIncompatible.java"

rcljava/include/org_ros2_rcljava_node_NodeImpl.h

@@ -92,6 +92,15 @@ JNIEXPORT jlong
 JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeCreateTimerHandle(
   JNIEnv *, jclass, jlong, jlong, jlong);
 
+/*
+ * Class:     org_ros2_rcljava_node_NodeImpl
+ * Method:    nativeGetTopicNamesAndTypes
+ * Signature: (JLjava/util/Collection;)V
+ */
+JNIEXPORT void
+JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetTopicNamesAndTypes(
+  JNIEnv *, jclass, jlong, jobject);
+
 #ifdef __cplusplus
 }
 #endif

rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp

@@ -19,6 +19,7 @@
 #include <string>
 
 #include "rcl/error_handling.h"
+#include "rcl/graph.h"
 #include "rcl/node.h"
 #include "rcl/rcl.h"
 #include "rmw/rmw.h"
@@ -29,6 +30,7 @@
 
 #include "org_ros2_rcljava_node_NodeImpl.h"
 
+using rcljava_common::exceptions::rcljava_throw_exception;
 using rcljava_common::exceptions::rcljava_throw_rclexception;
 
 JNIEXPORT jstring JNICALL
@@ -250,3 +252,60 @@ Java_org_ros2_rcljava_node_NodeImpl_nativeCreateTimerHandle(
   jlong jtimer = reinterpret_cast<jlong>(timer);
   return jtimer;
 }
+
+JNIEXPORT void JNICALL
+Java_org_ros2_rcljava_node_NodeImpl_nativeGetTopicNamesAndTypes(
+  JNIEnv * env, jclass, jlong handle, 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;
+  }
+
+  jclass collection_clazz = env->FindClass("java/util/Collection");
+  jmethodID collection_add_mid = env->GetMethodID(
+    collection_clazz, "add", "(Ljava/lang/Object;)Z");
+  RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env);
+  jclass name_and_types_clazz = env->FindClass("org/ros2/rcljava/graph/NameAndTypes");
+  RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env);
+  jmethodID name_and_types_init_mid = env->GetMethodID(name_and_types_clazz, "<init>", "()V");
+  RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env);
+  jfieldID name_fid = env->GetFieldID(name_and_types_clazz, "name", "Ljava/lang/String;");
+  RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env);
+  jfieldID types_fid = env->GetFieldID(name_and_types_clazz, "types", "Ljava/util/Collection;");
+  RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env);
+
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  rcl_names_and_types_t topic_names_and_types = rcl_get_zero_initialized_names_and_types();
+
+  rcl_ret_t ret = rcl_get_topic_names_and_types(
+    node,
+    &allocator,
+    false,
+    &topic_names_and_types);
+  RCLJAVA_COMMON_THROW_FROM_RCL(env, ret, "failed to get topic names and types");
+
+  for (size_t i = 0; i < topic_names_and_types.names.size; i++) {
+    jobject jitem = env->NewObject(name_and_types_clazz, name_and_types_init_mid);
+    RCLJAVA_COMMON_CHECK_FOR_EXCEPTION_WITH_ERROR_STATEMENT(env, goto cleanup);
+    jstring jname = env->NewStringUTF(topic_names_and_types.names.data[i]);
+    RCLJAVA_COMMON_CHECK_FOR_EXCEPTION_WITH_ERROR_STATEMENT(env, goto cleanup);
+    env->SetObjectField(jitem, name_fid, jname);
+    // the default constructor already inits types to an empty ArrayList
+    jobject jtypes = env->GetObjectField(jitem, types_fid);
+    for (size_t j = 0; j < topic_names_and_types.types[i].size; j++) {
+      jstring jtype = env->NewStringUTF(topic_names_and_types.types[i].data[j]);
+      env->CallBooleanMethod(jtypes, collection_add_mid, jtype);
+      RCLJAVA_COMMON_CHECK_FOR_EXCEPTION_WITH_ERROR_STATEMENT(env, goto cleanup);
+    }
+    env->CallBooleanMethod(jnames_and_types, collection_add_mid, jitem);
+    RCLJAVA_COMMON_CHECK_FOR_EXCEPTION_WITH_ERROR_STATEMENT(env, goto cleanup);
+  }
+
+cleanup:
+  ret = rcl_names_and_types_fini(&topic_names_and_types);
+  if (!env->ExceptionCheck() && RCL_RET_OK != ret) {
+    rcljava_throw_rclexception(env, ret, "failed to fini topic names and types structure");
+  }
+}

rcljava/src/main/java/org/ros2/rcljava/graph/NameAndTypes.java

@@ -0,0 +1,73 @@
+/* Copyright 2020 Open Source Robotics Foundation, Inc.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+package org.ros2.rcljava.graph;
+
+import java.util.ArrayList;
+import java.util.Collection;
+import java.util.Objects;
+
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
+
+import org.ros2.rcljava.common.JNIUtils;
+
+/**
+ * Class that represents a topic/service/action name, together with all the types
+ * of message/service/action that are using that name.
+ */
+public class NameAndTypes {
+  /// Name of the topic/service/action.
+  public String name;
+  /// Types of the topic/service/action with the given name.
+  public Collection<String> types;
+
+  /**
+   * Construct from given name and types.
+   *
+   * @param name name of the topic/service/action.
+   * @param types types of the given topic/service/action.
+   *  A shallow copy of the given collection will be stored,
+   *  but given that String is immutable, this is not a problem.
+   * @param typesSize size of the \a typesHandle array.
+   */
+  public NameAndTypes(final String name, final Collection<String> types) {
+    this.name = name;
+    this.types = new ArrayList(types);
+  }
+
+  /// @internal Default constructor, only used from jni code.
+  private NameAndTypes() {
+    this.types = new ArrayList();
+  }
+
+  @Override
+  public boolean equals(final Object o) {
+    if (o == this) {
+      return true;
+    }
+    if (!(o instanceof NameAndTypes)) {
+      return false;
+    }
+    NameAndTypes other = (NameAndTypes) o;
+    return Objects.equals(this.name, other.name) &&
+      Objects.equals(this.types, other.types);
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(this.name, this.types);
+  }
+}

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

@@ -24,6 +24,7 @@
 import org.ros2.rcljava.concurrent.Callback;
 import org.ros2.rcljava.consumers.Consumer;
 import org.ros2.rcljava.consumers.TriConsumer;
+import org.ros2.rcljava.graph.NameAndTypes;
 import org.ros2.rcljava.qos.QoSProfile;
 import org.ros2.rcljava.interfaces.Disposable;
 import org.ros2.rcljava.interfaces.MessageDefinition;
@@ -549,4 +550,12 @@ <T extends ServiceDefinition> Client<T> createClient(final Class<T> serviceType,
    * @return rcl_interfaces.msg.ListParametersResult
    */
   rcl_interfaces.msg.ListParametersResult listParameters(List<String> prefixes, long depth);
+
+  /**
+   * Return the topics names and types that were detected in the graph.
+   * See @{link graph#NameAndTypes} for more information about the returned value.
+   *
+   * @return the detected topic names and types.
+   */
+  Collection<NameAndTypes> getTopicNamesAndTypes();
 }

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

@@ -23,6 +23,7 @@
 import org.ros2.rcljava.consumers.Consumer;
 import org.ros2.rcljava.consumers.TriConsumer;
 import org.ros2.rcljava.contexts.Context;
+import org.ros2.rcljava.graph.NameAndTypes;
 import org.ros2.rcljava.qos.QoSProfile;
 import org.ros2.rcljava.interfaces.MessageDefinition;
 import org.ros2.rcljava.interfaces.ServiceDefinition;
@@ -738,4 +739,13 @@ public rcl_interfaces.msg.ListParametersResult listParameters(
       return result;
     }
   }
+
+  public final Collection<NameAndTypes> getTopicNamesAndTypes() {
+    Collection<NameAndTypes> namesAndTypes = new ArrayList();
+    nativeGetTopicNamesAndTypes(this.handle, namesAndTypes);
+    return namesAndTypes;
+  }
+
+  private static native final void nativeGetTopicNamesAndTypes(
+    long handle, Collection<NameAndTypes> namesAndTypes);
 }

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

@@ -17,6 +17,7 @@
 
 import static org.junit.Assert.assertEquals;
 import static org.junit.Assert.assertNotEquals;
+import static org.junit.Assert.assertNotNull;
 import static org.junit.Assert.assertTrue;
 
 import org.junit.After;
@@ -27,7 +28,10 @@
 
 import java.lang.ref.WeakReference;
 
+import java.util.concurrent.TimeUnit;
+import java.util.ArrayList;
 import java.util.Arrays;
+import java.util.Collection;
 import java.util.List;
 
 import org.ros2.rcljava.RCLJava;
@@ -36,6 +40,7 @@
 import org.ros2.rcljava.executors.Executor;
 import org.ros2.rcljava.executors.MultiThreadedExecutor;
 import org.ros2.rcljava.executors.SingleThreadedExecutor;
+import org.ros2.rcljava.graph.NameAndTypes;
 import org.ros2.rcljava.node.Node;
 import org.ros2.rcljava.publisher.Publisher;
 import org.ros2.rcljava.subscription.Subscription;
@@ -857,4 +862,75 @@ public Node getNode() {
     subscriptionTwo.dispose();
     assertEquals(0, subscriptionTwo.getHandle());
   }
+
+  @Test
+  public final void testGetTopicNamesAndTypes() throws Exception {
+    Publisher<rcljava.msg.UInt32> publisher = node.<rcljava.msg.UInt32>createPublisher(
+      rcljava.msg.UInt32.class, "test_get_topic_names_and_types_one");
+    Publisher<rcljava.msg.UInt32> publisher2 = node.<rcljava.msg.UInt32>createPublisher(
+      rcljava.msg.UInt32.class, "test_get_topic_names_and_types_two");
+    Subscription<rcljava.msg.Empty> subscription = node.<rcljava.msg.Empty>createSubscription(
+      rcljava.msg.Empty.class, "test_get_topic_names_and_types_one",
+      new Consumer<rcljava.msg.Empty>() {
+        public void accept(final rcljava.msg.Empty msg) {}
+      });
+    Subscription<rcljava.msg.Empty> subscription2 = node.<rcljava.msg.Empty>createSubscription(
+      rcljava.msg.Empty.class, "test_get_topic_names_and_types_three",
+      new Consumer<rcljava.msg.Empty>() {
+        public void accept(final rcljava.msg.Empty msg) {}
+      });
+
+    Consumer<Collection<NameAndTypes>> validateNameAndTypes =
+    new Consumer<Collection<NameAndTypes>>() {
+      public void accept(final Collection<NameAndTypes> namesAndTypes) {
+        // TODO(ivanpauno): Using assertj may help a lot here https://assertj.github.io/doc/.
+        assertEquals(namesAndTypes.size(), 3);
+        assertTrue(
+          "topic 'test_get_topic_names_and_types_one' was not discovered",
+          namesAndTypes.contains(
+            new NameAndTypes(
+              "/test_get_topic_names_and_types_one",
+              new ArrayList(Arrays.asList("rcljava/msg/Empty", "rcljava/msg/UInt32")))));
+        assertTrue(
+          "topic 'test_get_topic_names_and_types_two' was not discovered",
+          namesAndTypes.contains(
+            new NameAndTypes(
+              "/test_get_topic_names_and_types_two",
+              new ArrayList(Arrays.asList("rcljava/msg/UInt32")))));
+        assertTrue(
+          "topic 'test_get_topic_names_and_types_three' was not discovered",
+          namesAndTypes.contains(
+            new NameAndTypes(
+              "/test_get_topic_names_and_types_three",
+              new ArrayList(Arrays.asList("rcljava/msg/Empty")))));
+      }
+    };
+
+    long start = System.currentTimeMillis();
+    boolean ok = false;
+    Collection<NameAndTypes> namesAndTypes = null;
+    do {
+      namesAndTypes = this.node.getTopicNamesAndTypes();
+      try {
+        validateNameAndTypes.accept(namesAndTypes);
+        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(namesAndTypes);
+    validateNameAndTypes.accept(namesAndTypes);
+
+    publisher.dispose();
+    publisher2.dispose();
+    subscription.dispose();
+    subscription2.dispose();
+  }
 }