From fd2dd7bc3ed39e26854cd55bfd6c85f85bd845c8 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 9 Sep 2020 14:51:51 -0300 Subject: [PATCH 1/9] Update qos profile -- take 2 (osrf#25) * Improve QoSProfile constructors * Add documentation to QoSProfile class * Add fromRCL method to QoSProfile * Use fromRCL to create the built-in qos profiles. Signed-off-by: Ivan Santiago Paunovic --- rcljava/CMakeLists.txt | 1 + .../include/org_ros2_rcljava_qos_QoSProfile.h | 41 +++ .../cpp/org_ros2_rcljava_qos_QoSProfile.cpp | 250 ++++++++++++++++++ .../java/org/ros2/rcljava/qos/QoSProfile.java | 217 ++++++++++++--- .../ros2/rcljava/qos/policies/Durability.java | 3 +- .../ros2/rcljava/qos/policies/History.java | 3 +- .../ros2/rcljava/qos/policies/Liveliness.java | 3 +- .../rcljava/qos/policies/Reliability.java | 3 +- 8 files changed, 484 insertions(+), 37 deletions(-) create mode 100644 rcljava/include/org_ros2_rcljava_qos_QoSProfile.h create mode 100644 rcljava/src/main/cpp/org_ros2_rcljava_qos_QoSProfile.cpp diff --git a/rcljava/CMakeLists.txt b/rcljava/CMakeLists.txt index c454fab7..bf71991c 100644 --- a/rcljava/CMakeLists.txt +++ b/rcljava/CMakeLists.txt @@ -66,6 +66,7 @@ set(${PROJECT_NAME}_jni_sources "src/main/cpp/org_ros2_rcljava_publisher_statuses_OfferedQosIncompatible.cpp" "src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp" "src/main/cpp/org_ros2_rcljava_publisher_PublisherImpl.cpp" + "src/main/cpp/org_ros2_rcljava_qos_QoSProfile.cpp" "src/main/cpp/org_ros2_rcljava_service_ServiceImpl.cpp" "src/main/cpp/org_ros2_rcljava_subscription_SubscriptionImpl.cpp" "src/main/cpp/org_ros2_rcljava_subscription_statuses_LivelinessChanged.cpp" diff --git a/rcljava/include/org_ros2_rcljava_qos_QoSProfile.h b/rcljava/include/org_ros2_rcljava_qos_QoSProfile.h new file mode 100644 index 00000000..ada8004c --- /dev/null +++ b/rcljava/include/org_ros2_rcljava_qos_QoSProfile.h @@ -0,0 +1,41 @@ +// 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_qos_QoSProfile */ + +#ifndef ORG_ROS2_RCLJAVA_QOS_QOSPROFILE_H_ +#define ORG_ROS2_RCLJAVA_QOS_QOSPROFILE_H_ +#ifdef __cplusplus +extern "C" { +#endif +/* + * Class: org_ros2_rcljava_qos_QoSProfile + * Method: nativeFromRCL + * Signature: (J)V + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_qos_QoSProfile_nativeFromRCL(JNIEnv *, jobject, jlong); + +/* + * Class: org_ros2_rcljava_qos_QoSProfile + * Method: nativeGetHandleFromName + * Signature: (Ljava/lang/String;)J + */ +JNIEXPORT jlong +JNICALL Java_org_ros2_rcljava_qos_QoSProfile_nativeGetHandleFromName(JNIEnv *, jclass, jstring); +#ifdef __cplusplus +} +#endif +#endif // ORG_ROS2_RCLJAVA_QOS_QOSPROFILE_H_ diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_qos_QoSProfile.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_qos_QoSProfile.cpp new file mode 100644 index 00000000..16239f7b --- /dev/null +++ b/rcljava/src/main/cpp/org_ros2_rcljava_qos_QoSProfile.cpp @@ -0,0 +1,250 @@ +// 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_qos_QoSProfile.h" + +#include +#include + +#include "rcljava_common/exceptions.hpp" +#include "rmw/qos_profiles.h" +#include "rmw/types.h" + +using rcljava_common::exceptions::rcljava_throw_exception; + +static void +qos_set_duration(JNIEnv * env, uint64_t seconds, uint64_t nanos, jobject jqos, jfieldID fid) +{ + if (static_cast(std::numeric_limits::max()) < seconds) { + // Throwing an exception here would be weird, as we cannot control the durability that was set + // on all other endpoints in the network. Set jqos seconds to jlong max. + seconds = static_cast(std::numeric_limits::max()); + } + if (static_cast(std::numeric_limits::max()) < nanos) { + // This should never happen, as nanoseconds within a second can perfectly be represented + // in a jlong. + nanos = static_cast(std::numeric_limits::max()); + } + jclass duration_clazz = env->FindClass("java/time/Duration"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jmethodID factory_mid = env->GetStaticMethodID( + duration_clazz, "ofSeconds", "(JJ)Ljava/time/Duration;"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jobject jduration = env->CallStaticObjectMethod( + duration_clazz, factory_mid, static_cast(seconds), static_cast(nanos)); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + env->SetObjectField(jqos, fid, jduration); +} + +static void +qos_from_rcl(JNIEnv * env, const rmw_qos_profile_t & qos, jobject jqos) +{ + // TODO(ivanpauno): class and field lookup could be done at startup time + jclass clazz = env->GetObjectClass(jqos); + const char * history_class_path = "Lorg/ros2/rcljava/qos/policies/History;"; + jfieldID history_fid = env->GetFieldID(clazz, "history", history_class_path); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jfieldID depth_fid = env->GetFieldID(clazz, "depth", "I"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + const char * reliability_class_path = "Lorg/ros2/rcljava/qos/policies/Reliability;"; + jfieldID reliability_fid = env->GetFieldID(clazz, "reliability", reliability_class_path); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + const char * durability_class_path = "Lorg/ros2/rcljava/qos/policies/Durability;"; + jfieldID durability_fid = env->GetFieldID(clazz, "durability", durability_class_path); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jfieldID deadline_fid = env->GetFieldID(clazz, "deadline", "Ljava/time/Duration;"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jfieldID lifespan_fid = env->GetFieldID(clazz, "lifespan", "Ljava/time/Duration;"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + const char * liveliness_class_path = "Lorg/ros2/rcljava/qos/policies/Liveliness;"; + jfieldID liveliness_fid = env->GetFieldID(clazz, "liveliness", liveliness_class_path); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jfieldID liveliness_lease_fid = env->GetFieldID( + clazz, "livelinessLeaseDuration", "Ljava/time/Duration;"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jfieldID avoid_ros_conventions_fid = env->GetFieldID( + clazz, "avoidROSNamespaceConventions", "Z"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + + jclass history_clazz = env->FindClass("org/ros2/rcljava/qos/policies/History"); + jfieldID history_value_fid; + switch (qos.history) { + case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT: + history_value_fid = env->GetStaticFieldID( + history_clazz, "SYSTEM_DEFAULT", history_class_path); + break; + case RMW_QOS_POLICY_HISTORY_KEEP_LAST: + history_value_fid = env->GetStaticFieldID( + history_clazz, "KEEP_LAST", history_class_path); + break; + case RMW_QOS_POLICY_HISTORY_KEEP_ALL: + history_value_fid = env->GetStaticFieldID( + history_clazz, "KEEP_ALL", history_class_path); + break; + case RMW_QOS_POLICY_HISTORY_UNKNOWN: + history_value_fid = env->GetStaticFieldID( + history_clazz, "UNKNOWN", history_class_path); + break; + default: + rcljava_throw_exception( + env, "java/lang/IllegalStateException", "unknown history policy value"); + break; + } + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jobject history_value = env->GetStaticObjectField(history_clazz, history_value_fid); + env->SetObjectField(jqos, history_fid, history_value); + + env->SetIntField(jqos, depth_fid, qos.depth); + + jclass reliability_clazz = env->FindClass("org/ros2/rcljava/qos/policies/Reliability"); + jfieldID reliability_value_fid; + switch (qos.reliability) { + case RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT: + reliability_value_fid = env->GetStaticFieldID( + reliability_clazz, "SYSTEM_DEFAULT", reliability_class_path); + break; + case RMW_QOS_POLICY_RELIABILITY_RELIABLE: + reliability_value_fid = env->GetStaticFieldID( + reliability_clazz, "RELIABLE", reliability_class_path); + break; + case RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT: + reliability_value_fid = env->GetStaticFieldID( + reliability_clazz, "BEST_EFFORT", reliability_class_path); + break; + case RMW_QOS_POLICY_RELIABILITY_UNKNOWN: + reliability_value_fid = env->GetStaticFieldID( + reliability_clazz, "UNKNOWN", reliability_class_path); + break; + default: + rcljava_throw_exception( + env, "java/lang/IllegalStateException", "unknown reliability policy value"); + break; + } + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jobject reliability_value = env->GetStaticObjectField(reliability_clazz, reliability_value_fid); + env->SetObjectField(jqos, reliability_fid, reliability_value); + + jclass durability_clazz = env->FindClass("org/ros2/rcljava/qos/policies/Durability"); + jfieldID durability_value_fid; + switch (qos.durability) { + case RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT: + durability_value_fid = env->GetStaticFieldID( + durability_clazz, "SYSTEM_DEFAULT", durability_class_path); + break; + case RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL: + durability_value_fid = env->GetStaticFieldID( + durability_clazz, "TRANSIENT_LOCAL", durability_class_path); + break; + case RMW_QOS_POLICY_DURABILITY_VOLATILE: + durability_value_fid = env->GetStaticFieldID( + durability_clazz, "VOLATILE", durability_class_path); + break; + case RMW_QOS_POLICY_DURABILITY_UNKNOWN: + durability_value_fid = env->GetStaticFieldID( + durability_clazz, "UNKNOWN", durability_class_path); + break; + default: + rcljava_throw_exception( + env, "java/lang/IllegalStateException", "unknown durability policy value"); + break; + } + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jobject durability_value = env->GetStaticObjectField(durability_clazz, durability_value_fid); + env->SetObjectField(jqos, durability_fid, durability_value); + + qos_set_duration(env, qos.deadline.sec, qos.deadline.nsec, jqos, deadline_fid); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + qos_set_duration(env, qos.lifespan.sec, qos.lifespan.nsec, jqos, lifespan_fid); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + qos_set_duration( + env, qos.liveliness_lease_duration.sec, + qos.liveliness_lease_duration.nsec, jqos, liveliness_lease_fid); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + + jclass liveliness_clazz = env->FindClass("org/ros2/rcljava/qos/policies/Liveliness"); + jfieldID liveliness_value_fid; + switch (qos.liveliness) { + case RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT: + liveliness_value_fid = env->GetStaticFieldID( + liveliness_clazz, "SYSTEM_DEFAULT", liveliness_class_path); + break; + case RMW_QOS_POLICY_LIVELINESS_AUTOMATIC: + liveliness_value_fid = env->GetStaticFieldID( + liveliness_clazz, "AUTOMATIC", liveliness_class_path); + break; + case RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC: + liveliness_value_fid = env->GetStaticFieldID( + liveliness_clazz, "MANUAL_BY_TOPIC", liveliness_class_path); + break; + case RMW_QOS_POLICY_LIVELINESS_UNKNOWN: + liveliness_value_fid = env->GetStaticFieldID( + liveliness_clazz, "UNKNOWN", liveliness_class_path); + break; + default: + rcljava_throw_exception( + env, "java/lang/IllegalStateException", "unknown liveliness policy value"); + break; + } + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jobject liveliness_value = env->GetStaticObjectField(liveliness_clazz, liveliness_value_fid); + env->SetObjectField(jqos, liveliness_fid, liveliness_value); + + env->SetBooleanField(jqos, avoid_ros_conventions_fid, qos.avoid_ros_namespace_conventions); +} + +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_qos_QoSProfile_nativeFromRCL(JNIEnv * env, jobject jqos, jlong handle) +{ + auto * qos_profile = reinterpret_cast(handle); + if (!handle) { + rcljava_throw_exception( + env, "java/lang/IllegalArgumentException", "rmw qos profile handle is NULL"); + return; + } + qos_from_rcl(env, *qos_profile, jqos); +} + +JNIEXPORT jlong JNICALL +Java_org_ros2_rcljava_qos_QoSProfile_nativeGetHandleFromName( + JNIEnv * env, jclass, jstring jprofile_name) +{ + const rmw_qos_profile_t * qos = NULL; + const char * profile_name = env->GetStringUTFChars(jprofile_name, NULL); + if (strcmp(profile_name, "default") == 0) { + qos = &rmw_qos_profile_default; + } + if (strcmp(profile_name, "system_default") == 0) { + qos = &rmw_qos_profile_system_default; + } + if (strcmp(profile_name, "sensor_data") == 0) { + qos = &rmw_qos_profile_sensor_data; + } + if (strcmp(profile_name, "parameters") == 0) { + qos = &rmw_qos_profile_parameters; + } + if (strcmp(profile_name, "services") == 0) { + qos = &rmw_qos_profile_services_default; + } + if (strcmp(profile_name, "parameter_events") == 0) { + qos = &rmw_qos_profile_parameter_events; + } + + env->ReleaseStringUTFChars(jprofile_name, profile_name); + if (qos) { + return reinterpret_cast(qos); + } + rcljava_throw_exception( + env, "java/lang/IllegalArgumentException", "unexpected profile identifier"); + return 0; +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/qos/QoSProfile.java b/rcljava/src/main/java/org/ros2/rcljava/qos/QoSProfile.java index 15b81712..73961faa 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/qos/QoSProfile.java +++ b/rcljava/src/main/java/org/ros2/rcljava/qos/QoSProfile.java @@ -17,37 +17,56 @@ import java.time.Duration; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import org.ros2.rcljava.common.JNIUtils; import org.ros2.rcljava.qos.policies.Durability; import org.ros2.rcljava.qos.policies.History; import org.ros2.rcljava.qos.policies.Liveliness; import org.ros2.rcljava.qos.policies.QoSPolicy; import org.ros2.rcljava.qos.policies.Reliability; +/** + * Implementation of the ROS QoS profile abstraction. + * + * It works as a bridge with rmw_qos_profile_t. + * This class provides several static methods to instantiate the default profiles, + * getters for the different policies, and setters that allow method chaining. + * + * Examples: + * + * QoSProfile.keepLast(10); // default qos profile with depth 10 + * // keep all qos profile with best effort reliability and transient local durability + * QoSProfile.keepAll(). + * setReliability(Reliability.BEST_EFFORT).setDurability(Durability.TRANSIENT_LOCAL); + * QoSProfile.sensorData(); // get predefined sensor data qos profile + */ public class QoSProfile { // TODO(ivanpauno): Update all qos policies in a way that the objects are created from RCL, // to avoid depending on the enum values. - private History history; - - private int depth; - - private Reliability reliability; - - private Durability durability; - - private Duration deadline = Duration.ofSeconds(0, 0); - - private Duration lifespan = Duration.ofSeconds(0, 0); - - private Liveliness liveliness = Liveliness.SYSTEM_DEFAULT; - private Duration livelinessLeaseDuration = Duration.ofSeconds(0, 0); - - private boolean avoidROSNamespaceConventions; + /** + * Construct a keep last QoS profile with the specified depth. + * + * @param depth history depth. + * @return a new QoSProfile. + */ + public static QoSProfile keepLast(int depth) { + return defaultProfile().setDepth(depth); + } - public QoSProfile(int depth) { - this(History.KEEP_LAST, depth, Reliability.RELIABLE, Durability.VOLATILE, false); + /** + * Construct a keep all qos profile. + * + * @param depth history depth. + * @return a new QoSProfile. + */ + public static QoSProfile keepAll() { + return defaultProfile().setHistory(History.KEEP_ALL); } + // TODO(ivanpauno): Please deprecate me public QoSProfile(History history, int depth, Reliability reliability, Durability durability, boolean avoidROSNamespaceConventions) { this.history = history; @@ -57,131 +76,263 @@ public QoSProfile(History history, int depth, Reliability reliability, Durabilit this.avoidROSNamespaceConventions = avoidROSNamespaceConventions; } + /** + * @return history policy of the profile. + */ public final History getHistory() { return this.history; } + /** + * @param history history policy to be set. + * @return reference to `this` object, allowing method chaining. + */ public final QoSProfile setHistory(final History history) { this.history = history; return this; } + /** + * @return history depth of the profile. + */ public final int getDepth() { return this.depth; } + /** + * @param depth history depth to be set. + * @return reference to `this` object, allowing method chaining. + */ public final QoSProfile setDepth(final int depth) { this.depth = depth; return this; } + /** + * @return reliability policy of the profile. + */ public final Reliability getReliability() { return this.reliability; } + /** + * @param reliability reliability policy to be set. + * @return reference to `this` object, allowing method chaining. + */ public final QoSProfile setReliability(final Reliability reliability) { this.reliability = reliability; return this; } + /** + * @return durability policy of the profile. + */ public final Durability getDurability() { return this.durability; } + /** + * @param durability durability policy to be set. + * @return reference to `this` object, allowing method chaining. + */ public final QoSProfile setDurability(final Durability durability) { this.durability = durability; return this; } + /** + * @return deadline policy of the profile. + */ public final Duration getDeadline() { return this.deadline; } + /** + * @param deadline deadline policy to be set. See + * here. + * @return reference to `this` object, allowing method chaining. + */ public final QoSProfile setDeadline(final Duration deadline) { this.deadline = deadline; return this; } + /** + * @return lifespan policy of the profile. + */ public final Duration getLifespan() { return this.lifespan; } + /** + * @param lifespan lifespan policy to be set. See + * here. + * @return reference to `this` object, allowing method chaining. + */ public final QoSProfile setLifespan(final Duration lifespan) { this.lifespan = lifespan; return this; } + /** + * @return liveliness policy of the profile. + */ public final Liveliness getLiveliness() { return this.liveliness; } + /** + * @param liveliness liveliness policy to be set. See + * here. + * @return reference to `this` object, allowing method chaining. + */ public final QoSProfile setLiveliness(final Liveliness liveliness) { this.liveliness = liveliness; return this; } + /** + * @return liveliness lease duration of the profile. + */ public final Duration getLivelinessLeaseDuration() { return this.livelinessLeaseDuration; } + /** + * @param livelinessLeaseDuration liveliness lease duration to be set. See + * here. + * @return reference to `this` object, allowing method chaining. + */ public final QoSProfile setLivelinessLeaseDuration(final Duration livelinessLeaseDuration) { this.livelinessLeaseDuration = livelinessLeaseDuration; return this; } + /** + * @return `true` if ROS namespace conventions are ignored, else `false`. + */ public final boolean getAvoidROSNamespaceConventions() { return this.avoidROSNamespaceConventions; } + /** + * @param avoidROSConventions when `true`, ROS name mangling conventions will be ignored. + * @return reference to `this` object, allowing method chaining. + */ public final QoSProfile setAvoidROSNamespaceConventions(final boolean avoidROSConventions) { this.avoidROSNamespaceConventions = avoidROSConventions; return this; } - // TODO(ivanpauno): refactor all static default profiles methods, - // so that the return value is get from the rmw definition directly - // (leveraging a native function). + private static final Logger logger = LoggerFactory.getLogger(QoSProfile.class); + static { + try { + JNIUtils.loadImplementation(QoSProfile.class); + } catch (UnsatisfiedLinkError ule) { + logger.error("Native code library failed to load.\n" + ule); + System.exit(1); + } + } + + // TODO(ivanpauno): Deprecate and use a private alternative, to match rclcpp/rclpy. public static final QoSProfile defaultProfile() { - return new QoSProfile(10); + QoSProfile qos = new QoSProfile(); + qos.nativeFromRCL(qos.nativeGetHandleFromName("default")); + return qos; } + /** + * @return Construct a new instance of the predefined system default profile. + */ public static final QoSProfile systemDefault() { - return new QoSProfile( - History.SYSTEM_DEFAULT, QoSProfile.DEPTH_SYSTEM_DEFAULT, - Reliability.SYSTEM_DEFAULT, Durability.SYSTEM_DEFAULT, false); + QoSProfile qos = new QoSProfile(); + qos.nativeFromRCL(qos.nativeGetHandleFromName("system_default")); + return qos; } + /** + * @return Construct a new instance of the predefined sensor data profile. + */ public static final QoSProfile sensorData() { - return new QoSProfile( - History.KEEP_LAST, 5, Reliability.BEST_EFFORT, Durability.VOLATILE, false); + QoSProfile qos = new QoSProfile(); + qos.nativeFromRCL(qos.nativeGetHandleFromName("sensor_data")); + return qos; } + /** + * @return Construct a new instance of the predefined parameters default profile. + */ public static final QoSProfile parametersDefault() { - return new QoSProfile( - History.KEEP_LAST, 1000, Reliability.RELIABLE, Durability.VOLATILE, false); + QoSProfile qos = new QoSProfile(); + qos.nativeFromRCL(qos.nativeGetHandleFromName("parameters")); + return qos; } + /** + * @return Construct a new instance of the predefined services default profile. + */ public static final QoSProfile servicesDefault() { - return new QoSProfile( - History.KEEP_LAST, 10, Reliability.RELIABLE, Durability.VOLATILE, false); + QoSProfile qos = new QoSProfile(); + qos.nativeFromRCL(qos.nativeGetHandleFromName("services")); + return qos; } + /** + * @return Construct a new instance of the predefined parameter events default profile. + */ public static final QoSProfile parameterEventsDefault() { - return new QoSProfile( - History.KEEP_ALL, 1000, Reliability.RELIABLE, Durability.VOLATILE, false); + QoSProfile qos = new QoSProfile(); + qos.nativeFromRCL(qos.nativeGetHandleFromName("parameter_events")); + return qos; } + // TODO(ivanpauno): Deprecate. public static final int DEPTH_SYSTEM_DEFAULT = 0; + // TODO(ivanpauno): Deprecate. public static final QoSProfile SENSOR_DATA = sensorData(); + // TODO(ivanpauno): Deprecate. public static final QoSProfile PARAMETERS = parametersDefault(); + // TODO(ivanpauno): Deprecate. public static final QoSProfile DEFAULT = defaultProfile(); + // TODO(ivanpauno): Deprecate. public static final QoSProfile SERVICES_DEFAULT = servicesDefault(); + // TODO(ivanpauno): Deprecate. public static final QoSProfile PARAMETER_EVENTS = parameterEventsDefault(); + // TODO(ivanpauno): Deprecate. public static final QoSProfile SYSTEM_DEFAULT = systemDefault(); + + /** + * @internal + * Private default constructor. + * + * Used from native code or private methods. + * Typically combined with @{link QoSProfile#nativeFromRCL(handle)}. + */ + private QoSProfile() {} + + /** + * @internal + * Load from an rmw_qos_profile_t. + * + * @param handle A `rmw_qos_profile_t *`, from where the profile is loaded. + * The handle is only borrowed, and a reference to it is NOT kept. + */ + private final native void nativeFromRCL(long handle); + private final static native long nativeGetHandleFromName(String profileName); + + private History history; + private int depth; + private Reliability reliability; + private Durability durability; + private Duration deadline = Duration.ofSeconds(0, 0); + private Duration lifespan = Duration.ofSeconds(0, 0); + private Liveliness liveliness = Liveliness.SYSTEM_DEFAULT; + private Duration livelinessLeaseDuration = Duration.ofSeconds(0, 0); + private boolean avoidROSNamespaceConventions; } diff --git a/rcljava/src/main/java/org/ros2/rcljava/qos/policies/Durability.java b/rcljava/src/main/java/org/ros2/rcljava/qos/policies/Durability.java index 215602df..08a45329 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/qos/policies/Durability.java +++ b/rcljava/src/main/java/org/ros2/rcljava/qos/policies/Durability.java @@ -18,7 +18,8 @@ public enum Durability implements QoSPolicy { SYSTEM_DEFAULT(0), TRANSIENT_LOCAL(1), - VOLATILE(2); + VOLATILE(2), + UNKNOWN(3); private final int value; diff --git a/rcljava/src/main/java/org/ros2/rcljava/qos/policies/History.java b/rcljava/src/main/java/org/ros2/rcljava/qos/policies/History.java index 461d13d9..69d5b4c3 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/qos/policies/History.java +++ b/rcljava/src/main/java/org/ros2/rcljava/qos/policies/History.java @@ -18,7 +18,8 @@ public enum History implements QoSPolicy { SYSTEM_DEFAULT(0), KEEP_LAST(1), - KEEP_ALL(2); + KEEP_ALL(2), + UNKNOWN(3); private final int value; diff --git a/rcljava/src/main/java/org/ros2/rcljava/qos/policies/Liveliness.java b/rcljava/src/main/java/org/ros2/rcljava/qos/policies/Liveliness.java index 0a814e7f..3b25997f 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/qos/policies/Liveliness.java +++ b/rcljava/src/main/java/org/ros2/rcljava/qos/policies/Liveliness.java @@ -18,7 +18,8 @@ public enum Liveliness implements QoSPolicy { SYSTEM_DEFAULT(0), AUTOMATIC(1), - MANUAL_BY_TOPIC(3); + MANUAL_BY_TOPIC(3), + UNKNOWN(4); private final int value; diff --git a/rcljava/src/main/java/org/ros2/rcljava/qos/policies/Reliability.java b/rcljava/src/main/java/org/ros2/rcljava/qos/policies/Reliability.java index 3764d0bb..ea53f124 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/qos/policies/Reliability.java +++ b/rcljava/src/main/java/org/ros2/rcljava/qos/policies/Reliability.java @@ -18,7 +18,8 @@ public enum Reliability implements QoSPolicy { SYSTEM_DEFAULT(0), RELIABLE(1), - BEST_EFFORT(2); + BEST_EFFORT(2), + UNKNOWN(3); private final int value; From d6c5085040d2bcf0fbdfe3bcb59ad3958aab6f08 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 10 Sep 2020 17:47:58 -0300 Subject: [PATCH 2/9] Add get topic names and type method to Node class (osrf#32) * 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 bf71991c..994dbc4f 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 e2c2e829..86c542b3 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 @@ -238,3 +240,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_CHECK_FOR_EXCEPTION(env); + jclass name_and_types_clazz = env->FindClass("org/ros2/rcljava/graph/NameAndTypes"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jmethodID name_and_types_init_mid = env->GetMethodID(name_and_types_clazz, "", "()V"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jfieldID name_fid = env->GetFieldID(name_and_types_clazz, "name", "Ljava/lang/String;"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jfieldID types_fid = env->GetFieldID(name_and_types_clazz, "types", "Ljava/util/Collection;"); + 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(); + + 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_CHECK_FOR_EXCEPTION_WITH_ERROR_STATEMENT(env, goto cleanup); + jstring jname = env->NewStringUTF(topic_names_and_types.names.data[i]); + 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_CHECK_FOR_EXCEPTION_WITH_ERROR_STATEMENT(env, goto cleanup); + } + env->CallBooleanMethod(jnames_and_types, collection_add_mid, jitem); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION_WITH_ERROR_STATEMENT(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 1f638133..9906463f 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.Disposable; import org.ros2.rcljava.interfaces.MessageDefinition; @@ -753,4 +754,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 8df0cb7f..450fc651 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; @@ -28,7 +29,10 @@ import java.lang.ref.WeakReference; import java.lang.reflect.Method; +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; @@ -37,6 +41,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; @@ -869,4 +874,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 2ece6fb3438c0ae813c059eed54a16383135a526 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 11 Sep 2020 16:18:04 -0300 Subject: [PATCH 3/9] Add getPublishersInfo method to Node (osrf#26) ** Add EndpointInfo class. * Add getPublishersInfo method to Node. Signed-off-by: Ivan Santiago Paunovic --- rcljava/CMakeLists.txt | 2 + .../org_ros2_rcljava_graph_EndpointInfo.h | 34 ++++++ .../include/org_ros2_rcljava_node_NodeImpl.h | 9 ++ rcljava/package.xml | 2 + .../org_ros2_rcljava_graph_EndpointInfo.cpp | 109 ++++++++++++++++++ .../cpp/org_ros2_rcljava_node_NodeImpl.cpp | 64 ++++++++++ .../org/ros2/rcljava/graph/EndpointInfo.java | 58 ++++++++++ .../main/java/org/ros2/rcljava/node/Node.java | 15 ++- .../java/org/ros2/rcljava/node/NodeImpl.java | 12 +- .../java/org/ros2/rcljava/node/NodeTest.java | 62 +++++++++- 10 files changed, 364 insertions(+), 3 deletions(-) create mode 100644 rcljava/include/org_ros2_rcljava_graph_EndpointInfo.h create mode 100644 rcljava/src/main/cpp/org_ros2_rcljava_graph_EndpointInfo.cpp create mode 100644 rcljava/src/main/java/org/ros2/rcljava/graph/EndpointInfo.java diff --git a/rcljava/CMakeLists.txt b/rcljava/CMakeLists.txt index 994dbc4f..e6dfc34e 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 340edf33..0ca75741 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 86c542b3..94a5166d 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" @@ -297,3 +298,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 9906463f..caeb59ca 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.Disposable; import org.ros2.rcljava.interfaces.MessageDefinition; import org.ros2.rcljava.interfaces.ServiceDefinition; @@ -37,6 +37,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; @@ -763,4 +764,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 450fc651..620ee729 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; @@ -33,7 +34,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; @@ -41,9 +44,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 { @@ -945,4 +951,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 From dba9ea63e23a0ab1d6f604e2121f4b00a637074f Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 16 Sep 2020 13:41:06 -0300 Subject: [PATCH 4/9] Add get node names method to Node class (osrf#31) * Add NodeNameInfo class * Implement getNodeNames Signed-off-by: Ivan Santiago Paunovic --- rcljava/CMakeLists.txt | 7 +- .../include/org_ros2_rcljava_node_NodeImpl.h | 9 +++ .../cpp/org_ros2_rcljava_node_NodeImpl.cpp | 78 +++++++++++++++++++ .../org/ros2/rcljava/graph/NodeNameInfo.java | 68 ++++++++++++++++ .../main/java/org/ros2/rcljava/node/Node.java | 7 ++ .../java/org/ros2/rcljava/node/NodeImpl.java | 9 +++ .../java/org/ros2/rcljava/node/NodeTest.java | 59 ++++++++++++++ 7 files changed, 234 insertions(+), 3 deletions(-) create mode 100644 rcljava/src/main/java/org/ros2/rcljava/graph/NodeNameInfo.java diff --git a/rcljava/CMakeLists.txt b/rcljava/CMakeLists.txt index e6dfc34e..e61a8a16 100644 --- a/rcljava/CMakeLists.txt +++ b/rcljava/CMakeLists.txt @@ -143,9 +143,7 @@ set(${PROJECT_NAME}_sources "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" + "src/main/java/org/ros2/rcljava/graph/NodeNameInfo.java" "src/main/java/org/ros2/rcljava/executors/AnyExecutable.java" "src/main/java/org/ros2/rcljava/executors/BaseExecutor.java" "src/main/java/org/ros2/rcljava/executors/Executor.java" @@ -173,6 +171,9 @@ set(${PROJECT_NAME}_sources "src/main/java/org/ros2/rcljava/parameters/service/ParameterServiceImpl.java" "src/main/java/org/ros2/rcljava/publisher/Publisher.java" "src/main/java/org/ros2/rcljava/publisher/PublisherImpl.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" "src/main/java/org/ros2/rcljava/qos/policies/Durability.java" "src/main/java/org/ros2/rcljava/qos/policies/History.java" "src/main/java/org/ros2/rcljava/qos/policies/Liveliness.java" diff --git a/rcljava/include/org_ros2_rcljava_node_NodeImpl.h b/rcljava/include/org_ros2_rcljava_node_NodeImpl.h index 8f25d1f7..46729339 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: nativeGetNodeNames + * Signature: (JLjava/util/List;)V + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetNodeNames( + JNIEnv *, jclass, jlong, jobject); + /* * Class: org_ros2_rcljava_node_NodeImpl * Method: nativeGetTopicNamesAndTypes 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 94a5166d..42b2c2a5 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp @@ -242,6 +242,84 @@ Java_org_ros2_rcljava_node_NodeImpl_nativeCreateTimerHandle( return jtimer; } +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_node_NodeImpl_nativeGetNodeNames( + JNIEnv * env, jclass, jlong handle, jobject jnode_names_info) +{ + rcl_node_t * node = reinterpret_cast(handle); + if (!node) { + rcljava_throw_exception(env, "java/lang/IllegalArgumentException", "node handle is NULL"); + return; + } + + jclass list_clazz = env->GetObjectClass(jnode_names_info); + jmethodID list_add_mid = env->GetMethodID(list_clazz, "add", "(Ljava/lang/Object;)Z"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jclass node_info_clazz = env->FindClass("org/ros2/rcljava/graph/NodeNameInfo"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jmethodID node_info_init_mid = env->GetMethodID(node_info_clazz, "", "()V"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jfieldID name_fid = env->GetFieldID(node_info_clazz, "name", "Ljava/lang/String;"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jfieldID namespace_fid = env->GetFieldID(node_info_clazz, "namespace", "Ljava/lang/String;"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jfieldID enclave_fid = env->GetFieldID(node_info_clazz, "enclave", "Ljava/lang/String;"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcutils_string_array_t node_names = rcutils_get_zero_initialized_string_array(); + rcutils_string_array_t node_namespaces = rcutils_get_zero_initialized_string_array(); + rcutils_string_array_t enclaves = rcutils_get_zero_initialized_string_array(); + + rcl_ret_t ret = rcl_get_node_names_with_enclaves( + node, + allocator, + &node_names, + &node_namespaces, + &enclaves); + RCLJAVA_COMMON_THROW_FROM_RCL(env, ret, "rcl_get_node_names_with_enclaves failed"); + auto on_scope_exit = rcpputils::make_scope_exit( + [pnames = &node_names, pnamespaces = &node_namespaces, penclaves = &enclaves, env]() { + rcl_ret_t ret = rcutils_string_array_fini(pnames); + if (!env->ExceptionCheck() && RCL_RET_OK != ret) { + rcljava_throw_rclexception(env, ret, "failed to fini node names string array"); + } + ret = rcutils_string_array_fini(pnamespaces); + if (!env->ExceptionCheck() && RCL_RET_OK != ret) { + rcljava_throw_rclexception(env, ret, "failed to fini node namespaces string array"); + } + ret = rcutils_string_array_fini(penclaves); + if (!env->ExceptionCheck() && RCL_RET_OK != ret) { + rcljava_throw_rclexception(env, ret, "failed to fini enclaves string array"); + } + } + ); + + if (node_names.size != node_namespaces.size || node_names.size != enclaves.size) { + rcljava_throw_exception( + env, + "java/lang/IllegalStateException", + "names, namespaces and enclaves array leghts don't match"); + return; + } + + for (size_t i = 0; i < node_names.size; i++) { + jstring jnode_name = env->NewStringUTF(node_names.data[i]); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jstring jnode_namespace = env->NewStringUTF(node_namespaces.data[i]); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jstring jenclave = env->NewStringUTF(enclaves.data[i]); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jobject jitem = env->NewObject(node_info_clazz, node_info_init_mid); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + env->SetObjectField(jitem, name_fid, jnode_name); + env->SetObjectField(jitem, namespace_fid, jnode_namespace); + env->SetObjectField(jitem, enclave_fid, jenclave); + env->CallBooleanMethod(jnode_names_info, list_add_mid, jitem); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + } +} + JNIEXPORT void JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetTopicNamesAndTypes( JNIEnv * env, jclass, jlong handle, jobject jnames_and_types) diff --git a/rcljava/src/main/java/org/ros2/rcljava/graph/NodeNameInfo.java b/rcljava/src/main/java/org/ros2/rcljava/graph/NodeNameInfo.java new file mode 100644 index 00000000..2cd29806 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/graph/NodeNameInfo.java @@ -0,0 +1,68 @@ +/* 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.Objects; + +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import org.ros2.rcljava.common.JNIUtils; + +/** + * Class that represents the queried information of a node in the graph. + */ +public class NodeNameInfo { + /// The node name + public String name; + /// The node namespace + public String namespace; + /// The security enclave of the node. + /** + * For further details, see: + * @{link http://design.ros2.org/articles/ros2_security_enclaves.html} + */ + public String enclave; + + /// Constructor + public NodeNameInfo(final String name, final String namespace, final String enclave) { + this.name = name; + this.namespace = namespace; + this.enclave = enclave; + } + + /// Default constructor, used from jni. + private NodeNameInfo() {} + + @Override + public boolean equals(Object o) { + if (o == this) { + return true; + } + if (!(o instanceof NodeNameInfo)) { + return false; + } + NodeNameInfo other = (NodeNameInfo) o; + return Objects.equals(this.name, other.name) && + Objects.equals(this.namespace, other.namespace) && + Objects.equals(this.enclave, other.enclave); + } + + @Override + public int hashCode() { + return Objects.hash(this.name, this.namespace, this.enclave); + } +} 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 42c1281b..e86b3ab5 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/Node.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/Node.java @@ -26,6 +26,7 @@ import org.ros2.rcljava.consumers.TriConsumer; import org.ros2.rcljava.graph.EndpointInfo; import org.ros2.rcljava.graph.NameAndTypes; +import org.ros2.rcljava.graph.NodeNameInfo; import org.ros2.rcljava.interfaces.Disposable; import org.ros2.rcljava.interfaces.MessageDefinition; import org.ros2.rcljava.interfaces.ServiceDefinition; @@ -552,6 +553,12 @@ Client createClient(final Class serviceType, */ rcl_interfaces.msg.ListParametersResult listParameters(List prefixes, long depth); + /** + * Returns a collection of node names that were detected in the ROS graph. + * See @{link NodeNameInfo} for more information about the return value. + */ + Collection getNodeNames(); + /** * Return the topics names and types that were detected in the graph. * See @{link graph#NameAndTypes} for more information about the returned value. 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 caeb59ca..048ed32a 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java @@ -22,6 +22,7 @@ import org.ros2.rcljava.concurrent.Callback; import org.ros2.rcljava.consumers.Consumer; import org.ros2.rcljava.consumers.TriConsumer; +import org.ros2.rcljava.graph.NodeNameInfo; import org.ros2.rcljava.contexts.Context; import org.ros2.rcljava.graph.EndpointInfo; import org.ros2.rcljava.graph.NameAndTypes; @@ -756,6 +757,14 @@ public rcl_interfaces.msg.ListParametersResult listParameters( } } + public final Collection getNodeNames() { + ArrayList nodeNames = new ArrayList(); + nativeGetNodeNames(this.handle, nodeNames); + return nodeNames; + } + + private native static final void nativeGetNodeNames(long handle, ArrayList nodeNames); + public final Collection getTopicNamesAndTypes() { Collection namesAndTypes = new ArrayList(); nativeGetTopicNamesAndTypes(this.handle, 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 620ee729..4d88a830 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java @@ -46,6 +46,7 @@ import org.ros2.rcljava.executors.SingleThreadedExecutor; import org.ros2.rcljava.graph.EndpointInfo; import org.ros2.rcljava.graph.NameAndTypes; +import org.ros2.rcljava.graph.NodeNameInfo; import org.ros2.rcljava.node.Node; import org.ros2.rcljava.publisher.Publisher; import org.ros2.rcljava.qos.policies.Reliability; @@ -879,6 +880,64 @@ public Node getNode() { assertEquals(0, subscriptionOne.getHandle()); subscriptionTwo.dispose(); assertEquals(0, subscriptionTwo.getHandle()); + publisherNode.dispose(); + assertEquals(0, publisherNode.getHandle()); + subscriptionNodeOne.dispose(); + assertEquals(0, subscriptionNodeOne.getHandle()); + subscriptionNodeTwo.dispose(); + assertEquals(0, subscriptionNodeTwo.getHandle()); + } + + @Test + public final void testGetNodeNames() throws Exception { + final Node node1 = RCLJava.createNode("test_get_node_names_1"); + final Node node2 = RCLJava.createNode("test_get_node_names_2"); + final Node node3 = RCLJava.createNode("test_get_node_names_3"); + + Consumer> validateNodeNameInfo = + new Consumer>() { + public void accept(final Collection nodeNamesInfo) { + assertEquals(4, nodeNamesInfo.size()); + assertTrue( + "node 'test_node' was not discovered", + nodeNamesInfo.contains(new NodeNameInfo("test_node", "/", "/"))); + assertTrue( + "node 'test_get_node_names_1' was not discovered", + nodeNamesInfo.contains(new NodeNameInfo("test_get_node_names_1", "/", "/"))); + assertTrue( + "node 'test_get_node_names_2' was not discovered", + nodeNamesInfo.contains(new NodeNameInfo("test_get_node_names_1", "/", "/"))); + assertTrue( + "node 'test_get_node_names_3' was not discovered", + nodeNamesInfo.contains(new NodeNameInfo("test_get_node_names_1", "/", "/"))); + } + }; + + long start = System.currentTimeMillis(); + boolean ok = false; + Collection nodeNamesInfo = null; + do { + nodeNamesInfo = this.node.getNodeNames(); + try { + validateNodeNameInfo.accept(nodeNamesInfo); + 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(nodeNamesInfo); + validateNodeNameInfo.accept(nodeNamesInfo); + + node1.dispose(); + node2.dispose(); + node3.dispose(); } @Test From d0c6d43cd9d47c2d94d54971c5dce445fa1f4026 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 16 Sep 2020 14:57:22 -0300 Subject: [PATCH 5/9] Use rcpputils::scope_exit in native getTopicNamesAndTypes Node method (osrf#38) Signed-off-by: Ivan Santiago Paunovic --- .../cpp/org_ros2_rcljava_node_NodeImpl.cpp | 22 ++++++++++--------- 1 file changed, 12 insertions(+), 10 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 42b2c2a5..24de7e66 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp @@ -352,28 +352,30 @@ Java_org_ros2_rcljava_node_NodeImpl_nativeGetTopicNamesAndTypes( false, &topic_names_and_types); RCLJAVA_COMMON_THROW_FROM_RCL(env, ret, "failed to get topic names and types"); + auto cleanup_names_and_types = rcpputils::make_scope_exit( + [pnames_and_types = &topic_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 topic names and types structure"); + } + } + ); 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_CHECK_FOR_EXCEPTION_WITH_ERROR_STATEMENT(env, goto cleanup); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); jstring jname = env->NewStringUTF(topic_names_and_types.names.data[i]); - RCLJAVA_COMMON_CHECK_FOR_EXCEPTION_WITH_ERROR_STATEMENT(env, goto cleanup); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); 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_CHECK_FOR_EXCEPTION_WITH_ERROR_STATEMENT(env, goto cleanup); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); } env->CallBooleanMethod(jnames_and_types, collection_add_mid, jitem); - RCLJAVA_COMMON_CHECK_FOR_EXCEPTION_WITH_ERROR_STATEMENT(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"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); } } From 22fd2cd52c332c8d90a681c74bab7d0405b61437 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 17 Sep 2020 14:35:59 -0300 Subject: [PATCH 6/9] Add getSubscriptionsInfo to Node class (osrf#27) Signed-off-by: Ivan Santiago Paunovic --- .../include/org_ros2_rcljava_node_NodeImpl.h | 9 +++ .../cpp/org_ros2_rcljava_node_NodeImpl.cpp | 41 +++++++++---- .../main/java/org/ros2/rcljava/node/Node.java | 12 ++++ .../java/org/ros2/rcljava/node/NodeImpl.java | 9 +++ .../java/org/ros2/rcljava/node/NodeTest.java | 59 ++++++++++++++++++- 5 files changed, 117 insertions(+), 13 deletions(-) diff --git a/rcljava/include/org_ros2_rcljava_node_NodeImpl.h b/rcljava/include/org_ros2_rcljava_node_NodeImpl.h index 46729339..de24e820 100644 --- a/rcljava/include/org_ros2_rcljava_node_NodeImpl.h +++ b/rcljava/include/org_ros2_rcljava_node_NodeImpl.h @@ -119,6 +119,15 @@ JNIEXPORT void JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetPublishersInfo( JNIEnv *, jclass, jlong, jstring, jobject); +/* + * Class: org_ros2_rcljava_node_NodeImpl + * Method: nativeGetSubscriptionsInfo + * Signature: (JLjava/lang/String;Ljava/util/List;)V + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetSubscriptionsInfo( + JNIEnv *, jclass, jlong, 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 24de7e66..bb18e0c1 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp @@ -379,9 +379,10 @@ Java_org_ros2_rcljava_node_NodeImpl_nativeGetTopicNamesAndTypes( } } -JNIEXPORT void JNICALL -Java_org_ros2_rcljava_node_NodeImpl_nativeGetPublishersInfo( - JNIEnv * env, jclass, jlong handle, jstring jtopic_name, jobject jpublishers_info) +template +void +get_endpoint_info_common( + JNIEnv * env, jlong handle, jstring jtopic_name, jobject jendpoints_info, FunctorT get_info) { rcl_node_t * node = reinterpret_cast(handle); if (!node) { @@ -391,7 +392,7 @@ Java_org_ros2_rcljava_node_NodeImpl_nativeGetPublishersInfo( } rcutils_allocator_t allocator = rcutils_get_default_allocator(); - rcl_topic_endpoint_info_array_t publishers_info = + rcl_topic_endpoint_info_array_t endpoints_info = rcl_get_zero_initialized_topic_endpoint_info_array(); const char * topic_name = env->GetStringUTFChars(jtopic_name, NULL); @@ -401,26 +402,26 @@ Java_org_ros2_rcljava_node_NodeImpl_nativeGetPublishersInfo( return; } - rcl_ret_t ret = rcl_get_publishers_info_by_topic( + rcl_ret_t ret = get_info( node, &allocator, topic_name, false, // use ros mangling conventions - &publishers_info); + &endpoints_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]() { + [info_ptr = &endpoints_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"); + rcljava_throw_rclexception(env, ret, "failed to destroy rcl endpoints info"); } } ); - jclass list_clazz = env->GetObjectClass(jpublishers_info); + jclass list_clazz = env->GetObjectClass(jendpoints_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"); @@ -431,13 +432,29 @@ Java_org_ros2_rcljava_node_NodeImpl_nativeGetPublishersInfo( endpoint_info_clazz, "nativeFromRCL", "(J)V"); RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); - for (size_t i = 0; i < publishers_info.size; i++) { + for (size_t i = 0; i < endpoints_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]); + env->CallVoidMethod(item, endpoint_info_from_rcl_mid, &endpoints_info.info_array[i]); RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); - env->CallBooleanMethod(jpublishers_info, list_add_mid, item); + env->CallBooleanMethod(jendpoints_info, list_add_mid, item); RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); env->DeleteLocalRef(item); } } + +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_node_NodeImpl_nativeGetPublishersInfo( + JNIEnv * env, jclass, jlong handle, jstring jtopic_name, jobject jpublishers_info) +{ + get_endpoint_info_common( + env, handle, jtopic_name, jpublishers_info, rcl_get_publishers_info_by_topic); +} + +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_node_NodeImpl_nativeGetSubscriptionsInfo( + JNIEnv * env, jclass, jlong handle, jstring jtopic_name, jobject jsubscriptions_info) +{ + get_endpoint_info_common( + env, handle, jtopic_name, jsubscriptions_info, rcl_get_subscriptions_info_by_topic); +} 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 e86b3ab5..ceb7d3ec 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/Node.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/Node.java @@ -578,4 +578,16 @@ Client createClient(final Class serviceType, * passed topic. */ Collection getPublishersInfo(final String topicName); + + /** + * Get information of all subscriptions 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 subscriptions in the + * passed topic. + */ + Collection getSubscriptionsInfo(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 048ed32a..d7f6caa8 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java @@ -782,4 +782,13 @@ public final Collection getPublishersInfo(final String topicName) private native static final void nativeGetPublishersInfo( final long handle, final String topicName, ArrayList endpointInfo); + + public final Collection getSubscriptionsInfo(final String topicName) { + ArrayList returnValue = new ArrayList(); + nativeGetSubscriptionsInfo(this.handle, topicName, returnValue); + return returnValue; + } + + private native static final void nativeGetSubscriptionsInfo( + 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 4d88a830..9f5700ab 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java @@ -1064,4 +1064,61 @@ public void accept(final Collection info) { publisher.dispose(); publisher2.dispose(); } -} \ No newline at end of file + + @Test + public final void testGetSubscriptionsInfo() { + Subscription subscription = node.createSubscription( + rcljava.msg.UInt32.class, "test_get_subscriptions_info", new Consumer() { + public void accept(final rcljava.msg.UInt32 msg) {} + }); + Subscription subscription2 = node.createSubscription( + rcljava.msg.UInt32.class, "test_get_subscriptions_info", new Consumer() { + public void accept(final rcljava.msg.UInt32 msg) {} + }, 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.SUBSCRIPTION); + 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.SUBSCRIPTION); + assertEquals(item.qos.getReliability(), Reliability.BEST_EFFORT); + assertFalse(it.hasNext()); + } + }; + + long start = System.currentTimeMillis(); + boolean ok = false; + Collection subscriptionsInfo = null; + do { + subscriptionsInfo = node.getSubscriptionsInfo("/test_get_subscriptions_info"); + try { + validateEndpointInfo.accept(subscriptionsInfo); + 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(subscriptionsInfo); + validateEndpointInfo.accept(subscriptionsInfo); + subscription.dispose(); + subscription2.dispose(); + } +} From 710fc2a211f52b421323d69d4e89a11520962121 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 17 Sep 2020 18:54:57 -0300 Subject: [PATCH 7/9] Implement nativeGetServiceNamesAndTypes method (osrf#33) Signed-off-by: Ivan Santiago Paunovic --- .../include/org_ros2_rcljava_node_NodeImpl.h | 9 ++ .../cpp/org_ros2_rcljava_node_NodeImpl.cpp | 91 ++++++++++++------- .../main/java/org/ros2/rcljava/node/Node.java | 8 ++ .../java/org/ros2/rcljava/node/NodeImpl.java | 9 ++ .../java/org/ros2/rcljava/node/NodeTest.java | 65 +++++++++++++ 5 files changed, 151 insertions(+), 31 deletions(-) diff --git a/rcljava/include/org_ros2_rcljava_node_NodeImpl.h b/rcljava/include/org_ros2_rcljava_node_NodeImpl.h index de24e820..75b52e61 100644 --- a/rcljava/include/org_ros2_rcljava_node_NodeImpl.h +++ b/rcljava/include/org_ros2_rcljava_node_NodeImpl.h @@ -110,6 +110,15 @@ JNIEXPORT void JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetTopicNamesAndTypes( JNIEnv *, jclass, jlong, jobject); +/* + * Class: org_ros2_rcljava_node_NodeImpl + * Method: nativeGetServiceNamesAndTypes + * Signature: (JLjava/util/Collection;)V + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetServiceNamesAndTypes( + JNIEnv *, jclass, jlong, jobject); + /* * Class: org_ros2_rcljava_node_NodeImpl * Method: nativeGetPublishersInfo 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 bb18e0c1..a7c662f1 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp @@ -320,16 +320,10 @@ Java_org_ros2_rcljava_node_NodeImpl_nativeGetNodeNames( } } -JNIEXPORT void JNICALL -Java_org_ros2_rcljava_node_NodeImpl_nativeGetTopicNamesAndTypes( - JNIEnv * env, jclass, jlong handle, jobject jnames_and_types) +void +fill_jnames_and_types( + JNIEnv * env, const rcl_names_and_types_t & names_and_types, 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"); @@ -343,34 +337,16 @@ Java_org_ros2_rcljava_node_NodeImpl_nativeGetTopicNamesAndTypes( jfieldID types_fid = env->GetFieldID(name_and_types_clazz, "types", "Ljava/util/Collection;"); 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(); - - 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"); - auto cleanup_names_and_types = rcpputils::make_scope_exit( - [pnames_and_types = &topic_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 topic names and types structure"); - } - } - ); - - for (size_t i = 0; i < topic_names_and_types.names.size; i++) { + for (size_t i = 0; i < names_and_types.names.size; i++) { jobject jitem = env->NewObject(name_and_types_clazz, name_and_types_init_mid); RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); - jstring jname = env->NewStringUTF(topic_names_and_types.names.data[i]); + jstring jname = env->NewStringUTF(names_and_types.names.data[i]); RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); 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]); + for (size_t j = 0; j < names_and_types.types[i].size; j++) { + jstring jtype = env->NewStringUTF(names_and_types.types[i].data[j]); env->CallBooleanMethod(jtypes, collection_add_mid, jtype); RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); } @@ -379,6 +355,59 @@ Java_org_ros2_rcljava_node_NodeImpl_nativeGetTopicNamesAndTypes( } } +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; + } + + 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"); + fill_jnames_and_types(env, topic_names_and_types, jnames_and_types); + + 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"); + } +} + +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_node_NodeImpl_nativeGetServiceNamesAndTypes( + 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; + } + + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_names_and_types_t service_names_and_types = rcl_get_zero_initialized_names_and_types(); + + rcl_ret_t ret = rcl_get_service_names_and_types( + node, + &allocator, + &service_names_and_types); + RCLJAVA_COMMON_THROW_FROM_RCL(env, ret, "failed to get service names and types"); + fill_jnames_and_types(env, service_names_and_types, jnames_and_types); + + ret = rcl_names_and_types_fini(&service_names_and_types); + if (!env->ExceptionCheck() && RCL_RET_OK != ret) { + rcljava_throw_rclexception(env, ret, "failed to fini service names and types structure"); + } +} + template void get_endpoint_info_common( 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 ceb7d3ec..5059adee 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/Node.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/Node.java @@ -567,6 +567,14 @@ Client createClient(final Class serviceType, */ Collection getTopicNamesAndTypes(); + /** + * Return the service names and types that were detected in the graph. + * See @{link graph#NameAndTypes} for more information about the returned value. + * + * @return the detected service names and types. + */ + Collection getServiceNamesAndTypes(); + /** * Get information of all publishers in a topic. * 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 d7f6caa8..1d2d8061 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java @@ -774,6 +774,15 @@ public final Collection getTopicNamesAndTypes() { private static native final void nativeGetTopicNamesAndTypes( long handle, Collection namesAndTypes); + public final Collection getServiceNamesAndTypes() { + Collection namesAndTypes = new ArrayList(); + nativeGetServiceNamesAndTypes(this.handle, namesAndTypes); + return namesAndTypes; + } + + private static native final void nativeGetServiceNamesAndTypes( + long handle, Collection namesAndTypes); + public final Collection getPublishersInfo(final String topicName) { ArrayList returnValue = new ArrayList(); nativeGetPublishersInfo(this.handle, topicName, returnValue); 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 9f5700ab..220bb2ae 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java @@ -39,8 +39,10 @@ import java.util.concurrent.TimeUnit; import org.ros2.rcljava.RCLJava; +import org.ros2.rcljava.client.Client; import org.ros2.rcljava.concurrent.RCLFuture; import org.ros2.rcljava.consumers.Consumer; +import org.ros2.rcljava.consumers.TriConsumer; import org.ros2.rcljava.executors.Executor; import org.ros2.rcljava.executors.MultiThreadedExecutor; import org.ros2.rcljava.executors.SingleThreadedExecutor; @@ -51,6 +53,8 @@ import org.ros2.rcljava.publisher.Publisher; import org.ros2.rcljava.qos.policies.Reliability; import org.ros2.rcljava.qos.QoSProfile; +import org.ros2.rcljava.service.RMWRequestId; +import org.ros2.rcljava.service.Service; import org.ros2.rcljava.subscription.Subscription; public class NodeTest { @@ -1011,6 +1015,67 @@ public void accept(final Collection namesAndTypes) { subscription2.dispose(); } + @Test + public final void testGetServiceNamesAndTypes() throws Exception { + Service service = node.createService( + rcljava.srv.AddTwoInts.class, "test_service_names_and_types_one", + new TriConsumer< + RMWRequestId, rcljava.srv.AddTwoInts_Request, rcljava.srv.AddTwoInts_Response>() + { + public final void accept( + final RMWRequestId header, + final rcljava.srv.AddTwoInts_Request request, + final rcljava.srv.AddTwoInts_Response response) + {} + }); + Client client = node.createClient( + rcljava.srv.AddTwoInts.class, "test_service_names_and_types_two"); + + Consumer> validateNameAndTypes = + new Consumer>() { + public void accept(final Collection namesAndTypes) { + assertEquals(namesAndTypes.size(), 2); + assertTrue( + "service 'test_service_names_and_types_one' was not discovered", + namesAndTypes.contains( + new NameAndTypes( + "/test_service_names_and_types_one", + new ArrayList(Arrays.asList("rcljava/srv/AddTwoInts"))))); + assertTrue( + "service 'test_service_names_and_types_two' was not discovered", + namesAndTypes.contains( + new NameAndTypes( + "/test_service_names_and_types_two", + new ArrayList(Arrays.asList("rcljava/srv/AddTwoInts"))))); + } + }; + + long start = System.currentTimeMillis(); + boolean ok = false; + Collection namesAndTypes = null; + do { + namesAndTypes = this.node.getServiceNamesAndTypes(); + 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); + + service.dispose(); + client.dispose(); + } + @Test public final void testGetPublishersInfo() { Publisher publisher = From b0b30e48f3e34c9bb5aa441596c68e66d5fba2a1 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 14 Apr 2021 10:24:50 -0300 Subject: [PATCH 8/9] Do not use multiple message types in the same topic (osrf#61) Signed-off-by: Ivan Santiago Paunovic --- .../src/test/java/org/ros2/rcljava/node/NodeTest.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) 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 220bb2ae..794075b0 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java @@ -950,10 +950,10 @@ public final void testGetTopicNamesAndTypes() throws Exception { 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 subscription = node.createSubscription( + rcljava.msg.UInt32.class, "test_get_topic_names_and_types_one", + new Consumer() { + public void accept(final rcljava.msg.UInt32 msg) {} }); Subscription subscription2 = node.createSubscription( rcljava.msg.Empty.class, "test_get_topic_names_and_types_three", @@ -971,7 +971,7 @@ public void accept(final Collection namesAndTypes) { namesAndTypes.contains( new NameAndTypes( "/test_get_topic_names_and_types_one", - new ArrayList(Arrays.asList("rcljava/msg/Empty", "rcljava/msg/UInt32"))))); + new ArrayList(Arrays.asList("rcljava/msg/UInt32"))))); assertTrue( "topic 'test_get_topic_names_and_types_two' was not discovered", namesAndTypes.contains( From 18faaa767596a808f82c76c0115a9187a9486748 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Wed, 13 Apr 2022 13:54:15 -0700 Subject: [PATCH 9/9] Add missing target dependency on rcpputils (osrf#90) Fixes a build issue with newer versions of ROS Rolling: fatal error: rcpputils/scope_exit.hpp: No such file or directory Signed-off-by: Jacob Perron --- rcljava/CMakeLists.txt | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/rcljava/CMakeLists.txt b/rcljava/CMakeLists.txt index e61a8a16..adf05123 100644 --- a/rcljava/CMakeLists.txt +++ b/rcljava/CMakeLists.txt @@ -6,6 +6,7 @@ find_package(ament_cmake REQUIRED) find_package(ament_cmake_export_jars REQUIRED) find_package(ament_cmake_export_jni_libraries REQUIRED) find_package(builtin_interfaces REQUIRED) +find_package(rcpputils REQUIRED) find_package(rcl REQUIRED) find_package(rcl_interfaces REQUIRED) find_package(rcljava_common REQUIRED) @@ -104,10 +105,11 @@ foreach(_jni_source ${${PROJECT_NAME}_jni_sources}) endif() ament_target_dependencies(${_target_name} + "builtin_interfaces" "rcl" "rcljava_common" - "builtin_interfaces" "rcl_interfaces" + "rcpputils" ) target_include_directories(${_target_name}