From 49e0bb85bfa01753c88a4409ac0fb6b1509a8827 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 11 Sep 2020 15:48:14 -0300 Subject: [PATCH 1/3] Add getClientNamesAndTypes by Node Signed-off-by: Ivan Santiago Paunovic --- .../include/org_ros2_rcljava_node_NodeImpl.h | 9 ++ .../cpp/org_ros2_rcljava_node_NodeImpl.cpp | 32 +++++++ .../main/java/org/ros2/rcljava/node/Node.java | 12 +++ .../java/org/ros2/rcljava/node/NodeImpl.java | 11 +++ .../java/org/ros2/rcljava/node/NodeTest.java | 91 +++++++++++++++++++ 5 files changed, 155 insertions(+) diff --git a/rcljava/include/org_ros2_rcljava_node_NodeImpl.h b/rcljava/include/org_ros2_rcljava_node_NodeImpl.h index caedfbe5..8a4bc582 100644 --- a/rcljava/include/org_ros2_rcljava_node_NodeImpl.h +++ b/rcljava/include/org_ros2_rcljava_node_NodeImpl.h @@ -164,6 +164,15 @@ JNIEXPORT void JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetServiceNamesAndTypesByNode( JNIEnv *, jclass, jlong, jstring, jstring, jobject); +/* + * Class: org_ros2_rcljava_node_NodeImpl + * Method: nativeGetClientNamesAndTypesByNode + * Signature: (JLjava/lang/String;Ljava/lang/String;Ljava/util/Collection;)V + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetClientNamesAndTypesByNode( + JNIEnv *, jclass, jlong, jstring, jstring, jobject); + #ifdef __cplusplus } #endif 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 2f206832..cb337702 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp @@ -611,3 +611,35 @@ JNIEXPORT void JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetServiceNames 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); } + +JNIEXPORT void JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetClientNamesAndTypesByNode( + JNIEnv * env, jclass, jlong handle, jstring jname, jstring jnamespace, 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; + } + + const char * name = env->GetStringUTFChars(jname, NULL); + const char * namespace_ = env->GetStringUTFChars(jnamespace, NULL); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_names_and_types_t client_names_and_types = rcl_get_zero_initialized_names_and_types(); + + rcl_ret_t ret = rcl_get_client_names_and_types_by_node( + node, + &allocator, + name, + namespace_, + &client_names_and_types); + RCLJAVA_COMMON_THROW_FROM_RCL_X(env, ret, "failed to get client names and types", goto cleanup); + fill_jnames_and_types(env, client_names_and_types, jnames_and_types); + +cleanup: + env->ReleaseStringUTFChars(jname, name); + env->ReleaseStringUTFChars(jnamespace, namespace_); + ret = rcl_names_and_types_fini(&client_names_and_types); + if (!env->ExceptionCheck() && RCL_RET_OK != ret) { + rcljava_throw_rclexception(env, ret, "failed to fini client names and types structure"); + } +} 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 c91d74fd..b6750e39 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/Node.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/Node.java @@ -661,4 +661,16 @@ Collection getSubscriptionNamesAndTypesByNode( */ Collection getServiceNamesAndTypesByNode( String nodeName, String nodeNamespace); + + /** + * Return the client 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 clients. + * @param nodeNamespace namespace of the node we want to know its clients. + * @return the detected client names and types. + */ + Collection getClientNamesAndTypesByNode( + String nodeName, String nodeNamespace); } 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 eef9b0d0..041232d4 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java @@ -849,4 +849,15 @@ public final Collection getServiceNamesAndTypesByNode( private static native final Collection nativeGetServiceNamesAndTypesByNode( long handle, String nodeName, String nodeNamespace, Collection namesAndTypes); + + public final Collection getClientNamesAndTypesByNode( + String nodeName, String nodeNamespace) + { + Collection namesAndTypes = new ArrayList(); + nativeGetClientNamesAndTypesByNode(this.handle, nodeName, nodeNamespace, namesAndTypes); + return namesAndTypes; + } + + private static native final Collection nativeGetClientNamesAndTypesByNode( + long handle, String nodeName, String nodeNamespace, Collection namesAndTypes); } 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 8d6dd875..213179c0 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java @@ -1472,4 +1472,95 @@ public void accept(final Collection local, Collection client1 = node.createClient( + rcljava.srv.AddTwoInts.class, "test_get_client_names_and_types_one"); + Client client2 = node.createClient( + rcljava.srv.AddTwoInts.class, "test_get_client_names_and_types_two"); + Client client3 = remoteNode.createClient( + rcljava.srv.AddTwoInts.class, "test_get_client_names_and_types_two"); + Client client4 = remoteNode.createClient( + rcljava.srv.AddTwoInts.class, "test_get_client_names_and_types_three"); + Service service = node.createService( + rcljava.srv.AddTwoInts.class, "test_get_client_names_and_types_this_should_not_appear", + 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) + {} + }); + + BiConsumer, Collection> validateNameAndTypes = + new BiConsumer, Collection>() { + public void accept(final Collection local, Collection remote) { + // TODO(ivanpauno): Using assertj may help a lot here https://assertj.github.io/doc/. + assertEquals(local.size(), 2); + assertTrue( + "client 'test_get_client_names_and_types_one' was not discovered for local node", + local.contains( + new NameAndTypes( + "/test_get_client_names_and_types_one", + new ArrayList(Arrays.asList("rcljava/srv/AddTwoInts"))))); + assertTrue( + "client 'test_get_client_names_and_types_two' was not discovered for local node", + local.contains( + new NameAndTypes( + "/test_get_client_names_and_types_two", + new ArrayList(Arrays.asList("rcljava/srv/AddTwoInts"))))); + + assertEquals(remote.size(), 2); + assertTrue( + "client 'test_get_client_names_and_types_two' was not discovered for remote node", + remote.contains( + new NameAndTypes( + "/test_get_client_names_and_types_two", + new ArrayList(Arrays.asList("rcljava/srv/AddTwoInts"))))); + assertTrue( + "client 'test_get_client_names_and_types_three' was not discovered for remote node", + remote.contains( + new NameAndTypes( + "/test_get_client_names_and_types_three", + new ArrayList(Arrays.asList("rcljava/srv/AddTwoInts"))))); + } + }; + + long start = System.currentTimeMillis(); + boolean ok = false; + Collection local = null; + Collection remote = null; + do { + local = this.node.getClientNamesAndTypesByNode("test_node", "/"); + remote = this.node.getClientNamesAndTypesByNode( + "test_get_client_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); + + client1.dispose(); + client2.dispose(); + client3.dispose(); + client4.dispose(); + service.dispose(); + remoteNode.dispose(); + } } From 4b4e907829fc523d2527af03ab22af0830a0e4e4 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 22 Sep 2020 18:51:05 -0300 Subject: [PATCH 2/3] Use rcpputils::scope_exit() Signed-off-by: Ivan Santiago Paunovic --- .../cpp/org_ros2_rcljava_node_NodeImpl.cpp | 22 +++++++++++-------- 1 file changed, 13 insertions(+), 9 deletions(-) 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 cb337702..c0d7353d 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp @@ -622,9 +622,21 @@ JNIEXPORT void JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetClientNamesA } 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 client_names_and_types = rcl_get_zero_initialized_names_and_types(); + auto fini_names_and_types = rcpputils::make_scope_exit( + [pnames_and_types = &client_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 client names and types structure"); + } + }); rcl_ret_t ret = rcl_get_client_names_and_types_by_node( node, @@ -632,14 +644,6 @@ JNIEXPORT void JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetClientNamesA name, namespace_, &client_names_and_types); - RCLJAVA_COMMON_THROW_FROM_RCL_X(env, ret, "failed to get client names and types", goto cleanup); + RCLJAVA_COMMON_THROW_FROM_RCL_X(env, ret, "failed to get client names and types"); fill_jnames_and_types(env, client_names_and_types, jnames_and_types); - -cleanup: - env->ReleaseStringUTFChars(jname, name); - env->ReleaseStringUTFChars(jnamespace, namespace_); - ret = rcl_names_and_types_fini(&client_names_and_types); - if (!env->ExceptionCheck() && RCL_RET_OK != ret) { - rcljava_throw_rclexception(env, ret, "failed to fini client names and types structure"); - } } From fb038d3ba613d4a17c6724c4bcf84208fbdb0ca7 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 22 Sep 2020 18:51:58 -0300 Subject: [PATCH 3/3] Use rcpputils::scope_exit() Signed-off-by: Ivan Santiago Paunovic --- rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 c0d7353d..ed2d60cb 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp @@ -644,6 +644,6 @@ JNIEXPORT void JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetClientNamesA name, namespace_, &client_names_and_types); - RCLJAVA_COMMON_THROW_FROM_RCL_X(env, ret, "failed to get client names and types"); + RCLJAVA_COMMON_THROW_FROM_RCL(env, ret, "failed to get client names and types"); fill_jnames_and_types(env, client_names_and_types, jnames_and_types); }