Skip to content

Commit ece8300

Browse files
ivanpaunoivanpauno
committed
Add getPublishersInfo method to Node (osrf#26)
** Add EndpointInfo class. * Add getPublishersInfo method to Node. Signed-off-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>
1 parent 68b2b93 commit ece8300

File tree

10 files changed

+364
-3
lines changed

10 files changed

+364
-3
lines changed

‎rcljava/CMakeLists.txt‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -61,6 +61,7 @@ set(${PROJECT_NAME}_jni_sources
6161
"src/main/cpp/org_ros2_rcljava_detail_QosIncompatibleStatus.cpp"
6262
"src/main/cpp/org_ros2_rcljava_executors_BaseExecutor.cpp"
6363
"src/main/cpp/org_ros2_rcljava_events_EventHandlerImpl.cpp"
64+
"src/main/cpp/org_ros2_rcljava_graph_EndpointInfo"
6465
"src/main/cpp/org_ros2_rcljava_publisher_statuses_LivelinessLost.cpp"
6566
"src/main/cpp/org_ros2_rcljava_publisher_statuses_OfferedDeadlineMissed.cpp"
6667
"src/main/cpp/org_ros2_rcljava_publisher_statuses_OfferedQosIncompatible.cpp"
@@ -150,6 +151,7 @@ set(${PROJECT_NAME}_sources
150151
"src/main/java/org/ros2/rcljava/executors/Executor.java"
151152
"src/main/java/org/ros2/rcljava/executors/MultiThreadedExecutor.java"
152153
"src/main/java/org/ros2/rcljava/executors/SingleThreadedExecutor.java"
154+
"src/main/java/org/ros2/rcljava/graph/EndpointInfo.java"
153155
"src/main/java/org/ros2/rcljava/node/BaseComposableNode.java"
154156
"src/main/java/org/ros2/rcljava/node/ComposableNode.java"
155157
"src/main/java/org/ros2/rcljava/node/Node.java"

‎rcljava/include/org_ros2_rcljava_graph_EndpointInfo.h‎

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
// Copyright 2020 Open Source Robotics Foundation, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include <jni.h>
16+
/* Header for class org_ros2_rcljava_graph_EndpointInfo */
17+
18+
#ifndef ORG_ROS2_RCLJAVA_GRAPH_ENDPOINTINFO_H_
19+
#define ORG_ROS2_RCLJAVA_GRAPH_ENDPOINTINFO_H_
20+
#ifdef __cplusplus
21+
extern "C" {
22+
#endif
23+
/*
24+
* Class: org_ros2_rcljava_graph_EndpointInfo
25+
* Method: nativeFromRCL
26+
* Signature: (J)V
27+
*/
28+
JNIEXPORT void
29+
JNICALL Java_org_ros2_rcljava_graph_EndpointInfo_nativeFromRCL(JNIEnv *, jobject, jlong);
30+
31+
#ifdef __cplusplus
32+
}
33+
#endif
34+
#endif // ORG_ROS2_RCLJAVA_GRAPH_ENDPOINTINFO_H_

‎rcljava/include/org_ros2_rcljava_node_NodeImpl.h‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -101,6 +101,15 @@ JNIEXPORT void
101101
JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetTopicNamesAndTypes(
102102
JNIEnv *, jclass, jlong, jobject);
103103

104+
/*
105+
* Class: org_ros2_rcljava_node_NodeImpl
106+
* Method: nativeGetPublishersInfo
107+
* Signature: (JLjava/lang/String;Ljava/util/List;)V
108+
*/
109+
JNIEXPORT void
110+
JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetPublishersInfo(
111+
JNIEnv *, jclass, jlong, jstring, jobject);
112+
104113
#ifdef __cplusplus
105114
}
106115
#endif

‎rcljava/package.xml‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@
1515
<build_depend>builtin_interfaces</build_depend>
1616
<build_depend>rcl_interfaces</build_depend>
1717
<build_depend>rcl</build_depend>
18+
<build_depend>rcpputils</build_depend>
1819
<build_depend>rmw_implementation_cmake</build_depend>
1920
<build_depend>rmw</build_depend>
2021
<build_depend>rosidl_generator_c</build_depend>
@@ -29,6 +30,7 @@
2930
<exec_depend>builtin_interfaces</exec_depend>
3031
<exec_depend>rcl_interfaces</exec_depend>
3132
<exec_depend>rcl</exec_depend>
33+
<exec_depend>rcpputils</exec_depend>
3234
<exec_depend>rmw_implementation_cmake</exec_depend>
3335
<exec_depend>rmw_implementation</exec_depend>
3436
<exec_depend>rosidl_runtime_c</exec_depend>

‎rcljava/src/main/cpp/org_ros2_rcljava_graph_EndpointInfo.cpp‎

Lines changed: 109 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,109 @@
1+
// Copyright 2020 Open Source Robotics Foundation, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "org_ros2_rcljava_graph_EndpointInfo.h"
16+
17+
#include <jni.h>
18+
#include <limits>
19+
20+
#include "rcl/graph.h"
21+
#include "rcljava_common/exceptions.hpp"
22+
23+
using rcljava_common::exceptions::rcljava_throw_exception;
24+
25+
JNIEXPORT void JNICALL
26+
Java_org_ros2_rcljava_graph_EndpointInfo_nativeFromRCL(JNIEnv * env, jobject self, jlong handle)
27+
{
28+
auto * p = reinterpret_cast<rcl_topic_endpoint_info_t *>(handle);
29+
if (!p) {
30+
rcljava_throw_exception(
31+
env, "java/lang/IllegalArgumentException", "passed rcl endpoint info handle is NULL");
32+
return;
33+
}
34+
const char * endpoint_type_enum_path =
35+
"Lorg/ros2/rcljava/graph/EndpointInfo$EndpointType;";
36+
// TODO(ivanpauno): class and field lookup could be done at startup time
37+
jclass endpoint_type_clazz = env->FindClass("org/ros2/rcljava/graph/EndpointInfo$EndpointType");
38+
RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env);
39+
jclass clazz = env->GetObjectClass(self);
40+
jfieldID node_name_fid = env->GetFieldID(clazz, "nodeName", "Ljava/lang/String;");
41+
RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env);
42+
jfieldID node_namespace_fid = env->GetFieldID(clazz, "nodeNamespace", "Ljava/lang/String;");
43+
RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env);
44+
jfieldID topic_type_fid = env->GetFieldID(clazz, "topicType", "Ljava/lang/String;");
45+
RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env);
46+
jfieldID endpoint_type_fid = env->GetFieldID(clazz, "endpointType", endpoint_type_enum_path);
47+
RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env);
48+
jfieldID endpoint_gid_fid = env->GetFieldID(clazz, "endpointGID", "[B");
49+
RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env);
50+
jfieldID qos_fid = env->GetFieldID(clazz, "qos", "Lorg/ros2/rcljava/qos/QoSProfile;");
51+
RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env);
52+
53+
jstring jnode_name = env->NewStringUTF(p->node_name);
54+
RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env);
55+
env->SetObjectField(self, node_name_fid, jnode_name);
56+
jstring jnode_namespace = env->NewStringUTF(p->node_namespace);
57+
RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env);
58+
env->SetObjectField(self, node_namespace_fid, jnode_namespace);
59+
jstring jtopic_type = env->NewStringUTF(p->topic_type);
60+
RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env);
61+
env->SetObjectField(self, topic_type_fid, jtopic_type);
62+
jfieldID enum_value_fid;
63+
switch (p->endpoint_type) {
64+
case RMW_ENDPOINT_INVALID:
65+
enum_value_fid = env->GetStaticFieldID(
66+
endpoint_type_clazz, "INVALID", endpoint_type_enum_path);
67+
break;
68+
case RMW_ENDPOINT_PUBLISHER:
69+
enum_value_fid = env->GetStaticFieldID(
70+
endpoint_type_clazz, "PUBLISHER", endpoint_type_enum_path);
71+
break;
72+
case RMW_ENDPOINT_SUBSCRIPTION:
73+
enum_value_fid = env->GetStaticFieldID(
74+
endpoint_type_clazz, "SUBSCRIPTION", endpoint_type_enum_path);
75+
break;
76+
default:
77+
rcljava_throw_exception(
78+
env, "java/lang/IllegalArgumentException", "unknown endpoint type");
79+
break;
80+
}
81+
RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env);
82+
jobject enum_value = env->GetStaticObjectField(endpoint_type_clazz, enum_value_fid);
83+
env->SetObjectField(self, endpoint_type_fid, enum_value);
84+
jbyteArray jgid = env->NewByteArray(RMW_GID_STORAGE_SIZE);
85+
if (jgid == NULL) {
86+
rcljava_throw_exception(
87+
env, "java/lang/OutOfMemoryError", "cannot allocate java gid byte array");
88+
return;
89+
}
90+
jbyte * gid_content = env->GetByteArrayElements(jgid, nullptr);
91+
RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env);
92+
for (size_t i = 0; i < RMW_GID_STORAGE_SIZE; ++i) {
93+
gid_content[i] = p->endpoint_gid[i];
94+
}
95+
env->ReleaseByteArrayElements(jgid, gid_content, 0);
96+
RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env);
97+
env->SetObjectField(self, endpoint_gid_fid, jgid);
98+
jclass qos_clazz = env->FindClass("org/ros2/rcljava/qos/QoSProfile");
99+
RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env);
100+
jmethodID qos_init_mid = env->GetMethodID(qos_clazz, "<init>", "()V");
101+
RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env);
102+
jobject jqos = env->NewObject(qos_clazz, qos_init_mid);
103+
RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env);
104+
jmethodID qos_from_rcl_mid = env->GetMethodID(qos_clazz, "nativeFromRCL", "(J)V");
105+
RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env);
106+
env->CallObjectMethod(jqos, qos_from_rcl_mid, &p->qos_profile);
107+
RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env);
108+
env->SetObjectField(self, qos_fid, jqos);
109+
}

‎rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp‎

Lines changed: 64 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,7 @@
2222
#include "rcl/graph.h"
2323
#include "rcl/node.h"
2424
#include "rcl/rcl.h"
25+
#include "rcpputils/scope_exit.hpp"
2526
#include "rmw/rmw.h"
2627
#include "rosidl_runtime_c/message_type_support_struct.h"
2728

@@ -297,3 +298,66 @@ Java_org_ros2_rcljava_node_NodeImpl_nativeGetTopicNamesAndTypes(
297298
rcljava_throw_rclexception(env, ret, "failed to fini topic names and types structure");
298299
}
299300
}
301+
302+
JNIEXPORT void JNICALL
303+
Java_org_ros2_rcljava_node_NodeImpl_nativeGetPublishersInfo(
304+
JNIEnv * env, jclass, jlong handle, jstring jtopic_name, jobject jpublishers_info)
305+
{
306+
rcl_node_t * node = reinterpret_cast<rcl_node_t *>(handle);
307+
if (!node) {
308+
rcljava_throw_exception(
309+
env, "java/lang/IllegalArgumentException", "passed node handle is NULL");
310+
return;
311+
}
312+
313+
rcutils_allocator_t allocator = rcutils_get_default_allocator();
314+
rcl_topic_endpoint_info_array_t publishers_info =
315+
rcl_get_zero_initialized_topic_endpoint_info_array();
316+
317+
const char * topic_name = env->GetStringUTFChars(jtopic_name, NULL);
318+
if (!topic_name) {
319+
rcljava_throw_exception(
320+
env, "java/lang/IllegalArgumentException", "failed to convert jstring to utf chars");
321+
return;
322+
}
323+
324+
rcl_ret_t ret = rcl_get_publishers_info_by_topic(
325+
node,
326+
&allocator,
327+
topic_name,
328+
false, // use ros mangling conventions
329+
&publishers_info);
330+
331+
env->ReleaseStringUTFChars(jtopic_name, topic_name);
332+
333+
RCLJAVA_COMMON_THROW_FROM_RCL(env, ret, "failed to get publisher info");
334+
auto cleanup_info_array = rcpputils::make_scope_exit(
335+
[info_ptr = &publishers_info, allocator_ptr = &allocator, env]() {
336+
rcl_ret_t ret = rcl_topic_endpoint_info_array_fini(info_ptr, allocator_ptr);
337+
if (!env->ExceptionCheck() && RCL_RET_OK != ret) {
338+
rcljava_throw_rclexception(env, ret, "failed to destroy rcl publisher info");
339+
}
340+
}
341+
);
342+
343+
jclass list_clazz = env->GetObjectClass(jpublishers_info);
344+
jmethodID list_add_mid = env->GetMethodID(list_clazz, "add", "(Ljava/lang/Object;)Z");
345+
RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env);
346+
jclass endpoint_info_clazz = env->FindClass("org/ros2/rcljava/graph/EndpointInfo");
347+
RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env);
348+
jmethodID endpoint_info_init_mid = env->GetMethodID(endpoint_info_clazz, "<init>", "()V");
349+
RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env);
350+
jmethodID endpoint_info_from_rcl_mid = env->GetMethodID(
351+
endpoint_info_clazz, "nativeFromRCL", "(J)V");
352+
RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env);
353+
354+
for (size_t i = 0; i < publishers_info.size; i++) {
355+
jobject item = env->NewObject(endpoint_info_clazz, endpoint_info_init_mid);
356+
RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env);
357+
env->CallVoidMethod(item, endpoint_info_from_rcl_mid, &publishers_info.info_array[i]);
358+
RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env);
359+
env->CallBooleanMethod(jpublishers_info, list_add_mid, item);
360+
RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env);
361+
env->DeleteLocalRef(item);
362+
}
363+
}

‎rcljava/src/main/java/org/ros2/rcljava/graph/EndpointInfo.java‎

Lines changed: 58 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,58 @@
1+
/* Copyright 2020 Open Source Robotics Foundation, Inc.
2+
*
3+
* Licensed under the Apache License, Version 2.0 (the "License");
4+
* you may not use this file except in compliance with the License.
5+
* You may obtain a copy of the License at
6+
*
7+
* http://www.apache.org/licenses/LICENSE-2.0
8+
*
9+
* Unless required by applicable law or agreed to in writing, software
10+
* distributed under the License is distributed on an "AS IS" BASIS,
11+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
* See the License for the specific language governing permissions and
13+
* limitations under the License.
14+
*/
15+
16+
package org.ros2.rcljava.graph;
17+
18+
import org.slf4j.Logger;
19+
import org.slf4j.LoggerFactory;
20+
21+
import org.ros2.rcljava.common.JNIUtils;
22+
import org.ros2.rcljava.qos.QoSProfile;
23+
24+
/**
25+
* Class that represents queried information of an endpoint.
26+
*/
27+
public class EndpointInfo {
28+
/// Name of the node that created the endpoint.
29+
public String nodeName;
30+
/// Namespace of the node that created the endpoint.
31+
public String nodeNamespace;
32+
/// Topic type.
33+
public String topicType;
34+
/// Kind of endpoint, i.e.: publisher or subscription.
35+
public EndpointType endpointType;
36+
/// Gid of the endpoint.
37+
public byte[] endpointGID;
38+
/// Quality of service of the endpoint.
39+
public QoSProfile qos;
40+
41+
public enum EndpointType {
42+
INVALID,
43+
PUBLISHER,
44+
SUBSCRIPTION;
45+
}
46+
47+
private native final void nativeFromRCL(long handle);
48+
49+
private static final Logger logger = LoggerFactory.getLogger(EndpointInfo.class);
50+
static {
51+
try {
52+
JNIUtils.loadImplementation(EndpointInfo.class);
53+
} catch (UnsatisfiedLinkError ule) {
54+
logger.error("Native code library failed to load.\n" + ule);
55+
System.exit(1);
56+
}
57+
}
58+
}

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

Lines changed: 14 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -24,15 +24,16 @@
2424
import org.ros2.rcljava.concurrent.Callback;
2525
import org.ros2.rcljava.consumers.Consumer;
2626
import org.ros2.rcljava.consumers.TriConsumer;
27+
import org.ros2.rcljava.graph.EndpointInfo;
2728
import org.ros2.rcljava.graph.NameAndTypes;
28-
import org.ros2.rcljava.qos.QoSProfile;
2929
import org.ros2.rcljava.interfaces.Disposable;
3030
import org.ros2.rcljava.interfaces.MessageDefinition;
3131
import org.ros2.rcljava.interfaces.ServiceDefinition;
3232
import org.ros2.rcljava.parameters.ParameterCallback;
3333
import org.ros2.rcljava.parameters.ParameterType;
3434
import org.ros2.rcljava.parameters.ParameterVariant;
3535
import org.ros2.rcljava.publisher.Publisher;
36+
import org.ros2.rcljava.qos.QoSProfile;
3637
import org.ros2.rcljava.service.RMWRequestId;
3738
import org.ros2.rcljava.service.Service;
3839
import org.ros2.rcljava.subscription.Subscription;
@@ -558,4 +559,16 @@ <T extends ServiceDefinition> Client<T> createClient(final Class<T> serviceType,
558559
* @return the detected topic names and types.
559560
*/
560561
Collection<NameAndTypes> getTopicNamesAndTypes();
562+
563+
/**
564+
* Get information of all publishers in a topic.
565+
*
566+
* The queried information includes the node that created the publisher, its qos, etc.
567+
* For more info, see @{link EndpointInfo}.
568+
*
569+
* @param topicName The topic name of interest.
570+
* @return A collection of `EndpointInfo` instances, describing all publishers in the
571+
* passed topic.
572+
*/
573+
Collection<EndpointInfo> getPublishersInfo(final String topicName);
561574
}

‎rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java‎

Lines changed: 11 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -23,8 +23,8 @@
2323
import org.ros2.rcljava.consumers.Consumer;
2424
import org.ros2.rcljava.consumers.TriConsumer;
2525
import org.ros2.rcljava.contexts.Context;
26+
import org.ros2.rcljava.graph.EndpointInfo;
2627
import org.ros2.rcljava.graph.NameAndTypes;
27-
import org.ros2.rcljava.qos.QoSProfile;
2828
import org.ros2.rcljava.interfaces.Disposable;
2929
import org.ros2.rcljava.interfaces.MessageDefinition;
3030
import org.ros2.rcljava.interfaces.ServiceDefinition;
@@ -37,6 +37,7 @@
3737
import org.ros2.rcljava.parameters.ParameterVariant;
3838
import org.ros2.rcljava.publisher.Publisher;
3939
import org.ros2.rcljava.publisher.PublisherImpl;
40+
import org.ros2.rcljava.qos.QoSProfile;
4041
import org.ros2.rcljava.service.RMWRequestId;
4142
import org.ros2.rcljava.service.Service;
4243
import org.ros2.rcljava.service.ServiceImpl;
@@ -763,4 +764,13 @@ public final Collection<NameAndTypes> getTopicNamesAndTypes() {
763764

764765
private static native final void nativeGetTopicNamesAndTypes(
765766
long handle, Collection<NameAndTypes> namesAndTypes);
767+
768+
public final Collection<EndpointInfo> getPublishersInfo(final String topicName) {
769+
ArrayList<EndpointInfo> returnValue = new ArrayList();
770+
nativeGetPublishersInfo(this.handle, topicName, returnValue);
771+
return returnValue;
772+
}
773+
774+
private native static final void nativeGetPublishersInfo(
775+
final long handle, final String topicName, ArrayList<EndpointInfo> endpointInfo);
766776
}

0 commit comments

Comments
 (0)