diff --git a/rcljava/include/org_ros2_rcljava_node_NodeImpl.h b/rcljava/include/org_ros2_rcljava_node_NodeImpl.h index 75b52e61..90c565ed 100644 --- a/rcljava/include/org_ros2_rcljava_node_NodeImpl.h +++ b/rcljava/include/org_ros2_rcljava_node_NodeImpl.h @@ -137,6 +137,15 @@ JNIEXPORT void JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetSubscriptionsInfo( JNIEnv *, jclass, jlong, jstring, jobject); +/* + * Class: org_ros2_rcljava_node_NodeImpl + * Method: nativeGetPublisherNamesAndTypesByNode + * Signature: (JLjava/lang/String;Ljava/lang/String;Ljava/util/Collection;)V + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetPublisherNamesAndTypesByNode( + 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 0e85cf92..b3beedd9 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp @@ -499,3 +499,41 @@ Java_org_ros2_rcljava_node_NodeImpl_nativeGetSubscriptionsInfo( get_endpoint_info_common( env, handle, jtopic_name, jsubscriptions_info, rcl_get_subscriptions_info_by_topic); } + +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_node_NodeImpl_nativeGetPublisherNamesAndTypesByNode( + 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); + 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 publisher_names_and_types = rcl_get_zero_initialized_names_and_types(); + auto fini_names_and_types = rcpputils::make_scope_exit( + [pnames_and_types = &publisher_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 publisher names and types structure"); + } + }); + + rcl_ret_t ret = rcl_get_publisher_names_and_types_by_node( + node, + &allocator, + false, + name, + namespace_, + &publisher_names_and_types); + RCLJAVA_COMMON_THROW_FROM_RCL(env, ret, "failed to get publisher names and types"); + fill_jnames_and_types(env, publisher_names_and_types, jnames_and_types); +} 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 5059adee..59c2cf50 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/Node.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/Node.java @@ -598,4 +598,15 @@ Client createClient(final Class serviceType, * passed topic. */ Collection getSubscriptionsInfo(final String topicName); + + /** + * Return the publisher 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 publishers. + * @param nodeNamespace namespace of the node we want to know its publishers. + * @return the detected publisher names and types. + */ + Collection getPublisherNamesAndTypesByNode(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 c253bb03..3e2de320 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java @@ -785,4 +785,15 @@ public final Collection getSubscriptionsInfo(final String topicNam private native static final void nativeGetSubscriptionsInfo( final long handle, final String topicName, ArrayList endpointInfo); + + public final Collection getPublisherNamesAndTypesByNode( + String nodeName, String nodeNamespace) + { + Collection namesAndTypes = new ArrayList(); + nativeGetPublisherNamesAndTypesByNode(this.handle, nodeName, nodeNamespace, namesAndTypes); + return namesAndTypes; + } + + private static native final Collection nativeGetPublisherNamesAndTypesByNode( + 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 c2d99da9..88c1c956 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java @@ -41,6 +41,7 @@ import org.ros2.rcljava.client.Client; import org.ros2.rcljava.concurrent.RCLFuture; import org.ros2.rcljava.consumers.Consumer; +import org.ros2.rcljava.consumers.BiConsumer; import org.ros2.rcljava.consumers.TriConsumer; import org.ros2.rcljava.executors.Executor; import org.ros2.rcljava.executors.MultiThreadedExecutor; @@ -1174,4 +1175,89 @@ public void accept(final Collection info) { subscription.dispose(); subscription2.dispose(); } + + @Test + public final void testGetPublisherNamesAndTypesByNode() throws Exception { + final Node remoteNode = RCLJava.createNode("test_get_publisher_names_and_types_remote_node"); + Publisher publisher1 = node.createPublisher( + rcljava.msg.UInt32.class, "test_get_publisher_names_and_types_one"); + Publisher publisher2 = node.createPublisher( + rcljava.msg.UInt32.class, "test_get_publisher_names_and_types_two"); + Publisher publisher3 = remoteNode.createPublisher( + rcljava.msg.UInt32.class, "test_get_publisher_names_and_types_two"); + Publisher publisher4 = remoteNode.createPublisher( + rcljava.msg.UInt32.class, "test_get_publisher_names_and_types_three"); + Subscription subscription = node.createSubscription( + rcljava.msg.Empty.class, "test_get_topic_names_and_types_this_should_not_appear", + new Consumer() { + public void accept(final rcljava.msg.Empty msg) {} + }); + + 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( + "topic 'test_get_publisher_names_and_types_one' was not discovered for local node", + local.contains( + new NameAndTypes( + "/test_get_publisher_names_and_types_one", + new ArrayList(Arrays.asList("rcljava/msg/UInt32"))))); + assertTrue( + "topic 'test_get_publisher_names_and_types_two' was not discovered for local node", + local.contains( + new NameAndTypes( + "/test_get_publisher_names_and_types_two", + new ArrayList(Arrays.asList("rcljava/msg/UInt32"))))); + + assertEquals(remote.size(), 2); + assertTrue( + "topic 'test_get_publisher_names_and_types_two' was not discovered for remote node", + remote.contains( + new NameAndTypes( + "/test_get_publisher_names_and_types_two", + new ArrayList(Arrays.asList("rcljava/msg/UInt32"))))); + assertTrue( + "topic 'test_get_publisher_names_and_types_three' was not discovered for remote node", + remote.contains( + new NameAndTypes( + "/test_get_publisher_names_and_types_three", + new ArrayList(Arrays.asList("rcljava/msg/UInt32"))))); + } + }; + + long start = System.currentTimeMillis(); + boolean ok = false; + Collection local = null; + Collection remote = null; + do { + local = this.node.getPublisherNamesAndTypesByNode("test_node", "/"); + remote = this.node.getPublisherNamesAndTypesByNode( + "test_get_publisher_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); + + publisher1.dispose(); + publisher2.dispose(); + publisher3.dispose(); + publisher4.dispose(); + subscription.dispose(); + remoteNode.dispose(); + } }