Implement nativeGetServiceNamesAndTypes method (osrf#33)

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

Author: ivanpauno

rcljava/include/org_ros2_rcljava_node_NodeImpl.h

@@ -110,6 +110,15 @@ JNIEXPORT void
 JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetTopicNamesAndTypes(
   JNIEnv *, jclass, jlong, jobject);
 
+/*
+ * Class:     org_ros2_rcljava_node_NodeImpl
+ * Method:    nativeGetServiceNamesAndTypes
+ * Signature: (JLjava/util/Collection;)V
+ */
+JNIEXPORT void
+JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetServiceNamesAndTypes(
+  JNIEnv *, jclass, jlong, jobject);
+
 /*
  * Class:     org_ros2_rcljava_node_NodeImpl
  * Method:    nativeGetPublishersInfo

rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp

@@ -320,16 +320,10 @@ Java_org_ros2_rcljava_node_NodeImpl_nativeGetNodeNames(
   }
 }
 
-JNIEXPORT void JNICALL
-Java_org_ros2_rcljava_node_NodeImpl_nativeGetTopicNamesAndTypes(
-  JNIEnv * env, jclass, jlong handle, jobject jnames_and_types)
+void
+fill_jnames_and_types(
+  JNIEnv * env, const rcl_names_and_types_t & names_and_types, 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");
@@ -343,34 +337,16 @@ Java_org_ros2_rcljava_node_NodeImpl_nativeGetTopicNamesAndTypes(
   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");
-  auto cleanup_names_and_types = rcpputils::make_scope_exit(
-    [pnames_and_types = &topic_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 topic names and types structure");
-      }
-    }
-  );
-
-  for (size_t i = 0; i < topic_names_and_types.names.size; i++) {
+  for (size_t i = 0; i < names_and_types.names.size; i++) {
     jobject jitem = env->NewObject(name_and_types_clazz, name_and_types_init_mid);
     RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env);
-    jstring jname = env->NewStringUTF(topic_names_and_types.names.data[i]);
+    jstring jname = env->NewStringUTF(names_and_types.names.data[i]);
     RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env);
     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]);
+    for (size_t j = 0; j < names_and_types.types[i].size; j++) {
+      jstring jtype = env->NewStringUTF(names_and_types.types[i].data[j]);
       env->CallBooleanMethod(jtypes, collection_add_mid, jtype);
       RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env);
     }
@@ -379,6 +355,59 @@ Java_org_ros2_rcljava_node_NodeImpl_nativeGetTopicNamesAndTypes(
   }
 }
 
+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;
+  }
+
+  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");
+  fill_jnames_and_types(env, topic_names_and_types, jnames_and_types);
+
+  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");
+  }
+}
+
+JNIEXPORT void JNICALL
+Java_org_ros2_rcljava_node_NodeImpl_nativeGetServiceNamesAndTypes(
+  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;
+  }
+
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  rcl_names_and_types_t service_names_and_types = rcl_get_zero_initialized_names_and_types();
+
+  rcl_ret_t ret = rcl_get_service_names_and_types(
+    node,
+    &allocator,
+    &service_names_and_types);
+  RCLJAVA_COMMON_THROW_FROM_RCL(env, ret, "failed to get service names and types");
+  fill_jnames_and_types(env, service_names_and_types, jnames_and_types);
+
+  ret = rcl_names_and_types_fini(&service_names_and_types);
+  if (!env->ExceptionCheck() && RCL_RET_OK != ret) {
+    rcljava_throw_rclexception(env, ret, "failed to fini service names and types structure");
+  }
+}
+
 template<typename FunctorT>
 void
 get_endpoint_info_common(

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

@@ -567,6 +567,14 @@ <T extends ServiceDefinition> Client<T> createClient(final Class<T> serviceType,
    */
   Collection<NameAndTypes> getTopicNamesAndTypes();
 
+  /**
+   * Return the service names and types that were detected in the graph.
+   * See @{link graph#NameAndTypes} for more information about the returned value.
+   *
+   * @return the detected service names and types.
+   */
+  Collection<NameAndTypes> getServiceNamesAndTypes();
+
   /**
    * Get information of all publishers in a topic.
    *

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

@@ -778,6 +778,15 @@ public final Collection<NameAndTypes> getTopicNamesAndTypes() {
   private static native final void nativeGetTopicNamesAndTypes(
     long handle, Collection<NameAndTypes> namesAndTypes);
 
+  public final Collection<NameAndTypes> getServiceNamesAndTypes() {
+    Collection<NameAndTypes> namesAndTypes = new ArrayList();
+    nativeGetServiceNamesAndTypes(this.handle, namesAndTypes);
+    return namesAndTypes;
+  }
+
+  private static native final void nativeGetServiceNamesAndTypes(
+    long handle, Collection<NameAndTypes> namesAndTypes);
+
   public final Collection<EndpointInfo> getPublishersInfo(final String topicName) {
     ArrayList<EndpointInfo> returnValue = new ArrayList();
     nativeGetPublishersInfo(this.handle, topicName, returnValue);

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

@@ -39,8 +39,10 @@
 import java.util.concurrent.TimeUnit;
 
 import org.ros2.rcljava.RCLJava;
+import org.ros2.rcljava.client.Client;
 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.executors.MultiThreadedExecutor;
 import org.ros2.rcljava.executors.SingleThreadedExecutor;
@@ -51,6 +53,8 @@
 import org.ros2.rcljava.publisher.Publisher;
 import org.ros2.rcljava.qos.policies.Reliability;
 import org.ros2.rcljava.qos.QoSProfile;
+import org.ros2.rcljava.service.RMWRequestId;
+import org.ros2.rcljava.service.Service;
 import org.ros2.rcljava.subscription.Subscription;
 
 public class NodeTest {
@@ -1011,6 +1015,67 @@ public void accept(final Collection<NameAndTypes> namesAndTypes) {
     subscription2.dispose();
   }
 
+  @Test
+  public final void testGetServiceNamesAndTypes() throws Exception {
+    Service<rcljava.srv.AddTwoInts> service = node.<rcljava.srv.AddTwoInts>createService(
+      rcljava.srv.AddTwoInts.class, "test_service_names_and_types_one",
+      new TriConsumer<
+        RMWRequestId, rcljava.srv.AddTwoInts_Request, rcljava.srv.AddTwoInts_Response>()
+      {
+        public final void accept(
+          final RMWRequestId header,
+          final rcljava.srv.AddTwoInts_Request request,
+          final rcljava.srv.AddTwoInts_Response response)
+        {}
+      });
+    Client<rcljava.srv.AddTwoInts> client = node.<rcljava.srv.AddTwoInts>createClient(
+      rcljava.srv.AddTwoInts.class, "test_service_names_and_types_two");
+
+    Consumer<Collection<NameAndTypes>> validateNameAndTypes =
+    new Consumer<Collection<NameAndTypes>>() {
+      public void accept(final Collection<NameAndTypes> namesAndTypes) {
+        assertEquals(namesAndTypes.size(), 2);
+        assertTrue(
+          "service 'test_service_names_and_types_one' was not discovered",
+          namesAndTypes.contains(
+            new NameAndTypes(
+              "/test_service_names_and_types_one",
+              new ArrayList(Arrays.asList("rcljava/srv/AddTwoInts")))));
+        assertTrue(
+          "service 'test_service_names_and_types_two' was not discovered",
+          namesAndTypes.contains(
+            new NameAndTypes(
+              "/test_service_names_and_types_two",
+              new ArrayList(Arrays.asList("rcljava/srv/AddTwoInts")))));
+      }
+    };
+
+    long start = System.currentTimeMillis();
+    boolean ok = false;
+    Collection<NameAndTypes> namesAndTypes = null;
+    do {
+      namesAndTypes = this.node.getServiceNamesAndTypes();
+      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);
+
+    service.dispose();
+    client.dispose();
+  }
+
   @Test
   public final void testGetPublishersInfo() {
     Publisher<rcljava.msg.UInt32> publisher =