Add getPublishersInfo method to Node

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"

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 <jni.h>
+/* 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_

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

rcljava/package.xml

@@ -15,6 +15,7 @@
   <build_depend>builtin_interfaces</build_depend>
   <build_depend>rcl_interfaces</build_depend>
   <build_depend>rcl</build_depend>
+  <build_depend>rcpputils</build_depend>
   <build_depend>rmw_implementation_cmake</build_depend>
   <build_depend>rmw</build_depend>
   <build_depend>rosidl_generator_c</build_depend>
@@ -29,6 +30,7 @@
   <exec_depend>builtin_interfaces</exec_depend>
   <exec_depend>rcl_interfaces</exec_depend>
   <exec_depend>rcl</exec_depend>
+  <exec_depend>rcpputils</exec_depend>
   <exec_depend>rmw_implementation_cmake</exec_depend>
   <exec_depend>rmw_implementation</exec_depend>
   <exec_depend>rosidl_runtime_c</exec_depend>

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 <jni.h>
+#include <limits>
+
+#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<rcl_topic_endpoint_info_t *>(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, "<init>", "()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);
+}

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<rcl_node_t *>(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, "<init>", "()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);
+  }
+}

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);
+    }
+  }
+}

rcljava/src/main/java/org/ros2/rcljava/node/Node.java

@@ -24,15 +24,16 @@
 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;
 import org.ros2.rcljava.parameters.ParameterCallback;
 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 @@ <T extends ServiceDefinition> Client<T> createClient(final Class<T> serviceType,
    * @return the detected topic names and types.
    */
   Collection<NameAndTypes> 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<EndpointInfo> getPublishersInfo(final String topicName);
 }

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<NameAndTypes> getTopicNamesAndTypes() {
 
   private static native final void nativeGetTopicNamesAndTypes(
     long handle, Collection<NameAndTypes> namesAndTypes);
+
+  public final Collection<EndpointInfo> getPublishersInfo(final String topicName) {
+    ArrayList<EndpointInfo> returnValue = new ArrayList();
+    nativeGetPublishersInfo(this.handle, topicName, returnValue);
+    return returnValue;
+  }
+
+  private native static final void nativeGetPublishersInfo(
+    final long handle, final String topicName, ArrayList<EndpointInfo> endpointInfo);
 }