From 604268e425f951007a8bd3a2f5c242b86f054116 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 8 Sep 2020 13:27:08 -0300 Subject: [PATCH] Implement nativeGetServiceNamesAndTypes method Signed-off-by: Ivan Santiago Paunovic --- .../include/org_ros2_rcljava_node_NodeImpl.h | 9 ++ .../cpp/org_ros2_rcljava_node_NodeImpl.cpp | 91 ++++++++++++------- .../main/java/org/ros2/rcljava/node/Node.java | 8 ++ .../java/org/ros2/rcljava/node/NodeImpl.java | 9 ++ .../java/org/ros2/rcljava/node/NodeTest.java | 65 +++++++++++++ 5 files changed, 151 insertions(+), 31 deletions(-) diff --git a/rcljava/include/org_ros2_rcljava_node_NodeImpl.h b/rcljava/include/org_ros2_rcljava_node_NodeImpl.h index de24e820..75b52e61 100644 --- a/rcljava/include/org_ros2_rcljava_node_NodeImpl.h +++ b/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 diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp index 9ef2c287..0e85cf92 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp @@ -332,16 +332,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(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"); @@ -355,34 +349,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); } @@ -391,6 +367,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(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(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 void get_endpoint_info_common( diff --git a/rcljava/src/main/java/org/ros2/rcljava/node/Node.java b/rcljava/src/main/java/org/ros2/rcljava/node/Node.java index ceb7d3ec..5059adee 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/Node.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/Node.java @@ -567,6 +567,14 @@ Client createClient(final Class serviceType, */ Collection 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 getServiceNamesAndTypes(); + /** * Get information of all publishers in a topic. * 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 9bab0a29..c253bb03 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java @@ -759,6 +759,15 @@ public final Collection getTopicNamesAndTypes() { private static native final void nativeGetTopicNamesAndTypes( long handle, Collection namesAndTypes); + public final Collection getServiceNamesAndTypes() { + Collection namesAndTypes = new ArrayList(); + nativeGetServiceNamesAndTypes(this.handle, namesAndTypes); + return namesAndTypes; + } + + private static native final void nativeGetServiceNamesAndTypes( + long handle, Collection namesAndTypes); + public final Collection getPublishersInfo(final String topicName) { ArrayList returnValue = new ArrayList(); nativeGetPublishersInfo(this.handle, topicName, returnValue); diff --git a/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java b/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java index a431075e..c2d99da9 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java @@ -38,8 +38,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; @@ -50,6 +52,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 { @@ -999,6 +1003,67 @@ public void accept(final Collection namesAndTypes) { subscription2.dispose(); } + @Test + public final void testGetServiceNamesAndTypes() throws Exception { + Service service = node.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 client = node.createClient( + rcljava.srv.AddTwoInts.class, "test_service_names_and_types_two"); + + Consumer> validateNameAndTypes = + new Consumer>() { + public void accept(final Collection 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 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 publisher =