diff --git a/rcljava/CMakeLists.txt b/rcljava/CMakeLists.txt index 40619128..8f10aeb4 100644 --- a/rcljava/CMakeLists.txt +++ b/rcljava/CMakeLists.txt @@ -61,6 +61,7 @@ set(${PROJECT_NAME}_jni_sources "src/main/cpp/org_ros2_rcljava_detail_QosIncompatibleStatus.cpp" "src/main/cpp/org_ros2_rcljava_executors_BaseExecutor.cpp" "src/main/cpp/org_ros2_rcljava_events_EventHandlerImpl.cpp" + "src/main/cpp/org_ros2_rcljava_graph_EndpointInfo" "src/main/cpp/org_ros2_rcljava_publisher_statuses_LivelinessLost.cpp" "src/main/cpp/org_ros2_rcljava_publisher_statuses_OfferedDeadlineMissed.cpp" "src/main/cpp/org_ros2_rcljava_publisher_statuses_OfferedQosIncompatible.cpp" @@ -150,6 +151,7 @@ set(${PROJECT_NAME}_sources "src/main/java/org/ros2/rcljava/executors/Executor.java" "src/main/java/org/ros2/rcljava/executors/MultiThreadedExecutor.java" "src/main/java/org/ros2/rcljava/executors/SingleThreadedExecutor.java" + "src/main/java/org/ros2/rcljava/graph/EndpointInfo.java" "src/main/java/org/ros2/rcljava/node/BaseComposableNode.java" "src/main/java/org/ros2/rcljava/node/ComposableNode.java" "src/main/java/org/ros2/rcljava/node/Node.java" diff --git a/rcljava/include/org_ros2_rcljava_graph_EndpointInfo.h b/rcljava/include/org_ros2_rcljava_graph_EndpointInfo.h new file mode 100644 index 00000000..e391d84a --- /dev/null +++ b/rcljava/include/org_ros2_rcljava_graph_EndpointInfo.h @@ -0,0 +1,34 @@ +// 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. + +#include +/* Header for class org_ros2_rcljava_graph_EndpointInfo */ + +#ifndef ORG_ROS2_RCLJAVA_GRAPH_ENDPOINTINFO_H_ +#define ORG_ROS2_RCLJAVA_GRAPH_ENDPOINTINFO_H_ +#ifdef __cplusplus +extern "C" { +#endif +/* + * Class: org_ros2_rcljava_graph_EndpointInfo + * Method: nativeFromRCL + * Signature: (J)V + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_graph_EndpointInfo_nativeFromRCL(JNIEnv *, jobject, jlong); + +#ifdef __cplusplus +} +#endif +#endif // ORG_ROS2_RCLJAVA_GRAPH_ENDPOINTINFO_H_ diff --git a/rcljava/include/org_ros2_rcljava_node_NodeImpl.h b/rcljava/include/org_ros2_rcljava_node_NodeImpl.h index ff383475..8f25d1f7 100644 --- a/rcljava/include/org_ros2_rcljava_node_NodeImpl.h +++ b/rcljava/include/org_ros2_rcljava_node_NodeImpl.h @@ -101,6 +101,15 @@ JNIEXPORT void JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetTopicNamesAndTypes( JNIEnv *, jclass, jlong, jobject); +/* + * Class: org_ros2_rcljava_node_NodeImpl + * Method: nativeGetPublishersInfo + * Signature: (JLjava/lang/String;Ljava/util/List;)V + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetPublishersInfo( + JNIEnv *, jclass, jlong, jstring, jobject); + #ifdef __cplusplus } #endif diff --git a/rcljava/package.xml b/rcljava/package.xml index 9f7e58d1..2f4e5f0d 100644 --- a/rcljava/package.xml +++ b/rcljava/package.xml @@ -15,6 +15,7 @@ builtin_interfaces rcl_interfaces rcl + rcpputils rmw_implementation_cmake rmw rosidl_generator_c @@ -29,6 +30,7 @@ builtin_interfaces rcl_interfaces rcl + rcpputils rmw_implementation_cmake rmw_implementation rosidl_runtime_c diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_graph_EndpointInfo.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_graph_EndpointInfo.cpp new file mode 100644 index 00000000..1e7763d4 --- /dev/null +++ b/rcljava/src/main/cpp/org_ros2_rcljava_graph_EndpointInfo.cpp @@ -0,0 +1,109 @@ +// 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. + +#include "org_ros2_rcljava_graph_EndpointInfo.h" + +#include +#include + +#include "rcl/graph.h" +#include "rcljava_common/exceptions.hpp" + +using rcljava_common::exceptions::rcljava_throw_exception; + +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_graph_EndpointInfo_nativeFromRCL(JNIEnv * env, jobject self, jlong handle) +{ + auto * p = reinterpret_cast(handle); + if (!p) { + rcljava_throw_exception( + env, "java/lang/IllegalArgumentException", "passed rcl endpoint info handle is NULL"); + return; + } + const char * endpoint_type_enum_path = + "Lorg/ros2/rcljava/graph/EndpointInfo$EndpointType;"; + // TODO(ivanpauno): class and field lookup could be done at startup time + jclass endpoint_type_clazz = env->FindClass("org/ros2/rcljava/graph/EndpointInfo$EndpointType"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jclass clazz = env->GetObjectClass(self); + jfieldID node_name_fid = env->GetFieldID(clazz, "nodeName", "Ljava/lang/String;"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jfieldID node_namespace_fid = env->GetFieldID(clazz, "nodeNamespace", "Ljava/lang/String;"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jfieldID topic_type_fid = env->GetFieldID(clazz, "topicType", "Ljava/lang/String;"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jfieldID endpoint_type_fid = env->GetFieldID(clazz, "endpointType", endpoint_type_enum_path); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jfieldID endpoint_gid_fid = env->GetFieldID(clazz, "endpointGID", "[B"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jfieldID qos_fid = env->GetFieldID(clazz, "qos", "Lorg/ros2/rcljava/qos/QoSProfile;"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + + jstring jnode_name = env->NewStringUTF(p->node_name); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + env->SetObjectField(self, node_name_fid, jnode_name); + jstring jnode_namespace = env->NewStringUTF(p->node_namespace); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + env->SetObjectField(self, node_namespace_fid, jnode_namespace); + jstring jtopic_type = env->NewStringUTF(p->topic_type); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + env->SetObjectField(self, topic_type_fid, jtopic_type); + jfieldID enum_value_fid; + switch (p->endpoint_type) { + case RMW_ENDPOINT_INVALID: + enum_value_fid = env->GetStaticFieldID( + endpoint_type_clazz, "INVALID", endpoint_type_enum_path); + break; + case RMW_ENDPOINT_PUBLISHER: + enum_value_fid = env->GetStaticFieldID( + endpoint_type_clazz, "PUBLISHER", endpoint_type_enum_path); + break; + case RMW_ENDPOINT_SUBSCRIPTION: + enum_value_fid = env->GetStaticFieldID( + endpoint_type_clazz, "SUBSCRIPTION", endpoint_type_enum_path); + break; + default: + rcljava_throw_exception( + env, "java/lang/IllegalArgumentException", "unknown endpoint type"); + break; + } + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jobject enum_value = env->GetStaticObjectField(endpoint_type_clazz, enum_value_fid); + env->SetObjectField(self, endpoint_type_fid, enum_value); + jbyteArray jgid = env->NewByteArray(RMW_GID_STORAGE_SIZE); + if (jgid == NULL) { + rcljava_throw_exception( + env, "java/lang/OutOfMemoryError", "cannot allocate java gid byte array"); + return; + } + jbyte * gid_content = env->GetByteArrayElements(jgid, nullptr); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + for (size_t i = 0; i < RMW_GID_STORAGE_SIZE; ++i) { + gid_content[i] = p->endpoint_gid[i]; + } + env->ReleaseByteArrayElements(jgid, gid_content, 0); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + env->SetObjectField(self, endpoint_gid_fid, jgid); + jclass qos_clazz = env->FindClass("org/ros2/rcljava/qos/QoSProfile"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jmethodID qos_init_mid = env->GetMethodID(qos_clazz, "", "()V"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jobject jqos = env->NewObject(qos_clazz, qos_init_mid); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jmethodID qos_from_rcl_mid = env->GetMethodID(qos_clazz, "nativeFromRCL", "(J)V"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + env->CallObjectMethod(jqos, qos_from_rcl_mid, &p->qos_profile); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + env->SetObjectField(self, qos_fid, jqos); +} 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 99ce66f1..726f48a2 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp @@ -22,6 +22,7 @@ #include "rcl/graph.h" #include "rcl/node.h" #include "rcl/rcl.h" +#include "rcpputils/scope_exit.hpp" #include "rmw/rmw.h" #include "rosidl_runtime_c/message_type_support_struct.h" @@ -309,3 +310,66 @@ Java_org_ros2_rcljava_node_NodeImpl_nativeGetTopicNamesAndTypes( rcljava_throw_rclexception(env, ret, "failed to fini topic names and types structure"); } } + +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_node_NodeImpl_nativeGetPublishersInfo( + JNIEnv * env, jclass, jlong handle, jstring jtopic_name, jobject jpublishers_info) +{ + rcl_node_t * node = reinterpret_cast(handle); + if (!node) { + rcljava_throw_exception( + env, "java/lang/IllegalArgumentException", "passed node handle is NULL"); + return; + } + + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + rcl_topic_endpoint_info_array_t publishers_info = + rcl_get_zero_initialized_topic_endpoint_info_array(); + + const char * topic_name = env->GetStringUTFChars(jtopic_name, NULL); + if (!topic_name) { + rcljava_throw_exception( + env, "java/lang/IllegalArgumentException", "failed to convert jstring to utf chars"); + return; + } + + rcl_ret_t ret = rcl_get_publishers_info_by_topic( + node, + &allocator, + topic_name, + false, // use ros mangling conventions + &publishers_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]() { + 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"); + } + } + ); + + jclass list_clazz = env->GetObjectClass(jpublishers_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"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jmethodID endpoint_info_init_mid = env->GetMethodID(endpoint_info_clazz, "", "()V"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jmethodID endpoint_info_from_rcl_mid = env->GetMethodID( + endpoint_info_clazz, "nativeFromRCL", "(J)V"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + + for (size_t i = 0; i < publishers_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]); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + env->CallBooleanMethod(jpublishers_info, list_add_mid, item); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + env->DeleteLocalRef(item); + } +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/graph/EndpointInfo.java b/rcljava/src/main/java/org/ros2/rcljava/graph/EndpointInfo.java new file mode 100644 index 00000000..f0891093 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/graph/EndpointInfo.java @@ -0,0 +1,58 @@ +/* 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 org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import org.ros2.rcljava.common.JNIUtils; +import org.ros2.rcljava.qos.QoSProfile; + +/** + * Class that represents queried information of an endpoint. + */ +public class EndpointInfo { + /// Name of the node that created the endpoint. + public String nodeName; + /// Namespace of the node that created the endpoint. + public String nodeNamespace; + /// Topic type. + public String topicType; + /// Kind of endpoint, i.e.: publisher or subscription. + public EndpointType endpointType; + /// Gid of the endpoint. + public byte[] endpointGID; + /// Quality of service of the endpoint. + public QoSProfile qos; + + public enum EndpointType { + INVALID, + PUBLISHER, + SUBSCRIPTION; + } + + private native final void nativeFromRCL(long handle); + + private static final Logger logger = LoggerFactory.getLogger(EndpointInfo.class); + static { + try { + JNIUtils.loadImplementation(EndpointInfo.class); + } catch (UnsatisfiedLinkError ule) { + logger.error("Native code library failed to load.\n" + ule); + System.exit(1); + } + } +} 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 0c5b40ad..42c1281b 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/Node.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/Node.java @@ -24,8 +24,8 @@ import org.ros2.rcljava.concurrent.Callback; import org.ros2.rcljava.consumers.Consumer; import org.ros2.rcljava.consumers.TriConsumer; +import org.ros2.rcljava.graph.EndpointInfo; import org.ros2.rcljava.graph.NameAndTypes; -import org.ros2.rcljava.qos.QoSProfile; import org.ros2.rcljava.interfaces.Disposable; import org.ros2.rcljava.interfaces.MessageDefinition; import org.ros2.rcljava.interfaces.ServiceDefinition; @@ -33,6 +33,7 @@ import org.ros2.rcljava.parameters.ParameterType; import org.ros2.rcljava.parameters.ParameterVariant; import org.ros2.rcljava.publisher.Publisher; +import org.ros2.rcljava.qos.QoSProfile; import org.ros2.rcljava.service.RMWRequestId; import org.ros2.rcljava.service.Service; import org.ros2.rcljava.subscription.Subscription; @@ -558,4 +559,16 @@ Client createClient(final Class serviceType, * @return the detected topic names and types. */ Collection getTopicNamesAndTypes(); + + /** + * Get information of all publishers 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 publishers in the + * passed topic. + */ + Collection getPublishersInfo(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 e596f49c..2736e686 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java @@ -23,8 +23,8 @@ import org.ros2.rcljava.consumers.Consumer; import org.ros2.rcljava.consumers.TriConsumer; import org.ros2.rcljava.contexts.Context; +import org.ros2.rcljava.graph.EndpointInfo; import org.ros2.rcljava.graph.NameAndTypes; -import org.ros2.rcljava.qos.QoSProfile; import org.ros2.rcljava.interfaces.MessageDefinition; import org.ros2.rcljava.interfaces.ServiceDefinition; import org.ros2.rcljava.parameters.InvalidParametersException; @@ -36,6 +36,7 @@ import org.ros2.rcljava.parameters.ParameterVariant; import org.ros2.rcljava.publisher.Publisher; import org.ros2.rcljava.publisher.PublisherImpl; +import org.ros2.rcljava.qos.QoSProfile; import org.ros2.rcljava.service.RMWRequestId; import org.ros2.rcljava.service.Service; import org.ros2.rcljava.service.ServiceImpl; @@ -748,4 +749,13 @@ public final Collection getTopicNamesAndTypes() { private static native final void nativeGetTopicNamesAndTypes( long handle, Collection namesAndTypes); + + public final Collection getPublishersInfo(final String topicName) { + ArrayList returnValue = new ArrayList(); + nativeGetPublishersInfo(this.handle, topicName, returnValue); + return returnValue; + } + + private native static final void nativeGetPublishersInfo( + 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 d010fe05..4ab40171 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java @@ -16,6 +16,7 @@ package org.ros2.rcljava.node; import static org.junit.Assert.assertEquals; +import static org.junit.Assert.assertFalse; import static org.junit.Assert.assertNotEquals; import static org.junit.Assert.assertNotNull; import static org.junit.Assert.assertTrue; @@ -32,7 +33,9 @@ import java.util.ArrayList; import java.util.Arrays; import java.util.Collection; +import java.util.Iterator; import java.util.List; +import java.util.concurrent.TimeUnit; import org.ros2.rcljava.RCLJava; import org.ros2.rcljava.concurrent.RCLFuture; @@ -40,9 +43,12 @@ import org.ros2.rcljava.executors.Executor; import org.ros2.rcljava.executors.MultiThreadedExecutor; import org.ros2.rcljava.executors.SingleThreadedExecutor; +import org.ros2.rcljava.graph.EndpointInfo; import org.ros2.rcljava.graph.NameAndTypes; import org.ros2.rcljava.node.Node; import org.ros2.rcljava.publisher.Publisher; +import org.ros2.rcljava.qos.policies.Reliability; +import org.ros2.rcljava.qos.QoSProfile; import org.ros2.rcljava.subscription.Subscription; public class NodeTest { @@ -933,4 +939,58 @@ public void accept(final Collection namesAndTypes) { subscription.dispose(); subscription2.dispose(); } -} + + @Test + public final void testGetPublishersInfo() { + Publisher publisher = + node.createPublisher(rcljava.msg.UInt32.class, "test_get_publishers_info"); + Publisher publisher2 = + node.createPublisher( + rcljava.msg.UInt32.class, "test_get_publishers_info", 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.PUBLISHER); + 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.PUBLISHER); + assertEquals(item.qos.getReliability(), Reliability.BEST_EFFORT); + assertFalse(it.hasNext()); + } + }; + + long start = System.currentTimeMillis(); + boolean ok = false; + Collection publishersInfo = null; + do { + publishersInfo = node.getPublishersInfo("/test_get_publishers_info"); + try { + validateEndpointInfo.accept(publishersInfo); + 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(publishersInfo); + validateEndpointInfo.accept(publishersInfo); + publisher.dispose(); + publisher2.dispose(); + } +} \ No newline at end of file