From 224754a8f1a88ebb455e84508e5aa05ed77e890f Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 1 Sep 2020 17:43:00 -0300 Subject: [PATCH] Add getSubscriptionsInfo to Node class Signed-off-by: Ivan Santiago Paunovic --- .../include/org_ros2_rcljava_node_NodeImpl.h | 9 +++ .../cpp/org_ros2_rcljava_node_NodeImpl.cpp | 41 +++++++++---- .../main/java/org/ros2/rcljava/node/Node.java | 12 ++++ .../java/org/ros2/rcljava/node/NodeImpl.java | 9 +++ .../java/org/ros2/rcljava/node/NodeTest.java | 59 ++++++++++++++++++- 5 files changed, 117 insertions(+), 13 deletions(-) diff --git a/rcljava/include/org_ros2_rcljava_node_NodeImpl.h b/rcljava/include/org_ros2_rcljava_node_NodeImpl.h index 8f25d1f7..c6ba1b08 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_nativeGetPublishersInfo( JNIEnv *, jclass, jlong, jstring, jobject); +/* + * Class: org_ros2_rcljava_node_NodeImpl + * Method: nativeGetSubscriptionsInfo + * Signature: (JLjava/lang/String;Ljava/util/List;)V + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetSubscriptionsInfo( + JNIEnv *, jclass, jlong, 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 726f48a2..f0bb620d 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp @@ -311,9 +311,10 @@ Java_org_ros2_rcljava_node_NodeImpl_nativeGetTopicNamesAndTypes( } } -JNIEXPORT void JNICALL -Java_org_ros2_rcljava_node_NodeImpl_nativeGetPublishersInfo( - JNIEnv * env, jclass, jlong handle, jstring jtopic_name, jobject jpublishers_info) +template +void +get_endpoint_info_common( + JNIEnv * env, jlong handle, jstring jtopic_name, jobject jendpoints_info, FunctorT get_info) { rcl_node_t * node = reinterpret_cast(handle); if (!node) { @@ -323,7 +324,7 @@ Java_org_ros2_rcljava_node_NodeImpl_nativeGetPublishersInfo( } rcutils_allocator_t allocator = rcutils_get_default_allocator(); - rcl_topic_endpoint_info_array_t publishers_info = + rcl_topic_endpoint_info_array_t endpoints_info = rcl_get_zero_initialized_topic_endpoint_info_array(); const char * topic_name = env->GetStringUTFChars(jtopic_name, NULL); @@ -333,26 +334,26 @@ Java_org_ros2_rcljava_node_NodeImpl_nativeGetPublishersInfo( return; } - rcl_ret_t ret = rcl_get_publishers_info_by_topic( + rcl_ret_t ret = get_info( node, &allocator, topic_name, false, // use ros mangling conventions - &publishers_info); + &endpoints_info); env->ReleaseStringUTFChars(jtopic_name, topic_name); RCLJAVA_COMMON_THROW_FROM_RCL(env, ret, "failed to get publisher info"); auto cleanup_info_array = rcpputils::make_scope_exit( - [info_ptr = &publishers_info, allocator_ptr = &allocator, env]() { + [info_ptr = &endpoints_info, allocator_ptr = &allocator, env]() { rcl_ret_t ret = rcl_topic_endpoint_info_array_fini(info_ptr, allocator_ptr); if (!env->ExceptionCheck() && RCL_RET_OK != ret) { - rcljava_throw_rclexception(env, ret, "failed to destroy rcl publisher info"); + rcljava_throw_rclexception(env, ret, "failed to destroy rcl endpoints info"); } } ); - jclass list_clazz = env->GetObjectClass(jpublishers_info); + jclass list_clazz = env->GetObjectClass(jendpoints_info); jmethodID list_add_mid = env->GetMethodID(list_clazz, "add", "(Ljava/lang/Object;)Z"); RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); jclass endpoint_info_clazz = env->FindClass("org/ros2/rcljava/graph/EndpointInfo"); @@ -363,13 +364,29 @@ Java_org_ros2_rcljava_node_NodeImpl_nativeGetPublishersInfo( endpoint_info_clazz, "nativeFromRCL", "(J)V"); RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); - for (size_t i = 0; i < publishers_info.size; i++) { + for (size_t i = 0; i < endpoints_info.size; i++) { jobject item = env->NewObject(endpoint_info_clazz, endpoint_info_init_mid); RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); - env->CallVoidMethod(item, endpoint_info_from_rcl_mid, &publishers_info.info_array[i]); + env->CallVoidMethod(item, endpoint_info_from_rcl_mid, &endpoints_info.info_array[i]); RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); - env->CallBooleanMethod(jpublishers_info, list_add_mid, item); + env->CallBooleanMethod(jendpoints_info, list_add_mid, item); RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); env->DeleteLocalRef(item); } } + +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_node_NodeImpl_nativeGetPublishersInfo( + JNIEnv * env, jclass, jlong handle, jstring jtopic_name, jobject jpublishers_info) +{ + get_endpoint_info_common( + env, handle, jtopic_name, jpublishers_info, rcl_get_publishers_info_by_topic); +} + +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_node_NodeImpl_nativeGetSubscriptionsInfo( + JNIEnv * env, jclass, jlong handle, jstring jtopic_name, jobject jsubscriptions_info) +{ + get_endpoint_info_common( + env, handle, jtopic_name, jsubscriptions_info, rcl_get_subscriptions_info_by_topic); +} 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 42c1281b..fe3a6189 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/Node.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/Node.java @@ -571,4 +571,16 @@ Client createClient(final Class serviceType, * passed topic. */ Collection getPublishersInfo(final String topicName); + + /** + * Get information of all subscriptions in a topic. + * + * The queried information includes the node that created the publisher, its qos, etc. + * For more info, see @{link EndpointInfo}. + * + * @param topicName The topic name of interest. + * @return A collection of `EndpointInfo` instances, describing all subscriptions in the + * passed topic. + */ + Collection getSubscriptionsInfo(final String topicName); } 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 2736e686..aa600396 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java @@ -758,4 +758,13 @@ public final Collection getPublishersInfo(final String topicName) private native static final void nativeGetPublishersInfo( final long handle, final String topicName, ArrayList endpointInfo); + + public final Collection getSubscriptionsInfo(final String topicName) { + ArrayList returnValue = new ArrayList(); + nativeGetSubscriptionsInfo(this.handle, topicName, returnValue); + return returnValue; + } + + private native static final void nativeGetSubscriptionsInfo( + final long handle, final String topicName, ArrayList endpointInfo); } 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 4ab40171..5ed378f2 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java @@ -993,4 +993,61 @@ public void accept(final Collection info) { publisher.dispose(); publisher2.dispose(); } -} \ No newline at end of file + + @Test + public final void testGetSubscriptionsInfo() { + Subscription subscription = node.createSubscription( + rcljava.msg.UInt32.class, "test_get_subscriptions_info", new Consumer() { + public void accept(final rcljava.msg.UInt32 msg) {} + }); + Subscription subscription2 = node.createSubscription( + rcljava.msg.UInt32.class, "test_get_subscriptions_info", new Consumer() { + public void accept(final rcljava.msg.UInt32 msg) {} + }, QoSProfile.sensorData()); + + Consumer> validateEndpointInfo = + new Consumer>() { + public void accept(final Collection info) { + assertEquals(info.size(), 2); + Iterator it = info.iterator(); + EndpointInfo item = it.next(); + assertEquals("test_node", item.nodeName); + assertEquals("/", item.nodeNamespace); + assertEquals("rcljava/msg/UInt32", item.topicType); + assertEquals(item.endpointType, EndpointInfo.EndpointType.SUBSCRIPTION); + assertEquals(item.qos.getReliability(), Reliability.RELIABLE); + item = it.next(); + assertEquals("test_node", item.nodeName); + assertEquals("/", item.nodeNamespace); + assertEquals("rcljava/msg/UInt32", item.topicType); + assertEquals(item.endpointType, EndpointInfo.EndpointType.SUBSCRIPTION); + assertEquals(item.qos.getReliability(), Reliability.BEST_EFFORT); + assertFalse(it.hasNext()); + } + }; + + long start = System.currentTimeMillis(); + boolean ok = false; + Collection subscriptionsInfo = null; + do { + subscriptionsInfo = node.getSubscriptionsInfo("/test_get_subscriptions_info"); + try { + validateEndpointInfo.accept(subscriptionsInfo); + 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(subscriptionsInfo); + validateEndpointInfo.accept(subscriptionsInfo); + subscription.dispose(); + subscription2.dispose(); + } +}