From bd62cb29cd3225af53b20844d8646ab1f4848fbc Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 4 Sep 2020 16:28:07 -0300 Subject: [PATCH 1/2] * Add NameAndTypes class * Add getTopicNamesAndTypes method to Node Signed-off-by: Ivan Santiago Paunovic --- rcljava/CMakeLists.txt | 1 + .../include/org_ros2_rcljava_node_NodeImpl.h | 9 +++ .../cpp/org_ros2_rcljava_node_NodeImpl.cpp | 59 ++++++++++++++ .../org/ros2/rcljava/graph/NameAndTypes.java | 73 ++++++++++++++++++ .../main/java/org/ros2/rcljava/node/Node.java | 9 +++ .../java/org/ros2/rcljava/node/NodeImpl.java | 10 +++ .../java/org/ros2/rcljava/node/NodeTest.java | 76 +++++++++++++++++++ 7 files changed, 237 insertions(+) create mode 100644 rcljava/src/main/java/org/ros2/rcljava/graph/NameAndTypes.java diff --git a/rcljava/CMakeLists.txt b/rcljava/CMakeLists.txt index 0f89a59b..40619128 100644 --- a/rcljava/CMakeLists.txt +++ b/rcljava/CMakeLists.txt @@ -141,6 +141,7 @@ set(${PROJECT_NAME}_sources "src/main/java/org/ros2/rcljava/events/EventStatus.java" "src/main/java/org/ros2/rcljava/events/PublisherEventStatus.java" "src/main/java/org/ros2/rcljava/events/SubscriptionEventStatus.java" + "src/main/java/org/ros2/rcljava/graph/NameAndTypes.java" "src/main/java/org/ros2/rcljava/publisher/statuses/LivelinessLost.java" "src/main/java/org/ros2/rcljava/publisher/statuses/OfferedDeadlineMissed.java" "src/main/java/org/ros2/rcljava/publisher/statuses/OfferedQosIncompatible.java" diff --git a/rcljava/include/org_ros2_rcljava_node_NodeImpl.h b/rcljava/include/org_ros2_rcljava_node_NodeImpl.h index 40d40867..ff383475 100644 --- a/rcljava/include/org_ros2_rcljava_node_NodeImpl.h +++ b/rcljava/include/org_ros2_rcljava_node_NodeImpl.h @@ -92,6 +92,15 @@ JNIEXPORT jlong JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeCreateTimerHandle( JNIEnv *, jclass, jlong, jlong, jlong); +/* + * Class: org_ros2_rcljava_node_NodeImpl + * Method: nativeGetTopicNamesAndTypes + * Signature: (JLjava/util/Collection;)V + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetTopicNamesAndTypes( + JNIEnv *, jclass, jlong, 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 868fc2e4..f5fc9e08 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp @@ -19,6 +19,7 @@ #include #include "rcl/error_handling.h" +#include "rcl/graph.h" #include "rcl/node.h" #include "rcl/rcl.h" #include "rmw/rmw.h" @@ -29,6 +30,7 @@ #include "org_ros2_rcljava_node_NodeImpl.h" +using rcljava_common::exceptions::rcljava_throw_exception; using rcljava_common::exceptions::rcljava_throw_rclexception; JNIEXPORT jstring JNICALL @@ -250,3 +252,60 @@ Java_org_ros2_rcljava_node_NodeImpl_nativeCreateTimerHandle( jlong jtimer = reinterpret_cast(timer); return jtimer; } + +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; + } + + jclass collection_clazz = env->FindClass("java/util/Collection"); + jmethodID collection_add_mid = env->GetMethodID( + collection_clazz, "add", "(Ljava/lang/Object;)Z"); + RCLJAVA_COMMON_EXCEPTION_CHECK(env); + jclass name_and_types_clazz = env->FindClass("org/ros2/rcljava/graph/NameAndTypes"); + RCLJAVA_COMMON_EXCEPTION_CHECK(env); + jmethodID name_and_types_init_mid = env->GetMethodID(name_and_types_clazz, "", "()V"); + RCLJAVA_COMMON_EXCEPTION_CHECK(env); + jfieldID name_fid = env->GetFieldID(name_and_types_clazz, "name", "Ljava/lang/String;"); + RCLJAVA_COMMON_EXCEPTION_CHECK(env); + jfieldID types_fid = env->GetFieldID(name_and_types_clazz, "types", "Ljava/util/Collection;"); + RCLJAVA_COMMON_EXCEPTION_CHECK(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"); + + for (size_t i = 0; i < topic_names_and_types.names.size; i++) { + jobject jitem = env->NewObject(name_and_types_clazz, name_and_types_init_mid); + RCLJAVA_COMMON_EXCEPTION_CHECK_X(env, goto cleanup); + jstring jname = env->NewStringUTF(topic_names_and_types.names.data[i]); + RCLJAVA_COMMON_EXCEPTION_CHECK_X(env, goto cleanup); + 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]); + env->CallBooleanMethod(jtypes, collection_add_mid, jtype); + RCLJAVA_COMMON_EXCEPTION_CHECK_X(env, goto cleanup); + } + env->CallBooleanMethod(jnames_and_types, collection_add_mid, jitem); + RCLJAVA_COMMON_EXCEPTION_CHECK_X(env, goto cleanup); + } + +cleanup: + 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"); + } +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/graph/NameAndTypes.java b/rcljava/src/main/java/org/ros2/rcljava/graph/NameAndTypes.java new file mode 100644 index 00000000..580ad329 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/graph/NameAndTypes.java @@ -0,0 +1,73 @@ +/* Copyright 2020 Open Source Robotics Foundation, Inc. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package org.ros2.rcljava.graph; + +import java.util.ArrayList; +import java.util.Collection; +import java.util.Objects; + +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import org.ros2.rcljava.common.JNIUtils; + +/** + * Class that represents a topic/service/action name, together with all the types + * of message/service/action that are using that name. + */ +public class NameAndTypes { + /// Name of the topic/service/action. + public String name; + /// Types of the topic/service/action with the given name. + public Collection types; + + /** + * Construct from given name and types. + * + * @param name name of the topic/service/action. + * @param types types of the given topic/service/action. + * A shallow copy of the given collection will be stored, + * but given that String is immutable, this is not a problem. + * @param typesSize size of the \a typesHandle array. + */ + public NameAndTypes(final String name, final Collection types) { + this.name = name; + this.types = new ArrayList(types); + } + + /// @internal Default constructor, only used from jni code. + private NameAndTypes() { + this.types = new ArrayList(); + } + + @Override + public boolean equals(final Object o) { + if (o == this) { + return true; + } + if (!(o instanceof NameAndTypes)) { + return false; + } + NameAndTypes other = (NameAndTypes) o; + return Objects.equals(this.name, other.name) && + Objects.equals(this.types, other.types); + } + + @Override + public int hashCode() { + return Objects.hash(this.name, this.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 f413ab04..0c5b40ad 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/Node.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/Node.java @@ -24,6 +24,7 @@ import org.ros2.rcljava.concurrent.Callback; import org.ros2.rcljava.consumers.Consumer; import org.ros2.rcljava.consumers.TriConsumer; +import org.ros2.rcljava.graph.NameAndTypes; import org.ros2.rcljava.qos.QoSProfile; import org.ros2.rcljava.interfaces.Disposable; import org.ros2.rcljava.interfaces.MessageDefinition; @@ -549,4 +550,12 @@ Client createClient(final Class serviceType, * @return rcl_interfaces.msg.ListParametersResult */ rcl_interfaces.msg.ListParametersResult listParameters(List prefixes, long depth); + + /** + * Return the topics names and types that were detected in the graph. + * See @{link graph#NameAndTypes} for more information about the returned value. + * + * @return the detected topic names and types. + */ + Collection getTopicNamesAndTypes(); } 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 0189e302..e596f49c 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java @@ -23,6 +23,7 @@ import org.ros2.rcljava.consumers.Consumer; import org.ros2.rcljava.consumers.TriConsumer; import org.ros2.rcljava.contexts.Context; +import org.ros2.rcljava.graph.NameAndTypes; import org.ros2.rcljava.qos.QoSProfile; import org.ros2.rcljava.interfaces.MessageDefinition; import org.ros2.rcljava.interfaces.ServiceDefinition; @@ -738,4 +739,13 @@ public rcl_interfaces.msg.ListParametersResult listParameters( return result; } } + + public final Collection getTopicNamesAndTypes() { + Collection namesAndTypes = new ArrayList(); + nativeGetTopicNamesAndTypes(this.handle, namesAndTypes); + return namesAndTypes; + } + + private static native final void nativeGetTopicNamesAndTypes( + long handle, 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 b6408b52..d010fe05 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java @@ -17,6 +17,7 @@ import static org.junit.Assert.assertEquals; import static org.junit.Assert.assertNotEquals; +import static org.junit.Assert.assertNotNull; import static org.junit.Assert.assertTrue; import org.junit.After; @@ -27,7 +28,10 @@ import java.lang.ref.WeakReference; +import java.util.concurrent.TimeUnit; +import java.util.ArrayList; import java.util.Arrays; +import java.util.Collection; import java.util.List; import org.ros2.rcljava.RCLJava; @@ -36,6 +40,7 @@ import org.ros2.rcljava.executors.Executor; import org.ros2.rcljava.executors.MultiThreadedExecutor; import org.ros2.rcljava.executors.SingleThreadedExecutor; +import org.ros2.rcljava.graph.NameAndTypes; import org.ros2.rcljava.node.Node; import org.ros2.rcljava.publisher.Publisher; import org.ros2.rcljava.subscription.Subscription; @@ -857,4 +862,75 @@ public Node getNode() { subscriptionTwo.dispose(); assertEquals(0, subscriptionTwo.getHandle()); } + + @Test + public final void testGetTopicNamesAndTypes() throws Exception { + Publisher publisher = node.createPublisher( + rcljava.msg.UInt32.class, "test_get_topic_names_and_types_one"); + Publisher publisher2 = node.createPublisher( + rcljava.msg.UInt32.class, "test_get_topic_names_and_types_two"); + Subscription subscription = node.createSubscription( + rcljava.msg.Empty.class, "test_get_topic_names_and_types_one", + new Consumer() { + public void accept(final rcljava.msg.Empty msg) {} + }); + Subscription subscription2 = node.createSubscription( + rcljava.msg.Empty.class, "test_get_topic_names_and_types_three", + new Consumer() { + public void accept(final rcljava.msg.Empty msg) {} + }); + + Consumer> validateNameAndTypes = + new Consumer>() { + public void accept(final Collection namesAndTypes) { + // TODO(ivanpauno): Using assertj may help a lot here https://assertj.github.io/doc/. + assertEquals(namesAndTypes.size(), 3); + assertTrue( + "topic 'test_get_topic_names_and_types_one' was not discovered", + namesAndTypes.contains( + new NameAndTypes( + "/test_get_topic_names_and_types_one", + new ArrayList(Arrays.asList("rcljava/msg/Empty", "rcljava/msg/UInt32"))))); + assertTrue( + "topic 'test_get_topic_names_and_types_two' was not discovered", + namesAndTypes.contains( + new NameAndTypes( + "/test_get_topic_names_and_types_two", + new ArrayList(Arrays.asList("rcljava/msg/UInt32"))))); + assertTrue( + "topic 'test_get_topic_names_and_types_three' was not discovered", + namesAndTypes.contains( + new NameAndTypes( + "/test_get_topic_names_and_types_three", + new ArrayList(Arrays.asList("rcljava/msg/Empty"))))); + } + }; + + long start = System.currentTimeMillis(); + boolean ok = false; + Collection namesAndTypes = null; + do { + namesAndTypes = this.node.getTopicNamesAndTypes(); + 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); + + publisher.dispose(); + publisher2.dispose(); + subscription.dispose(); + subscription2.dispose(); + } } From 7fba8a16eb9dc82388b3da6850be55de5cea1b22 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 9 Sep 2020 17:26:10 -0300 Subject: [PATCH 2/2] Fix renamed macros Signed-off-by: Ivan Santiago Paunovic --- .../cpp/org_ros2_rcljava_node_NodeImpl.cpp | 18 +++++++++--------- 1 file changed, 9 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 f5fc9e08..99ce66f1 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp @@ -266,15 +266,15 @@ Java_org_ros2_rcljava_node_NodeImpl_nativeGetTopicNamesAndTypes( jclass collection_clazz = env->FindClass("java/util/Collection"); jmethodID collection_add_mid = env->GetMethodID( collection_clazz, "add", "(Ljava/lang/Object;)Z"); - RCLJAVA_COMMON_EXCEPTION_CHECK(env); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); jclass name_and_types_clazz = env->FindClass("org/ros2/rcljava/graph/NameAndTypes"); - RCLJAVA_COMMON_EXCEPTION_CHECK(env); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); jmethodID name_and_types_init_mid = env->GetMethodID(name_and_types_clazz, "", "()V"); - RCLJAVA_COMMON_EXCEPTION_CHECK(env); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); jfieldID name_fid = env->GetFieldID(name_and_types_clazz, "name", "Ljava/lang/String;"); - RCLJAVA_COMMON_EXCEPTION_CHECK(env); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); jfieldID types_fid = env->GetFieldID(name_and_types_clazz, "types", "Ljava/util/Collection;"); - RCLJAVA_COMMON_EXCEPTION_CHECK(env); + 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(); @@ -288,19 +288,19 @@ Java_org_ros2_rcljava_node_NodeImpl_nativeGetTopicNamesAndTypes( for (size_t i = 0; i < topic_names_and_types.names.size; i++) { jobject jitem = env->NewObject(name_and_types_clazz, name_and_types_init_mid); - RCLJAVA_COMMON_EXCEPTION_CHECK_X(env, goto cleanup); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION_WITH_ERROR_STATEMENT(env, goto cleanup); jstring jname = env->NewStringUTF(topic_names_and_types.names.data[i]); - RCLJAVA_COMMON_EXCEPTION_CHECK_X(env, goto cleanup); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION_WITH_ERROR_STATEMENT(env, goto cleanup); 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]); env->CallBooleanMethod(jtypes, collection_add_mid, jtype); - RCLJAVA_COMMON_EXCEPTION_CHECK_X(env, goto cleanup); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION_WITH_ERROR_STATEMENT(env, goto cleanup); } env->CallBooleanMethod(jnames_and_types, collection_add_mid, jitem); - RCLJAVA_COMMON_EXCEPTION_CHECK_X(env, goto cleanup); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION_WITH_ERROR_STATEMENT(env, goto cleanup); } cleanup: