From ae53ab741bb716416b60e528d13db2823a2b1ee6 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 2 Sep 2020 14:55:55 -0300 Subject: [PATCH 01/51] Add MessageLost subscription event status (osrf/ros2_java#29) Signed-off-by: Ivan Santiago Paunovic --- rcljava/CMakeLists.txt | 2 + ...cljava_subscription_statuses_MessageLost.h | 63 ++++++++++++++++ ...java_subscription_statuses_MessageLost.cpp | 74 +++++++++++++++++++ .../subscription/statuses/MessageLost.java | 65 ++++++++++++++++ .../subscription/SubscriptionTest.java | 29 ++++++++ 5 files changed, 233 insertions(+) create mode 100644 rcljava/include/org_ros2_rcljava_subscription_statuses_MessageLost.h create mode 100644 rcljava/src/main/cpp/org_ros2_rcljava_subscription_statuses_MessageLost.cpp create mode 100644 rcljava/src/main/java/org/ros2/rcljava/subscription/statuses/MessageLost.java diff --git a/rcljava/CMakeLists.txt b/rcljava/CMakeLists.txt index dbd3d649..c454fab7 100644 --- a/rcljava/CMakeLists.txt +++ b/rcljava/CMakeLists.txt @@ -69,6 +69,7 @@ set(${PROJECT_NAME}_jni_sources "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" + "src/main/cpp/org_ros2_rcljava_subscription_statuses_MessageLost.cpp" "src/main/cpp/org_ros2_rcljava_subscription_statuses_RequestedDeadlineMissed.cpp" "src/main/cpp/org_ros2_rcljava_subscription_statuses_RequestedQosIncompatible.cpp" "src/main/cpp/org_ros2_rcljava_time_Clock.cpp" @@ -180,6 +181,7 @@ set(${PROJECT_NAME}_sources "src/main/java/org/ros2/rcljava/subscription/Subscription.java" "src/main/java/org/ros2/rcljava/subscription/SubscriptionImpl.java" "src/main/java/org/ros2/rcljava/subscription/statuses/LivelinessChanged.java" + "src/main/java/org/ros2/rcljava/subscription/statuses/MessageLost.java" "src/main/java/org/ros2/rcljava/subscription/statuses/RequestedDeadlineMissed.java" "src/main/java/org/ros2/rcljava/subscription/statuses/RequestedQosIncompatible.java" "src/main/java/org/ros2/rcljava/time/Clock.java" diff --git a/rcljava/include/org_ros2_rcljava_subscription_statuses_MessageLost.h b/rcljava/include/org_ros2_rcljava_subscription_statuses_MessageLost.h new file mode 100644 index 00000000..3139afa9 --- /dev/null +++ b/rcljava/include/org_ros2_rcljava_subscription_statuses_MessageLost.h @@ -0,0 +1,63 @@ +// 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_subscription_statuses_MessageLost */ + +#ifndef ORG_ROS2_RCLJAVA_SUBSCRIPTION_STATUSES_MESSAGELOST_H_ +#define ORG_ROS2_RCLJAVA_SUBSCRIPTION_STATUSES_MESSAGELOST_H_ +#ifdef __cplusplus +extern "C" { +#endif + +/* + * Class: org_ros2_rcljava_subscription_statuses_MessageLost + * Method: nativeAllocateRCLStatusEvent + * Signature: ()J + */ +JNIEXPORT jlong JNICALL +Java_org_ros2_rcljava_subscription_statuses_MessageLost_nativeAllocateRCLStatusEvent( + JNIEnv *, jclass); + +/* + * Class: org_ros2_rcljava_subscription_statuses_MessageLost + * Method: nativeDeallocateRCLStatusEvent + * Signature: (J)V + */ +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_subscription_statuses_MessageLost_nativeDeallocateRCLStatusEvent( + JNIEnv *, jclass, jlong); + +/* + * Class: org_ros2_rcljava_subscription_statuses_MessageLost + * Method: nativeFromRCLEvent + * Signature: (J)V + */ +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_subscription_statuses_MessageLost_nativeFromRCLEvent( + JNIEnv *, jobject, jlong); + +/* + * Class: org_ros2_rcljava_subscription_statuses_MessageLost + * Method: nativeGetsubscriptionEventType + * Signature: ()I + */ +JNIEXPORT jint JNICALL +Java_org_ros2_rcljava_subscription_statuses_MessageLost_nativeGetSubscriptionEventType( + JNIEnv *, jclass); + +#ifdef __cplusplus +} +#endif +#endif // ORG_ROS2_RCLJAVA_SUBSCRIPTION_STATUSES_MESSAGELOST_H_ diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_subscription_statuses_MessageLost.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_subscription_statuses_MessageLost.cpp new file mode 100644 index 00000000..d2f935d5 --- /dev/null +++ b/rcljava/src/main/cpp/org_ros2_rcljava_subscription_statuses_MessageLost.cpp @@ -0,0 +1,74 @@ +// 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_subscription_statuses_MessageLost.h" + +#include +#include + +#include "rmw/types.h" +#include "rcl/event.h" +#include "rcljava_common/exceptions.hpp" + +using rcljava_common::exceptions::rcljava_throw_exception; + +JNIEXPORT jlong JNICALL +Java_org_ros2_rcljava_subscription_statuses_MessageLost_nativeAllocateRCL( + JNIEnv * env, jclass) +{ + void * p = malloc(sizeof(rmw_message_lost_status_t)); + if (!p) { + rcljava_throw_exception( + env, "java/lang/OutOfMemoryError", "failed to allocate missed deadline status"); + } + return reinterpret_cast(p); +} + +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_subscription_statuses_MessageLost_nativeDeallocateRCL( + JNIEnv *, jclass, jlong handle) +{ + free(reinterpret_cast(handle)); +} + +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_subscription_statuses_MessageLost_nativeFromRCL( + JNIEnv * env, jobject self, jlong handle) +{ + auto * p = reinterpret_cast(handle); + if (!p) { + rcljava_throw_exception( + env, "java/lang/IllegalArgumentException", "passed rmw object handle is NULL"); + return; + } + // TODO(ivanpauno): class and field lookup could be done at startup time + jclass clazz = env->GetObjectClass(self); + jfieldID total_count_fid = env->GetFieldID(clazz, "totalCount", "I"); + if (env->ExceptionCheck()) { + return; + } + jfieldID total_count_change_fid = env->GetFieldID(clazz, "totalCountChange", "I"); + if (env->ExceptionCheck()) { + return; + } + env->SetIntField(self, total_count_fid, p->total_count); + env->SetIntField(self, total_count_change_fid, p->total_count_change); +} + +JNIEXPORT jint JNICALL +Java_org_ros2_rcljava_subscription_statuses_MessageLost_nativeGetEventType( + JNIEnv *, jclass) +{ + return RCL_SUBSCRIPTION_MESSAGE_LOST; +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/subscription/statuses/MessageLost.java b/rcljava/src/main/java/org/ros2/rcljava/subscription/statuses/MessageLost.java new file mode 100644 index 00000000..d136b4df --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/subscription/statuses/MessageLost.java @@ -0,0 +1,65 @@ +// 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.subscription.statuses; + +import java.util.function.Supplier; + +import org.ros2.rcljava.common.JNIUtils; +import org.ros2.rcljava.events.SubscriptionEventStatus; + +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +/** + * This class serves as a bridge between a rmw_message_lost_status_t and RCLJava. + */ +public class MessageLost implements SubscriptionEventStatus { + public int totalCount; + public int totalCountChange; + + public final long allocateRCLStatusEvent() { + return nativeAllocateRCL(); + } + public final void deallocateRCLStatusEvent(long handle) { + nativeDeallocateRCL(handle); + } + public final void fromRCLEvent(long handle) { + nativeFromRCL(handle); + } + public final int getSubscriptionEventType() { + return nativeGetEventType(); + } + // TODO(ivanpauno): Remove this when -source 8 can be used (method references for the win) + public static final Supplier factory = new Supplier() { + public MessageLost get() { + return new MessageLost(); + } + }; + + private static final Logger logger = LoggerFactory.getLogger(MessageLost.class); + static { + try { + JNIUtils.loadImplementation(MessageLost.class); + } catch (UnsatisfiedLinkError ule) { + logger.error("Native code library failed to load.\n" + ule); + System.exit(1); + } + } + + private static native long nativeAllocateRCL(); + private static native void nativeDeallocateRCL(long handle); + private native void nativeFromRCL(long handle); + private static native int nativeGetEventType(); +} diff --git a/rcljava/src/test/java/org/ros2/rcljava/subscription/SubscriptionTest.java b/rcljava/src/test/java/org/ros2/rcljava/subscription/SubscriptionTest.java index 21299024..59976add 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/subscription/SubscriptionTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/subscription/SubscriptionTest.java @@ -28,6 +28,7 @@ import org.ros2.rcljava.events.EventHandler; import org.ros2.rcljava.node.Node; import org.ros2.rcljava.subscription.statuses.LivelinessChanged; +import org.ros2.rcljava.subscription.statuses.MessageLost; import org.ros2.rcljava.subscription.statuses.RequestedDeadlineMissed; import org.ros2.rcljava.subscription.statuses.RequestedQosIncompatible; @@ -149,4 +150,32 @@ public void accept(final RequestedQosIncompatible status) { RCLJava.shutdown(); assertEquals(0, eventHandler.getHandle()); } + + @Test + public final void testCreateMessageLost() { + String identifier = RCLJava.getRMWIdentifier(); + if (identifier.equals("rmw_fastrtps_cpp") || identifier.equals("rmw_fastrtps_dynamic_cpp")) { + // event not supported in these implementations + return; + } + RCLJava.rclJavaInit(); + Node node = RCLJava.createNode("test_node"); + Subscription subscription = node.createSubscription( + std_msgs.msg.String.class, "test_topic", new Consumer() { + public void accept(final std_msgs.msg.String msg) {} + }); + EventHandler eventHandler = subscription.createEventHandler( + MessageLost.factory, new Consumer() { + public void accept(final MessageLost status) { + assertEquals(status.totalCount, 0); + assertEquals(status.totalCountChange, 0); + } + } + ); + assertNotEquals(0, eventHandler.getHandle()); + // force executing the callback, so we check that taking an event works + eventHandler.executeCallback(); + RCLJava.shutdown(); + assertEquals(0, eventHandler.getHandle()); + } } From c63d8063cc3b281c378fd0f84cae5a74038fb279 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 3 Sep 2020 13:57:03 -0300 Subject: [PATCH 02/51] Add convenient error handling macros (osrf/ros2_java#30) Add macros to handle java exceptions Add macros to throw a java exception from an rcl error Always use `base_message` instead of `message` in docblocks Signed-off-by: Ivan Santiago Paunovic --- .../include/rcljava_common/exceptions.hpp | 51 +++++++++++++++++++ 1 file changed, 51 insertions(+) diff --git a/rcljava_common/include/rcljava_common/exceptions.hpp b/rcljava_common/include/rcljava_common/exceptions.hpp index 8a148611..b7f2d3c5 100644 --- a/rcljava_common/include/rcljava_common/exceptions.hpp +++ b/rcljava_common/include/rcljava_common/exceptions.hpp @@ -19,6 +19,57 @@ #include "rcljava_common/visibility_control.hpp" +/// Execute \a error_statement if an exception has happened. +/** + * \param env a JNIEnv pointer, used to check for exceptions. + * \param error_statement statement executed if an exception has happened. + */ +#define RCLJAVA_COMMON_CHECK_FOR_EXCEPTION_WITH_ERROR_STATEMENT(env, error_statement) \ + do { \ + if (env->ExceptionCheck()) { \ + error_statement; \ + } \ + } while (0) + +/// Return from the current function if a java exception has happened. +/** + * \param env a JNIEnv pointer, used to check for exceptions. + */ +#define RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env) \ + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION_WITH_ERROR_STATEMENT(env, return ) + +/// Call \ref rcljava_throw_rclexception if \a ret is not RCL_RET_OK, +/// and execute \a error_statement in that case. +/** + * The rcl error message will be added to \a base_message, and the rcl error state will be reset. + * + * \param env a JNIEnv pointer, used to throw a java exception from the rcl error. + * \param ret rcl_ret_t error that will be checked. + * \param base_message error message that will be passed to the thrown exception. + * The complete error will be "${base_message}: ${rcl_error_string}". + * \a base_message can be either a `const char *` or as `std::string`. + * \param error_statement statement executed if ret was not RCL_RET_OK. + */ +#define RCLJAVA_COMMON_THROW_FROM_RCL_WITH_ERROR_STATEMENT( \ + env, ret, base_message, error_statement) \ + do { \ + if (RCL_RET_OK != ret) { \ + rcljava_common::exceptions::rcljava_throw_rclexception( \ + env, ret, static_cast(base_message) + ": " + rcl_get_error_string().str); \ + rcl_reset_error(); \ + error_statement; \ + } \ + } while (0) + +/// Call \ref rcljava_throw_rclexception if \a ret is not RCL_RET_OK and return. +/** + * \param env a JNIEnv pointer, used to check for exceptions. + * \param ret rcl_ret_t error that will be checked. + * \param base_message error message that will be passed to the thrown exception. + */ +#define RCLJAVA_COMMON_THROW_FROM_RCL(env, ret, base_message) \ + RCLJAVA_COMMON_THROW_FROM_RCL_WITH_ERROR_STATEMENT(env, ret, base_message, return ) + namespace rcljava_common { namespace exceptions From 56b6135cc4386c40ffa0686aec78109e948b0b63 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 9 Sep 2020 14:51:51 -0300 Subject: [PATCH 03/51] Update qos profile -- take 2 (osrf/ros2_java#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 2a03d23f5eac0c716ad3d5d2c6a7b71e8931500f Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 10 Sep 2020 17:47:58 -0300 Subject: [PATCH 04/51] Add get topic names and type method to Node class (osrf/ros2_java#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 dfea32929a4452420288e88005ce0b187d5d5818 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 11 Sep 2020 16:18:04 -0300 Subject: [PATCH 05/51] Add getPublishersInfo method to Node (osrf/ros2_java#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 83c0681e0683ff414c6e558e1b3d699ef5386ad8 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 16 Sep 2020 13:41:06 -0300 Subject: [PATCH 06/51] Add get node names method to Node class (osrf/ros2_java#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 9a5ca1b969dd1dbaadcaa233d89ecd86d6e05615 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 16 Sep 2020 14:57:22 -0300 Subject: [PATCH 07/51] Use rcpputils::scope_exit in native getTopicNamesAndTypes Node method (osrf/ros2_java#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 8c2054d169f1a8d0c4283cef0b478c1aed3d45ee Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 17 Sep 2020 14:35:59 -0300 Subject: [PATCH 08/51] Add getSubscriptionsInfo to Node class (osrf/ros2_java#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 158f7c7ac17503f900e4f6f28daacbc913935b53 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 17 Sep 2020 18:54:57 -0300 Subject: [PATCH 09/51] Implement nativeGetServiceNamesAndTypes method (osrf/ros2_java#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 a938544657509e29ff60b2fd01ff7727b348468f Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Fri, 18 Sep 2020 12:19:59 -0700 Subject: [PATCH 10/51] Log error if calls to dispose fail (osrf/ros2_java#40) In these cases, we are likely leaking memory. Signed-off-by: Jacob Perron --- .../main/java/org/ros2/rcljava/client/ClientImpl.java | 10 ++++++---- .../java/org/ros2/rcljava/publisher/PublisherImpl.java | 10 ++++++---- .../java/org/ros2/rcljava/service/ServiceImpl.java | 10 ++++++---- .../ros2/rcljava/subscription/SubscriptionImpl.java | 10 ++++++---- 4 files changed, 24 insertions(+), 16 deletions(-) diff --git a/rcljava/src/main/java/org/ros2/rcljava/client/ClientImpl.java b/rcljava/src/main/java/org/ros2/rcljava/client/ClientImpl.java index 70ff3074..c5189b49 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/client/ClientImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/client/ClientImpl.java @@ -135,11 +135,13 @@ public final Class getResponseType() { */ public final void dispose() { Node node = this.nodeReference.get(); - if (node != null) { - node.removeClient(this); - nativeDispose(node.getHandle(), this.handle); - this.handle = 0; + if (node == null) { + logger.error("Node reference is null. Failed to dispose of Client."); + return; } + node.removeClient(this); + nativeDispose(node.getHandle(), this.handle); + this.handle = 0; } /** diff --git a/rcljava/src/main/java/org/ros2/rcljava/publisher/PublisherImpl.java b/rcljava/src/main/java/org/ros2/rcljava/publisher/PublisherImpl.java index 0e93531e..7911916b 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/publisher/PublisherImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/publisher/PublisherImpl.java @@ -187,10 +187,12 @@ public final void dispose() { } this.eventHandlers.clear(); Node node = this.nodeReference.get(); - if (node != null) { - node.removePublisher(this); - nativeDispose(node.getHandle(), this.handle); - this.handle = 0; + if (node == null) { + logger.error("Node reference is null. Failed to dispose of Publisher."); + return; } + node.removePublisher(this); + nativeDispose(node.getHandle(), this.handle); + this.handle = 0; } } diff --git a/rcljava/src/main/java/org/ros2/rcljava/service/ServiceImpl.java b/rcljava/src/main/java/org/ros2/rcljava/service/ServiceImpl.java index 8d7a3254..a02896b1 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/service/ServiceImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/service/ServiceImpl.java @@ -84,11 +84,13 @@ public final Class getResponseType() { */ public final void dispose() { Node node = this.nodeReference.get(); - if (node != null) { - node.removeService(this); - nativeDispose(node.getHandle(), this.handle); - this.handle = 0; + if (node == null) { + logger.error("Node reference is null. Failed to dispose of Service."); + return; } + node.removeService(this); + nativeDispose(node.getHandle(), this.handle); + this.handle = 0; } /** diff --git a/rcljava/src/main/java/org/ros2/rcljava/subscription/SubscriptionImpl.java b/rcljava/src/main/java/org/ros2/rcljava/subscription/SubscriptionImpl.java index 8e20d81d..7a969af0 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/subscription/SubscriptionImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/subscription/SubscriptionImpl.java @@ -200,11 +200,13 @@ public final void dispose() { } this.eventHandlers.clear(); Node node = this.nodeReference.get(); - if (node != null) { - node.removeSubscription(this); - nativeDispose(node.getHandle(), this.handle); - this.handle = 0; + if (node == null) { + logger.error("Node reference is null. Failed to dispose of Subscription."); + return; } + node.removeSubscription(this); + nativeDispose(node.getHandle(), this.handle); + this.handle = 0; } public void executeCallback(T message) { From 60ac29380e7e2ba6f7b6a1ef7c1feae0bdccb99c Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 18 Sep 2020 16:33:52 -0300 Subject: [PATCH 11/51] Add get publishers by node (osrf/ros2_java#34) Signed-off-by: Ivan Santiago Paunovic --- .../include/org_ros2_rcljava_node_NodeImpl.h | 9 ++ .../cpp/org_ros2_rcljava_node_NodeImpl.cpp | 38 ++++++++ .../main/java/org/ros2/rcljava/node/Node.java | 11 +++ .../java/org/ros2/rcljava/node/NodeImpl.java | 11 +++ .../java/org/ros2/rcljava/node/NodeTest.java | 86 +++++++++++++++++++ 5 files changed, 155 insertions(+) diff --git a/rcljava/include/org_ros2_rcljava_node_NodeImpl.h b/rcljava/include/org_ros2_rcljava_node_NodeImpl.h index 75b52e61..90c565ed 100644 --- a/rcljava/include/org_ros2_rcljava_node_NodeImpl.h +++ b/rcljava/include/org_ros2_rcljava_node_NodeImpl.h @@ -137,6 +137,15 @@ JNIEXPORT void JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetSubscriptionsInfo( JNIEnv *, jclass, jlong, jstring, jobject); +/* + * Class: org_ros2_rcljava_node_NodeImpl + * Method: nativeGetPublisherNamesAndTypesByNode + * Signature: (JLjava/lang/String;Ljava/lang/String;Ljava/util/Collection;)V + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetPublisherNamesAndTypesByNode( + JNIEnv *, jclass, jlong, jstring, 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 a7c662f1..4b0d57d8 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp @@ -487,3 +487,41 @@ Java_org_ros2_rcljava_node_NodeImpl_nativeGetSubscriptionsInfo( get_endpoint_info_common( env, handle, jtopic_name, jsubscriptions_info, rcl_get_subscriptions_info_by_topic); } + +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_node_NodeImpl_nativeGetPublisherNamesAndTypesByNode( + JNIEnv * env, jclass, jlong handle, jstring jname, jstring jnamespace, 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; + } + + const char * name = env->GetStringUTFChars(jname, NULL); + auto release_jname = rcpputils::make_scope_exit( + [jname, name, env]() {env->ReleaseStringUTFChars(jname, name);}); + const char * namespace_ = env->GetStringUTFChars(jnamespace, NULL); + auto release_jnamespace = rcpputils::make_scope_exit( + [jnamespace, namespace_, env]() {env->ReleaseStringUTFChars(jnamespace, namespace_);}); + + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_names_and_types_t publisher_names_and_types = rcl_get_zero_initialized_names_and_types(); + auto fini_names_and_types = rcpputils::make_scope_exit( + [pnames_and_types = &publisher_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 publisher names and types structure"); + } + }); + + rcl_ret_t ret = rcl_get_publisher_names_and_types_by_node( + node, + &allocator, + false, + name, + namespace_, + &publisher_names_and_types); + RCLJAVA_COMMON_THROW_FROM_RCL(env, ret, "failed to get publisher names and types"); + fill_jnames_and_types(env, publisher_names_and_types, jnames_and_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 5059adee..59c2cf50 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/Node.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/Node.java @@ -598,4 +598,15 @@ Client createClient(final Class serviceType, * passed topic. */ Collection getSubscriptionsInfo(final String topicName); + + /** + * Return the publisher names and types that were created from the node specified by the given + * node name and namespace. + * See @{link graph#NameAndTypes} for more information about the returned value. + * + * @param nodeName name of the node we want to know its publishers. + * @param nodeNamespace namespace of the node we want to know its publishers. + * @return the detected publisher names and types. + */ + Collection getPublisherNamesAndTypesByNode(String nodeName, String nodeNamespace); } 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 1d2d8061..5d240728 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java @@ -800,4 +800,15 @@ public final Collection getSubscriptionsInfo(final String topicNam private native static final void nativeGetSubscriptionsInfo( final long handle, final String topicName, ArrayList endpointInfo); + + public final Collection getPublisherNamesAndTypesByNode( + String nodeName, String nodeNamespace) + { + Collection namesAndTypes = new ArrayList(); + nativeGetPublisherNamesAndTypesByNode(this.handle, nodeName, nodeNamespace, namesAndTypes); + return namesAndTypes; + } + + private static native final Collection nativeGetPublisherNamesAndTypesByNode( + long handle, String nodeName, String nodeNamespace, 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 220bb2ae..6afbb25b 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java @@ -42,6 +42,7 @@ import org.ros2.rcljava.client.Client; import org.ros2.rcljava.concurrent.RCLFuture; import org.ros2.rcljava.consumers.Consumer; +import org.ros2.rcljava.consumers.BiConsumer; import org.ros2.rcljava.consumers.TriConsumer; import org.ros2.rcljava.executors.Executor; import org.ros2.rcljava.executors.MultiThreadedExecutor; @@ -1186,4 +1187,89 @@ public void accept(final Collection info) { subscription.dispose(); subscription2.dispose(); } + + @Test + public final void testGetPublisherNamesAndTypesByNode() throws Exception { + final Node remoteNode = RCLJava.createNode("test_get_publisher_names_and_types_remote_node"); + Publisher publisher1 = node.createPublisher( + rcljava.msg.UInt32.class, "test_get_publisher_names_and_types_one"); + Publisher publisher2 = node.createPublisher( + rcljava.msg.UInt32.class, "test_get_publisher_names_and_types_two"); + Publisher publisher3 = remoteNode.createPublisher( + rcljava.msg.UInt32.class, "test_get_publisher_names_and_types_two"); + Publisher publisher4 = remoteNode.createPublisher( + rcljava.msg.UInt32.class, "test_get_publisher_names_and_types_three"); + Subscription subscription = node.createSubscription( + rcljava.msg.Empty.class, "test_get_topic_names_and_types_this_should_not_appear", + new Consumer() { + public void accept(final rcljava.msg.Empty msg) {} + }); + + BiConsumer, Collection> validateNameAndTypes = + new BiConsumer, Collection>() { + public void accept(final Collection local, Collection remote) { + // TODO(ivanpauno): Using assertj may help a lot here https://assertj.github.io/doc/. + assertEquals(local.size(), 2); + assertTrue( + "topic 'test_get_publisher_names_and_types_one' was not discovered for local node", + local.contains( + new NameAndTypes( + "/test_get_publisher_names_and_types_one", + new ArrayList(Arrays.asList("rcljava/msg/UInt32"))))); + assertTrue( + "topic 'test_get_publisher_names_and_types_two' was not discovered for local node", + local.contains( + new NameAndTypes( + "/test_get_publisher_names_and_types_two", + new ArrayList(Arrays.asList("rcljava/msg/UInt32"))))); + + assertEquals(remote.size(), 2); + assertTrue( + "topic 'test_get_publisher_names_and_types_two' was not discovered for remote node", + remote.contains( + new NameAndTypes( + "/test_get_publisher_names_and_types_two", + new ArrayList(Arrays.asList("rcljava/msg/UInt32"))))); + assertTrue( + "topic 'test_get_publisher_names_and_types_three' was not discovered for remote node", + remote.contains( + new NameAndTypes( + "/test_get_publisher_names_and_types_three", + new ArrayList(Arrays.asList("rcljava/msg/UInt32"))))); + } + }; + + long start = System.currentTimeMillis(); + boolean ok = false; + Collection local = null; + Collection remote = null; + do { + local = this.node.getPublisherNamesAndTypesByNode("test_node", "/"); + remote = this.node.getPublisherNamesAndTypesByNode( + "test_get_publisher_names_and_types_remote_node", "/"); + try { + validateNameAndTypes.accept(local, remote); + 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(local); + assertNotNull(remote); + validateNameAndTypes.accept(local, remote); + + publisher1.dispose(); + publisher2.dispose(); + publisher3.dispose(); + publisher4.dispose(); + subscription.dispose(); + remoteNode.dispose(); + } } From 54ebfb33d6e0acf4ab6eff244d20a2b32f55e4a6 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 24 Aug 2020 19:50:36 -0400 Subject: [PATCH 12/51] Revamp Time (osrf/ros2_java#6) * Start revamping Time and Clock classes. Signed-off-by: Chris Lalancette * Fixes from review. Signed-off-by: Chris Lalancette * Add in logging to the TimeSource class. Signed-off-by: Chris Lalancette --- rcljava/CMakeLists.txt | 15 ++ rcljava/include/org_ros2_rcljava_time_Clock.h | 34 ++++ rcljava/package.xml | 3 + .../main/cpp/org_ros2_rcljava_time_Clock.cpp | 85 ++++++++- .../src/main/java/org/ros2/rcljava/Time.java | 61 +++--- .../java/org/ros2/rcljava/node/NodeImpl.java | 7 +- .../java/org/ros2/rcljava/time/Clock.java | 54 +++++- .../org/ros2/rcljava/time/TimeSource.java | 173 ++++++++++++++++++ .../test/java/org/ros2/rcljava/TimeTest.java | 64 +++---- .../ros2/rcljava/node/NodeParametersTest.java | 3 +- 10 files changed, 410 insertions(+), 89 deletions(-) create mode 100644 rcljava/src/main/java/org/ros2/rcljava/time/TimeSource.java diff --git a/rcljava/CMakeLists.txt b/rcljava/CMakeLists.txt index e61a8a16..6c3a7e29 100644 --- a/rcljava/CMakeLists.txt +++ b/rcljava/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(rcl_interfaces REQUIRED) find_package(rcljava_common REQUIRED) find_package(rmw REQUIRED) find_package(rmw_implementation_cmake REQUIRED) +find_package(rosgraph_msgs REQUIRED) include(CrossCompilingExtra) @@ -108,6 +109,7 @@ foreach(_jni_source ${${PROJECT_NAME}_jni_sources}) "rcljava_common" "builtin_interfaces" "rcl_interfaces" + "rosgraph_msgs" ) target_include_directories(${_target_name} @@ -191,6 +193,7 @@ set(${PROJECT_NAME}_sources "src/main/java/org/ros2/rcljava/subscription/statuses/RequestedQosIncompatible.java" "src/main/java/org/ros2/rcljava/time/Clock.java" "src/main/java/org/ros2/rcljava/time/ClockType.java" + "src/main/java/org/ros2/rcljava/time/TimeSource.java" "src/main/java/org/ros2/rcljava/timer/Timer.java" "src/main/java/org/ros2/rcljava/timer/WallTimer.java" "src/main/java/org/ros2/rcljava/timer/WallTimerImpl.java" @@ -204,6 +207,7 @@ add_jar("${PROJECT_NAME}_jar" ${rcljava_common_JARS} ${builtin_interfaces_JARS} ${rcl_interfaces_JARS} + ${rosgraph_msgs_JARS} ) install_jar("${PROJECT_NAME}_jar" "share/${PROJECT_NAME}/java") @@ -241,6 +245,7 @@ if(BUILD_TESTING) DEPENDENCIES builtin_interfaces rcl_interfaces + rosgraph_msgs ${_java_type_supports} SKIP_INSTALL ) @@ -323,6 +328,15 @@ if(BUILD_TESTING) list_append_unique(_deps_library_dirs ${_dep_dir}) endforeach() + foreach(_dep_lib ${rosgraph_msgs_interfaces_LIBRARIES}) + get_filename_component(_dep_dir "${_dep_lib}" DIRECTORY) + list_append_unique(_deps_library_dirs ${_dep_dir}) + endforeach() + foreach(_dep_lib ${rosgraph_msgs_JNI_LIBRARIES}) + get_filename_component(_dep_dir "${_dep_lib}" DIRECTORY) + list_append_unique(_deps_library_dirs ${_dep_dir}) + endforeach() + list_append_unique(_deps_library_dirs ${CMAKE_CURRENT_BINARY_DIR}) list_append_unique(_deps_library_dirs ${CMAKE_CURRENT_BINARY_DIR}/rcljava) list_append_unique(_deps_library_dirs ${CMAKE_CURRENT_BINARY_DIR}/rosidl_generator_java/rcljava/msg/) @@ -343,6 +357,7 @@ if(BUILD_TESTING) "${std_msgs_JARS}" "${builtin_interfaces_JARS}" "${rcl_interfaces_JARS}" + "${rosgraph_msgs_JARS}" "${_${PROJECT_NAME}_jar_file}" "${_${PROJECT_NAME}_messages_jar_file}" APPEND_LIBRARY_DIRS diff --git a/rcljava/include/org_ros2_rcljava_time_Clock.h b/rcljava/include/org_ros2_rcljava_time_Clock.h index 430ca4b0..e903f2e1 100644 --- a/rcljava/include/org_ros2_rcljava_time_Clock.h +++ b/rcljava/include/org_ros2_rcljava_time_Clock.h @@ -28,6 +28,40 @@ extern "C" { JNIEXPORT jlong JNICALL Java_org_ros2_rcljava_time_Clock_nativeCreateClockHandle(JNIEnv *, jclass, jobject); +/* + * Class: org_ros2_rcljava_time_Clock + * Method: nativeGetNow + * Signature: (J)J + */ +JNIEXPORT jlong +JNICALL Java_org_ros2_rcljava_time_Clock_nativeGetNow(JNIEnv * env, jclass, jlong); + +/* + * Class: org_ros2_rcljava_time_Clock + * Method: nativeRosTimeOverrideEnabled + * Signature: (J)Z + */ +JNIEXPORT jboolean +JNICALL Java_org_ros2_rcljava_time_Clock_nativeRosTimeOverrideEnabled(JNIEnv * env, jclass, jlong); + +/* + * Class: org_ros2_rcljava_time_Clock + * Method: nativeSetRosTimeOverrideEnabled + * Signature: (JZ)V + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_time_Clock_nativeSetRosTimeOverrideEnabled( + JNIEnv * env, jclass, jlong, jboolean); + +/* + * Class: org_ros2_rcljava_time_Clock + * Method: nativeSetRosTimeOverride + * Signature: (JJ)V + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_time_Clock_nativeSetRosTimeOverride( + JNIEnv * env, jclass, jlong, jlong); + /* * Class: org_ros2_rcljava_time_Clock * Method: nativeDispose diff --git a/rcljava/package.xml b/rcljava/package.xml index 0ca75741..a6be77ba 100644 --- a/rcljava/package.xml +++ b/rcljava/package.xml @@ -18,11 +18,13 @@ rcpputils rmw_implementation_cmake rmw + rosgraph_msgs rosidl_generator_c rosidl_typesupport_c builtin_interfaces rcl_interfaces rmw + rosgraph_msgs rosidl_runtime_c rosidl_generator_java rosidl_typesupport_c @@ -33,6 +35,7 @@ rcpputils rmw_implementation_cmake rmw_implementation + rosgraph_msgs rosidl_runtime_c rosidl_parser diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_time_Clock.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_time_Clock.cpp index 3a4638b2..8e954c22 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_time_Clock.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_time_Clock.cpp @@ -62,8 +62,89 @@ Java_org_ros2_rcljava_time_Clock_nativeCreateClockHandle(JNIEnv * env, jclass, j return 0; } - jlong clock_handle = reinterpret_cast(clock); - return clock_handle; + return reinterpret_cast(clock); +} + +JNIEXPORT jlong JNICALL +Java_org_ros2_rcljava_time_Clock_nativeGetNow(JNIEnv * env, jclass, jlong clock_handle) +{ + rcl_clock_t * clock = reinterpret_cast(clock_handle); + rcl_time_point_value_t nanoseconds; + rcl_ret_t ret = rcl_clock_get_now(clock, &nanoseconds); + if (ret != RCL_RET_OK) { + std::string msg = "Failed to get time: " + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + return 0; + } + + return static_cast(nanoseconds); +} + +JNIEXPORT jboolean JNICALL +Java_org_ros2_rcljava_time_Clock_nativeRosTimeOverrideEnabled( + JNIEnv * env, jclass, jlong clock_handle) +{ + rcl_clock_t * clock = reinterpret_cast(clock_handle); + + if (!rcl_clock_valid(clock)) { + return false; + } + + bool is_enabled = false; + rcl_ret_t ret = rcl_is_enabled_ros_time_override(clock, &is_enabled); + if (ret != RCL_RET_OK) { + std::string msg = "Failed to check ros_time_override_status: " + + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + return false; + } + + return is_enabled; +} + +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_time_Clock_nativeSetRosTimeOverrideEnabled( + JNIEnv * env, jclass, jlong clock_handle, jboolean enabled) +{ + rcl_clock_t * clock = reinterpret_cast(clock_handle); + + if (!rcl_clock_valid(clock)) { + return; + } + + rcl_ret_t ret; + if (enabled) { + ret = rcl_enable_ros_time_override(clock); + } else { + ret = rcl_disable_ros_time_override(clock); + } + if (ret != RCL_RET_OK) { + std::string msg = "Failed to set ROS time override enable for clock: " + + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + } +} + +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_time_Clock_nativeSetRosTimeOverride( + JNIEnv * env, jclass, jlong clock_handle, jlong nanos) +{ + rcl_clock_t * clock = reinterpret_cast(clock_handle); + + if (!rcl_clock_valid(clock)) { + return; + } + + rcl_ret_t ret = rcl_set_ros_time_override(clock, static_cast(nanos)); + if (ret != RCL_RET_OK) { + std::string msg = "Failed to set ROS time override for clock: " + + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + } } JNIEXPORT void JNICALL diff --git a/rcljava/src/main/java/org/ros2/rcljava/Time.java b/rcljava/src/main/java/org/ros2/rcljava/Time.java index 4bca10af..f5b864e0 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/Time.java +++ b/rcljava/src/main/java/org/ros2/rcljava/Time.java @@ -35,56 +35,37 @@ public final class Time { } } - /** - * Private constructor so this cannot be instantiated. - */ - private Time() {} + private final ClockType clockType; - public static builtin_interfaces.msg.Time now() { - return Time.now(ClockType.SYSTEM_TIME); - } - - public static builtin_interfaces.msg.Time now(ClockType clockType) { - long rclTime = 0; - - switch (clockType) { - case ROS_TIME: - throw new UnsupportedOperationException("ROS Time is currently not implemented"); - case SYSTEM_TIME: - rclTime = rclSystemTimeNow(); - break; - case STEADY_TIME: - rclTime = rclSteadyTimeNow(); - break; - } + private final long nanoseconds; - builtin_interfaces.msg.Time msgTime = new builtin_interfaces.msg.Time(); - msgTime.setSec((int) TimeUnit.SECONDS.convert(rclTime, TimeUnit.NANOSECONDS)); - msgTime.setNanosec((int) rclTime % (1000 * 1000 * 1000)); - return msgTime; + public Time() { + this(0, 0, ClockType.SYSTEM_TIME); } - public static long toNanoseconds(builtin_interfaces.msg.Time msgTime) { - return TimeUnit.NANOSECONDS.convert(msgTime.getSec(), TimeUnit.SECONDS) - + (msgTime.getNanosec() & 0x00000000ffffffffL); + public Time(final long nanos, final ClockType ct) { + this(0, nanos, ct); } - public static long difference( - builtin_interfaces.msg.Time msgTime1, builtin_interfaces.msg.Time msgTime2) { - long difference = - (Time.toNanoseconds(msgTime1) - Time.toNanoseconds(msgTime2)) & 0x00000000ffffffffL; - return difference; + public Time(final builtin_interfaces.msg.Time time_msg, final ClockType ct) { + this(time_msg.getSec(), time_msg.getNanosec(), ct); } - private static long rclSystemTimeNow() { - return nativeRCLSystemTimeNow(); // new RuntimeException("Could not get current time"); + public Time(final long secs, final long nanos, final ClockType ct) { + if (secs < 0 || nanos < 0) { + // TODO(clalancette): Make this a custom exception + throw new IllegalArgumentException("seconds and nanoseconds must not be negative"); + } + this.clockType = ct; + this.nanoseconds = TimeUnit.SECONDS.toNanos(secs) + nanos; } - private static long rclSteadyTimeNow() { - return nativeRCLSteadyTimeNow(); // new RuntimeException("Could not get current time"); - } - private static native long nativeRCLSystemTimeNow(); + public long nanoseconds() { + return nanoseconds; + } - private static native long nativeRCLSteadyTimeNow(); + public ClockType clockType() { + return clockType; + } } 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 5d240728..7d195264 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java @@ -46,6 +46,7 @@ import org.ros2.rcljava.subscription.SubscriptionImpl; import org.ros2.rcljava.time.Clock; import org.ros2.rcljava.time.ClockType; +import org.ros2.rcljava.time.TimeSource; import org.ros2.rcljava.timer.Timer; import org.ros2.rcljava.timer.WallTimer; import org.ros2.rcljava.timer.WallTimerImpl; @@ -98,6 +99,8 @@ public class NodeImpl implements Node { */ private Clock clock; + private TimeSource timeSource; + /** * All the @{link Subscription}s that have been created through this instance. */ @@ -143,7 +146,6 @@ class ParameterAndDescriptor { * be zero. */ public NodeImpl(final long handle, final Context context, final boolean allowUndeclaredParameters) { - this.clock = new Clock(ClockType.SYSTEM_TIME); this.handle = handle; this.context = context; this.publishers = new LinkedBlockingQueue(); @@ -156,6 +158,9 @@ public NodeImpl(final long handle, final Context context, final boolean allowUnd this.allowUndeclaredParameters = allowUndeclaredParameters; this.parameterCallbacksMutex = new Object(); this.parameterCallbacks = new ArrayList(); + this.clock = new Clock(ClockType.ROS_TIME); + this.timeSource = new TimeSource(this); + this.timeSource.attachClock(this.clock); } /** diff --git a/rcljava/src/main/java/org/ros2/rcljava/time/Clock.java b/rcljava/src/main/java/org/ros2/rcljava/time/Clock.java index 69936c33..f4428121 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/time/Clock.java +++ b/rcljava/src/main/java/org/ros2/rcljava/time/Clock.java @@ -15,13 +15,14 @@ package org.ros2.rcljava.time; -import org.ros2.rcljava.common.JNIUtils; - -import org.ros2.rcljava.interfaces.Disposable; import org.slf4j.Logger; import org.slf4j.LoggerFactory; +import org.ros2.rcljava.common.JNIUtils; +import org.ros2.rcljava.interfaces.Disposable; +import org.ros2.rcljava.Time; + public class Clock implements Disposable { private static final Logger logger = LoggerFactory.getLogger(Clock.class); @@ -44,6 +45,8 @@ public class Clock implements Disposable { */ private final ClockType clockType; + private Object clockHandleMutex; + /** * Create an RCL clock (rcl_clock_t). * @@ -52,6 +55,10 @@ public class Clock implements Disposable { */ private static native long nativeCreateClockHandle(ClockType clockType); + public Clock() { + this(ClockType.SYSTEM_TIME); + } + /** * Constructor. * @@ -60,6 +67,31 @@ public class Clock implements Disposable { public Clock(ClockType clockType) { this.clockType = clockType; this.handle = Clock.nativeCreateClockHandle(clockType); + this.clockHandleMutex = new Object(); + } + + public Time now() { + synchronized(this.clockHandleMutex) { + return new Time(0, nativeGetNow(this.handle), this.clockType); + } + } + + public boolean getRosTimeIsActive() { + synchronized(this.clockHandleMutex) { + return nativeRosTimeOverrideEnabled(this.handle); + } + } + + public void setRosTimeIsActive(boolean enabled) { + synchronized(this.clockHandleMutex) { + nativeSetRosTimeOverrideEnabled(this.handle, enabled); + } + } + + public void setRosTimeOverride(Time time) { + synchronized(this.clockHandleMutex) { + nativeSetRosTimeOverride(this.handle, time.nanoseconds()); + } } /** @@ -69,6 +101,18 @@ public ClockType getClockType() { return clockType; } + // TODO(clalancette): The rclcpp and rclpy implementations of the Clock class + // both have the ability to register "time jump" callbacks with rcl. That is + // only used with tf2, so we don't need it in rcljava until we have tf2 support. + + private static native long nativeGetNow(long handle); + + private static native boolean nativeRosTimeOverrideEnabled(long handle); + + private static native void nativeSetRosTimeOverrideEnabled(long handle, boolean enabled); + + private static native void nativeSetRosTimeOverride(long handle, long nanos); + /** * Destroy an RCL clock (rcl_clock_t). * @@ -80,7 +124,9 @@ public ClockType getClockType() { * {@inheritDoc} */ public final void dispose() { - Clock.nativeDispose(this.handle); + synchronized(this.clockHandleMutex) { + nativeDispose(this.handle); + } this.handle = 0; } diff --git a/rcljava/src/main/java/org/ros2/rcljava/time/TimeSource.java b/rcljava/src/main/java/org/ros2/rcljava/time/TimeSource.java new file mode 100644 index 00000000..44318c2d --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/time/TimeSource.java @@ -0,0 +1,173 @@ +/* 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.time; + +import org.ros2.rcljava.consumers.Consumer; +import org.ros2.rcljava.node.Node; +import org.ros2.rcljava.parameters.ParameterCallback; +import org.ros2.rcljava.parameters.ParameterType; +import org.ros2.rcljava.parameters.ParameterVariant; +import org.ros2.rcljava.subscription.Subscription; +import org.ros2.rcljava.time.Clock; +import org.ros2.rcljava.time.ClockType; +import org.ros2.rcljava.Time; + +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.util.Collection; +import java.util.List; + +import java.util.concurrent.LinkedBlockingQueue; + +public final class TimeSource { + private Node node; + private boolean rosTimeIsActive; + private final Collection associatedClocks; + private Subscription clockSub; + private Time lastTimeSet; + private ParameterCallback simTimeCB; + + private static final Logger logger = LoggerFactory.getLogger(TimeSource.class); + + public TimeSource() { + this(null); + } + + public TimeSource(Node node) { + this.rosTimeIsActive = false; + this.associatedClocks = new LinkedBlockingQueue(); + this.clockSub = null; + this.lastTimeSet = new Time(0, 0, ClockType.ROS_TIME); + if (node != null) { + this.attachNode(node); + } + } + + public boolean getRosTimeIsActive() { + return this.rosTimeIsActive; + } + + class SubscriptionCallback implements Consumer { + private TimeSource timeSource; + + SubscriptionCallback(TimeSource ts) { + this.timeSource = ts; + } + + public void accept(final rosgraph_msgs.msg.Clock msg) { + Time timeFromMsg = new Time(msg.getClock(), ClockType.ROS_TIME); + this.timeSource.lastTimeSet = timeFromMsg; + for (Clock clock : this.timeSource.associatedClocks) { + clock.setRosTimeOverride(timeFromMsg); + } + } + } + + public void setRosTimeIsActive(boolean enabled) { + if (this.rosTimeIsActive == enabled) { + return; + } + this.rosTimeIsActive = enabled; + for (Clock clock : this.associatedClocks) { + clock.setRosTimeIsActive(enabled); + } + if (enabled) { + if (this.node != null) { + this.clockSub = node.createSubscription(rosgraph_msgs.msg.Clock.class, "/clock", new SubscriptionCallback(this)); + } + } else { + if (this.node != null && this.clockSub != null) { + this.node.removeSubscription(this.clockSub); + this.clockSub = null; + } + } + } + + public void attachNode(Node node) { + if (this.node != null) { + detachNode(); + } + + this.node = node; + + if (!this.node.hasParameter("use_sim_time")) { + this.node.declareParameter(new ParameterVariant("use_sim_time", false)); + } + + ParameterVariant useSimTime = this.node.getParameter("use_sim_time"); + ParameterType useSimTimeType = useSimTime.getType(); + if (useSimTimeType != ParameterType.PARAMETER_NOT_SET) { + if (useSimTimeType == ParameterType.PARAMETER_BOOL) { + this.rosTimeIsActive = useSimTime.asBool(); + } else { + logger.warn("The 'use_sim_time' parameter must be a boolean"); + } + } + + class SimTimeCB implements ParameterCallback { + private TimeSource timeSource; + + public SimTimeCB(TimeSource ts) { + this.timeSource = ts; + } + + public rcl_interfaces.msg.SetParametersResult callback(List parameters) { + rcl_interfaces.msg.SetParametersResult result = new rcl_interfaces.msg.SetParametersResult(); + result.setSuccessful(true); + for (ParameterVariant param : parameters) { + if (param.getName() == "use_sim_time") { + if (param.getType() == ParameterType.PARAMETER_BOOL) { + this.timeSource.rosTimeIsActive = param.asBool(); + } else { + result.setSuccessful(false); + result.setReason("'use_sim_time' parameter must be a boolean"); + break; + } + } + } + return result; + } + } + + this.simTimeCB = new SimTimeCB(this); + + this.node.addOnSetParametersCallback(this.simTimeCB); + } + + public void detachNode() { + if (this.node == null) { + return; + } + this.setRosTimeIsActive(false); + this.node.removeOnSetParametersCallback(this.simTimeCB); + this.node = null; + } + + public void attachClock(Clock clock) { + if (clock.getClockType() != ClockType.ROS_TIME) { + // TODO(clalancette): Make this a custom exception + throw new IllegalArgumentException("Cannot attach a clock that is not ROS_TIME to a timesource"); + } + clock.setRosTimeOverride(this.lastTimeSet); + clock.setRosTimeIsActive(this.rosTimeIsActive); + this.associatedClocks.add(clock); + } + + public void detachClock(Clock clock) { + this.associatedClocks.remove(clock); + } +} diff --git a/rcljava/src/test/java/org/ros2/rcljava/TimeTest.java b/rcljava/src/test/java/org/ros2/rcljava/TimeTest.java index 9bd0e01c..a8d6b298 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/TimeTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/TimeTest.java @@ -16,10 +16,8 @@ package org.ros2.rcljava; import static org.junit.Assert.assertEquals; -import static org.junit.Assert.fail; import java.lang.reflect.Method; -import java.util.concurrent.TimeUnit; import org.junit.BeforeClass; import org.junit.Test; @@ -45,50 +43,34 @@ public static void setupOnce() throws Exception { } } - // @Test - public final void testSystemTime() { - builtin_interfaces.msg.Time now = Time.now(ClockType.SYSTEM_TIME); - long javaNowMillis = System.currentTimeMillis(); - - long nowNanos = Time.toNanoseconds(now); - long javaNowNanos = TimeUnit.NANOSECONDS.convert(javaNowMillis, TimeUnit.MILLISECONDS); - - long tolerance = TimeUnit.NANOSECONDS.convert(1000, TimeUnit.MILLISECONDS); - assertEquals(javaNowNanos, nowNanos, tolerance); + @Test + public final void testTimeNoArgConstructor() { + Time time = new Time(); + assertEquals(0, time.nanoseconds()); + assertEquals(ClockType.SYSTEM_TIME, time.clockType()); } - // @Test - public final void testSteadyTime() { - int millisecondsToSleep = 100; - builtin_interfaces.msg.Time now = Time.now(ClockType.STEADY_TIME); - long javaNowNanos = System.nanoTime(); - - try { - Thread.sleep(millisecondsToSleep); - } catch (InterruptedException iex) { - fail("Failed to sleep for " + millisecondsToSleep + " milliseconds"); - } - - builtin_interfaces.msg.Time later = Time.now(ClockType.STEADY_TIME); - - long javaLaterNanos = System.nanoTime(); - - long tolerance = TimeUnit.NANOSECONDS.convert(1, TimeUnit.MILLISECONDS); - - long javaDifference = javaLaterNanos - javaNowNanos; - long difference = Time.difference(later, now); - assertEquals(javaDifference, difference, tolerance); + @Test + public final void testTimeNanos() { + Time time = new Time(45, ClockType.SYSTEM_TIME); + assertEquals(45, time.nanoseconds()); + assertEquals(ClockType.SYSTEM_TIME, time.clockType()); } @Test - public final void testROSTime() { - builtin_interfaces.msg.Time startTime; - try { - startTime = Time.now(ClockType.ROS_TIME); - } catch (UnsupportedOperationException uoe) { - return; - } + public final void testTimeAllArgs() { + Time time = new Time(0, 45, ClockType.SYSTEM_TIME); + assertEquals(45, time.nanoseconds()); + assertEquals(ClockType.SYSTEM_TIME, time.clockType()); + } + + @Test(expected = IllegalArgumentException.class) + public final void testTimeBadSecs() { + Time time = new Time(-1, 0, ClockType.SYSTEM_TIME); + } - fail("ROS time is not supported yet"); + @Test(expected = IllegalArgumentException.class) + public final void testTimeBadNanos() { + Time time = new Time(0, -45, ClockType.SYSTEM_TIME); } } diff --git a/rcljava/src/test/java/org/ros2/rcljava/node/NodeParametersTest.java b/rcljava/src/test/java/org/ros2/rcljava/node/NodeParametersTest.java index 72af9f8d..ccf1b697 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/node/NodeParametersTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/node/NodeParametersTest.java @@ -349,7 +349,8 @@ public final void testListParameters() { node.declareParameter(new ParameterVariant("thing.do", true)); rcl_interfaces.msg.ListParametersResult result = node.listParameters(new ArrayList(), rcl_interfaces.srv.ListParameters_Request.DEPTH_RECURSIVE); - assertEquals(2, result.getNames().size()); + // 2 we added above plus the implicit "use_sim_time" + assertEquals(3, result.getNames().size()); } @Test From 69524944b97509e072658e576b07010ef7c0a110 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Tue, 25 Aug 2020 09:56:59 -0700 Subject: [PATCH 13/51] Document TimeSource class (osrf/ros2_java#21) * Document TimeSource class Signed-off-by: Jacob Perron * Address feedback * Empty -> Default * Add more detail to class docblock Signed-off-by: Jacob Perron --- .../org/ros2/rcljava/time/TimeSource.java | 63 +++++++++++++++++++ 1 file changed, 63 insertions(+) diff --git a/rcljava/src/main/java/org/ros2/rcljava/time/TimeSource.java b/rcljava/src/main/java/org/ros2/rcljava/time/TimeSource.java index 44318c2d..27e4bbc2 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/time/TimeSource.java +++ b/rcljava/src/main/java/org/ros2/rcljava/time/TimeSource.java @@ -33,6 +33,21 @@ import java.util.concurrent.LinkedBlockingQueue; +/** + * Implementation of the ROS time source abstraction. + * + * This class updates the time of one or more @{link Clock}s with a ROS time source. + * A ROS time source is expected to be published to the "/clock" topic. + * A TimeSource object needs a @{link Node} in order create a subscription to the "/clock" topic. + * + * There are two ways to enable (or disable) using a ROS time source: + * + * 1. Calling the method @{link #setRosTimeIsActive(boolean)}, or + * 2. Setting the 'use_sim_time' parameter on the @{link Node} that is attached to this TimeSource + * + * More information can be found in the design document + * Clock and Time. + */ public final class TimeSource { private Node node; private boolean rosTimeIsActive; @@ -43,10 +58,18 @@ public final class TimeSource { private static final Logger logger = LoggerFactory.getLogger(TimeSource.class); + /** + * Default constructor. + */ public TimeSource() { this(null); } + /** + * Attach a node to the time source. + * + * @param node The node to attach to the time source. + */ public TimeSource(Node node) { this.rosTimeIsActive = false; this.associatedClocks = new LinkedBlockingQueue(); @@ -57,6 +80,10 @@ public TimeSource(Node node) { } } + /** + * @return true if ROS time is active, false otherwise. + * Being active means we are listening to the "/clock" topic. + */ public boolean getRosTimeIsActive() { return this.rosTimeIsActive; } @@ -77,6 +104,11 @@ public void accept(final rosgraph_msgs.msg.Clock msg) { } } + /** + * Enable or disable ROS time. + * + * @param enabled Flag to enable or disable ROS time. + */ public void setRosTimeIsActive(boolean enabled) { if (this.rosTimeIsActive == enabled) { return; @@ -97,6 +129,14 @@ public void setRosTimeIsActive(boolean enabled) { } } + /** + * Attach a @{link Node} to the time source. + * + * This node is used to create a subscription to the "/clock" topic. + * Any previously attached @{link Node} is detached. + * + * @param node The node to attach to the time source. + */ public void attachNode(Node node) { if (this.node != null) { detachNode(); @@ -148,6 +188,13 @@ public rcl_interfaces.msg.SetParametersResult callback(List pa this.node.addOnSetParametersCallback(this.simTimeCB); } + /** + * Detach a @{link Node} from the time source. + * + * Unsubscribes from the "/clock" topic, effectively disabling ROS time. + * + * If no Node is attached, nothing happens. + */ public void detachNode() { if (this.node == null) { return; @@ -157,6 +204,15 @@ public void detachNode() { this.node = null; } + /** + * Attach a @{link Clock} to the time source. + * + * More than one clock may be attached to a time source. + * Attached clocks will be updated to the the time received on the "/clock" topic. + * + * @param clock The Clock to attach. + * Must be of type @{link ClockType.ROS_TIME}, otherwise an exception is thrown. + */ public void attachClock(Clock clock) { if (clock.getClockType() != ClockType.ROS_TIME) { // TODO(clalancette): Make this a custom exception @@ -167,6 +223,13 @@ public void attachClock(Clock clock) { this.associatedClocks.add(clock); } + /** + * Detach a @{link Clock} from the time source. + * + * The Clock will no longer receive time updates from this time source. + * + * @param clock The Clock to detach. + */ public void detachClock(Clock clock) { this.associatedClocks.remove(clock); } From e8837c8a31f4f591b3706c299263b94cc8c37de2 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Thu, 17 Sep 2020 11:10:36 -0700 Subject: [PATCH 14/51] Add tests for TimeSource (osrf/ros2_java#22) * Notify clocks when use_sim_time parameter changes Signed-off-by: Jacob Perron * Add unit tests for TimeSource class Introduce Mockito dependency to help with creating mock objects. Signed-off-by: Jacob Perron * Add test dependency on libmockito-java Depends on https://github.com/ros/rosdistro/pull/26308 Signed-off-by: Jacob Perron * Minor refactor - Switch to using @Mock annotation. - Cleanup Signed-off-by: Jacob Perron * Depend on mockito_vendor package Signed-off-by: Jacob Perron * Add mockito_vendor to repos file This should make CI green. Signed-off-by: Jacob Perron * Update branch for mockito_vendor Signed-off-by: Jacob Perron --- rcljava/CMakeLists.txt | 4 + rcljava/package.xml | 1 + .../org/ros2/rcljava/time/TimeSource.java | 2 +- .../org/ros2/rcljava/time/TimeSourceTest.java | 183 ++++++++++++++++++ ros2_java_desktop.repos | 4 + 5 files changed, 193 insertions(+), 1 deletion(-) create mode 100644 rcljava/src/test/java/org/ros2/rcljava/time/TimeSourceTest.java diff --git a/rcljava/CMakeLists.txt b/rcljava/CMakeLists.txt index 6c3a7e29..ec567872 100644 --- a/rcljava/CMakeLists.txt +++ b/rcljava/CMakeLists.txt @@ -216,6 +216,7 @@ ament_export_jars("share/${PROJECT_NAME}/java/${PROJECT_NAME}.jar") if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) find_package(std_msgs REQUIRED) + find_package(mockito_vendor REQUIRED) ament_lint_auto_find_test_dependencies() set(${PROJECT_NAME}_message_files @@ -272,6 +273,7 @@ if(BUILD_TESTING) "src/test/java/org/ros2/rcljava/publisher/PublisherTest.java" "src/test/java/org/ros2/rcljava/qos/QoSProfileTest.java" "src/test/java/org/ros2/rcljava/subscription/SubscriptionTest.java" + "src/test/java/org/ros2/rcljava/time/TimeSourceTest.java" "src/test/java/org/ros2/rcljava/timer/TimerTest.java" ) @@ -288,6 +290,7 @@ if(BUILD_TESTING) "org.ros2.rcljava.publisher.PublisherTest" "org.ros2.rcljava.qos.QoSProfileTest" "org.ros2.rcljava.subscription.SubscriptionTest" + "org.ros2.rcljava.time.TimeSourceTest" "org.ros2.rcljava.timer.TimerTest" ) @@ -358,6 +361,7 @@ if(BUILD_TESTING) "${builtin_interfaces_JARS}" "${rcl_interfaces_JARS}" "${rosgraph_msgs_JARS}" + "${mockito_vendor_JARS}" "${_${PROJECT_NAME}_jar_file}" "${_${PROJECT_NAME}_messages_jar_file}" APPEND_LIBRARY_DIRS diff --git a/rcljava/package.xml b/rcljava/package.xml index a6be77ba..eb817669 100644 --- a/rcljava/package.xml +++ b/rcljava/package.xml @@ -42,6 +42,7 @@ ament_lint_auto ament_lint_common builtin_interfaces + mockito_vendor rcl_interfaces rcljava_common rmw_implementation_cmake diff --git a/rcljava/src/main/java/org/ros2/rcljava/time/TimeSource.java b/rcljava/src/main/java/org/ros2/rcljava/time/TimeSource.java index 27e4bbc2..862cacb7 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/time/TimeSource.java +++ b/rcljava/src/main/java/org/ros2/rcljava/time/TimeSource.java @@ -171,7 +171,7 @@ public rcl_interfaces.msg.SetParametersResult callback(List pa for (ParameterVariant param : parameters) { if (param.getName() == "use_sim_time") { if (param.getType() == ParameterType.PARAMETER_BOOL) { - this.timeSource.rosTimeIsActive = param.asBool(); + this.timeSource.setRosTimeIsActive(param.asBool()); } else { result.setSuccessful(false); result.setReason("'use_sim_time' parameter must be a boolean"); diff --git a/rcljava/src/test/java/org/ros2/rcljava/time/TimeSourceTest.java b/rcljava/src/test/java/org/ros2/rcljava/time/TimeSourceTest.java new file mode 100644 index 00000000..84958b86 --- /dev/null +++ b/rcljava/src/test/java/org/ros2/rcljava/time/TimeSourceTest.java @@ -0,0 +1,183 @@ +/* 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.time; + +import static org.junit.Assert.assertTrue; +import static org.junit.Assert.assertFalse; + +import org.junit.AfterClass; +import org.junit.BeforeClass; +import org.junit.Test; + +import org.junit.runner.RunWith; + +import static org.mockito.Mockito.*; +import org.mockito.Mock; +import org.mockito.junit.MockitoJUnitRunner; + +import org.ros2.rcljava.consumers.Consumer; +import org.ros2.rcljava.RCLJava; +import org.ros2.rcljava.node.Node; +import org.ros2.rcljava.parameters.ParameterVariant; +import org.ros2.rcljava.subscription.Subscription; +import org.ros2.rcljava.time.TimeSource; + +@RunWith(MockitoJUnitRunner.StrictStubs.class) +public class TimeSourceTest { + @Mock + private Node mockedNode; + + @Mock + private Clock mockedClock; + + @Mock + private Subscription mockSubscription; + + @BeforeClass + public static void setupOnce() throws Exception { + // Just to quiet down warnings + org.apache.log4j.BasicConfigurator.configure(); + + RCLJava.rclJavaInit(); + } + + @AfterClass + public static void tearDownOnce() { + RCLJava.shutdown(); + } + + @Test + public final void testEmptyConstructor() { + TimeSource timeSource = new TimeSource(); + assertFalse(timeSource.getRosTimeIsActive()); + } + + @Test + public final void testConstructorWithNode() { + when(mockedNode.getParameter("use_sim_time")).thenReturn(new ParameterVariant("use_sim_time", false)); + + TimeSource timeSource = new TimeSource(mockedNode); + assertFalse(timeSource.getRosTimeIsActive()); + } + + @Test + public final void testAttachNodeUseSimTimeFalse() { + when(mockedNode.getParameter("use_sim_time")).thenReturn(new ParameterVariant("use_sim_time", false)); + + TimeSource timeSource = new TimeSource(); + timeSource.attachNode(mockedNode); + assertFalse(timeSource.getRosTimeIsActive()); + } + + @Test + public final void testAttachNodeUseSimTimeTrue() { + when(mockedNode.getParameter("use_sim_time")).thenReturn(new ParameterVariant("use_sim_time", true)); + + TimeSource timeSource = new TimeSource(); + timeSource.attachNode(mockedNode); + assertTrue(timeSource.getRosTimeIsActive()); + } + + @Test + public final void testAttachNodeTwice() { + when(mockedNode.getParameter("use_sim_time")).thenReturn(new ParameterVariant("use_sim_time", true)); + + TimeSource timeSource = new TimeSource(); + timeSource.attachNode(mockedNode); + assertTrue(timeSource.getRosTimeIsActive()); + + // Attach the same node again + timeSource.attachNode(mockedNode); + assertTrue(timeSource.getRosTimeIsActive()); + } + + @Test + public final void testDetachNode() { + when(mockedNode.getParameter("use_sim_time")).thenReturn(new ParameterVariant("use_sim_time", true)); + + // Attaches node with ROS time active + TimeSource timeSource = new TimeSource(mockedNode); + assertTrue(timeSource.getRosTimeIsActive()); + + timeSource.detachNode(); + assertFalse(timeSource.getRosTimeIsActive()); + + // Calling detach again shouldn't change anything + timeSource.detachNode(); + assertFalse(timeSource.getRosTimeIsActive()); + } + + @Test + public final void testAttachClock() { + when(mockedClock.getClockType()).thenReturn(ClockType.ROS_TIME); + + TimeSource timeSource = new TimeSource(); + // Attaching a clock should notifiy the clock + timeSource.attachClock(mockedClock); + verify(mockedClock).setRosTimeIsActive(false); + + // Setting ROS time active should notify clock + timeSource.setRosTimeIsActive(true); + verify(mockedClock).setRosTimeIsActive(true); + } + + @Test(expected = IllegalArgumentException.class) + public final void testAttachClockInvalidType() { + TimeSource timeSource = new TimeSource(); + timeSource.attachClock(mockedClock); + } + + @Test + public final void testDetachClock() { + when(mockedClock.getClockType()).thenReturn(ClockType.ROS_TIME); + + TimeSource timeSource = new TimeSource(); + timeSource.attachClock(mockedClock); + timeSource.detachClock(mockedClock); + + // Setting ROS time active should not notify a detached clock + timeSource.setRosTimeIsActive(true); + verify(mockedClock, never()).setRosTimeIsActive(true); + } + + @Test + public final void testSetRosTimeIsActiveNoNode() { + TimeSource timeSource = new TimeSource(); + timeSource.setRosTimeIsActive(false); + assertFalse(timeSource.getRosTimeIsActive()); + timeSource.setRosTimeIsActive(true); + assertTrue(timeSource.getRosTimeIsActive()); + } + + @Test + public final void testSetRosTimeIsActiveWithNode() { + when(mockedNode.getParameter("use_sim_time")).thenReturn(new ParameterVariant("use_sim_time", false)); + when(mockedNode.createSubscription(eq(rosgraph_msgs.msg.Clock.class), anyString(), any(Consumer.class))) + .thenReturn(mockSubscription); + + TimeSource timeSource = new TimeSource(mockedNode); + timeSource.setRosTimeIsActive(false); + assertFalse(timeSource.getRosTimeIsActive()); + timeSource.setRosTimeIsActive(true); + assertTrue(timeSource.getRosTimeIsActive()); + // Expect subscription for the "/clock" topic when set active + verify(mockedNode).createSubscription(eq(rosgraph_msgs.msg.Clock.class), eq("/clock"), any(Consumer.class)); + timeSource.setRosTimeIsActive(false); + assertFalse(timeSource.getRosTimeIsActive()); + // Expect subscription removed when set not active + verify(mockedNode).removeSubscription(any(Subscription.class)); + } +} diff --git a/ros2_java_desktop.repos b/ros2_java_desktop.repos index 1b10ef90..9ed0e919 100644 --- a/ros2_java_desktop.repos +++ b/ros2_java_desktop.repos @@ -23,6 +23,10 @@ repositories: type: git url: https://github.com/ros2-java/ament_java.git version: main + ros2-java/mockito_vendor: + type: git + url: https://github.com/ros2-java/mockito_vendor.git + version: main ros2-java/ros2_java: type: git url: https://github.com/ros2-java/ros2_java.git From 6cfc727857b02b0667346d7df9a058de700d7098 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Fri, 18 Sep 2020 11:33:31 -0700 Subject: [PATCH 15/51] Support wall-time and ROS-time timers (osrf/ros2_java#39) * Support wall-time and ROS-time timers Add a new method to Node for creating ROS timers. Refactor timer implementation to account for the two different types of timers. Deprecate WallTimer interface and WallTimerImpl and replace with unified Timer interface and TimerImpl. Signed-off-by: Jacob Perron * Add test Signed-off-by: Jacob Perron * Log error if failed to dispose timer Signed-off-by: Jacob Perron --- rcljava/CMakeLists.txt | 3 +- .../org_ros2_rcljava_timer_TimerImpl.h | 108 +++++++++++++++ .../org_ros2_rcljava_timer_WallTimerImpl.h | 108 --------------- ...p => org_ros2_rcljava_timer_TimerImpl.cpp} | 80 +++++------ .../main/java/org/ros2/rcljava/node/Node.java | 24 +++- .../java/org/ros2/rcljava/node/NodeImpl.java | 29 +++- .../java/org/ros2/rcljava/timer/Timer.java | 14 ++ .../org/ros2/rcljava/timer/TimerImpl.java | 131 ++++++++++++++++++ .../org/ros2/rcljava/timer/WallTimer.java | 18 +-- .../org/ros2/rcljava/timer/WallTimerImpl.java | 112 ++------------- .../test/java/org/ros2/rcljava/SpinTest.java | 30 ++-- .../org/ros2/rcljava/timer/TimerTest.java | 47 +++++-- 12 files changed, 407 insertions(+), 297 deletions(-) create mode 100644 rcljava/include/org_ros2_rcljava_timer_TimerImpl.h delete mode 100644 rcljava/include/org_ros2_rcljava_timer_WallTimerImpl.h rename rcljava/src/main/cpp/{org_ros2_rcljava_timer_WallTimerImpl.cpp => org_ros2_rcljava_timer_TimerImpl.cpp} (67%) create mode 100644 rcljava/src/main/java/org/ros2/rcljava/timer/TimerImpl.java diff --git a/rcljava/CMakeLists.txt b/rcljava/CMakeLists.txt index ec567872..4165ab57 100644 --- a/rcljava/CMakeLists.txt +++ b/rcljava/CMakeLists.txt @@ -76,7 +76,7 @@ set(${PROJECT_NAME}_jni_sources "src/main/cpp/org_ros2_rcljava_subscription_statuses_RequestedDeadlineMissed.cpp" "src/main/cpp/org_ros2_rcljava_subscription_statuses_RequestedQosIncompatible.cpp" "src/main/cpp/org_ros2_rcljava_time_Clock.cpp" - "src/main/cpp/org_ros2_rcljava_timer_WallTimerImpl.cpp" + "src/main/cpp/org_ros2_rcljava_timer_TimerImpl.cpp" ) foreach(_jni_source ${${PROJECT_NAME}_jni_sources}) @@ -195,6 +195,7 @@ set(${PROJECT_NAME}_sources "src/main/java/org/ros2/rcljava/time/ClockType.java" "src/main/java/org/ros2/rcljava/time/TimeSource.java" "src/main/java/org/ros2/rcljava/timer/Timer.java" + "src/main/java/org/ros2/rcljava/timer/TimerImpl.java" "src/main/java/org/ros2/rcljava/timer/WallTimer.java" "src/main/java/org/ros2/rcljava/timer/WallTimerImpl.java" ) diff --git a/rcljava/include/org_ros2_rcljava_timer_TimerImpl.h b/rcljava/include/org_ros2_rcljava_timer_TimerImpl.h new file mode 100644 index 00000000..d02a7efa --- /dev/null +++ b/rcljava/include/org_ros2_rcljava_timer_TimerImpl.h @@ -0,0 +1,108 @@ +// Copyright 2017-2018 Esteve Fernandez +// +// 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_timer_TimerImpl */ + +#ifndef ORG_ROS2_RCLJAVA_TIMER_TIMERIMPL_H_ +#define ORG_ROS2_RCLJAVA_TIMER_TIMERIMPL_H_ +#ifdef __cplusplus +extern "C" { +#endif + +/* + * Class: org_ros2_rcljava_timer_TimerImpl + * Method: nativeDispose + * Signature: (J)V + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_timer_TimerImpl_nativeDispose(JNIEnv *, jclass, jlong); + +/* + * Class: org_ros2_rcljava_timer_TimerImpl + * Method: nativeIsReady + * Signature: (J)Z + */ +JNIEXPORT jboolean +JNICALL Java_org_ros2_rcljava_timer_TimerImpl_nativeIsReady(JNIEnv *, jclass, jlong); + +/* + * Class: org_ros2_rcljava_timer_TimerImpl + * Method: nativeIsCanceled + * Signature: (J)Z + */ +JNIEXPORT jboolean +JNICALL Java_org_ros2_rcljava_timer_TimerImpl_nativeIsCanceled(JNIEnv *, jclass, jlong); + +/* + * Class: org_ros2_rcljava_timer_TimerImpl + * Method: nativeReset + * Signature: (J)Z + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_timer_TimerImpl_nativeReset(JNIEnv *, jclass, jlong); + +/* + * Class: org_ros2_rcljava_timer_TimerImpl + * Method: nativeCancel + * Signature: (J)Z + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_timer_TimerImpl_nativeCancel(JNIEnv *, jclass, jlong); + +/* + * Class: org_ros2_rcljava_timer_TimerImpl + * Method: nativeTimeUntilNextCall + * Signature: (J)J + */ +JNIEXPORT jlong +JNICALL Java_org_ros2_rcljava_timer_TimerImpl_nativeTimeUntilNextCall(JNIEnv *, jclass, jlong); + +/* + * Class: org_ros2_rcljava_timer_TimerImpl + * Method: nativeTimeSinceLastCall + * Signature: (J)J + */ +JNIEXPORT jlong +JNICALL Java_org_ros2_rcljava_timer_TimerImpl_nativeTimeSinceLastCall(JNIEnv *, jclass, jlong); + +/* + * Class: org_ros2_rcljava_timer_TimerImpl + * Method: nativeGetTimerPeriodNS + * Signature: (J)J + */ +JNIEXPORT jlong +JNICALL Java_org_ros2_rcljava_timer_TimerImpl_nativeGetTimerPeriodNS(JNIEnv *, jclass, jlong); + +/* + * Class: org_ros2_rcljava_timer_TimerImpl + * Method: nativeSetTimerPeriodNS + * Signature: (JJ)Z + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_timer_TimerImpl_nativeSetTimerPeriodNS( + JNIEnv *, jclass, jlong, jlong); + +/* + * Class: org_ros2_rcljava_timer_TimerImpl + * Method: nativeCallTimer + * Signature: (J)Z + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_timer_TimerImpl_nativeCallTimer(JNIEnv *, jclass, jlong); + +#ifdef __cplusplus +} +#endif +#endif // ORG_ROS2_RCLJAVA_TIMER_TIMERIMPL_H_ diff --git a/rcljava/include/org_ros2_rcljava_timer_WallTimerImpl.h b/rcljava/include/org_ros2_rcljava_timer_WallTimerImpl.h deleted file mode 100644 index 88c9daeb..00000000 --- a/rcljava/include/org_ros2_rcljava_timer_WallTimerImpl.h +++ /dev/null @@ -1,108 +0,0 @@ -// Copyright 2017-2018 Esteve Fernandez -// -// 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_timer_WallTimerImpl */ - -#ifndef ORG_ROS2_RCLJAVA_TIMER_WALLTIMERIMPL_H_ -#define ORG_ROS2_RCLJAVA_TIMER_WALLTIMERIMPL_H_ -#ifdef __cplusplus -extern "C" { -#endif - -/* - * Class: org_ros2_rcljava_timer_WallTimerImpl - * Method: nativeDispose - * Signature: (J)V - */ -JNIEXPORT void -JNICALL Java_org_ros2_rcljava_timer_WallTimerImpl_nativeDispose(JNIEnv *, jclass, jlong); - -/* - * Class: org_ros2_rcljava_timer_WallTimerImpl - * Method: nativeIsReady - * Signature: (J)Z - */ -JNIEXPORT jboolean -JNICALL Java_org_ros2_rcljava_timer_WallTimerImpl_nativeIsReady(JNIEnv *, jclass, jlong); - -/* - * Class: org_ros2_rcljava_timer_WallTimerImpl - * Method: nativeIsCanceled - * Signature: (J)Z - */ -JNIEXPORT jboolean -JNICALL Java_org_ros2_rcljava_timer_WallTimerImpl_nativeIsCanceled(JNIEnv *, jclass, jlong); - -/* - * Class: org_ros2_rcljava_timer_WallTimerImpl - * Method: nativeReset - * Signature: (J)Z - */ -JNIEXPORT void -JNICALL Java_org_ros2_rcljava_timer_WallTimerImpl_nativeReset(JNIEnv *, jclass, jlong); - -/* - * Class: org_ros2_rcljava_timer_WallTimerImpl - * Method: nativeCancel - * Signature: (J)Z - */ -JNIEXPORT void -JNICALL Java_org_ros2_rcljava_timer_WallTimerImpl_nativeCancel(JNIEnv *, jclass, jlong); - -/* - * Class: org_ros2_rcljava_timer_WallTimerImpl - * Method: nativeTimeUntilNextCall - * Signature: (J)J - */ -JNIEXPORT jlong -JNICALL Java_org_ros2_rcljava_timer_WallTimerImpl_nativeTimeUntilNextCall(JNIEnv *, jclass, jlong); - -/* - * Class: org_ros2_rcljava_timer_WallTimerImpl - * Method: nativeTimeSinceLastCall - * Signature: (J)J - */ -JNIEXPORT jlong -JNICALL Java_org_ros2_rcljava_timer_WallTimerImpl_nativeTimeSinceLastCall(JNIEnv *, jclass, jlong); - -/* - * Class: org_ros2_rcljava_timer_WallTimerImpl - * Method: nativeGetTimerPeriodNS - * Signature: (J)J - */ -JNIEXPORT jlong -JNICALL Java_org_ros2_rcljava_timer_WallTimerImpl_nativeGetTimerPeriodNS(JNIEnv *, jclass, jlong); - -/* - * Class: org_ros2_rcljava_timer_WallTimerImpl - * Method: nativeSetTimerPeriodNS - * Signature: (JJ)Z - */ -JNIEXPORT void -JNICALL Java_org_ros2_rcljava_timer_WallTimerImpl_nativeSetTimerPeriodNS( - JNIEnv *, jclass, jlong, jlong); - -/* - * Class: org_ros2_rcljava_timer_WallTimerImpl - * Method: nativeCallTimer - * Signature: (J)Z - */ -JNIEXPORT void -JNICALL Java_org_ros2_rcljava_timer_WallTimerImpl_nativeCallTimer(JNIEnv *, jclass, jlong); - -#ifdef __cplusplus -} -#endif -#endif // ORG_ROS2_RCLJAVA_TIMER_WALLTIMERIMPL_H_ diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_timer_WallTimerImpl.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_timer_TimerImpl.cpp similarity index 67% rename from rcljava/src/main/cpp/org_ros2_rcljava_timer_WallTimerImpl.cpp rename to rcljava/src/main/cpp/org_ros2_rcljava_timer_TimerImpl.cpp index 8a7559af..d2c45c3a 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_timer_WallTimerImpl.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_timer_TimerImpl.cpp @@ -27,19 +27,19 @@ #include "rcljava_common/exceptions.hpp" #include "rcljava_common/signatures.hpp" -#include "org_ros2_rcljava_timer_WallTimerImpl.h" +#include "org_ros2_rcljava_timer_TimerImpl.h" using rcljava_common::exceptions::rcljava_throw_exception; using rcljava_common::signatures::convert_from_java_signature; using rcljava_common::signatures::destroy_ros_message_signature; JNIEXPORT jboolean JNICALL -Java_org_ros2_rcljava_timer_WallTimerImpl_nativeIsReady( - JNIEnv * env, jclass, jlong wall_timer_handle) +Java_org_ros2_rcljava_timer_TimerImpl_nativeIsReady( + JNIEnv * env, jclass, jlong timer_handle) { - assert(wall_timer_handle != 0); + assert(timer_handle != 0); - rcl_timer_t * timer = reinterpret_cast(wall_timer_handle); + rcl_timer_t * timer = reinterpret_cast(timer_handle); bool is_ready; rcl_ret_t ret = rcl_timer_is_ready(timer, &is_ready); @@ -54,12 +54,12 @@ Java_org_ros2_rcljava_timer_WallTimerImpl_nativeIsReady( } JNIEXPORT jboolean JNICALL -Java_org_ros2_rcljava_timer_WallTimerImpl_nativeIsCanceled( - JNIEnv * env, jclass, jlong wall_timer_handle) +Java_org_ros2_rcljava_timer_TimerImpl_nativeIsCanceled( + JNIEnv * env, jclass, jlong timer_handle) { - assert(wall_timer_handle != 0); + assert(timer_handle != 0); - rcl_timer_t * timer = reinterpret_cast(wall_timer_handle); + rcl_timer_t * timer = reinterpret_cast(timer_handle); bool is_canceled; rcl_ret_t ret = rcl_timer_is_canceled(timer, &is_canceled); @@ -74,15 +74,15 @@ Java_org_ros2_rcljava_timer_WallTimerImpl_nativeIsCanceled( } JNIEXPORT void JNICALL -Java_org_ros2_rcljava_timer_WallTimerImpl_nativeDispose( - JNIEnv * env, jclass, jlong wall_timer_handle) +Java_org_ros2_rcljava_timer_TimerImpl_nativeDispose( + JNIEnv * env, jclass, jlong timer_handle) { - if (wall_timer_handle == 0) { + if (timer_handle == 0) { // everything is ok, already destroyed return; } - rcl_timer_t * timer = reinterpret_cast(wall_timer_handle); + rcl_timer_t * timer = reinterpret_cast(timer_handle); assert(timer != NULL); @@ -96,11 +96,11 @@ Java_org_ros2_rcljava_timer_WallTimerImpl_nativeDispose( } JNIEXPORT void JNICALL -Java_org_ros2_rcljava_timer_WallTimerImpl_nativeReset(JNIEnv * env, jclass, jlong wall_timer_handle) +Java_org_ros2_rcljava_timer_TimerImpl_nativeReset(JNIEnv * env, jclass, jlong timer_handle) { - assert(wall_timer_handle != 0); + assert(timer_handle != 0); - rcl_timer_t * timer = reinterpret_cast(wall_timer_handle); + rcl_timer_t * timer = reinterpret_cast(timer_handle); assert(timer != NULL); @@ -114,12 +114,12 @@ Java_org_ros2_rcljava_timer_WallTimerImpl_nativeReset(JNIEnv * env, jclass, jlon } JNIEXPORT void JNICALL -Java_org_ros2_rcljava_timer_WallTimerImpl_nativeCancel( - JNIEnv * env, jclass, jlong wall_timer_handle) +Java_org_ros2_rcljava_timer_TimerImpl_nativeCancel( + JNIEnv * env, jclass, jlong timer_handle) { - assert(wall_timer_handle != 0); + assert(timer_handle != 0); - rcl_timer_t * timer = reinterpret_cast(wall_timer_handle); + rcl_timer_t * timer = reinterpret_cast(timer_handle); assert(timer != NULL); @@ -133,12 +133,12 @@ Java_org_ros2_rcljava_timer_WallTimerImpl_nativeCancel( } JNIEXPORT jlong JNICALL -Java_org_ros2_rcljava_timer_WallTimerImpl_nativeTimeUntilNextCall( - JNIEnv * env, jclass, jlong wall_timer_handle) +Java_org_ros2_rcljava_timer_TimerImpl_nativeTimeUntilNextCall( + JNIEnv * env, jclass, jlong timer_handle) { - assert(wall_timer_handle != 0); + assert(timer_handle != 0); - rcl_timer_t * timer = reinterpret_cast(wall_timer_handle); + rcl_timer_t * timer = reinterpret_cast(timer_handle); assert(timer != NULL); @@ -157,12 +157,12 @@ Java_org_ros2_rcljava_timer_WallTimerImpl_nativeTimeUntilNextCall( } JNIEXPORT jlong JNICALL -Java_org_ros2_rcljava_timer_WallTimerImpl_nativeTimeSinceLastCall( - JNIEnv * env, jclass, jlong wall_timer_handle) +Java_org_ros2_rcljava_timer_TimerImpl_nativeTimeSinceLastCall( + JNIEnv * env, jclass, jlong timer_handle) { - assert(wall_timer_handle != 0); + assert(timer_handle != 0); - rcl_timer_t * timer = reinterpret_cast(wall_timer_handle); + rcl_timer_t * timer = reinterpret_cast(timer_handle); assert(timer != NULL); @@ -181,12 +181,12 @@ Java_org_ros2_rcljava_timer_WallTimerImpl_nativeTimeSinceLastCall( } JNIEXPORT jlong JNICALL -Java_org_ros2_rcljava_timer_WallTimerImpl_nativeGetTimerPeriodNS( - JNIEnv * env, jclass, jlong wall_timer_handle) +Java_org_ros2_rcljava_timer_TimerImpl_nativeGetTimerPeriodNS( + JNIEnv * env, jclass, jlong timer_handle) { - assert(wall_timer_handle != 0); + assert(timer_handle != 0); - rcl_timer_t * timer = reinterpret_cast(wall_timer_handle); + rcl_timer_t * timer = reinterpret_cast(timer_handle); assert(timer != NULL); @@ -204,12 +204,12 @@ Java_org_ros2_rcljava_timer_WallTimerImpl_nativeGetTimerPeriodNS( } JNIEXPORT void JNICALL -Java_org_ros2_rcljava_timer_WallTimerImpl_nativeSetTimerPeriodNS( - JNIEnv * env, jclass, jlong wall_timer_handle, jlong timer_period) +Java_org_ros2_rcljava_timer_TimerImpl_nativeSetTimerPeriodNS( + JNIEnv * env, jclass, jlong timer_handle, jlong timer_period) { - assert(wall_timer_handle != 0); + assert(timer_handle != 0); - rcl_timer_t * timer = reinterpret_cast(wall_timer_handle); + rcl_timer_t * timer = reinterpret_cast(timer_handle); assert(timer != NULL); @@ -224,12 +224,12 @@ Java_org_ros2_rcljava_timer_WallTimerImpl_nativeSetTimerPeriodNS( } JNIEXPORT void JNICALL -Java_org_ros2_rcljava_timer_WallTimerImpl_nativeCallTimer( - JNIEnv * env, jclass, jlong wall_timer_handle) +Java_org_ros2_rcljava_timer_TimerImpl_nativeCallTimer( + JNIEnv * env, jclass, jlong timer_handle) { - assert(wall_timer_handle != 0); + assert(timer_handle != 0); - rcl_timer_t * timer = reinterpret_cast(wall_timer_handle); + rcl_timer_t * timer = reinterpret_cast(timer_handle); assert(timer != NULL); 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 59c2cf50..7cdbec41 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/Node.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/Node.java @@ -175,9 +175,31 @@ Client createClient(final Class serviceType, */ boolean removeClient(final Client client); - + /** + * Create a wall timer. + * + * A wall timer uses steady time. + * To create a timer that uses ROS time, see @{link Node#createTimer}. + * + * @param period Time between calls to the provided callback function. + * @param unit Time unit of the timer period. + * @param callback Function that is called when the timer expires. + * @return The created timer. + */ WallTimer createWallTimer(final long period, final TimeUnit unit, final Callback callback); + /** + * Create a timer. + * + * The timer will use the same time source as the node, ie. ROS time. + * + * @param period Time between calls to the provided callback function. + * @param unit Time unit of the timer period. + * @param callback Function that is called when the timer expires. + * @return The created timer. + */ + Timer createTimer(final long period, final TimeUnit unit, final Callback callback); + /** Get the name of the node. * * @return The name of the node. 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 7d195264..588c4f6b 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java @@ -48,6 +48,7 @@ import org.ros2.rcljava.time.ClockType; import org.ros2.rcljava.time.TimeSource; import org.ros2.rcljava.timer.Timer; +import org.ros2.rcljava.timer.TimerImpl; import org.ros2.rcljava.timer.WallTimer; import org.ros2.rcljava.timer.WallTimerImpl; @@ -99,6 +100,11 @@ public class NodeImpl implements Node { */ private Clock clock; + /** + * The clock used for creating wall timers. + */ + private Clock wall_clock; + private TimeSource timeSource; /** @@ -159,6 +165,7 @@ public NodeImpl(final long handle, final Context context, final boolean allowUnd this.parameterCallbacksMutex = new Object(); this.parameterCallbacks = new ArrayList(); this.clock = new Clock(ClockType.ROS_TIME); + this.wall_clock = new Clock(ClockType.STEADY_TIME); this.timeSource = new TimeSource(this); this.timeSource.attachClock(this.clock); } @@ -414,16 +421,28 @@ public final long getHandle() { private static native long nativeCreateTimerHandle(long clockHandle, long contextHandle, long timerPeriod); - public WallTimer createWallTimer( - final long period, final TimeUnit unit, final Callback callback) { + private Timer createTimer(Clock clock, final long period, final TimeUnit unit, final Callback callback) { long timerPeriodNS = TimeUnit.NANOSECONDS.convert(period, unit); - long timerHandle = nativeCreateTimerHandle(clock.getHandle(), context.getHandle(), timerPeriodNS); - WallTimer timer = - new WallTimerImpl(new WeakReference(this), timerHandle, callback, timerPeriodNS); + long timerHandle = nativeCreateTimerHandle(clock.getHandle(), this.context.getHandle(), timerPeriodNS); + WallTimer timer = new WallTimerImpl(new WeakReference(this), timerHandle, callback, timerPeriodNS); this.timers.add(timer); return timer; } + /** + * {@inheritDoc} + */ + public WallTimer createWallTimer(final long period, final TimeUnit unit, final Callback callback) { + return (WallTimer) this.createTimer(this.wall_clock, period, unit, callback); + } + + /** + * {@inheritDoc} + */ + public Timer createTimer(final long period, final TimeUnit unit, final Callback callback) { + return this.createTimer(this.clock, period, unit, callback); + } + /** * {@inheritDoc} */ diff --git a/rcljava/src/main/java/org/ros2/rcljava/timer/Timer.java b/rcljava/src/main/java/org/ros2/rcljava/timer/Timer.java index 46aa6d43..dc2b47ab 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/timer/Timer.java +++ b/rcljava/src/main/java/org/ros2/rcljava/timer/Timer.java @@ -23,4 +23,18 @@ public interface Timer extends Disposable { void executeCallback(); boolean isReady(); + + long getTimerPeriodNS(); + + void setTimerPeriodNS(long period); + + boolean isCanceled(); + + void cancel(); + + void reset(); + + long timeSinceLastCall(); + + long timeUntilNextCall(); } diff --git a/rcljava/src/main/java/org/ros2/rcljava/timer/TimerImpl.java b/rcljava/src/main/java/org/ros2/rcljava/timer/TimerImpl.java new file mode 100644 index 00000000..5f9084de --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/timer/TimerImpl.java @@ -0,0 +1,131 @@ +/* Copyright 2017-2018 Esteve Fernandez + * + * 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.timer; + +import java.lang.ref.WeakReference; + +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import org.ros2.rcljava.common.JNIUtils; +import org.ros2.rcljava.concurrent.Callback; +import org.ros2.rcljava.node.Node; + +public class TimerImpl implements Timer { + private static final Logger logger = LoggerFactory.getLogger(TimerImpl.class); + + static { + try { + JNIUtils.loadImplementation(TimerImpl.class); + } catch (UnsatisfiedLinkError ule) { + logger.error("Native code library failed to load.\n" + ule); + System.exit(1); + } + } + + private long timerPeriodNS; + + private final WeakReference nodeReference; + + private long handle; + + private final Callback callback; + + private static native boolean nativeIsReady(long handle); + + private static native boolean nativeIsCanceled(long handle); + + private static native long nativeTimeSinceLastCall(long handle); + + private static native long nativeTimeUntilNextCall(long handle); + + private static native long nativeReset(long handle); + + private static native long nativeCancel(long handle); + + private static native long nativeGetTimerPeriodNS(long handle); + + private static native void nativeSetTimerPeriodNS(long handle, long period); + + private static native long nativeCallTimer(long handle); + + public TimerImpl(final WeakReference nodeReference, final long handle, + final Callback callback, final long timerPeriodNS) { + this.nodeReference = nodeReference; + this.handle = handle; + this.callback = callback; + this.timerPeriodNS = timerPeriodNS; + } + + public long timeSinceLastCall() { + return nativeTimeSinceLastCall(this.handle); + } + + public long timeUntilNextCall() { + return nativeTimeUntilNextCall(this.handle); + } + + public void reset() { + nativeReset(this.handle); + } + + public void cancel() { + nativeCancel(this.handle); + } + + public boolean isCanceled() { + return nativeIsCanceled(this.handle); + } + + public boolean isReady() { + return nativeIsReady(this.handle); + } + + public void setTimerPeriodNS(final long timerPeriodNS) { + nativeSetTimerPeriodNS(this.handle, timerPeriodNS); + this.timerPeriodNS = timerPeriodNS; + } + + public long getTimerPeriodNS() { + long timerPeriodNS = nativeGetTimerPeriodNS(this.handle); + this.timerPeriodNS = timerPeriodNS; + return this.timerPeriodNS; + } + + public long getHandle() { + return this.handle; + } + + private static native void nativeDispose(long handle); + + public void dispose() { + Node node = this.nodeReference.get(); + if (node == null) { + logger.error("Node reference is null. Failed to dispose of Timer."); + return; + } + nativeDispose(this.handle); + this.handle = 0; + } + + public void callTimer() { + nativeCallTimer(this.handle); + } + + public void executeCallback() { + this.callback.call(); + } +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/timer/WallTimer.java b/rcljava/src/main/java/org/ros2/rcljava/timer/WallTimer.java index f21a96f4..49de0910 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/timer/WallTimer.java +++ b/rcljava/src/main/java/org/ros2/rcljava/timer/WallTimer.java @@ -17,18 +17,10 @@ import org.ros2.rcljava.concurrent.Callback; +/** + * @deprecated + * Use @{link Timer} interface instead. + */ +@Deprecated public interface WallTimer extends Timer { - long getTimerPeriodNS(); - - void setTimerPeriodNS(long period); - - boolean isCanceled(); - - void cancel(); - - void reset(); - - long timeSinceLastCall(); - - long timeUntilNextCall(); } diff --git a/rcljava/src/main/java/org/ros2/rcljava/timer/WallTimerImpl.java b/rcljava/src/main/java/org/ros2/rcljava/timer/WallTimerImpl.java index 560c4548..077c6386 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/timer/WallTimerImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/timer/WallTimerImpl.java @@ -17,113 +17,19 @@ import java.lang.ref.WeakReference; -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; - -import org.ros2.rcljava.common.JNIUtils; import org.ros2.rcljava.concurrent.Callback; import org.ros2.rcljava.node.Node; +import org.ros2.rcljava.timer.TimerImpl; +import org.ros2.rcljava.timer.WallTimer; -public class WallTimerImpl implements WallTimer { - private static final Logger logger = LoggerFactory.getLogger(WallTimerImpl.class); - - static { - try { - JNIUtils.loadImplementation(WallTimerImpl.class); - } catch (UnsatisfiedLinkError ule) { - logger.error("Native code library failed to load.\n" + ule); - System.exit(1); - } - } - - private long timerPeriodNS; - - private final WeakReference nodeReference; - - private long handle; - - private final Callback callback; - - private static native boolean nativeIsReady(long handle); - - private static native boolean nativeIsCanceled(long handle); - - private static native long nativeTimeSinceLastCall(long handle); - - private static native long nativeTimeUntilNextCall(long handle); - - private static native long nativeReset(long handle); - - private static native long nativeCancel(long handle); - - private static native long nativeGetTimerPeriodNS(long handle); - - private static native void nativeSetTimerPeriodNS(long handle, long period); - - private static native long nativeCallTimer(long handle); - +/** + * @deprecated + * Use @{link TimerImpl} instead. + */ +@Deprecated +public class WallTimerImpl extends TimerImpl implements WallTimer { public WallTimerImpl(final WeakReference nodeReference, final long handle, final Callback callback, final long timerPeriodNS) { - this.nodeReference = nodeReference; - this.handle = handle; - this.callback = callback; - this.timerPeriodNS = timerPeriodNS; - } - - public long timeSinceLastCall() { - return nativeTimeSinceLastCall(this.handle); - } - - public long timeUntilNextCall() { - return nativeTimeUntilNextCall(this.handle); - } - - public void reset() { - nativeReset(this.handle); - } - - public void cancel() { - nativeCancel(this.handle); - } - - public boolean isCanceled() { - return nativeIsCanceled(this.handle); - } - - public boolean isReady() { - return nativeIsReady(this.handle); - } - - public void setTimerPeriodNS(final long timerPeriodNS) { - nativeSetTimerPeriodNS(this.handle, timerPeriodNS); - this.timerPeriodNS = timerPeriodNS; - } - - public long getTimerPeriodNS() { - long timerPeriodNS = nativeGetTimerPeriodNS(this.handle); - this.timerPeriodNS = timerPeriodNS; - return this.timerPeriodNS; - } - - public long getHandle() { - return this.handle; - } - - private static native void nativeDispose(long handle); - - public void dispose() { - Node node = this.nodeReference.get(); - if (node != null) { - nativeDispose(this.handle); - this.handle = 0; - } - } - - public void callTimer() { - nativeCallTimer(this.handle); - } - - public void executeCallback() { - this.callback.call(); + super(nodeReference, handle, callback, timerPeriodNS); } } diff --git a/rcljava/src/test/java/org/ros2/rcljava/SpinTest.java b/rcljava/src/test/java/org/ros2/rcljava/SpinTest.java index c5b8beb3..21740e87 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/SpinTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/SpinTest.java @@ -35,7 +35,7 @@ import org.ros2.rcljava.executors.SingleThreadedExecutor; import org.ros2.rcljava.node.ComposableNode; import org.ros2.rcljava.node.Node; -import org.ros2.rcljava.timer.WallTimer; +import org.ros2.rcljava.timer.Timer; import org.ros2.rcljava.publisher.Publisher; import org.ros2.rcljava.qos.policies.Durability; import org.ros2.rcljava.qos.policies.History; @@ -97,7 +97,7 @@ public final void testSpinOnce() { Executor executor = new SingleThreadedExecutor(); final Node node = RCLJava.createNode("spin_once_node"); TimerCallback timerCallback = new TimerCallback(0); - WallTimer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); + Timer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); assertNotEquals(0, timer.getHandle()); ComposableNode composableNode = new ComposableNode() { @@ -117,7 +117,7 @@ public final void testSpinOnceTimeout() { Executor executor = new SingleThreadedExecutor(); final Node node = RCLJava.createNode("spin_once_timeout"); TimerCallback timerCallback = new TimerCallback(0); - WallTimer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); + Timer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); assertNotEquals(0, timer.getHandle()); ComposableNode composableNode = new ComposableNode() { @@ -137,7 +137,7 @@ public final void testSpinOnceTimeoutTooShort() { Executor executor = new SingleThreadedExecutor(); final Node node = RCLJava.createNode("spin_once_timeout_too_short_node"); TimerCallback timerCallback = new TimerCallback(0); - WallTimer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); + Timer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); assertNotEquals(0, timer.getHandle()); ComposableNode composableNode = new ComposableNode() { @@ -157,7 +157,7 @@ public final void testSpinSome() { Executor executor = new SingleThreadedExecutor(); final Node node = RCLJava.createNode("spin_some_node"); TimerCallback timerCallback = new TimerCallback(0); - WallTimer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); + Timer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); assertNotEquals(0, timer.getHandle()); ComposableNode composableNode = new ComposableNode() { @@ -185,7 +185,7 @@ public final void testSpinSomeMaxDuration() { Executor executor = new SingleThreadedExecutor(); final Node node = RCLJava.createNode("spin_some_max_duration_node"); TimerCallback timerCallback = new TimerCallback(0); - WallTimer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); + Timer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); assertNotEquals(0, timer.getHandle()); ComposableNode composableNode = new ComposableNode() { @@ -213,8 +213,8 @@ public final void testSpinSomeMaxDurationMultipleTimers() { Executor executor = new SingleThreadedExecutor(); final Node node = RCLJava.createNode("spin_some_max_duration_multiple_timers_node"); TimerCallback timerCallback = new TimerCallback(200); - WallTimer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); - WallTimer timer2 = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); + Timer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); + Timer timer2 = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); assertNotEquals(0, timer.getHandle()); assertNotEquals(0, timer2.getHandle()); @@ -243,8 +243,8 @@ public final void testSpinSomeMaxDurationTooShortMultipleTimers() { Executor executor = new SingleThreadedExecutor(); final Node node = RCLJava.createNode("spin_some_max_duration_too_short_node"); TimerCallback timerCallback = new TimerCallback(200); - WallTimer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); - WallTimer timer2 = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); + Timer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); + Timer timer2 = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); assertNotEquals(0, timer.getHandle()); assertNotEquals(0, timer2.getHandle()); @@ -273,7 +273,7 @@ public final void testSpinAll() { Executor executor = new SingleThreadedExecutor(); final Node node = RCLJava.createNode("spin_all_node"); TimerCallback timerCallback = new TimerCallback(0); - WallTimer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); + Timer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); assertNotEquals(0, timer.getHandle()); ComposableNode composableNode = new ComposableNode() { @@ -302,8 +302,8 @@ public final void testSpinAllMaxDurationMultipleTimers() { Executor executor = new SingleThreadedExecutor(); final Node node = RCLJava.createNode("spin_all_max_duration_multiple_timers_node"); TimerCallback timerCallback = new TimerCallback(200); - WallTimer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); - WallTimer timer2 = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); + Timer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); + Timer timer2 = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); assertNotEquals(0, timer.getHandle()); assertNotEquals(0, timer2.getHandle()); @@ -332,8 +332,8 @@ public final void testSpinAllMaxDurationTooShortMultipleTimers() { Executor executor = new SingleThreadedExecutor(); final Node node = RCLJava.createNode("spin_all_max_duration_too_short_node"); TimerCallback timerCallback = new TimerCallback(200); - WallTimer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); - WallTimer timer2 = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); + Timer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); + Timer timer2 = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); assertNotEquals(0, timer.getHandle()); assertNotEquals(0, timer2.getHandle()); diff --git a/rcljava/src/test/java/org/ros2/rcljava/timer/TimerTest.java b/rcljava/src/test/java/org/ros2/rcljava/timer/TimerTest.java index 9eff6b17..a38d2901 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/timer/TimerTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/timer/TimerTest.java @@ -25,6 +25,7 @@ import java.util.concurrent.Future; import java.util.concurrent.TimeUnit; +import org.junit.AfterClass; import org.junit.BeforeClass; import org.junit.Test; @@ -72,32 +73,56 @@ public static void setupOnce() throws Exception { { e.printStackTrace(); } + + RCLJava.rclJavaInit(); + } + + @AfterClass + public static void tearDownOnce() { + RCLJava.shutdown(); } @Test - public final void testCreate() { + public final void testCreateWallTimer() throws Exception { int max_iterations = 4; - RCLJava.rclJavaInit(); - Node node = RCLJava.createNode("test_node"); + Node node = RCLJava.createNode("test_timer_node"); RCLFuture future = new RCLFuture(new WeakReference(node)); TimerCallback timerCallback = new TimerCallback(future, max_iterations); - WallTimer timer = node.createWallTimer(250, TimeUnit.MILLISECONDS, timerCallback); + Timer timer = node.createWallTimer(250, TimeUnit.MILLISECONDS, timerCallback); assertNotEquals(0, timer.getHandle()); + assertFalse(timer.isCanceled()); + assertEquals( + TimeUnit.NANOSECONDS.convert(250, TimeUnit.MILLISECONDS), timer.getTimerPeriodNS()); - while (RCLJava.ok() && !future.isDone()) { - RCLJava.spinOnce(node); - } + boolean result = future.get(3, TimeUnit.SECONDS); + assertTrue(result); + assertEquals(4, timerCallback.getCounter()); - assertFalse(timer.isCanceled()); timer.cancel(); + assertFalse(timer.isReady()); + assertTrue(timer.isCanceled()); + } + @Test + public final void testCreateTimer() throws Exception { + int max_iterations = 4; + + Node node = RCLJava.createNode("test_timer_node"); + RCLFuture future = new RCLFuture(new WeakReference(node)); + TimerCallback timerCallback = new TimerCallback(future, max_iterations); + Timer timer = node.createTimer(250, TimeUnit.MILLISECONDS, timerCallback); + assertNotEquals(0, timer.getHandle()); + assertFalse(timer.isCanceled()); assertEquals( TimeUnit.NANOSECONDS.convert(250, TimeUnit.MILLISECONDS), timer.getTimerPeriodNS()); - assertFalse(timer.isReady()); - assertTrue(timer.isCanceled()); + boolean result = future.get(3, TimeUnit.SECONDS); + assertTrue(result); assertEquals(4, timerCallback.getCounter()); - RCLJava.shutdown(); + + timer.cancel(); + assertFalse(timer.isReady()); + assertTrue(timer.isCanceled()); } } From aa9f8d786c74bc3a1cad1f03420ae09a01a84ce3 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Tue, 22 Sep 2020 09:34:18 -0700 Subject: [PATCH 16/51] Add getClock method to Node interface (osrf/ros2_java#43) Useful for getting the current time from the Clock instance. Signed-off-by: Jacob Perron --- rcljava/src/main/java/org/ros2/rcljava/node/Node.java | 6 ++++++ rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java | 7 +++++++ 2 files changed, 13 insertions(+) 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 7cdbec41..11297139 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/Node.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/Node.java @@ -38,6 +38,7 @@ import org.ros2.rcljava.service.RMWRequestId; import org.ros2.rcljava.service.Service; import org.ros2.rcljava.subscription.Subscription; +import org.ros2.rcljava.time.Clock; import org.ros2.rcljava.timer.Timer; import org.ros2.rcljava.timer.WallTimer; @@ -46,6 +47,11 @@ * A Node must be created via @{link RCLJava#createNode(String)} */ public interface Node extends Disposable { + /** + * @return The nodes @{link Clock}. + */ + Clock getClock(); + /** * return All the @{link Client}s that have been created by this instance. */ 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 588c4f6b..2019f8c7 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java @@ -220,6 +220,13 @@ private static native long nativeCreatePublisherHa private static native long nativeCreateSubscriptionHandle( long handle, Class messageType, String topic, long qosProfileHandle); + /** + * {@inheritDoc} + */ + public final Clock getClock() { + return this.clock; + } + /** * {@inheritDoc} */ From 9487121ad17b37a7ff6fe43acbcf02b5c8186e13 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 22 Sep 2020 14:39:53 -0300 Subject: [PATCH 17/51] Add get subscription names and types by node method to Node class (osrf/ros2_java#35) Signed-off-by: Ivan Santiago Paunovic --- .../include/org_ros2_rcljava_node_NodeImpl.h | 9 ++ .../cpp/org_ros2_rcljava_node_NodeImpl.cpp | 38 ++++++++ .../main/java/org/ros2/rcljava/node/Node.java | 12 +++ .../java/org/ros2/rcljava/node/NodeImpl.java | 11 +++ .../java/org/ros2/rcljava/node/NodeTest.java | 94 +++++++++++++++++++ 5 files changed, 164 insertions(+) diff --git a/rcljava/include/org_ros2_rcljava_node_NodeImpl.h b/rcljava/include/org_ros2_rcljava_node_NodeImpl.h index 90c565ed..4a169cb8 100644 --- a/rcljava/include/org_ros2_rcljava_node_NodeImpl.h +++ b/rcljava/include/org_ros2_rcljava_node_NodeImpl.h @@ -146,6 +146,15 @@ JNIEXPORT void JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetPublisherNamesAndTypesByNode( JNIEnv *, jclass, jlong, jstring, jstring, jobject); +/* + * Class: org_ros2_rcljava_node_NodeImpl + * Method: nativeGetSubscriptionNamesAndTypesByNode + * Signature: (JLjava/lang/String;Ljava/lang/String;Ljava/util/Collection;)V + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetSubscriptionNamesAndTypesByNode( + JNIEnv *, jclass, jlong, jstring, 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 4b0d57d8..ae476276 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp @@ -525,3 +525,41 @@ Java_org_ros2_rcljava_node_NodeImpl_nativeGetPublisherNamesAndTypesByNode( RCLJAVA_COMMON_THROW_FROM_RCL(env, ret, "failed to get publisher names and types"); fill_jnames_and_types(env, publisher_names_and_types, jnames_and_types); } + +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_node_NodeImpl_nativeGetSubscriptionNamesAndTypesByNode( + JNIEnv * env, jclass, jlong handle, jstring jname, jstring jnamespace, 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; + } + + const char * name = env->GetStringUTFChars(jname, NULL); + auto release_jname = rcpputils::make_scope_exit( + [jname, name, env]() {env->ReleaseStringUTFChars(jname, name);}); + const char * namespace_ = env->GetStringUTFChars(jnamespace, NULL); + auto release_jnamespace = rcpputils::make_scope_exit( + [jnamespace, namespace_, env]() {env->ReleaseStringUTFChars(jnamespace, namespace_);}); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_names_and_types_t subscription_names_and_types = rcl_get_zero_initialized_names_and_types(); + auto fini_names_and_types = rcpputils::make_scope_exit( + [pnames_and_types = &subscription_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 subscription names and types structure"); + } + }); + + rcl_ret_t ret = rcl_get_subscriber_names_and_types_by_node( + node, + &allocator, + false, + name, + namespace_, + &subscription_names_and_types); + RCLJAVA_COMMON_THROW_FROM_RCL(env, ret, "failed to get subscription names and types"); + fill_jnames_and_types(env, subscription_names_and_types, jnames_and_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 11297139..4e9b0db5 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/Node.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/Node.java @@ -637,4 +637,16 @@ Client createClient(final Class serviceType, * @return the detected publisher names and types. */ Collection getPublisherNamesAndTypesByNode(String nodeName, String nodeNamespace); + + /** + * Return the subscription names and types that were created from the node specified by the given + * node name and namespace. + * See @{link graph#NameAndTypes} for more information about the returned value. + * + * @param nodeName name of the node we want to know its subscriptions. + * @param nodeNamespace namespace of the node we want to know its subscriptions. + * @return the detected subscription names and types. + */ + Collection getSubscriptionNamesAndTypesByNode( + String nodeName, String nodeNamespace); } 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 2019f8c7..9710a57d 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java @@ -842,4 +842,15 @@ public final Collection getPublisherNamesAndTypesByNode( private static native final Collection nativeGetPublisherNamesAndTypesByNode( long handle, String nodeName, String nodeNamespace, Collection namesAndTypes); + + public final Collection getSubscriptionNamesAndTypesByNode( + String nodeName, String nodeNamespace) + { + Collection namesAndTypes = new ArrayList(); + nativeGetSubscriptionNamesAndTypesByNode(this.handle, nodeName, nodeNamespace, namesAndTypes); + return namesAndTypes; + } + + private static native final Collection nativeGetSubscriptionNamesAndTypesByNode( + long handle, String nodeName, String nodeNamespace, 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 6afbb25b..4657568a 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java @@ -1272,4 +1272,98 @@ public void accept(final Collection local, Collection subscription1 = node.createSubscription( + rcljava.msg.Empty.class, "test_get_subscription_names_and_types_one", + new Consumer() { + public void accept(final rcljava.msg.Empty msg) {} + }); + Subscription subscription2 = node.createSubscription( + rcljava.msg.Empty.class, "test_get_subscription_names_and_types_two", + new Consumer() { + public void accept(final rcljava.msg.Empty msg) {} + }); + Subscription subscription3 = remoteNode.createSubscription( + rcljava.msg.Empty.class, "test_get_subscription_names_and_types_two", + new Consumer() { + public void accept(final rcljava.msg.Empty msg) {} + }); + Subscription subscription4 = remoteNode.createSubscription( + rcljava.msg.Empty.class, "test_get_subscription_names_and_types_three", + new Consumer() { + public void accept(final rcljava.msg.Empty msg) {} + }); + Publisher publisher = node.createPublisher( + rcljava.msg.UInt32.class, "test_get_topic_names_and_types_this_should_not_appear"); + + BiConsumer, Collection> validateNameAndTypes = + new BiConsumer, Collection>() { + public void accept(final Collection local, Collection remote) { + // TODO(ivanpauno): Using assertj may help a lot here https://assertj.github.io/doc/. + assertEquals(local.size(), 2); + assertTrue( + "topic 'test_get_subscription_names_and_types_one' was not discovered for local node", + local.contains( + new NameAndTypes( + "/test_get_subscription_names_and_types_one", + new ArrayList(Arrays.asList("rcljava/msg/Empty"))))); + assertTrue( + "topic 'test_get_subscription_names_and_types_two' was not discovered for local node", + local.contains( + new NameAndTypes( + "/test_get_subscription_names_and_types_two", + new ArrayList(Arrays.asList("rcljava/msg/Empty"))))); + + assertEquals(remote.size(), 2); + assertTrue( + "topic 'test_get_subscription_names_and_types_two' was not discovered for remote node", + remote.contains( + new NameAndTypes( + "/test_get_subscription_names_and_types_two", + new ArrayList(Arrays.asList("rcljava/msg/Empty"))))); + assertTrue( + "topic 'test_get_subscription_names_and_types_three' was not discovered for remote node", + remote.contains( + new NameAndTypes( + "/test_get_subscription_names_and_types_three", + new ArrayList(Arrays.asList("rcljava/msg/Empty"))))); + } + }; + + long start = System.currentTimeMillis(); + boolean ok = false; + Collection local = null; + Collection remote = null; + do { + local = this.node.getSubscriptionNamesAndTypesByNode("test_node", "/"); + remote = this.node.getSubscriptionNamesAndTypesByNode( + "test_get_subscription_names_and_types_remote_node", "/"); + try { + validateNameAndTypes.accept(local, remote); + 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(local); + assertNotNull(remote); + validateNameAndTypes.accept(local, remote); + + subscription1.dispose(); + subscription2.dispose(); + subscription3.dispose(); + subscription4.dispose(); + publisher.dispose(); + remoteNode.dispose(); + } } From 43e9d32c7789a428e31de8b3cedcd51b94fd7d3b Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 22 Sep 2020 18:44:32 -0300 Subject: [PATCH 18/51] Add getServiceNamesAndTypes method to Node class (osrf/ros2_java#36) Signed-off-by: Ivan Santiago Paunovic --- .../include/org_ros2_rcljava_node_NodeImpl.h | 9 ++ .../cpp/org_ros2_rcljava_node_NodeImpl.cpp | 36 ++++++ .../main/java/org/ros2/rcljava/node/Node.java | 12 ++ .../java/org/ros2/rcljava/node/NodeImpl.java | 11 ++ .../java/org/ros2/rcljava/node/NodeTest.java | 118 ++++++++++++++++++ 5 files changed, 186 insertions(+) diff --git a/rcljava/include/org_ros2_rcljava_node_NodeImpl.h b/rcljava/include/org_ros2_rcljava_node_NodeImpl.h index 4a169cb8..caedfbe5 100644 --- a/rcljava/include/org_ros2_rcljava_node_NodeImpl.h +++ b/rcljava/include/org_ros2_rcljava_node_NodeImpl.h @@ -155,6 +155,15 @@ JNIEXPORT void JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetSubscriptionNamesAndTypesByNode( JNIEnv *, jclass, jlong, jstring, jstring, jobject); +/* + * Class: org_ros2_rcljava_node_NodeImpl + * Method: nativeServiceNamesAndTypesByNode + * Signature: (JLjava/lang/String;Ljava/lang/String;Ljava/util/Collection;)V + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetServiceNamesAndTypesByNode( + JNIEnv *, jclass, jlong, jstring, 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 ae476276..1dcfc361 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp @@ -563,3 +563,39 @@ Java_org_ros2_rcljava_node_NodeImpl_nativeGetSubscriptionNamesAndTypesByNode( RCLJAVA_COMMON_THROW_FROM_RCL(env, ret, "failed to get subscription names and types"); fill_jnames_and_types(env, subscription_names_and_types, jnames_and_types); } + +JNIEXPORT void JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetServiceNamesAndTypesByNode( + JNIEnv * env, jclass, jlong handle, jstring jname, jstring jnamespace, 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; + } + + const char * name = env->GetStringUTFChars(jname, NULL); + auto release_jname = rcpputils::make_scope_exit( + [jname, name, env]() {env->ReleaseStringUTFChars(jname, name);}); + const char * namespace_ = env->GetStringUTFChars(jnamespace, NULL); + auto release_jnamespace = rcpputils::make_scope_exit( + [jnamespace, namespace_, env]() {env->ReleaseStringUTFChars(jnamespace, namespace_);}); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_names_and_types_t service_names_and_types = rcl_get_zero_initialized_names_and_types(); + auto fini_names_and_types = rcpputils::make_scope_exit( + [pnames_and_types = &service_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 service names and types structure"); + } + }); + + rcl_ret_t ret = rcl_get_service_names_and_types_by_node( + node, + &allocator, + name, + namespace_, + &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); +} 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 4e9b0db5..c91d74fd 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/Node.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/Node.java @@ -649,4 +649,16 @@ Client createClient(final Class serviceType, */ Collection getSubscriptionNamesAndTypesByNode( String nodeName, String nodeNamespace); + + /** + * Return the service server names and types that were created from the node specified by the + * given node name and namespace. + * See @{link graph#NameAndTypes} for more information about the returned value. + * + * @param nodeName name of the node we want to know its services. + * @param nodeNamespace namespace of the node we want to know its services. + * @return the detected service server names and types. + */ + Collection getServiceNamesAndTypesByNode( + String nodeName, String nodeNamespace); } 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 9710a57d..f3096e25 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java @@ -853,4 +853,15 @@ public final Collection getSubscriptionNamesAndTypesByNode( private static native final Collection nativeGetSubscriptionNamesAndTypesByNode( long handle, String nodeName, String nodeNamespace, Collection namesAndTypes); + + public final Collection getServiceNamesAndTypesByNode( + String nodeName, String nodeNamespace) + { + Collection namesAndTypes = new ArrayList(); + nativeGetServiceNamesAndTypesByNode(this.handle, nodeName, nodeNamespace, namesAndTypes); + return namesAndTypes; + } + + private static native final Collection nativeGetServiceNamesAndTypesByNode( + long handle, String nodeName, String nodeNamespace, 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 4657568a..49137cf2 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java @@ -1366,4 +1366,122 @@ public void accept(final Collection local, Collection service1 = node.createService( + rcljava.srv.AddTwoInts.class, "test_get_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) + {} + }); + Service service2 = node.createService( + rcljava.srv.AddTwoInts.class, "test_get_service_names_and_types_two", + 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) + {} + }); + Service service3 = remoteNode.createService( + rcljava.srv.AddTwoInts.class, "test_get_service_names_and_types_two", + 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) + {} + }); + Service service4 = remoteNode.createService( + rcljava.srv.AddTwoInts.class, "test_get_service_names_and_types_three", + 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_get_service_names_and_types_this_should_not_appear"); + + BiConsumer, Collection> validateNameAndTypes = + new BiConsumer, Collection>() { + public void accept(final Collection local, Collection remote) { + // TODO(ivanpauno): Using assertj may help a lot here https://assertj.github.io/doc/. + assertEquals(local.size(), 2); + assertTrue( + "service 'test_get_service_names_and_types_one' was not discovered for local node", + local.contains( + new NameAndTypes( + "/test_get_service_names_and_types_one", + new ArrayList(Arrays.asList("rcljava/srv/AddTwoInts"))))); + assertTrue( + "service 'test_get_service_names_and_types_two' was not discovered for local node", + local.contains( + new NameAndTypes( + "/test_get_service_names_and_types_two", + new ArrayList(Arrays.asList("rcljava/srv/AddTwoInts"))))); + + assertEquals(remote.size(), 2); + assertTrue( + "service 'test_get_service_names_and_types_two' was not discovered for remote node", + remote.contains( + new NameAndTypes( + "/test_get_service_names_and_types_two", + new ArrayList(Arrays.asList("rcljava/srv/AddTwoInts"))))); + assertTrue( + "service 'test_get_service_names_and_types_three' was not discovered for remote node", + remote.contains( + new NameAndTypes( + "/test_get_service_names_and_types_three", + new ArrayList(Arrays.asList("rcljava/srv/AddTwoInts"))))); + } + }; + + long start = System.currentTimeMillis(); + boolean ok = false; + Collection local = null; + Collection remote = null; + do { + local = this.node.getServiceNamesAndTypesByNode("test_node", "/"); + remote = this.node.getServiceNamesAndTypesByNode( + "test_get_service_names_and_types_remote_node", "/"); + try { + validateNameAndTypes.accept(local, remote); + 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(local); + assertNotNull(remote); + validateNameAndTypes.accept(local, remote); + + service1.dispose(); + service2.dispose(); + service3.dispose(); + service4.dispose(); + client.dispose(); + remoteNode.dispose(); + } } From 978787071f41da3b49cef488980e120c2366bfc5 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 23 Sep 2020 10:32:51 -0300 Subject: [PATCH 19/51] Add getClientNamesAndTypes by Node (osrf/ros2_java#37) Signed-off-by: Ivan Santiago Paunovic --- .../include/org_ros2_rcljava_node_NodeImpl.h | 9 ++ .../cpp/org_ros2_rcljava_node_NodeImpl.cpp | 36 ++++++++ .../main/java/org/ros2/rcljava/node/Node.java | 12 +++ .../java/org/ros2/rcljava/node/NodeImpl.java | 11 +++ .../java/org/ros2/rcljava/node/NodeTest.java | 91 +++++++++++++++++++ 5 files changed, 159 insertions(+) diff --git a/rcljava/include/org_ros2_rcljava_node_NodeImpl.h b/rcljava/include/org_ros2_rcljava_node_NodeImpl.h index caedfbe5..8a4bc582 100644 --- a/rcljava/include/org_ros2_rcljava_node_NodeImpl.h +++ b/rcljava/include/org_ros2_rcljava_node_NodeImpl.h @@ -164,6 +164,15 @@ JNIEXPORT void JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetServiceNamesAndTypesByNode( JNIEnv *, jclass, jlong, jstring, jstring, jobject); +/* + * Class: org_ros2_rcljava_node_NodeImpl + * Method: nativeGetClientNamesAndTypesByNode + * Signature: (JLjava/lang/String;Ljava/lang/String;Ljava/util/Collection;)V + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetClientNamesAndTypesByNode( + JNIEnv *, jclass, jlong, jstring, 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 1dcfc361..a85bd726 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp @@ -599,3 +599,39 @@ JNIEXPORT void JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetServiceNames 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); } + +JNIEXPORT void JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetClientNamesAndTypesByNode( + JNIEnv * env, jclass, jlong handle, jstring jname, jstring jnamespace, 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; + } + + const char * name = env->GetStringUTFChars(jname, NULL); + auto release_jname = rcpputils::make_scope_exit( + [jname, name, env]() {env->ReleaseStringUTFChars(jname, name);}); + const char * namespace_ = env->GetStringUTFChars(jnamespace, NULL); + auto release_jnamespace = rcpputils::make_scope_exit( + [jnamespace, namespace_, env]() {env->ReleaseStringUTFChars(jnamespace, namespace_);}); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_names_and_types_t client_names_and_types = rcl_get_zero_initialized_names_and_types(); + auto fini_names_and_types = rcpputils::make_scope_exit( + [pnames_and_types = &client_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 client names and types structure"); + } + }); + + rcl_ret_t ret = rcl_get_client_names_and_types_by_node( + node, + &allocator, + name, + namespace_, + &client_names_and_types); + RCLJAVA_COMMON_THROW_FROM_RCL(env, ret, "failed to get client names and types"); + fill_jnames_and_types(env, client_names_and_types, jnames_and_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 c91d74fd..b6750e39 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/Node.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/Node.java @@ -661,4 +661,16 @@ Collection getSubscriptionNamesAndTypesByNode( */ Collection getServiceNamesAndTypesByNode( String nodeName, String nodeNamespace); + + /** + * Return the client names and types that were created from the node specified by the + * given node name and namespace. + * See @{link graph#NameAndTypes} for more information about the returned value. + * + * @param nodeName name of the node we want to know its clients. + * @param nodeNamespace namespace of the node we want to know its clients. + * @return the detected client names and types. + */ + Collection getClientNamesAndTypesByNode( + String nodeName, String nodeNamespace); } 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 f3096e25..4d23ac70 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java @@ -864,4 +864,15 @@ public final Collection getServiceNamesAndTypesByNode( private static native final Collection nativeGetServiceNamesAndTypesByNode( long handle, String nodeName, String nodeNamespace, Collection namesAndTypes); + + public final Collection getClientNamesAndTypesByNode( + String nodeName, String nodeNamespace) + { + Collection namesAndTypes = new ArrayList(); + nativeGetClientNamesAndTypesByNode(this.handle, nodeName, nodeNamespace, namesAndTypes); + return namesAndTypes; + } + + private static native final Collection nativeGetClientNamesAndTypesByNode( + long handle, String nodeName, String nodeNamespace, 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 49137cf2..7c58344f 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java @@ -1484,4 +1484,95 @@ public void accept(final Collection local, Collection client1 = node.createClient( + rcljava.srv.AddTwoInts.class, "test_get_client_names_and_types_one"); + Client client2 = node.createClient( + rcljava.srv.AddTwoInts.class, "test_get_client_names_and_types_two"); + Client client3 = remoteNode.createClient( + rcljava.srv.AddTwoInts.class, "test_get_client_names_and_types_two"); + Client client4 = remoteNode.createClient( + rcljava.srv.AddTwoInts.class, "test_get_client_names_and_types_three"); + Service service = node.createService( + rcljava.srv.AddTwoInts.class, "test_get_client_names_and_types_this_should_not_appear", + 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) + {} + }); + + BiConsumer, Collection> validateNameAndTypes = + new BiConsumer, Collection>() { + public void accept(final Collection local, Collection remote) { + // TODO(ivanpauno): Using assertj may help a lot here https://assertj.github.io/doc/. + assertEquals(local.size(), 2); + assertTrue( + "client 'test_get_client_names_and_types_one' was not discovered for local node", + local.contains( + new NameAndTypes( + "/test_get_client_names_and_types_one", + new ArrayList(Arrays.asList("rcljava/srv/AddTwoInts"))))); + assertTrue( + "client 'test_get_client_names_and_types_two' was not discovered for local node", + local.contains( + new NameAndTypes( + "/test_get_client_names_and_types_two", + new ArrayList(Arrays.asList("rcljava/srv/AddTwoInts"))))); + + assertEquals(remote.size(), 2); + assertTrue( + "client 'test_get_client_names_and_types_two' was not discovered for remote node", + remote.contains( + new NameAndTypes( + "/test_get_client_names_and_types_two", + new ArrayList(Arrays.asList("rcljava/srv/AddTwoInts"))))); + assertTrue( + "client 'test_get_client_names_and_types_three' was not discovered for remote node", + remote.contains( + new NameAndTypes( + "/test_get_client_names_and_types_three", + new ArrayList(Arrays.asList("rcljava/srv/AddTwoInts"))))); + } + }; + + long start = System.currentTimeMillis(); + boolean ok = false; + Collection local = null; + Collection remote = null; + do { + local = this.node.getClientNamesAndTypesByNode("test_node", "/"); + remote = this.node.getClientNamesAndTypesByNode( + "test_get_client_names_and_types_remote_node", "/"); + try { + validateNameAndTypes.accept(local, remote); + 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(local); + assertNotNull(remote); + validateNameAndTypes.accept(local, remote); + + client1.dispose(); + client2.dispose(); + client3.dispose(); + client4.dispose(); + service.dispose(); + remoteNode.dispose(); + } } From 6a94c9dfb2f6dfec440fc617a508ac090e125d06 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Wed, 23 Sep 2020 09:42:12 -0700 Subject: [PATCH 20/51] Add toMsg method to Time class (osrf/ros2_java#44) Signed-off-by: Jacob Perron --- .../src/main/java/org/ros2/rcljava/Time.java | 8 +++++ .../test/java/org/ros2/rcljava/TimeTest.java | 35 +++++++++++++++++++ 2 files changed, 43 insertions(+) diff --git a/rcljava/src/main/java/org/ros2/rcljava/Time.java b/rcljava/src/main/java/org/ros2/rcljava/Time.java index f5b864e0..02d517ed 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/Time.java +++ b/rcljava/src/main/java/org/ros2/rcljava/Time.java @@ -60,6 +60,14 @@ public Time(final long secs, final long nanos, final ClockType ct) { this.nanoseconds = TimeUnit.SECONDS.toNanos(secs) + nanos; } + public builtin_interfaces.msg.Time toMsg() { + long seconds = this.nanoseconds / 1000000000; + long nanos = this.nanoseconds % 1000000000; + builtin_interfaces.msg.Time msg = new builtin_interfaces.msg.Time(); + msg.setSec((int)seconds); + msg.setNanosec((int)nanos); + return msg; + } public long nanoseconds() { return nanoseconds; diff --git a/rcljava/src/test/java/org/ros2/rcljava/TimeTest.java b/rcljava/src/test/java/org/ros2/rcljava/TimeTest.java index a8d6b298..62aa37a8 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/TimeTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/TimeTest.java @@ -50,6 +50,16 @@ public final void testTimeNoArgConstructor() { assertEquals(ClockType.SYSTEM_TIME, time.clockType()); } + @Test + public final void testTimeFromMsgConstructor() { + builtin_interfaces.msg.Time timeMsg = new builtin_interfaces.msg.Time(); + timeMsg.setSec(42); + timeMsg.setNanosec(100); + Time time = new Time(timeMsg, ClockType.SYSTEM_TIME); + assertEquals(42000000100L, time.nanoseconds()); + assertEquals(ClockType.SYSTEM_TIME, time.clockType()); + } + @Test public final void testTimeNanos() { Time time = new Time(45, ClockType.SYSTEM_TIME); @@ -73,4 +83,29 @@ public final void testTimeBadSecs() { public final void testTimeBadNanos() { Time time = new Time(0, -45, ClockType.SYSTEM_TIME); } + + @Test + public final void testTimeToMsg() { + { + Time time = new Time(); + builtin_interfaces.msg.Time timeMsgOut = time.toMsg(); + assertEquals(0, timeMsgOut.getSec()); + assertEquals(0, timeMsgOut.getNanosec()); + } + { + builtin_interfaces.msg.Time timeMsg = new builtin_interfaces.msg.Time(); + timeMsg.setSec(42); + timeMsg.setNanosec(100); + Time time = new Time(timeMsg, ClockType.SYSTEM_TIME); + builtin_interfaces.msg.Time timeMsgOut = time.toMsg(); + assertEquals(42, timeMsgOut.getSec()); + assertEquals(100, timeMsgOut.getNanosec()); + } + { + Time time = new Time(0, 45, ClockType.SYSTEM_TIME); + builtin_interfaces.msg.Time timeMsgOut = time.toMsg(); + assertEquals(0, timeMsgOut.getSec()); + assertEquals(45, timeMsgOut.getNanosec()); + } + } } From d6523f30afb75b85de9d69c9ced8c214727a255d Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 29 Sep 2020 17:43:38 -0300 Subject: [PATCH 21/51] Pass command line arguments in context init method (osrf/ros2_java#45) Signed-off-by: Ivan Santiago Paunovic --- rcljava/CMakeLists.txt | 2 + .../org_ros2_rcljava_contexts_ContextImpl.h | 3 +- .../org_ros2_rcljava_contexts_ContextImpl.cpp | 53 +++++++++++- .../main/java/org/ros2/rcljava/RCLJava.java | 10 ++- .../org/ros2/rcljava/contexts/Context.java | 6 +- .../ros2/rcljava/contexts/ContextImpl.java | 24 ++++-- .../ros2/rcljava/contexts/ContextTest.java | 82 +++++++++++++++++++ 7 files changed, 167 insertions(+), 13 deletions(-) create mode 100644 rcljava/src/test/java/org/ros2/rcljava/contexts/ContextTest.java diff --git a/rcljava/CMakeLists.txt b/rcljava/CMakeLists.txt index 4165ab57..ded6f663 100644 --- a/rcljava/CMakeLists.txt +++ b/rcljava/CMakeLists.txt @@ -265,6 +265,7 @@ if(BUILD_TESTING) "src/test/java/org/ros2/rcljava/SpinTest.java" "src/test/java/org/ros2/rcljava/TimeTest.java" "src/test/java/org/ros2/rcljava/client/ClientTest.java" + "src/test/java/org/ros2/rcljava/contexts/ContextTest.java" "src/test/java/org/ros2/rcljava/node/NodeOptionsTest.java" "src/test/java/org/ros2/rcljava/node/NodeParametersTest.java" "src/test/java/org/ros2/rcljava/node/NodeUndeclaredParametersTest.java" @@ -283,6 +284,7 @@ if(BUILD_TESTING) "org.ros2.rcljava.SpinTest" "org.ros2.rcljava.TimeTest" "org.ros2.rcljava.client.ClientTest" + "org.ros2.rcljava.contexts.ContextTest" "org.ros2.rcljava.node.NodeOptionsTest" "org.ros2.rcljava.node.NodeParametersTest" "org.ros2.rcljava.node.NodeUndeclaredParametersTest" diff --git a/rcljava/include/org_ros2_rcljava_contexts_ContextImpl.h b/rcljava/include/org_ros2_rcljava_contexts_ContextImpl.h index 049891d6..b6cef1fb 100644 --- a/rcljava/include/org_ros2_rcljava_contexts_ContextImpl.h +++ b/rcljava/include/org_ros2_rcljava_contexts_ContextImpl.h @@ -27,7 +27,8 @@ extern "C" { * Signature: (J)V */ JNIEXPORT void -JNICALL Java_org_ros2_rcljava_contexts_ContextImpl_nativeInit(JNIEnv *, jclass, jlong); +JNICALL Java_org_ros2_rcljava_contexts_ContextImpl_nativeInit( + JNIEnv *, jclass, jlong, jobjectArray); /* * Class: org_ros2_rcljava_contexts_ContextImpl diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_contexts_ContextImpl.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_contexts_ContextImpl.cpp index 8f70afad..4ae9d471 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_contexts_ContextImpl.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_contexts_ContextImpl.cpp @@ -14,7 +14,9 @@ #include +#include #include +#include #include "rcl/context.h" #include "rcl/error_handling.h" @@ -25,6 +27,7 @@ #include "org_ros2_rcljava_contexts_ContextImpl.h" +using rcljava_common::exceptions::rcljava_throw_exception; using rcljava_common::exceptions::rcljava_throw_rclexception; JNIEXPORT jboolean JNICALL @@ -36,7 +39,8 @@ Java_org_ros2_rcljava_contexts_ContextImpl_nativeIsValid(JNIEnv *, jclass, jlong } JNIEXPORT void JNICALL -Java_org_ros2_rcljava_contexts_ContextImpl_nativeInit(JNIEnv * env, jclass, jlong context_handle) +Java_org_ros2_rcljava_contexts_ContextImpl_nativeInit( + JNIEnv * env, jclass, jlong context_handle, jobjectArray jargs) { // TODO(jacobperron): Encapsulate init options into a Java class rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); @@ -49,8 +53,42 @@ Java_org_ros2_rcljava_contexts_ContextImpl_nativeInit(JNIEnv * env, jclass, jlon } rcl_context_t * context = reinterpret_cast(context_handle); - // TODO(esteve): parse args - ret = rcl_init(0, nullptr, &init_options, context); + + // jsize should always fit in a size_t, so the following cast is safe. + const auto argc = static_cast(env->GetArrayLength(jargs)); + // rcl_init takes an int, check for overflow just in case + if (argc > static_cast(std::numeric_limits::max())) { + std::ostringstream oss( + "args length is longer than expected, maximum length is ", std::ios_base::ate); + oss << std::numeric_limits::max(); + rcljava_throw_exception( + env, "java/lang/IllegalArgumentException", oss.str().c_str()); + return; + } + const char ** argv = nullptr; + if (argc) { + argv = static_cast(malloc(argc * sizeof(char *))); + for (size_t i = 0; i < argc; ++i) { + auto item = static_cast(env->GetObjectArrayElement(jargs, i)); + // an exception here should never happen, + // as the only possible exception is array out of bounds + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + argv[i] = env->GetStringUTFChars(item, NULL); + } + } + + ret = rcl_init(static_cast(argc), argv, &init_options, context); + if (argc) { + for (size_t i = 0; i < argc; ++i) { + auto item = static_cast(env->GetObjectArrayElement(jargs, i)); + // an exception here should never happen, + // as the only possible exception is array out of bounds + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + env->ReleaseStringUTFChars(item, argv[i]); + argv[i] = env->GetStringUTFChars(item, NULL); + } + free(argv); + } if (RCL_RET_OK != ret) { std::string msg = "Failed to init context: " + std::string(rcl_get_error_string().str); rcl_ret_t ignored_ret = rcl_init_options_fini(&init_options); @@ -98,8 +136,15 @@ Java_org_ros2_rcljava_contexts_ContextImpl_nativeDispose(JNIEnv * env, jclass, j rcl_context_t * context = reinterpret_cast(context_handle); - rcl_ret_t ret = rcl_context_fini(context); + // TODO(ivanpauno): Currently, calling `rcl_context_fini` in a zero initialized context fails: + // That's incongruent with most other rcl objects. + // rcl issue: https://github.com/ros2/rcl/issues/814 + if (!context->impl) { + return; + } + + rcl_ret_t ret = rcl_context_fini(context); if (RCL_RET_OK != ret) { std::string msg = "Failed to destroy context: " + std::string(rcl_get_error_string().str); rcl_reset_error(); diff --git a/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java b/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java index 15a703c8..d4d28c3c 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java +++ b/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java @@ -137,12 +137,20 @@ public static boolean isInitialized() { * This also initializes the default context. */ public static synchronized void rclJavaInit() { + String args[] = {}; + rclJavaInit(args); + } + + /** + * Initialize the RCLJava API, passing command line arguments. + */ + public static synchronized void rclJavaInit(String args[]) { if (RCLJava.ok()) { return; } // Initialize default context - getDefaultContext().init(); + getDefaultContext().init(args); logger.info("Using RMW implementation: {}", RCLJava.getRMWIdentifier()); } diff --git a/rcljava/src/main/java/org/ros2/rcljava/contexts/Context.java b/rcljava/src/main/java/org/ros2/rcljava/contexts/Context.java index 717eea77..3a458cb1 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/contexts/Context.java +++ b/rcljava/src/main/java/org/ros2/rcljava/contexts/Context.java @@ -32,11 +32,15 @@ public interface Context extends Disposable { /** * Initialize the context. - * // TODO(jacobperron): Pass arguments for parsing * // TODO(jacobperron): Pass in InitOptions object */ void init(); + /** + * Initialize the context passing command line arguments. + */ + void init(String args[]); + /** * Shutdown the context. */ diff --git a/rcljava/src/main/java/org/ros2/rcljava/contexts/ContextImpl.java b/rcljava/src/main/java/org/ros2/rcljava/contexts/ContextImpl.java index 3d6bf9f0..d39e09d0 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/contexts/ContextImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/contexts/ContextImpl.java @@ -61,10 +61,14 @@ public ContextImpl(final long handle) { * {@inheritDoc} */ public final void dispose() { - // Ensure the context is shutdown first - shutdown(); - nativeDispose(this.handle); - this.handle = 0; + if (0 != this.handle) { + // Ensure the context is shutdown first. + if (this.isValid()) { + shutdown(); + } + nativeDispose(this.handle); + this.handle = 0; + } } /** @@ -79,13 +83,21 @@ public final long getHandle() { * * @param contextHandle The pointer to the context structure. */ - private static native void nativeInit(long contextHandle); + private static native void nativeInit(long contextHandle, String args[]); /** * {@inheritDoc} */ public final void init() { - nativeInit(this.handle); + String args[] = {}; + nativeInit(this.handle, args); + } + + /** + * {@inheritDoc} + */ + public final void init(String args[]) { + nativeInit(this.handle, args); } /** diff --git a/rcljava/src/test/java/org/ros2/rcljava/contexts/ContextTest.java b/rcljava/src/test/java/org/ros2/rcljava/contexts/ContextTest.java new file mode 100644 index 00000000..52216587 --- /dev/null +++ b/rcljava/src/test/java/org/ros2/rcljava/contexts/ContextTest.java @@ -0,0 +1,82 @@ +/* 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.contexts; + +import static org.junit.Assert.assertEquals; + +import org.junit.After; +import org.junit.AfterClass; +import org.junit.BeforeClass; +import org.junit.Before; +import org.junit.Test; + +import org.ros2.rcljava.RCLJava; +import org.ros2.rcljava.contexts.Context; + +public class ContextTest { + public Context context = null; + + @BeforeClass + public static void setupOnce() throws Exception { + // Just to quiet down warnings + org.apache.log4j.BasicConfigurator.configure(); + + RCLJava.rclJavaInit(); + } + + @AfterClass + public static void tearDownOnce() { + RCLJava.shutdown(); + } + + @Before + public void setup() throws Exception { + this.context = RCLJava.createContext(); + } + + @After + public void tearDown() { + this.context.dispose(); + } + + @Test + public final void testCreateAndDispose() { + assertEquals(false, this.context.isValid()); + } + + @Test + public final void testInitShutdown() { + assertEquals(false, this.context.isValid()); + this.context.init(); + assertEquals(true, this.context.isValid()); + this.context.shutdown(); + assertEquals(false, this.context.isValid()); + } + + @Test + public final void testInitWithArgsAndShutdown() { + String args[] = { + "--user-arg-1", "user-arg-2", "-p,", + "--ros-args", "-r", "asd:=bsd" + }; + assertEquals(false, this.context.isValid()); + this.context.init(args); + assertEquals(true, this.context.isValid()); + this.context.shutdown(); + assertEquals(false, this.context.isValid()); + } +} + From a65edc5897253f2b0fb9b3e8d082123934b4e96d Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 30 Sep 2020 10:55:55 -0300 Subject: [PATCH 22/51] Initialize parameter services based on node options (osrf/ros2_java#46) Signed-off-by: Ivan Santiago Paunovic --- .../main/java/org/ros2/rcljava/RCLJava.java | 55 +++++++++++++------ .../java/org/ros2/rcljava/node/NodeImpl.java | 24 +++++++- .../org/ros2/rcljava/node/NodeOptions.java | 37 +++++++++---- .../parameters/service/ParameterService.java | 2 +- .../service/ParameterServiceImpl.java | 2 +- .../ros2/rcljava/node/NodeOptionsTest.java | 2 +- .../java/org/ros2/rcljava/node/NodeTest.java | 12 +++- .../node/NodeUndeclaredParametersTest.java | 2 +- 8 files changed, 98 insertions(+), 38 deletions(-) diff --git a/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java b/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java index d4d28c3c..6c820ed9 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java +++ b/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java @@ -43,7 +43,7 @@ import org.ros2.rcljava.timer.Timer; /** - * Entry point for the ROS2 Java API, similar to the rclcpp API. + * Entry point for the ROS 2 Java API, similar to the rclcpp API. */ public final class RCLJava { private static final Logger logger = LoggerFactory.getLogger(RCLJava.class); @@ -132,7 +132,7 @@ public static boolean isInitialized() { /** * Initialize the RCLJava API. If successful, a valid RMW implementation will - * be loaded and accessible, enabling the creating of ROS2 entities + * be loaded and accessible, enabling the creating of ROS 2 entities * (@{link Node}s, @{link Publisher}s and @{link Subscription}s. * This also initializes the default context. */ @@ -163,18 +163,18 @@ public static synchronized void rclJavaInit(String args[]) { private static native long nativeCreateContextHandle(); /** - * Create a ROS2 node (rcl_node_t) and return a pointer to it as an integer. + * Create a ROS 2 node (rcl_node_t) and return a pointer to it as an integer. * - * @param nodeName The name that will identify this node in a ROS2 graph. + * @param nodeName The name that will identify this node in a ROS 2 graph. * @param namespace The namespace of the node. * @param contextHandle Pointer to a context (rcl_context_t) with which to associated the node. - * @return A pointer to the underlying ROS2 node structure. + * @return A pointer to the underlying ROS 2 node structure. */ private static native long nativeCreateNodeHandle(String nodeName, String namespace, long contextHandle, ArrayList arguments, boolean useGlobalArguments, boolean enableRosout); /** * @return The identifier of the currently active RMW implementation via the - * native ROS2 API. + * native ROS 2 API. */ private static native String nativeGetRMWIdentifier(); @@ -186,7 +186,7 @@ public static String getRMWIdentifier() { } /** - * Call the underlying ROS2 rcl mechanism to check if ROS2 has been shut + * Call the underlying ROS 2 rcl mechanism to check if ROS 2 has been shut * down. * * @return true if RCLJava hasn't been shut down, false otherwise. @@ -222,29 +222,50 @@ public static Context createContext() { /** * Create a @{link Node}. * - * @param nodeName The name that will identify this node in a ROS2 graph. - * @return A @{link Node} that represents the underlying ROS2 node - * structure. + * @param nodeName The name that will identify this node in a ROS 2 graph. + * @return A @{link Node} that represents the underlying ROS 2 node structure. */ public static Node createNode(final String nodeName) { - return createNode(nodeName, "", RCLJava.getDefaultContext(), new NodeOptions()); + return createNode(nodeName, "", new NodeOptions()); } /** * Create a @{link Node}. * - * @param nodeName The name that will identify this node in a ROS2 graph. + * @param nodeName The name that will identify this node in a ROS 2 graph. * @param namespace The namespace of the node. - * @return A @{link Node} that represents the underlying ROS2 node - * structure. + * @return A @{link Node} that represents the underlying ROS 2 node structure. */ + public static Node createNode(final String nodeName, final String namespace) { + return createNode(nodeName, namespace, new NodeOptions()); + } + + /** + * Create a @{link Node}. + * + * @deprecated Use `RCLJava.createNode(nodeName, namespace, new NodeOptions.setContext(context))` instead. + * @param nodeName The name that will identify this node in a ROS 2 graph. + * @param namespace The namespace of the node. + * @param context Context used for creating the node, @link{RCLJava.getDefaultContext()} will be used if `null`. + * @return A @{link Node} that represents the underlying ROS 2 node structure. + */ + @Deprecated public static Node createNode(final String nodeName, final String namespace, final Context context) { - return createNode(nodeName, namespace, context, new NodeOptions()); + return createNode(nodeName, namespace, new NodeOptions().setContext(context)); } - public static Node createNode(final String nodeName, final String namespace, final Context context, final NodeOptions options) { + /** + * Create a @{link Node}. + * + * @param nodeName The name that will identify this node in a ROS 2 graph. + * @param namespace The namespace of the node. + * @param options Additional options to customize the Node creation. See @link{org.ros2.rcljava.node.NodeOptions}. + * @return A @{link Node} that represents the underlying ROS 2 node structure. + */ + public static Node createNode(final String nodeName, final String namespace, final NodeOptions options) { + Context context = options.getContext() == null ? RCLJava.getDefaultContext() : options.getContext(); long nodeHandle = nativeCreateNodeHandle(nodeName, namespace, context.getHandle(), options.getCliArgs(), options.getUseGlobalArguments(), options.getEnableRosout()); - Node node = new NodeImpl(nodeHandle, context, options.getAllowUndeclaredParameters()); + Node node = new NodeImpl(nodeHandle, options); nodes.add(node); return node; } 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 4d23ac70..bc4d90d2 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java @@ -29,6 +29,7 @@ import org.ros2.rcljava.interfaces.Disposable; import org.ros2.rcljava.interfaces.MessageDefinition; import org.ros2.rcljava.interfaces.ServiceDefinition; +import org.ros2.rcljava.node.NodeOptions; import org.ros2.rcljava.parameters.InvalidParametersException; import org.ros2.rcljava.parameters.InvalidParameterValueException; import org.ros2.rcljava.parameters.ParameterAlreadyDeclaredException; @@ -36,6 +37,8 @@ import org.ros2.rcljava.parameters.ParameterNotDeclaredException; import org.ros2.rcljava.parameters.ParameterType; import org.ros2.rcljava.parameters.ParameterVariant; +import org.ros2.rcljava.parameters.service.ParameterService; +import org.ros2.rcljava.parameters.service.ParameterServiceImpl; import org.ros2.rcljava.publisher.Publisher; import org.ros2.rcljava.publisher.PublisherImpl; import org.ros2.rcljava.qos.QoSProfile; @@ -145,15 +148,18 @@ class ParameterAndDescriptor { private Object parameterCallbacksMutex; private List parameterCallbacks; + private final ParameterService parameterService; + /** * Constructor. * * @param handle A pointer to the underlying ROS2 node structure. Must not * be zero. */ - public NodeImpl(final long handle, final Context context, final boolean allowUndeclaredParameters) { + public NodeImpl(final long handle, final NodeOptions nodeOptions) { this.handle = handle; - this.context = context; + this.context = nodeOptions.getContext() == null ? + RCLJava.getDefaultContext() : nodeOptions.getContext(); this.publishers = new LinkedBlockingQueue(); this.subscriptions = new LinkedBlockingQueue(); this.services = new LinkedBlockingQueue(); @@ -161,13 +167,25 @@ public NodeImpl(final long handle, final Context context, final boolean allowUnd this.timers = new LinkedBlockingQueue(); this.parametersMutex = new Object(); this.parameters = new ConcurrentHashMap(); - this.allowUndeclaredParameters = allowUndeclaredParameters; + this.allowUndeclaredParameters = nodeOptions.getAllowUndeclaredParameters(); this.parameterCallbacksMutex = new Object(); this.parameterCallbacks = new ArrayList(); this.clock = new Clock(ClockType.ROS_TIME); this.wall_clock = new Clock(ClockType.STEADY_TIME); this.timeSource = new TimeSource(this); this.timeSource.attachClock(this.clock); + try { + this.parameterService = nodeOptions.getStartParameterServices() ? + new ParameterServiceImpl(this) : null; + // TODO(ivanpauno): Modify createService, createClient so they don't declare + // NoSuchFieldException and IllegalAccessException checked exceptions. + // That only happens if the user passed the wrong Class object, and the exception should + // be rethrown as an unchecked IllegalArgumentException. + } catch (NoSuchFieldException ex) { + throw new IllegalArgumentException(ex.getMessage()); + } catch (IllegalAccessException ex) { + throw new IllegalArgumentException(ex.getMessage()); + } } /** diff --git a/rcljava/src/main/java/org/ros2/rcljava/node/NodeOptions.java b/rcljava/src/main/java/org/ros2/rcljava/node/NodeOptions.java index c64d65e1..cd230123 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/NodeOptions.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/NodeOptions.java @@ -17,18 +17,15 @@ import java.util.ArrayList; +import org.ros2.rcljava.contexts.Context; + public class NodeOptions { - private boolean useGlobalArguments; - private boolean enableRosout; - private boolean allowUndeclaredParameters; - private ArrayList cliArgs; - - public NodeOptions() { - this.useGlobalArguments = true; - this.enableRosout = true; - this.allowUndeclaredParameters = false; - this.cliArgs = new ArrayList(); - } + private boolean useGlobalArguments = true; + private boolean enableRosout = true; + private boolean allowUndeclaredParameters = false; + private boolean startParameterServices = true; + private Context context = null; + private ArrayList cliArgs = new ArrayList(); public final boolean getUseGlobalArguments() { return this.useGlobalArguments; @@ -57,6 +54,24 @@ public NodeOptions setAllowUndeclaredParameters(boolean allow) { return this; } + public final boolean getStartParameterServices() { + return this.startParameterServices; + } + + public NodeOptions setStartParameterServices(boolean startParameterServices) { + this.startParameterServices = startParameterServices; + return this; + } + + public final Context getContext() { + return this.context; + } + + public NodeOptions setContext(Context context) { + this.context = context; + return this; + } + public final ArrayList getCliArgs() { return this.cliArgs; } diff --git a/rcljava/src/main/java/org/ros2/rcljava/parameters/service/ParameterService.java b/rcljava/src/main/java/org/ros2/rcljava/parameters/service/ParameterService.java index 0be72380..8e2106f7 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/parameters/service/ParameterService.java +++ b/rcljava/src/main/java/org/ros2/rcljava/parameters/service/ParameterService.java @@ -15,4 +15,4 @@ package org.ros2.rcljava.parameters.service; -public interface ParameterService {} \ No newline at end of file +public interface ParameterService {} diff --git a/rcljava/src/main/java/org/ros2/rcljava/parameters/service/ParameterServiceImpl.java b/rcljava/src/main/java/org/ros2/rcljava/parameters/service/ParameterServiceImpl.java index b13d8e86..c5baa7c0 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/parameters/service/ParameterServiceImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/parameters/service/ParameterServiceImpl.java @@ -149,4 +149,4 @@ public void accept(RMWRequestId rmwRequestId, public ParameterServiceImpl(final Node node) throws NoSuchFieldException, IllegalAccessException { this(node, QoSProfile.PARAMETERS); } -} \ No newline at end of file +} diff --git a/rcljava/src/test/java/org/ros2/rcljava/node/NodeOptionsTest.java b/rcljava/src/test/java/org/ros2/rcljava/node/NodeOptionsTest.java index bde90763..3fe2ef94 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/node/NodeOptionsTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/node/NodeOptionsTest.java @@ -58,7 +58,7 @@ public static void tearDownOnce() { public final void testCreateNodeWithArgs() { NodeOptions options = new NodeOptions(); options.setCliArgs(new ArrayList(Arrays.asList("--ros-args", "-r", "__ns:=/foo"))); - Node node = RCLJava.createNode("test_node", "", RCLJava.getDefaultContext(), options); + Node node = RCLJava.createNode("test_node", "", options); assertEquals("test_node", node.getName()); assertEquals("/foo", node.getNamespace()); 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 7c58344f..926f5b3b 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java @@ -51,6 +51,7 @@ import org.ros2.rcljava.graph.NameAndTypes; import org.ros2.rcljava.graph.NodeNameInfo; import org.ros2.rcljava.node.Node; +import org.ros2.rcljava.node.NodeOptions; import org.ros2.rcljava.publisher.Publisher; import org.ros2.rcljava.qos.policies.Reliability; import org.ros2.rcljava.qos.QoSProfile; @@ -136,7 +137,8 @@ public final void accept(final T msg) { @Before public void setUp() { - node = RCLJava.createNode("test_node"); + node = RCLJava.createNode( + "test_node", "/", new NodeOptions().setStartParameterServices(false)); primitives1 = new rcljava.msg.Primitives(); primitives2 = new rcljava.msg.Primitives(); @@ -1369,7 +1371,9 @@ public void accept(final Collection local, Collection service1 = node.createService( rcljava.srv.AddTwoInts.class, "test_get_service_names_and_types_one", new TriConsumer< @@ -1487,7 +1491,9 @@ public void accept(final Collection local, Collection client1 = node.createClient( rcljava.srv.AddTwoInts.class, "test_get_client_names_and_types_one"); Client client2 = node.createClient( diff --git a/rcljava/src/test/java/org/ros2/rcljava/node/NodeUndeclaredParametersTest.java b/rcljava/src/test/java/org/ros2/rcljava/node/NodeUndeclaredParametersTest.java index 454e62bd..92ee605b 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/node/NodeUndeclaredParametersTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/node/NodeUndeclaredParametersTest.java @@ -67,7 +67,7 @@ public static void tearDownOnce() { public void setUp() { NodeOptions options = new NodeOptions(); options.setAllowUndeclaredParameters(true); - node = RCLJava.createNode("test_node", "", RCLJava.getDefaultContext(), options); + node = RCLJava.createNode("test_node", "", options); } @After From e4ebbdb6f71543f8bb25969ba3fc80b27489f78c Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Tue, 10 Nov 2020 09:16:20 -0800 Subject: [PATCH 23/51] Fix issues with parameters descriptors (osrf/ros2_java#50) This fixes two issues: 1. The default constructor for ParameterAndDescriptor initializes it's members to null. This can cause a segfault when trying to access a parameters descriptor if undeclared parameters are allowed and it has been set without a descriptor (e.g. using setParameters()). This change defines the constructor for ParameterAndDescriptor so that we don't hit any null values. 2. Parameters that are not declared and set with setParameters() do not have their descriptors set to the correct value. This change makes sure that the descriptor is initialized with the values of the parameter being set. Signed-off-by: Jacob Perron --- .../java/org/ros2/rcljava/node/NodeImpl.java | 23 +++++++++++++++---- 1 file changed, 18 insertions(+), 5 deletions(-) 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 bc4d90d2..1527c020 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java @@ -138,6 +138,10 @@ public class NodeImpl implements Node { private Object parametersMutex; class ParameterAndDescriptor { + public ParameterAndDescriptor() { + this.parameter = new ParameterVariant(); + this.descriptor = new rcl_interfaces.msg.ParameterDescriptor(); + } public ParameterVariant parameter; public rcl_interfaces.msg.ParameterDescriptor descriptor; } @@ -633,7 +637,7 @@ private rcl_interfaces.msg.SetParametersResult internalSetParametersAtomically( // them in the parameter list before setting them to the new value. We // do this multi-stage thing so that all of the parameters are set, or // none of them. - List parametersToDeclare = new ArrayList(); + ListparametersToDeclare = new ArrayList(); List parametersToUndeclare = new ArrayList(); for (ParameterVariant parameter : parameters) { if (this.parameters.containsKey(parameter.getName())) { @@ -642,7 +646,7 @@ private rcl_interfaces.msg.SetParametersResult internalSetParametersAtomically( } } else { if (this.allowUndeclaredParameters) { - parametersToDeclare.add(parameter.getName()); + parametersToDeclare.add(parameter); } else { throw new ParameterNotDeclaredException(String.format("Parameter '%s' is not declared", parameter.getName())); } @@ -651,7 +655,8 @@ private rcl_interfaces.msg.SetParametersResult internalSetParametersAtomically( // Check to make sure that a parameter isn't both trying to be declared // and undeclared simultaneously. - for (String name : parametersToDeclare) { + for (ParameterVariant parameter : parametersToDeclare) { + String name = parameter.getName(); if (parametersToUndeclare.contains(name)) { throw new IllegalArgumentException(String.format("Cannot both declare and undeclare the same parameter name '%s'", name)); } @@ -666,8 +671,16 @@ private rcl_interfaces.msg.SetParametersResult internalSetParametersAtomically( this.parameters.remove(name); } - for (String name : parametersToDeclare) { - this.parameters.put(name, new ParameterAndDescriptor()); + for (ParameterVariant parameter : parametersToDeclare) { + String name = parameter.getName(); + ParameterAndDescriptor pandd = new ParameterAndDescriptor(); + rcl_interfaces.msg.ParameterDescriptor descriptor = + new rcl_interfaces.msg.ParameterDescriptor() + .setName(name) + .setType(parameter.getType().getValue()); + pandd.parameter = parameter; + pandd.descriptor = descriptor; + this.parameters.put(name, pandd); } for (ParameterVariant parameter : parameters) { From 0b20500afa9ee0b9ccaf2392d67d2347ab69937f Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Mon, 9 Nov 2020 18:34:19 -0800 Subject: [PATCH 24/51] Allow undeclared parameters in parameters client tests Signed-off-by: Jacob Perron --- .../ros2/rcljava/parameters/AsyncParametersClientTest.java | 5 ++++- .../ros2/rcljava/parameters/SyncParametersClientTest.java | 5 ++++- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/rcljava/src/test/java/org/ros2/rcljava/parameters/AsyncParametersClientTest.java b/rcljava/src/test/java/org/ros2/rcljava/parameters/AsyncParametersClientTest.java index 1c94bce5..f9bcdc9d 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/parameters/AsyncParametersClientTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/parameters/AsyncParametersClientTest.java @@ -38,6 +38,7 @@ import org.ros2.rcljava.concurrent.RCLFuture; import org.ros2.rcljava.consumers.Consumer; import org.ros2.rcljava.node.Node; +import org.ros2.rcljava.node.NodeOptions; import org.ros2.rcljava.parameters.ParameterVariant; import org.ros2.rcljava.parameters.client.AsyncParametersClient; import org.ros2.rcljava.parameters.client.AsyncParametersClientImpl; @@ -86,7 +87,9 @@ public static void setupOnce() throws Exception { @Before public void setUp() throws Exception { - node = RCLJava.createNode("test_node"); + NodeOptions opts = new NodeOptions(); + opts.setAllowUndeclaredParameters(true); + node = RCLJava.createNode("test_node", "", opts); parameterService = new ParameterServiceImpl(node); parametersClient = new AsyncParametersClientImpl(node); } diff --git a/rcljava/src/test/java/org/ros2/rcljava/parameters/SyncParametersClientTest.java b/rcljava/src/test/java/org/ros2/rcljava/parameters/SyncParametersClientTest.java index 197e5cef..d44c566a 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/parameters/SyncParametersClientTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/parameters/SyncParametersClientTest.java @@ -38,6 +38,7 @@ import org.ros2.rcljava.concurrent.RCLFuture; import org.ros2.rcljava.consumers.Consumer; import org.ros2.rcljava.node.Node; +import org.ros2.rcljava.node.NodeOptions; import org.ros2.rcljava.parameters.ParameterVariant; import org.ros2.rcljava.parameters.client.SyncParametersClient; import org.ros2.rcljava.parameters.client.SyncParametersClientImpl; @@ -69,7 +70,9 @@ public static void setupOnce() throws Exception { @Before public void setUp() throws Exception { - node = RCLJava.createNode("test_node"); + NodeOptions opts = new NodeOptions(); + opts.setAllowUndeclaredParameters(true); + node = RCLJava.createNode("test_node", "", opts); parameterService = new ParameterServiceImpl(node); parametersClient = new SyncParametersClientImpl(node); } From a4d41d9ef6a09887d22567f24c33060f67cca3b8 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Tue, 10 Nov 2020 09:54:14 -0800 Subject: [PATCH 25/51] Fix spinSome logic (osrf/ros2_java#49) The workAvailable flag was never being set to true. If I understand the original intention, I think that the flag should be set as long as there are things being executed. Signed-off-by: Jacob Perron --- .../src/main/java/org/ros2/rcljava/executors/BaseExecutor.java | 1 + 1 file changed, 1 insertion(+) diff --git a/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java b/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java index 1ed9218e..67c15fda 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java +++ b/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java @@ -404,6 +404,7 @@ private void spinSomeImpl(long maxDurationNs, boolean exhaustive) { AnyExecutable anyExecutable = getNextExecutable(); if (anyExecutable != null) { executeAnyExecutable(anyExecutable); + workAvailable = true; } else { if (!workAvailable || !exhaustive) { break; From 622982e0510897eae0efb7d388bd2c1527e86bc7 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Wed, 11 Nov 2020 09:46:24 -0800 Subject: [PATCH 26/51] Enable and fix AsyncParametersClientTest (osrf/ros2_java#51) Now we're actually running the tests. This also fixes a bug where we were comparing the wrong objects in one of the tests. Signed-off-by: Jacob Perron --- rcljava/CMakeLists.txt | 7 ++++--- .../rcljava/parameters/AsyncParametersClientTest.java | 8 +++++++- 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/rcljava/CMakeLists.txt b/rcljava/CMakeLists.txt index ded6f663..b357f0d4 100644 --- a/rcljava/CMakeLists.txt +++ b/rcljava/CMakeLists.txt @@ -270,8 +270,8 @@ if(BUILD_TESTING) "src/test/java/org/ros2/rcljava/node/NodeParametersTest.java" "src/test/java/org/ros2/rcljava/node/NodeUndeclaredParametersTest.java" "src/test/java/org/ros2/rcljava/node/NodeTest.java" - # "src/test/java/org/ros2/rcljava/parameters/AsyncParametersClientTest.java" - # "src/test/java/org/ros2/rcljava/parameters/SyncParametersClientTest.java" + "src/test/java/org/ros2/rcljava/parameters/AsyncParametersClientTest.java" + "src/test/java/org/ros2/rcljava/parameters/SyncParametersClientTest.java" "src/test/java/org/ros2/rcljava/publisher/PublisherTest.java" "src/test/java/org/ros2/rcljava/qos/QoSProfileTest.java" "src/test/java/org/ros2/rcljava/subscription/SubscriptionTest.java" @@ -289,7 +289,8 @@ if(BUILD_TESTING) "org.ros2.rcljava.node.NodeParametersTest" "org.ros2.rcljava.node.NodeUndeclaredParametersTest" "org.ros2.rcljava.node.NodeTest" - # "org.ros2.rcljava.parameters.SyncParametersClientTest" + "org.ros2.rcljava.parameters.AsyncParametersClientTest" + "org.ros2.rcljava.parameters.SyncParametersClientTest" "org.ros2.rcljava.publisher.PublisherTest" "org.ros2.rcljava.qos.QoSProfileTest" "org.ros2.rcljava.subscription.SubscriptionTest" diff --git a/rcljava/src/test/java/org/ros2/rcljava/parameters/AsyncParametersClientTest.java b/rcljava/src/test/java/org/ros2/rcljava/parameters/AsyncParametersClientTest.java index f9bcdc9d..c0ce876a 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/parameters/AsyncParametersClientTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/parameters/AsyncParametersClientTest.java @@ -119,8 +119,14 @@ public final void testSetParameters() throws Exception { List parameterNames = Arrays.asList(new String[] {"foo", "bar", "baz", "foo.first", "foo.second", "foobar"}); + List setParametersResults = future.get(); + assertEquals(6, setParametersResults.size()); + for (rcl_interfaces.msg.SetParametersResult result : setParametersResults) { + assertEquals(true, result.getSuccessful()); + } + List results = node.getParameters(parameterNames); - assertEquals(parameters, future.get()); + assertEquals(parameters, results); } @Test From 884963bb29957904baa467c4b4044ba31f2e8040 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Thu, 7 Jan 2021 10:33:52 -0800 Subject: [PATCH 27/51] Use array instead of List for ROS message types (osrf/ros2_java#55) * change logic(list -> array) in msg.java.em * change logic(list -> array) in msg.cpp.em * Support getting and setting as Lists in addition to arrays Signed-off-by: Jacob Perron * Update rcljava to work with new methods for list types Signed-off-by: Jacob Perron * Update tests to work with new methods for list types Signed-off-by: Jacob Perron * Fixes to template files * Avoid nullptr access when converting from Java arrays to C arrays * Make sure Java arrays are initialized Signed-off-by: Jacob Perron * Minor refactor Signed-off-by: Jacob Perron * A note about performance to *AsList docblocks Signed-off-by: Jacob Perron Co-authored-by: pluris --- .../java/org/ros2/rcljava/node/NodeImpl.java | 12 +- .../rcljava/parameters/ParameterVariant.java | 46 +- .../client/AsyncParametersClientImpl.java | 16 +- .../service/ParameterServiceImpl.java | 8 +- .../ros2/rcljava/node/NodeParametersTest.java | 2 +- .../java/org/ros2/rcljava/node/NodeTest.java | 102 +-- .../parameters/AsyncParametersClientTest.java | 5 +- .../parameters/SyncParametersClientTest.java | 7 +- rosidl_generator_java/resource/msg.cpp.em | 175 ++--- rosidl_generator_java/resource/msg.java.em | 53 +- .../org/ros2/generator/InterfacesTest.java | 613 +++++++++++------- 11 files changed, 611 insertions(+), 428 deletions(-) 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 1527c020..687138cb 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java @@ -781,6 +781,8 @@ public rcl_interfaces.msg.ListParametersResult listParameters( rcl_interfaces.msg.ListParametersResult result = new rcl_interfaces.msg.ListParametersResult(); + List resultNames = new ArrayList(); + List resultPrefixes = new ArrayList(); synchronized (parametersMutex) { for (Map.Entry entry : this.parameters.entrySet()) { boolean getAll = @@ -805,16 +807,18 @@ public rcl_interfaces.msg.ListParametersResult listParameters( } if (getAll || prefixMatches) { - result.getNames().add(entry.getKey()); + resultNames.add(entry.getKey()); int lastSeparator = entry.getKey().lastIndexOf(separator); if (-1 != lastSeparator) { String prefix = entry.getKey().substring(0, lastSeparator); - if (!result.getPrefixes().contains(prefix)) { - result.getPrefixes().add(prefix); + if (!resultPrefixes.contains(prefix)) { + resultPrefixes.add(prefix); } } } } + result.setNames(resultNames); + result.setPrefixes(resultPrefixes); return result; } } @@ -853,7 +857,7 @@ 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); diff --git a/rcljava/src/main/java/org/ros2/rcljava/parameters/ParameterVariant.java b/rcljava/src/main/java/org/ros2/rcljava/parameters/ParameterVariant.java index 8f7484c7..3f865ae3 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/parameters/ParameterVariant.java +++ b/rcljava/src/main/java/org/ros2/rcljava/parameters/ParameterVariant.java @@ -100,38 +100,38 @@ public ParameterVariant(final String name, final String stringValue) { this.value.setType(ParameterType.PARAMETER_STRING.getValue()); } - public ParameterVariant(final String name, final Byte[] byteArrayValue) { + public ParameterVariant(final String name, final byte[] byteArrayValue) { this.name = name; this.value = new rcl_interfaces.msg.ParameterValue(); - this.value.setByteArrayValue(Arrays.asList(byteArrayValue)); + this.value.setByteArrayValue(byteArrayValue); this.value.setType(ParameterType.PARAMETER_BYTE_ARRAY.getValue()); } - public ParameterVariant(final String name, final Boolean[] boolArrayValue) { + public ParameterVariant(final String name, final boolean[] boolArrayValue) { this.name = name; this.value = new rcl_interfaces.msg.ParameterValue(); - this.value.setBoolArrayValue(Arrays.asList(boolArrayValue)); + this.value.setBoolArrayValue(boolArrayValue); this.value.setType(ParameterType.PARAMETER_BOOL_ARRAY.getValue()); } - public ParameterVariant(final String name, final Long[] integerArrayValue) { + public ParameterVariant(final String name, final long[] integerArrayValue) { this.name = name; this.value = new rcl_interfaces.msg.ParameterValue(); - this.value.setIntegerArrayValue(Arrays.asList(integerArrayValue)); + this.value.setIntegerArrayValue(integerArrayValue); this.value.setType(ParameterType.PARAMETER_INTEGER_ARRAY.getValue()); } - public ParameterVariant(final String name, final Double[] doubleArrayValue) { + public ParameterVariant(final String name, final double[] doubleArrayValue) { this.name = name; this.value = new rcl_interfaces.msg.ParameterValue(); - this.value.setDoubleArrayValue(Arrays.asList(doubleArrayValue)); + this.value.setDoubleArrayValue(doubleArrayValue); this.value.setType(ParameterType.PARAMETER_DOUBLE_ARRAY.getValue()); } public ParameterVariant(final String name, final String[] stringArrayValue) { this.name = name; this.value = new rcl_interfaces.msg.ParameterValue(); - this.value.setStringArrayValue(Arrays.asList(stringArrayValue)); + this.value.setStringArrayValue(stringArrayValue); this.value.setType(ParameterType.PARAMETER_STRING_ARRAY.getValue()); } @@ -203,39 +203,39 @@ public final boolean asBool() { return this.value.getBoolValue(); } - public final Byte[] asByteArray() { + public final byte[] asByteArray() { if (getType() != ParameterType.PARAMETER_BYTE_ARRAY) { throw new IllegalArgumentException("Invalid type"); } - return this.value.getByteArrayValue().toArray(new Byte[0]); + return this.value.getByteArrayValue(); } - public final Boolean[] asBooleanArray() { + public final boolean[] asBooleanArray() { if (getType() != ParameterType.PARAMETER_BOOL_ARRAY) { throw new IllegalArgumentException("Invalid type"); } - return this.value.getBoolArrayValue().toArray(new Boolean[0]); + return this.value.getBoolArrayValue(); } - public final Long[] asIntegerArray() { + public final long[] asIntegerArray() { if (getType() != ParameterType.PARAMETER_INTEGER_ARRAY) { throw new IllegalArgumentException("Invalid type"); } - return this.value.getIntegerArrayValue().toArray(new Long[0]); + return this.value.getIntegerArrayValue(); } - public final Double[] asDoubleArray() { + public final double[] asDoubleArray() { if (getType() != ParameterType.PARAMETER_DOUBLE_ARRAY) { throw new IllegalArgumentException("Invalid type"); } - return this.value.getDoubleArrayValue().toArray(new Double[0]); + return this.value.getDoubleArrayValue(); } public final String[] asStringArray() { if (getType() != ParameterType.PARAMETER_STRING_ARRAY) { throw new IllegalArgumentException("Invalid type"); } - return this.value.getStringArrayValue().toArray(new String[0]); + return this.value.getStringArrayValue(); } public final rcl_interfaces.msg.Parameter toParameter() { @@ -256,15 +256,15 @@ public static ParameterVariant fromParameter(final rcl_interfaces.msg.Parameter case rcl_interfaces.msg.ParameterType.PARAMETER_STRING: return new ParameterVariant(parameter.getName(), parameter.getValue().getStringValue()); case rcl_interfaces.msg.ParameterType.PARAMETER_BYTE_ARRAY: - return new ParameterVariant(parameter.getName(), parameter.getValue().getByteArrayValue().toArray(new Byte[0])); + return new ParameterVariant(parameter.getName(), parameter.getValue().getByteArrayValue()); case rcl_interfaces.msg.ParameterType.PARAMETER_BOOL_ARRAY: - return new ParameterVariant(parameter.getName(), parameter.getValue().getBoolArrayValue().toArray(new Boolean[0])); + return new ParameterVariant(parameter.getName(), parameter.getValue().getBoolArrayValue()); case rcl_interfaces.msg.ParameterType.PARAMETER_INTEGER_ARRAY: - return new ParameterVariant(parameter.getName(), parameter.getValue().getIntegerArrayValue().toArray(new Long[0])); + return new ParameterVariant(parameter.getName(), parameter.getValue().getIntegerArrayValue()); case rcl_interfaces.msg.ParameterType.PARAMETER_DOUBLE_ARRAY: - return new ParameterVariant(parameter.getName(), parameter.getValue().getDoubleArrayValue().toArray(new Double[0])); + return new ParameterVariant(parameter.getName(), parameter.getValue().getDoubleArrayValue()); case rcl_interfaces.msg.ParameterType.PARAMETER_STRING_ARRAY: - return new ParameterVariant(parameter.getName(), parameter.getValue().getStringArrayValue().toArray(new String[0])); + return new ParameterVariant(parameter.getName(), parameter.getValue().getStringArrayValue()); case rcl_interfaces.msg.ParameterType.PARAMETER_NOT_SET: throw new IllegalArgumentException("Type from ParameterValue is not set"); default: diff --git a/rcljava/src/main/java/org/ros2/rcljava/parameters/client/AsyncParametersClientImpl.java b/rcljava/src/main/java/org/ros2/rcljava/parameters/client/AsyncParametersClientImpl.java index 6d60c226..bfd01708 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/parameters/client/AsyncParametersClientImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/parameters/client/AsyncParametersClientImpl.java @@ -114,16 +114,16 @@ public Future> getParameters( request, new Consumer>() { public void accept(final Future future) { List parameterVariants = new ArrayList(); - List pvalues = null; + rcl_interfaces.msg.ParameterValue[] pvalues = new rcl_interfaces.msg.ParameterValue[0]; try { pvalues = future.get().getValues(); } catch (Exception e) { // TODO(esteve): do something } - for (int i = 0; i < pvalues.size(); i++) { + for (int i = 0; i < pvalues.length; i++) { rcl_interfaces.msg.Parameter parameter = new rcl_interfaces.msg.Parameter(); - parameter.setName(request.getNames().get(i)); - parameter.setValue(pvalues.get(i)); + parameter.setName(request.getNames()[i]); + parameter.setValue(pvalues[i]); parameterVariants.add(ParameterVariant.fromParameter(parameter)); } futureResult.set(parameterVariants); @@ -151,13 +151,13 @@ public Future> getParameterTypes( request, new Consumer>() { public void accept(final Future future) { List parameterTypes = new ArrayList(); - List pts = null; + byte[] pts = new byte[0]; try { pts = future.get().getTypes(); } catch (Exception e) { // TODO(esteve): do something } - for (Byte pt : pts) { + for (byte pt : pts) { parameterTypes.add(ParameterType.fromByte(pt)); } futureResult.set(parameterTypes); @@ -194,7 +194,7 @@ public Future> setParameters( public void accept(final Future future) { List setParametersResult = null; try { - setParametersResult = future.get().getResults(); + setParametersResult = future.get().getResultsAsList(); } catch (Exception e) { // TODO(esteve): do something } @@ -297,7 +297,7 @@ public Future> describeParameters( public void accept(final Future future) { List parameterDescriptors = null; try { - parameterDescriptors = future.get().getDescriptors(); + parameterDescriptors = future.get().getDescriptorsAsList(); } catch (Exception e) { // TODO(esteve): do something } diff --git a/rcljava/src/main/java/org/ros2/rcljava/parameters/service/ParameterServiceImpl.java b/rcljava/src/main/java/org/ros2/rcljava/parameters/service/ParameterServiceImpl.java index c5baa7c0..9d65ddd7 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/parameters/service/ParameterServiceImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/parameters/service/ParameterServiceImpl.java @@ -48,7 +48,7 @@ public ParameterServiceImpl(final Node node, final QoSProfile qosProfile) public void accept(RMWRequestId rmwRequestId, rcl_interfaces.srv.GetParameters_Request request, rcl_interfaces.srv.GetParameters_Response response) { - List values = node.getParameters(request.getNames()); + List values = node.getParameters(request.getNamesAsList()); List pvalues = new ArrayList(); for (ParameterVariant pvariant : values) { @@ -67,7 +67,7 @@ public void accept(RMWRequestId rmwRequestId, public void accept(RMWRequestId rmwRequestId, rcl_interfaces.srv.GetParameterTypes_Request request, rcl_interfaces.srv.GetParameterTypes_Response response) { - List types = node.getParameterTypes(request.getNames()); + List types = node.getParameterTypes(request.getNamesAsList()); List ptypes = new ArrayList(); for (ParameterType type : types) { ptypes.add(type.getValue()); @@ -124,7 +124,7 @@ public void accept(RMWRequestId rmwRequestId, rcl_interfaces.srv.DescribeParameters_Request request, rcl_interfaces.srv.DescribeParameters_Response response) { List descriptors = - node.describeParameters(request.getNames()); + node.describeParameters(request.getNamesAsList()); response.setDescriptors(descriptors); } }, @@ -139,7 +139,7 @@ public void accept(RMWRequestId rmwRequestId, rcl_interfaces.srv.ListParameters_Request request, rcl_interfaces.srv.ListParameters_Response response) { rcl_interfaces.msg.ListParametersResult result = - node.listParameters(request.getPrefixes(), request.getDepth()); + node.listParameters(request.getPrefixesAsList(), request.getDepth()); response.setResult(result); } }, diff --git a/rcljava/src/test/java/org/ros2/rcljava/node/NodeParametersTest.java b/rcljava/src/test/java/org/ros2/rcljava/node/NodeParametersTest.java index ccf1b697..5843b359 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/node/NodeParametersTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/node/NodeParametersTest.java @@ -350,7 +350,7 @@ public final void testListParameters() { rcl_interfaces.msg.ListParametersResult result = node.listParameters(new ArrayList(), rcl_interfaces.srv.ListParameters_Request.DEPTH_RECURSIVE); // 2 we added above plus the implicit "use_sim_time" - assertEquals(3, result.getNames().size()); + assertEquals(3, result.getNames().length); } @Test 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 926f5b3b..f810abf7 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java @@ -273,8 +273,8 @@ public final void testPubSubBoundedArrayNested() throws Exception { rcljava.msg.BoundedArrayNested value = future.get(); assertNotEquals(null, value.getPrimitiveValues()); - rcljava.msg.Primitives primitivesValue1 = value.getPrimitiveValues().get(0); - rcljava.msg.Primitives primitivesValue2 = value.getPrimitiveValues().get(1); + rcljava.msg.Primitives primitivesValue1 = value.getPrimitiveValues()[0]; + rcljava.msg.Primitives primitivesValue2 = value.getPrimitiveValues()[1]; assertTrue(checkPrimitives(primitivesValue1, boolValue1, byteValue1, charValue1, float32Value1, float64Value1, int8Value1, uint8Value1, int16Value1, uint16Value1, int32Value1, @@ -343,20 +343,20 @@ public final void testPubSubBoundedArrayPrimitives() throws Exception { rcljava.msg.BoundedArrayPrimitives value = future.get(); - assertEquals(boolValues, value.getBoolValues()); - assertEquals(byteValues, value.getByteValues()); - assertEquals(charValues, value.getCharValues()); - assertEquals(float32Values, value.getFloat32Values()); - assertEquals(float64Values, value.getFloat64Values()); - assertEquals(int8Values, value.getInt8Values()); - assertEquals(uint8Values, value.getUint8Values()); - assertEquals(int16Values, value.getInt16Values()); - assertEquals(uint16Values, value.getUint16Values()); - assertEquals(int32Values, value.getInt32Values()); - assertEquals(uint32Values, value.getUint32Values()); - assertEquals(int64Values, value.getInt64Values()); - assertEquals(uint64Values, value.getUint64Values()); - assertEquals(stringValues, value.getStringValues()); + assertEquals(boolValues, value.getBoolValuesAsList()); + assertEquals(byteValues, value.getByteValuesAsList()); + assertEquals(charValues, value.getCharValuesAsList()); + assertEquals(float32Values, value.getFloat32ValuesAsList()); + assertEquals(float64Values, value.getFloat64ValuesAsList()); + assertEquals(int8Values, value.getInt8ValuesAsList()); + assertEquals(uint8Values, value.getUint8ValuesAsList()); + assertEquals(int16Values, value.getInt16ValuesAsList()); + assertEquals(uint16Values, value.getUint16ValuesAsList()); + assertEquals(int32Values, value.getInt32ValuesAsList()); + assertEquals(uint32Values, value.getUint32ValuesAsList()); + assertEquals(int64Values, value.getInt64ValuesAsList()); + assertEquals(uint64Values, value.getUint64ValuesAsList()); + assertEquals(stringValues, value.getStringValuesAsList()); publisher.dispose(); assertEquals(0, publisher.getHandle()); @@ -435,8 +435,8 @@ public final void testPubSubDynamicArrayNested() throws Exception { rcljava.msg.DynamicArrayNested value = future.get(); assertNotEquals(null, value.getPrimitiveValues()); - rcljava.msg.Primitives primitivesValue1 = value.getPrimitiveValues().get(0); - rcljava.msg.Primitives primitivesValue2 = value.getPrimitiveValues().get(1); + rcljava.msg.Primitives primitivesValue1 = value.getPrimitiveValues()[0]; + rcljava.msg.Primitives primitivesValue2 = value.getPrimitiveValues()[1]; assertTrue(checkPrimitives(primitivesValue1, boolValue1, byteValue1, charValue1, float32Value1, float64Value1, int8Value1, uint8Value1, int16Value1, uint16Value1, int32Value1, @@ -505,20 +505,20 @@ public final void testPubSubDynamicArrayPrimitives() throws Exception { rcljava.msg.DynamicArrayPrimitives value = future.get(); - assertEquals(boolValues, value.getBoolValues()); - assertEquals(byteValues, value.getByteValues()); - assertEquals(charValues, value.getCharValues()); - assertEquals(float32Values, value.getFloat32Values()); - assertEquals(float64Values, value.getFloat64Values()); - assertEquals(int8Values, value.getInt8Values()); - assertEquals(uint8Values, value.getUint8Values()); - assertEquals(int16Values, value.getInt16Values()); - assertEquals(uint16Values, value.getUint16Values()); - assertEquals(int32Values, value.getInt32Values()); - assertEquals(uint32Values, value.getUint32Values()); - assertEquals(int64Values, value.getInt64Values()); - assertEquals(uint64Values, value.getUint64Values()); - assertEquals(stringValues, value.getStringValues()); + assertEquals(boolValues, value.getBoolValuesAsList()); + assertEquals(byteValues, value.getByteValuesAsList()); + assertEquals(charValues, value.getCharValuesAsList()); + assertEquals(float32Values, value.getFloat32ValuesAsList()); + assertEquals(float64Values, value.getFloat64ValuesAsList()); + assertEquals(int8Values, value.getInt8ValuesAsList()); + assertEquals(uint8Values, value.getUint8ValuesAsList()); + assertEquals(int16Values, value.getInt16ValuesAsList()); + assertEquals(uint16Values, value.getUint16ValuesAsList()); + assertEquals(int32Values, value.getInt32ValuesAsList()); + assertEquals(uint32Values, value.getUint32ValuesAsList()); + assertEquals(int64Values, value.getInt64ValuesAsList()); + assertEquals(uint64Values, value.getUint64ValuesAsList()); + assertEquals(stringValues, value.getStringValuesAsList()); publisher.dispose(); assertEquals(0, publisher.getHandle()); @@ -689,12 +689,12 @@ public final void testPubSubStaticArrayNested() throws Exception { rcljava.msg.StaticArrayNested value = future.get(); assertNotEquals(null, value.getPrimitiveValues()); - assertEquals(4, value.getPrimitiveValues().size()); + assertEquals(4, value.getPrimitiveValues().length); - rcljava.msg.Primitives primitivesValue1 = value.getPrimitiveValues().get(0); - rcljava.msg.Primitives primitivesValue2 = value.getPrimitiveValues().get(1); - rcljava.msg.Primitives primitivesValue3 = value.getPrimitiveValues().get(2); - rcljava.msg.Primitives primitivesValue4 = value.getPrimitiveValues().get(3); + rcljava.msg.Primitives primitivesValue1 = value.getPrimitiveValues()[0]; + rcljava.msg.Primitives primitivesValue2 = value.getPrimitiveValues()[1]; + rcljava.msg.Primitives primitivesValue3 = value.getPrimitiveValues()[2]; + rcljava.msg.Primitives primitivesValue4 = value.getPrimitiveValues()[3]; assertTrue(checkPrimitives(primitivesValue1, boolValue1, byteValue1, charValue1, float32Value1, float64Value1, int8Value1, uint8Value1, int16Value1, uint16Value1, int32Value1, @@ -772,20 +772,20 @@ public final void testPubSubStaticArrayPrimitives() throws Exception { rcljava.msg.StaticArrayPrimitives value = future.get(); - assertEquals(boolValues, value.getBoolValues()); - assertEquals(byteValues, value.getByteValues()); - assertEquals(charValues, value.getCharValues()); - assertEquals(float32Values, value.getFloat32Values()); - assertEquals(float64Values, value.getFloat64Values()); - assertEquals(int8Values, value.getInt8Values()); - assertEquals(uint8Values, value.getUint8Values()); - assertEquals(int16Values, value.getInt16Values()); - assertEquals(uint16Values, value.getUint16Values()); - assertEquals(int32Values, value.getInt32Values()); - assertEquals(uint32Values, value.getUint32Values()); - assertEquals(int64Values, value.getInt64Values()); - assertEquals(uint64Values, value.getUint64Values()); - assertEquals(stringValues, value.getStringValues()); + assertEquals(boolValues, value.getBoolValuesAsList()); + assertEquals(byteValues, value.getByteValuesAsList()); + assertEquals(charValues, value.getCharValuesAsList()); + assertEquals(float32Values, value.getFloat32ValuesAsList()); + assertEquals(float64Values, value.getFloat64ValuesAsList()); + assertEquals(int8Values, value.getInt8ValuesAsList()); + assertEquals(uint8Values, value.getUint8ValuesAsList()); + assertEquals(int16Values, value.getInt16ValuesAsList()); + assertEquals(uint16Values, value.getUint16ValuesAsList()); + assertEquals(int32Values, value.getInt32ValuesAsList()); + assertEquals(uint32Values, value.getUint32ValuesAsList()); + assertEquals(int64Values, value.getInt64ValuesAsList()); + assertEquals(uint64Values, value.getUint64ValuesAsList()); + assertEquals(stringValues, value.getStringValuesAsList()); publisher.dispose(); assertEquals(0, publisher.getHandle()); diff --git a/rcljava/src/test/java/org/ros2/rcljava/parameters/AsyncParametersClientTest.java b/rcljava/src/test/java/org/ros2/rcljava/parameters/AsyncParametersClientTest.java index c0ce876a..df2f749f 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/parameters/AsyncParametersClientTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/parameters/AsyncParametersClientTest.java @@ -15,6 +15,7 @@ package org.ros2.rcljava.parameters; +import static org.junit.Assert.assertArrayEquals; import static org.junit.Assert.assertEquals; import static org.junit.Assert.assertNotEquals; import static org.junit.Assert.assertTrue; @@ -162,8 +163,8 @@ public final void testListParameters() throws Exception { parametersClient.listParameters( Arrays.asList(new String[] {"foo", "bar"}), 10, new TestConsumer(future)); - assertEquals(Arrays.asList(new String[] {"foo.first", "foo.second"}), future.get().getNames()); - assertEquals(Arrays.asList(new String[] {"foo"}), future.get().getPrefixes()); + assertArrayEquals(new String[] {"foo.first", "foo.second"}, future.get().getNames()); + assertArrayEquals(new String[] {"foo"}, future.get().getPrefixes()); } @Test diff --git a/rcljava/src/test/java/org/ros2/rcljava/parameters/SyncParametersClientTest.java b/rcljava/src/test/java/org/ros2/rcljava/parameters/SyncParametersClientTest.java index d44c566a..818314f8 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/parameters/SyncParametersClientTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/parameters/SyncParametersClientTest.java @@ -15,6 +15,7 @@ package org.ros2.rcljava.parameters; +import static org.junit.Assert.assertArrayEquals; import static org.junit.Assert.assertEquals; import static org.junit.Assert.assertNotEquals; import static org.junit.Assert.assertTrue; @@ -133,9 +134,9 @@ public final void testListParameters() throws Exception { rcl_interfaces.msg.ListParametersResult parametersAndPrefixes = parametersClient.listParameters(Arrays.asList(new String[] {"foo", "bar"}), 10); - assertEquals( - parametersAndPrefixes.getNames(), Arrays.asList(new String[] {"foo.first", "foo.second"})); - assertEquals(parametersAndPrefixes.getPrefixes(), Arrays.asList(new String[] {"foo"})); + assertArrayEquals( + parametersAndPrefixes.getNames(), new String[] {"foo.first", "foo.second"}); + assertEquals(parametersAndPrefixes.getPrefixes(), new String[] {"foo"}); } @Test diff --git a/rosidl_generator_java/resource/msg.cpp.em b/rosidl_generator_java/resource/msg.cpp.em index a7008a53..733b0513 100644 --- a/rosidl_generator_java/resource/msg.cpp.em +++ b/rosidl_generator_java/resource/msg.cpp.em @@ -22,15 +22,6 @@ from rosidl_parser.definition import NamespacedType msg_normalized_type = '__'.join(message.structure.namespaced_type.namespaced_name()) msg_jni_type = '/'.join(message.structure.namespaced_type.namespaced_name()) -list_java_type = "java.util.List" -array_list_java_type = "java.util.ArrayList" - -list_normalized_type = "java__util__List" -list_jni_type = "java/util/List" - -array_list_normalized_type = "java__util__ArrayList" -array_list_jni_type = "java/util/ArrayList" - # Collect JNI types and includes cache = defaultdict(lambda: False) cache[msg_normalized_type] = msg_jni_type @@ -39,8 +30,6 @@ member_includes = set() for member in message.structure.members: type_ = member.type if isinstance(type_, AbstractNestedType): - cache[list_normalized_type] = list_jni_type - cache[array_list_normalized_type] = array_list_jni_type type_ = type_.value_type if isinstance(type_, BasicType): member_includes.add('rosidl_runtime_c/primitives_sequence.h') @@ -220,32 +209,46 @@ JNIEXPORT jlong JNICALL Java_@(underscore_separated_jni_type_name)_getDestructor } @[for member in message.structure.members]@ @{ -normalized_type = get_normalized_type(member.type) +base_type = member.type.value_type if isinstance(member.type, AbstractNestedType) else member.type +normalized_type = get_normalized_type(base_type) +get_java_name = get_java_type(base_type, use_primitives=True) +get_method_name = get_java_name.capitalize() +jni_signature = get_jni_signature(base_type) }@ @[ if isinstance(member.type, AbstractNestedType)] - auto _jfield_@(member.name)_fid = env->GetFieldID(_j@(msg_normalized_type)_class_global, "@(member.name)", "L@(list_jni_type);"); - jobject _jlist_@(member.name)_object = env->GetObjectField(_jmessage_obj, _jfield_@(member.name)_fid); +@[ if isinstance(member.type.value_type, BasicType)]@ + auto _jfield_@(member.name)_fid = env->GetFieldID(_j@(msg_normalized_type)_class_global, "@(member.name)", "[@(jni_signature)"); + j@(get_java_name)Array _jarray_@(member.name)_obj = (j@(get_java_name)Array)env->GetObjectField(_jmessage_obj, _jfield_@(member.name)_fid); +@[ elif isinstance(member.type.value_type, AbstractGenericString)]@ + auto _jfield_@(member.name)_fid = env->GetFieldID(_j@(msg_normalized_type)_class_global, "@(member.name)", "[Ljava/lang/String;"); + jobjectArray _jarray_@(member.name)_obj = (jobjectArray)env->GetObjectField(_jmessage_obj, _jfield_@(member.name)_fid); +@[ else]@ + auto _jfield_@(member.name)_fid = env->GetFieldID(_j@(msg_normalized_type)_class_global, "@(member.name)", "[L@('/'.join(member.type.value_type.namespaced_name()));"); + jobjectArray _jarray_@(member.name)_obj = (jobjectArray)env->GetObjectField(_jmessage_obj, _jfield_@(member.name)_fid); +@[ end if]@ - if (_jlist_@(member.name)_object != nullptr) { - jmethodID _jlist_@(member.name)_get_mid = env->GetMethodID(_j@(list_normalized_type)_class_global, "get", "(I)Ljava/lang/Object;"); + if (_jarray_@(member.name)_obj != nullptr) { @[ if isinstance(member.type, AbstractSequence)]@ - jmethodID _jlist_@(member.name)_size_mid = env->GetMethodID(_j@(list_normalized_type)_class_global, "size", "()I"); - jint _jlist_@(member.name)_size = env->CallIntMethod(_jlist_@(member.name)_object, _jlist_@(member.name)_size_mid); +@[ if isinstance(member.type, Array)]@ + jint _jarray_@(member.name)_size = @(member.type.size); +@[ else]@ + jint _jarray_@(member.name)_size = env->GetArrayLength(_jarray_@(member.name)_obj); +@[ end if]@ @[ if isinstance(member.type.value_type, AbstractString)]@ - if (!rosidl_runtime_c__String__Sequence__init(&(ros_message->@(member.name)), _jlist_@(member.name)_size)) { + if (!rosidl_runtime_c__String__Sequence__init(&(ros_message->@(member.name)), _jarray_@(member.name)_size)) { rcljava_throw_exception(env, "java/lang/IllegalStateException", "unable to create String__Array ros_message"); } @[ elif isinstance(member.type.value_type, AbstractWString)]@ - if (!rosidl_runtime_c__U16String__Sequence__init(&(ros_message->@(member.name)), _jlist_@(member.name)_size)) { + if (!rosidl_runtime_c__U16String__Sequence__init(&(ros_message->@(member.name)), _jarray_@(member.name)_size)) { rcljava_throw_exception(env, "java/lang/IllegalStateException", "unable to create U16String__Array ros_message"); } @[ else]@ @[ if isinstance(member.type.value_type, BasicType)]@ - if (!rosidl_runtime_c__@(member.type.value_type.typename)__Sequence__init(&(ros_message->@(member.name)), _jlist_@(member.name)_size)) { + if (!rosidl_runtime_c__@(member.type.value_type.typename)__Sequence__init(&(ros_message->@(member.name)), _jarray_@(member.name)_size)) { rcljava_throw_exception(env, "java/lang/IllegalStateException", "unable to create @(member.type.value_type)__Array ros_message"); } @[ else]@ - if (!@('__'.join(member.type.value_type.namespaced_name()))__Sequence__init(&(ros_message->@(member.name)), _jlist_@(member.name)_size)) { + if (!@('__'.join(member.type.value_type.namespaced_name()))__Sequence__init(&(ros_message->@(member.name)), _jarray_@(member.name)_size)) { rcljava_throw_exception(env, "java/lang/IllegalStateException", "unable to create @(member.type.value_type)__Array ros_message"); } @@ -253,39 +256,43 @@ normalized_type = get_normalized_type(member.type) @[ end if]@ auto _dest_@(member.name) = ros_message->@(member.name).data; @[ else]@ - jint _jlist_@(member.name)_size = @(member.type.size); + jint _jarray_@(member.name)_size = @(member.type.size); auto _dest_@(member.name) = ros_message->@(member.name); @[ end if]@ - - for (jint i = 0; i < _jlist_@(member.name)_size; ++i) { - auto element = env->CallObjectMethod(_jlist_@(member.name)_object, _jlist_@(member.name)_get_mid, i); -@[ if isinstance(member.type.value_type, AbstractString)]@ +@[ if isinstance(member.type.value_type, BasicType)]@ + j@(get_java_name) *_jarray_@(member.name)_ptr = env->Get@(get_method_name)ArrayElements(_jarray_@(member.name)_obj, nullptr); + std::copy(_jarray_@(member.name)_ptr, _jarray_@(member.name)_ptr + _jarray_@(member.name)_size, _dest_@(member.name)); + env->Release@(get_method_name)ArrayElements(_jarray_@(member.name)_obj, _jarray_@(member.name)_ptr, 0); +@[ else]@ + for (jint i = 0; i < _jarray_@(member.name)_size; ++i) { + auto element = env->GetObjectArrayElement(_jarray_@(member.name)_obj, i); + if (element == nullptr) { + continue; + } +@[ if isinstance(member.type.value_type, AbstractString)]@ jstring _jfield_@(member.name)_value = static_cast(element); if (_jfield_@(member.name)_value != nullptr) { - const char * _str@(member.name) = env->GetStringUTFChars(_jfield_@(member.name)_value, 0); + const char * _str_@(member.name) = env->GetStringUTFChars(_jfield_@(member.name)_value, 0); rosidl_runtime_c__String__assign( - &_dest_@(member.name)[i], _str@(member.name)); - env->ReleaseStringUTFChars(_jfield_@(member.name)_value, _str@(member.name)); + &_dest_@(member.name)[i], _str_@(member.name)); + env->ReleaseStringUTFChars(_jfield_@(member.name)_value, _str_@(member.name)); } -@[ elif isinstance(member.type.value_type, AbstractWString)]@ +@[ elif isinstance(member.type.value_type, AbstractWString)]@ jstring _jfield_@(member.name)_value = static_cast(element); if (_jfield_@(member.name)_value != nullptr) { - const jchar * _str@(member.name) = env->GetStringChars(_jfield_@(member.name)_value, 0); + const jchar * _str_@(member.name) = env->GetStringChars(_jfield_@(member.name)_value, 0); rosidl_runtime_c__U16String__assign( - &_dest_@(member.name)[i], _str@(member.name)); - env->ReleaseStringChars(_jfield_@(member.name)_value, _str@(member.name)); + &_dest_@(member.name)[i], _str_@(member.name)); + env->ReleaseStringChars(_jfield_@(member.name)_value, _str_@(member.name)); } -@[ elif isinstance(member.type.value_type, BasicType)]@ -@{ -call_method_name = 'Call%sMethod' % get_java_type(member.type.value_type, use_primitives=True).capitalize() -}@ - _dest_@(member.name)[i] = env->@(call_method_name)(element, _j@(normalized_type)_value_global); -@[ else]@ +@[ else]@ _dest_@(member.name)[i] = *_j@(normalized_type)_from_java_function(element, nullptr); -@[ end if]@ +@[ end if]@ env->DeleteLocalRef(element); } +@[ end if]@ + env->DeleteLocalRef(_jarray_@(member.name)_obj); } @[ else]@ @[ if isinstance(member.type, AbstractGenericString)]@ @@ -349,61 +356,71 @@ jobject @(underscore_separated_type_name)__convert_to_java(@(msg_normalized_type } @[for member in message.structure.members]@ @{ -normalized_type = get_normalized_type(member.type) +base_type = member.type.value_type if isinstance(member.type, AbstractNestedType) else member.type +normalized_type = get_normalized_type(base_type) +get_java_name = get_java_type(base_type, use_primitives=True) +get_method_name = get_java_name.capitalize() +jni_signature = get_jni_signature(base_type) }@ @[ if isinstance(member.type, AbstractNestedType)]@ -@[ if isinstance(member.type.value_type, (BasicType, AbstractGenericString))]@ - auto _jfield_@(member.name)_fid = env->GetFieldID(_j@(msg_normalized_type)_class_global, "@(member.name)", "L@(list_jni_type);"); - jobject _jarray_list_@(member.name)_obj = env->NewObject(_j@(array_list_normalized_type)_class_global, _j@(array_list_normalized_type)_constructor_global); - // TODO(esteve): replace ArrayList with a jobjectArray to initialize the array beforehand - jmethodID _jlist_@(member.name)_add_mid = env->GetMethodID(_j@(array_list_normalized_type)_class_global, "add", "(Ljava/lang/Object;)Z"); +@[ if isinstance(member.type.value_type, BasicType)]@ + auto _jfield_@(member.name)_fid = env->GetFieldID(_j@(msg_normalized_type)_class_global, "@(member.name)", "[@(jni_signature)"); +@[ elif isinstance(member.type.value_type, AbstractGenericString)]@ + auto _jfield_@(member.name)_fid = env->GetFieldID(_j@(msg_normalized_type)_class_global, "@(member.name)", "[Ljava/lang/String;"); +@[ else]@ + auto _jfield_@(member.name)_fid = env->GetFieldID(_j@(msg_normalized_type)_class_global, "@(member.name)", "[L@('/'.join(member.type.value_type.namespaced_name()));"); +@[ end if]@ + +@[ if isinstance(member.type.value_type, BasicType)]@ @[ if isinstance(member.type, Array)]@ - for (size_t i = 0; i < @(member.type.size); ++i) { + j@(get_java_name)Array _jarray_@(member.name)_obj = env->New@(get_method_name)Array(@(member.type.size)); + j@(get_java_name) *_j@(get_java_name)_@(member.name)_buf = (j@(get_java_name) *)malloc(sizeof(j@(get_java_name)) * @(member.type.size)); + std::copy(_ros_message->@(member.name), _ros_message->@(member.name) + @(member.type.size), _j@(get_java_name)_@(member.name)_buf); + env->Set@(get_method_name)ArrayRegion(_jarray_@(member.name)_obj, 0, @(member.type.size), (const j@(get_java_name) *)_j@(get_java_name)_@(member.name)_buf); +@[ else]@ + j@(get_java_name)Array _jarray_@(member.name)_obj = env->New@(get_method_name)Array(_ros_message->@(member.name).size); + j@(get_java_name) *_j@(get_java_name)_@(member.name)_buf = (j@(get_java_name) *)malloc(sizeof(j@(get_java_name)) * _ros_message->@(member.name).size); + std::copy(_ros_message->@(member.name).data, _ros_message->@(member.name).data + _ros_message->@(member.name).size, _j@(get_java_name)_@(member.name)_buf); + env->Set@(get_method_name)ArrayRegion(_jarray_@(member.name)_obj, 0, _ros_message->@(member.name).size, (const j@(get_java_name) *)_j@(get_java_name)_@(member.name)_buf); +@[ end if]@ + free(_j@(get_java_name)_@(member.name)_buf); +@[ elif isinstance(member.type.value_type, AbstractGenericString)]@ +@[ if isinstance(member.type, Array)]@ + jobjectArray _jarray_@(member.name)_obj = (jobjectArray)env->NewObjectArray(@(member.type.size), env->FindClass("java/lang/String"), NULL); + for (size_t i = 0; i < @(member.type.size); i++) { auto _ros_@(member.name)_element = _ros_message->@(member.name)[i]; @[ else]@ - for (size_t i = 0; i < _ros_message->@(member.name).size; ++i) { + jobjectArray _jarray_@(member.name)_obj = (jobjectArray)env->NewObjectArray(_ros_message->@(member.name).size, env->FindClass("java/lang/String"), NULL); + for (size_t i = 0; i < _ros_message->@(member.name).size; i++) { auto _ros_@(member.name)_element = _ros_message->@(member.name).data[i]; @[ end if]@ -@[ if isinstance(member.type.value_type, AbstractGenericString)]@ - jobject _jlist_@(member.name)_element = nullptr; + jstring _jarray_@(member.name)_element = nullptr; if (_ros_@(member.name)_element.data != nullptr) { -@[ if isinstance(member.type.value_type, AbstractString)]@ - _jlist_@(member.name)_element = env->NewStringUTF(_ros_@(member.name)_element.data); -@[ else]@ - _jlist_@(member.name)_element = env->NewString(_ros_@(member.name)_element.data, _ros_@(member.name)_element.size); -@[ end if]@ - } +@[ if isinstance(member.type.value_type, AbstractString)]@ + _jarray_@(member.name)_element = env->NewStringUTF(_ros_@(member.name)_element.data); @[ else]@ - jobject _jlist_@(member.name)_element = env->NewObject( - _j@(normalized_type)_class_global, _j@(normalized_type)_constructor_global, _ros_@(member.name)_element); -@[ end if]@ - if (_jlist_@(member.name)_element != nullptr) { - jboolean _jlist_@(member.name)_add_result = env->CallBooleanMethod(_jarray_list_@(member.name)_obj, _jlist_@(member.name)_add_mid, _jlist_@(member.name)_element); - assert(_jlist_@(member.name)_add_result); + _jarray_@(member.name)_element = env->NewString(_ros_@(member.name)_element.data, _ros_@(member.name)_element.size); +@[ end if]@ } + env->SetObjectArrayElement(_jarray_@(member.name)_obj, i, _jarray_@(member.name)_element); + env->DeleteLocalRef(_jarray_@(member.name)_element); } @[ else]@ - - auto _jfield_@(member.name)_fid = env->GetFieldID(_j@(msg_normalized_type)_class_global, "@(member.name)", "L@(list_jni_type);"); - jobject _jarray_list_@(member.name)_obj = env->NewObject(_j@(array_list_normalized_type)_class_global, _j@(array_list_normalized_type)_constructor_global); - // TODO(esteve): replace ArrayList with a jobjectArray to initialize the array beforehand - jmethodID _jlist_@(member.name)_add_mid = env->GetMethodID(_j@(array_list_normalized_type)_class_global, "add", "(Ljava/lang/Object;)Z"); - @[ if isinstance(member.type, Array)]@ - for (size_t i = 0; i < @(member.type.size); ++i) { - jobject _jlist_@(member.name)_element = _j@(normalized_type)_to_java_function(&(_ros_message->@(member.name)[i]), nullptr); + jobjectArray _jarray_@(member.name)_obj = (jobjectArray)env->NewObjectArray(@(member.type.size), _j@(normalized_type)_class_global, NULL); + for (size_t i = 0; i < @(member.type.size); i++) { + jobject _jarray_@(member.name)_element = _j@(normalized_type)_to_java_function(&(_ros_message->@(member.name)[i]), nullptr); @[ else]@ - for (size_t i = 0; i < _ros_message->@(member.name).size; ++i) { - jobject _jlist_@(member.name)_element = _j@(normalized_type)_to_java_function(&(_ros_message->@(member.name).data[i]), nullptr); + jobjectArray _jarray_@(member.name)_obj = (jobjectArray)env->NewObjectArray(_ros_message->@(member.name).size, _j@(normalized_type)_class_global, NULL); + for (size_t i = 0; i < _ros_message->@(member.name).size; i++) { + jobject _jarray_@(member.name)_element = _j@(normalized_type)_to_java_function(&(_ros_message->@(member.name).data[i]), nullptr); @[ end if]@ - if (_jlist_@(member.name)_element != nullptr) { - jboolean _jlist_@(member.name)_add_result = env->CallBooleanMethod(_jarray_list_@(member.name)_obj, _jlist_@(member.name)_add_mid, _jlist_@(member.name)_element); - assert(_jlist_@(member.name)_add_result); - } + env->SetObjectArrayElement(_jarray_@(member.name)_obj, i, _jarray_@(member.name)_element); + env->DeleteLocalRef(_jarray_@(member.name)_element); } @[ end if]@ - env->SetObjectField(_jmessage_obj, _jfield_@(member.name)_fid, _jarray_list_@(member.name)_obj); - env->DeleteLocalRef(_jarray_list_@(member.name)_obj); + env->SetObjectField(_jmessage_obj, _jfield_@(member.name)_fid, _jarray_@(member.name)_obj); + env->DeleteLocalRef(_jarray_@(member.name)_obj); @[ else]@ @[ if isinstance(member.type, AbstractGenericString)]@ auto _jfield_@(member.name)_fid = env->GetFieldID(_j@(msg_normalized_type)_class_global, "@(member.name)", "Ljava/lang/String;"); diff --git a/rosidl_generator_java/resource/msg.java.em b/rosidl_generator_java/resource/msg.java.em index cb6e1b3a..c8912488 100644 --- a/rosidl_generator_java/resource/msg.java.em +++ b/rosidl_generator_java/resource/msg.java.em @@ -80,15 +80,29 @@ public final class @(type_name) implements MessageDefinition { @[ if isinstance(member.type, AbstractNestedType)]@ @[ if member.has_annotation('default')]@ - private java.util.List<@(get_java_type(member.type, use_primitives=False))> @(member.name) = java.util.Arrays.asList(new @(get_java_type(member.type, use_primitives=False))[] @(value_to_java(member.type, member.get_annotation_value('default')['value']))); + private @(get_java_type(member.type))[] @(member.name) = new @(get_java_type(member.type))[] @(value_to_java(member.type, member.get_annotation_value('default')['value'])); @[ else]@ @[ if isinstance(member.type, Array)]@ - private java.util.List<@(get_java_type(member.type, use_primitives=False))> @(member.name); + private @(get_java_type(member.type))[] @(member.name) = new @(get_java_type(member.type))[@(member.type.size)]; @[ else]@ - private java.util.List<@(get_java_type(member.type, use_primitives=False))> @(member.name) = new java.util.ArrayList<@(get_java_type(member.type, use_primitives=False))>(); + private @(get_java_type(member.type))[] @(member.name) = new @(get_java_type(member.type))[]{}; @[ end if]@ @[ end if]@ + public final @(type_name) set@(convert_lower_case_underscore_to_camel_case(member.name))(final @(get_java_type(member.type))[] @(member.name)) { +@[ if isinstance(member.type, BoundedSequence)]@ + if(@(member.name).length > @(member.type.maximum_size)) { + throw new IllegalArgumentException("Array too big, maximum size allowed: @(member.type.maximum_size)"); + } +@[ elif isinstance(member.type, Array)]@ + if(@(member.name).length != @(member.type.size)) { + throw new IllegalArgumentException("Invalid size for fixed array, must be exactly: @(member.type.size)"); + } +@[ end if]@ + this.@(member.name) = @(member.name); + return this; + } + public final @(type_name) set@(convert_lower_case_underscore_to_camel_case(member.name))(final java.util.List<@(get_java_type(member.type, use_primitives=False))> @(member.name)) { @[ if isinstance(member.type, BoundedSequence)]@ if(@(member.name).size() > @(member.type.maximum_size)) { @@ -99,22 +113,33 @@ public final class @(type_name) implements MessageDefinition { throw new IllegalArgumentException("Invalid size for fixed array, must be exactly: @(member.type.size)"); } @[ end if]@ - this.@(member.name) = @(member.name); +@[ if isinstance(member.type.value_type, BasicType)]@ + @(get_java_type(member.type, use_primitives=False))[] boxed_arr = @(member.name).toArray(new @(get_java_type(member.type, use_primitives=False))[]{}); + @(get_java_type(member.type))[] unboxed_arr = new @(get_java_type(member.type))[@(member.name).size()]; + for (int i = 0; i < @(member.name).size(); i++) { + unboxed_arr[i] = boxed_arr[i].@(get_java_type(member.type))Value(); + } + this.@(member.name) = unboxed_arr; +@[ else]@ + this.@(member.name) = @(member.name).toArray(new @(get_java_type(member.type))[0]); +@[ end if]@ return this; } -@[ if isinstance(member.type.value_type, (BasicType, AbstractGenericString))]@ - public final @(type_name) set@(convert_lower_case_underscore_to_camel_case(member.name))(final @(get_java_type(member.type, use_primitives=True))[] @(member.name)) { - java.util.List<@(get_java_type(member.type, use_primitives=False))> @(member.name)_tmp = new java.util.ArrayList<@(get_java_type(member.type, use_primitives=False))>(); - for(@(get_java_type(member.type, use_primitives=True)) @(member.name)_value : @(member.name)) { - @(member.name)_tmp.add(@(member.name)_value); - } - return set@(convert_lower_case_underscore_to_camel_case(member.name))(@(member.name)_tmp); + public final @(get_java_type(member.type))[] get@(convert_lower_case_underscore_to_camel_case(member.name))() { + return this.@(member.name); } -@[ end if]@ - public final java.util.List<@(get_java_type(member.type, use_primitives=False))> get@(convert_lower_case_underscore_to_camel_case(member.name))() { - return this.@(member.name); + /** + * For better performance, use @@{link @(type_name)#get@(convert_lower_case_underscore_to_camel_case(member.name))} instead. + */ + public final java.util.List<@(get_java_type(member.type, use_primitives=False))> get@(convert_lower_case_underscore_to_camel_case(member.name))AsList() { + // TODO(jacobperron): We could cache the List value for subsequent calls + java.util.List<@(get_java_type(member.type, use_primitives=False))> list = new java.util.ArrayList<@(get_java_type(member.type, use_primitives=False))>(this.@(member.name).length); + for (@(get_java_type(member.type)) element : this.@(member.name)) { + list.add(element); + } + return list; } @[ else]@ @[ if member.has_annotation('default')]@ diff --git a/rosidl_generator_java/src/test/java/org/ros2/generator/InterfacesTest.java b/rosidl_generator_java/src/test/java/org/ros2/generator/InterfacesTest.java index d64af3eb..0403d00a 100644 --- a/rosidl_generator_java/src/test/java/org/ros2/generator/InterfacesTest.java +++ b/rosidl_generator_java/src/test/java/org/ros2/generator/InterfacesTest.java @@ -15,6 +15,7 @@ package org.ros2.generator; +import static org.junit.Assert.assertArrayEquals; import static org.junit.Assert.assertEquals; import static org.junit.Assert.assertNotEquals; import static org.junit.Assert.assertTrue; @@ -29,6 +30,95 @@ import org.junit.rules.ExpectedException; public class InterfacesTest { + public class ListFixtureData { + public boolean[] boolArr = new boolean[]{true, false, true}; + public List boolList = Arrays.asList(true, false, true); + public boolean[] boolArrShort = new boolean[]{false}; + public List boolListShort = Arrays.asList(false); + public byte[] byteArr = new byte[]{(byte) 0, (byte) 1, (byte) 255}; + public List byteList = Arrays.asList((byte) 0, (byte) 1, (byte) 255); + public byte[] byteArrShort = new byte[]{1}; + public List byteListShort = Arrays.asList((byte) 1); + public byte[] charArr = new byte[]{(byte) ' ', (byte) 'a', (byte) 'Z'}; + public List charList = Arrays.asList(new Byte((byte) ' '), new Byte((byte) 'a'), new Byte((byte) 'Z')); + public byte[] charArrShort = new byte[]{(byte) 'z', (byte) 'A'}; + public List charListShort = Arrays.asList(new Byte((byte) 'z'), new Byte((byte) 'A')); + public float[] float32Arr = new float[]{0.0f, -1.125f, 1.125f}; + public List float32List = Arrays.asList(0.0f, -1.125f, 1.125f); + public float[] float32ArrShort = new float[]{1.125f, -1.125f}; + public List float32ListShort = Arrays.asList(1.125f, -1.125f); + public double[] float64Arr = new double[]{0.0, -3.1415, 3.1415}; + public List float64List = Arrays.asList(0.0, -3.1415, 3.1415); + public double[] float64ArrShort = new double[]{3.1415, -3.1415}; + public List float64ListShort = Arrays.asList(3.1415, -3.1415); + public byte[] int8Arr = new byte[]{0, -128, 127}; + public List int8List = Arrays.asList((byte) 0, (byte) -128, (byte) 127); + public byte[] int8ArrShort = new byte[]{127, -128}; + public List int8ListShort = Arrays.asList((byte) 127, (byte) -128); + public byte[] uint8Arr = new byte[]{(byte) 0, (byte) 1, (byte) 255}; + public List uint8List = Arrays.asList((byte) 0, (byte) 1, (byte) 255); + public byte[] uint8ArrShort = new byte[]{(byte) 255, (byte) 1}; + public List uint8ListShort = Arrays.asList((byte) 255, (byte) 1); + public short[] int16Arr = new short[]{0, -32768, 32767}; + public List int16List = Arrays.asList((short) 0, (short) -32768, (short) 32767); + public short[] int16ArrShort = new short[]{32767, -32768}; + public List int16ListShort = Arrays.asList((short) 32767, (short) -32768); + public short[] uint16Arr = new short[]{(short) 0, (short) 1, (short) 65535}; + public List uint16List = Arrays.asList((short) 0, (short) 1, (short) 65535); + public short[] uint16ArrShort = new short[]{(short) 0, (short) 1, (short) 65535}; + public List uint16ListShort = Arrays.asList((short) 0, (short) 1, (short) 65535); + public int[] int32Arr = new int[]{0, -2147483648, 2147483647}; + public List int32List = Arrays.asList(0, -2147483648, 2147483647); + public int[] int32ArrShort = new int[]{2147483647, -2147483648}; + public List int32ListShort = Arrays.asList(2147483647, -2147483648); + public int[] uint32Arr = new int[]{0, 1, (int) 4294967295L}; + public List uint32List = Arrays.asList(0, 1, (int) 4294967295L); + public int[] uint32ArrShort = new int[]{(int) 4294967295L, 1}; + public List uint32ListShort = Arrays.asList((int) 4294967295L, 1); + public long[] int64Arr = new long[]{0L, -9223372036854775808L, 9223372036854775807L}; + public List int64List = Arrays.asList(0L, -9223372036854775808L, 9223372036854775807L); + public long[] int64ArrShort = new long[]{0L, -9223372036854775808L, 9223372036854775807L}; + public List int64ListShort = Arrays.asList(0L, -9223372036854775808L, 9223372036854775807L); + public long[] uint64Arr = new long[]{0L, 1L, -1L}; + public List uint64List = Arrays.asList(0L, 1L, -1L); + public long[] uint64ArrShort = new long[]{0L, 1L, -1L}; + public List uint64ListShort = Arrays.asList(0L, 1L, -1L); + public String[] stringArr = new String[]{"", "min value", "max_value"}; + public List stringList = Arrays.asList(stringArr); + public String[] stringArrShort = new String[]{"max_value", ""}; + public List stringListShort = Arrays.asList(stringArrShort); + private rosidl_generator_java.msg.BasicTypes basicTypes = new rosidl_generator_java.msg.BasicTypes(); + public rosidl_generator_java.msg.BasicTypes[] basicTypesArr = + new rosidl_generator_java.msg.BasicTypes[] {basicTypes, basicTypes, basicTypes}; + public List basicTypesList = Arrays.asList(basicTypesArr); + public rosidl_generator_java.msg.BasicTypes[] basicTypesArrShort = + new rosidl_generator_java.msg.BasicTypes[] {basicTypes}; + public List basicTypesListShort = Arrays.asList(basicTypesArrShort); + public rosidl_generator_java.msg.BasicTypes[] basicTypesArrLong = + new rosidl_generator_java.msg.BasicTypes[] {basicTypes, basicTypes, basicTypes, basicTypes}; + public List basicTypesListLong = Arrays.asList(basicTypesArrLong); + private rosidl_generator_java.msg.Constants constants = new rosidl_generator_java.msg.Constants(); + public rosidl_generator_java.msg.Constants[] constantsArr = + new rosidl_generator_java.msg.Constants[] {constants, constants, constants}; + public List constantsList = Arrays.asList(constantsArr); + public rosidl_generator_java.msg.Constants[] constantsArrShort = + new rosidl_generator_java.msg.Constants[] {constants}; + public List constantsListShort = Arrays.asList(constantsArrShort); + public rosidl_generator_java.msg.Constants[] constantsArrLong = + new rosidl_generator_java.msg.Constants[] {constants, constants, constants, constants}; + public List constantsListLong = Arrays.asList(constantsArrLong); + private rosidl_generator_java.msg.Defaults defaults = new rosidl_generator_java.msg.Defaults(); + public rosidl_generator_java.msg.Defaults[] defaultsArr = + new rosidl_generator_java.msg.Defaults[] {defaults, defaults, defaults}; + public List defaultsList = Arrays.asList(defaultsArr); + public rosidl_generator_java.msg.Defaults[] defaultsArrShort = + new rosidl_generator_java.msg.Defaults[] {defaults}; + public List defaultsListShort = Arrays.asList(defaultsArrShort); + public rosidl_generator_java.msg.Defaults[] defaultsArrLong = + new rosidl_generator_java.msg.Defaults[] {defaults, defaults, defaults, defaults}; + public List defaultsListLong = Arrays.asList(defaultsArrLong); + } + @BeforeClass public static void setupOnce() { try @@ -204,106 +294,135 @@ public final void testCheckStringConstraints() { @Test public final void testArrays() { rosidl_generator_java.msg.Arrays arrays = new rosidl_generator_java.msg.Arrays(); + ListFixtureData fixture = new ListFixtureData(); // This value should not change and is asserted at end of test arrays.setAlignmentCheck(42); // Test setting/getting fixed length arrays of primitive types - List boolList = Arrays.asList(true, false, true); - arrays.setBoolValues(boolList); - assertEquals(boolList, arrays.getBoolValues()); + arrays.setBoolValues(fixture.boolList); + assertArrayEquals(fixture.boolArr, arrays.getBoolValues()); + assertEquals(fixture.boolList, arrays.getBoolValuesAsList()); + assertThrows(IllegalArgumentException.class, + () -> arrays.setBoolValues(new boolean[]{true, false, true, false})); assertThrows(IllegalArgumentException.class, () -> arrays.setBoolValues(Arrays.asList(true, false, true, false))); - List byteList = Arrays.asList((byte) 0, (byte) 1, (byte) 255); - arrays.setByteValues(byteList); - assertEquals(byteList, arrays.getByteValues()); + arrays.setByteValues(fixture.byteList); + assertArrayEquals(fixture.byteArr, arrays.getByteValues()); + assertEquals(fixture.byteList, arrays.getByteValuesAsList()); + assertThrows(IllegalArgumentException.class, + () -> arrays.setByteValues(new byte[]{(byte) 1, (byte) 2})); assertThrows(IllegalArgumentException.class, () -> arrays.setByteValues(Arrays.asList((byte) 1, (byte) 2))); - List charList = Arrays.asList(' ', 'a', 'Z'); - arrays.setCharValues(charList); - assertEquals(charList, arrays.getCharValues()); + arrays.setCharValues(fixture.charList); + assertArrayEquals(fixture.charArr, arrays.getCharValues()); + assertEquals(fixture.charList, arrays.getCharValuesAsList()); + assertThrows(IllegalArgumentException.class, + () -> arrays.setCharValues(new byte[]{(byte) 'a', (byte) 'b', (byte) 'c', (byte) 'd'})); assertThrows(IllegalArgumentException.class, () -> arrays.setCharValues(Arrays.asList((byte) 'a', (byte) 'b', (byte) 'c', (byte) 'd'))); - List float32List = Arrays.asList(0.0f, -1.125f, 1.125f); - arrays.setFloat32Values(float32List); - assertEquals(float32List, arrays.getFloat32Values()); + arrays.setFloat32Values(fixture.float32List); + assertTrue(Arrays.equals(fixture.float32Arr, arrays.getFloat32Values())); + assertEquals(fixture.float32List, arrays.getFloat32ValuesAsList()); + assertThrows(IllegalArgumentException.class, + () -> arrays.setFloat32Values(new float[]{1.0f, 2.0f})); assertThrows(IllegalArgumentException.class, () -> arrays.setFloat32Values(Arrays.asList(1.0f, 2.0f))); - List float64List = Arrays.asList(0.0f, -3.1415, 3.1415); - arrays.setFloat64Values(float64List); - assertEquals(float64List, arrays.getFloat64Values()); + arrays.setFloat64Values(fixture.float64List); + assertTrue(Arrays.equals(fixture.float64Arr, arrays.getFloat64Values())); + assertEquals(fixture.float64List, arrays.getFloat64ValuesAsList()); + assertThrows(IllegalArgumentException.class, + () -> arrays.setFloat64Values(new double[]{1.0, 2.0, 3.0, 4.0})); assertThrows(IllegalArgumentException.class, () -> arrays.setFloat64Values(Arrays.asList(1.0, 2.0, 3.0, 4.0))); - List int8List = Arrays.asList(0, -128, 127); - arrays.setInt8Values(int8List); - assertEquals(int8List, arrays.getInt8Values()); + arrays.setInt8Values(fixture.int8List); + assertArrayEquals(fixture.int8Arr, arrays.getInt8Values()); + assertEquals(fixture.int8List, arrays.getInt8ValuesAsList()); + assertThrows(IllegalArgumentException.class, + () ->arrays.setInt8Values(new byte[]{(byte) 1, (byte) 2})); assertThrows(IllegalArgumentException.class, () ->arrays.setInt8Values(Arrays.asList((byte) 1, (byte) 2))); - List uint8List = Arrays.asList(0, 1, 255); - arrays.setUint8Values(uint8List); - assertEquals(uint8List, arrays.getUint8Values()); + arrays.setUint8Values(fixture.uint8List); + assertArrayEquals(fixture.uint8Arr, arrays.getUint8Values()); + assertEquals(fixture.uint8List, arrays.getUint8ValuesAsList()); + assertThrows(IllegalArgumentException.class, + () -> arrays.setUint8Values(new byte[]{(byte) 1, (byte) 2, (byte) 3, (byte) 4})); assertThrows(IllegalArgumentException.class, () -> arrays.setUint8Values(Arrays.asList((byte) 1, (byte) 2, (byte) 3, (byte) 4))); - List int16List = Arrays.asList(0, -32768, 32767); - arrays.setInt16Values(int16List); - assertEquals(int16List, arrays.getInt16Values()); + arrays.setInt16Values(fixture.int16List); + assertTrue(Arrays.equals(fixture.int16Arr, arrays.getInt16Values())); + assertEquals(fixture.int16List, arrays.getInt16ValuesAsList()); + assertThrows(IllegalArgumentException.class, + () -> arrays.setInt16Values(new short[]{(short) 1, (short) 2})); assertThrows(IllegalArgumentException.class, () -> arrays.setInt16Values(Arrays.asList((short) 1, (short) 2))); - List uint16List = Arrays.asList(0, 1, 65535); - arrays.setUint16Values(uint16List); - assertEquals(uint16List, arrays.getUint16Values()); + arrays.setUint16Values(fixture.uint16List); + assertTrue(Arrays.equals(fixture.uint16Arr, arrays.getUint16Values())); + assertEquals(fixture.uint16List, arrays.getUint16ValuesAsList()); + assertThrows(IllegalArgumentException.class, + () -> arrays.setUint16Values(new short[]{(short) 1, (short) 2, (short) 3, (short) 4})); assertThrows(IllegalArgumentException.class, () -> arrays.setUint16Values(Arrays.asList((short) 1, (short) 2, (short) 3, (short) 4))); - List int32List = Arrays.asList(0, -2147483648, 2147483647); - arrays.setInt32Values(int32List); - assertEquals(int32List, arrays.getInt32Values()); + arrays.setInt32Values(fixture.int32List); + assertArrayEquals(fixture.int32Arr, arrays.getInt32Values()); + assertEquals(fixture.int32List, arrays.getInt32ValuesAsList()); + assertThrows(IllegalArgumentException.class, + () -> arrays.setInt32Values(new int[]{1, 2})); assertThrows(IllegalArgumentException.class, () -> arrays.setInt32Values(Arrays.asList(1, 2))); - List uint32List = Arrays.asList(0, 1, 4294967295L); - arrays.setUint32Values(uint32List); - assertEquals(uint32List, arrays.getUint32Values()); + arrays.setUint32Values(fixture.uint32List); + assertArrayEquals(fixture.uint32Arr, arrays.getUint32Values()); + assertEquals(fixture.uint32List, arrays.getUint32ValuesAsList()); + assertThrows(IllegalArgumentException.class, + () -> arrays.setUint32Values(new int[]{1, 2, 3, 4})); assertThrows(IllegalArgumentException.class, () -> arrays.setUint32Values(Arrays.asList(1, 2, 3, 4))); - List int64List = Arrays.asList(0, -9223372036854775808L, 9223372036854775807L); - arrays.setInt64Values(int64List); - assertEquals(int64List, arrays.getInt64Values()); + arrays.setInt64Values(fixture.int64List); + assertArrayEquals(fixture.int64Arr, arrays.getInt64Values()); + assertEquals(fixture.int64List, arrays.getInt64ValuesAsList()); + assertThrows(IllegalArgumentException.class, + () -> arrays.setInt64Values(new long[]{1L, 2L})); assertThrows(IllegalArgumentException.class, () -> arrays.setInt64Values(Arrays.asList(1L, 2L))); - List uint64List = Arrays.asList(0, 1, -1); - arrays.setUint64Values(uint64List); - assertEquals(uint64List, arrays.getUint64Values()); + arrays.setUint64Values(fixture.uint64List); + assertArrayEquals(fixture.uint64Arr, arrays.getUint64Values()); + assertEquals(fixture.uint64List, arrays.getUint64ValuesAsList()); + assertThrows(IllegalArgumentException.class, + () ->arrays.setUint64Values(new long[]{1L, 2L, 3L, 4L})); assertThrows(IllegalArgumentException.class, () ->arrays.setUint64Values(Arrays.asList(1L, 2L, 3L, 4L))); // Test setting/getting fixed length arrays of strings - List stringList = Arrays.asList("", "min value", "max_value"); - arrays.setStringValues(stringList); - assertEquals(stringList, arrays.getStringValues()); + arrays.setStringValues(fixture.stringList); + assertArrayEquals(fixture.stringArr, arrays.getStringValues()); + assertEquals(fixture.stringList, arrays.getStringValuesAsList()); + assertThrows(IllegalArgumentException.class, + () -> arrays.setStringValues(new String[]{"too", "few"})); assertThrows(IllegalArgumentException.class, () -> arrays.setStringValues(Arrays.asList("too", "few"))); // Test setting/getting fixed length arrays of nested types - rosidl_generator_java.msg.BasicTypes basicTypes = new rosidl_generator_java.msg.BasicTypes(); - List basicTypesList = Arrays.asList( - new rosidl_generator_java.msg.BasicTypes[] {basicTypes, basicTypes, basicTypes}); - arrays.setBasicTypesValues(basicTypesList); - assertEquals(basicTypesList, arrays.getBasicTypesValues()); - assertThrows(IllegalArgumentException.class, - () -> arrays.setBasicTypesValues(Arrays.asList(new rosidl_generator_java.msg.BasicTypes[] {basicTypes}))); - rosidl_generator_java.msg.Constants constants = new rosidl_generator_java.msg.Constants(); - List constantsList = Arrays.asList( - new rosidl_generator_java.msg.Constants[] {constants, constants, constants}); - arrays.setConstantsValues(constantsList); - assertEquals(constantsList, arrays.getConstantsValues()); - assertThrows(IllegalArgumentException.class, - () -> arrays.setConstantsValues(Arrays.asList(new rosidl_generator_java.msg.Constants[] {constants}))); - rosidl_generator_java.msg.Defaults defaults = new rosidl_generator_java.msg.Defaults(); - List defaultsList = Arrays.asList( - new rosidl_generator_java.msg.Defaults[] {defaults, defaults, defaults}); - arrays.setDefaultsValues(defaultsList); - assertEquals(defaultsList, arrays.getDefaultsValues()); + arrays.setBasicTypesValues(fixture.basicTypesList); + assertArrayEquals(fixture.basicTypesArr, arrays.getBasicTypesValues()); + assertEquals(fixture.basicTypesList, arrays.getBasicTypesValuesAsList()); + assertThrows(IllegalArgumentException.class, + () -> arrays.setBasicTypesValues(fixture.basicTypesArrShort)); + assertThrows(IllegalArgumentException.class, + () -> arrays.setBasicTypesValues(fixture.basicTypesListShort)); + arrays.setConstantsValues(fixture.constantsList); + assertArrayEquals(fixture.constantsArr, arrays.getConstantsValues()); + assertEquals(fixture.constantsList, arrays.getConstantsValuesAsList()); + assertThrows(IllegalArgumentException.class, + () -> arrays.setConstantsValues(fixture.constantsArrShort)); + assertThrows(IllegalArgumentException.class, + () -> arrays.setConstantsValues(fixture.constantsListShort)); + arrays.setDefaultsValues(fixture.defaultsList); + assertArrayEquals(fixture.defaultsArr, arrays.getDefaultsValues()); + assertEquals(fixture.defaultsList, arrays.getDefaultsValuesAsList()); + assertThrows(IllegalArgumentException.class, + () -> arrays.setDefaultsValues(fixture.defaultsArrShort)); assertThrows(IllegalArgumentException.class, - () -> arrays.setDefaultsValues(Arrays.asList(new rosidl_generator_java.msg.Defaults[] {defaults}))); + () -> arrays.setDefaultsValues(fixture.defaultsListShort)); assertEquals(42, arrays.getAlignmentCheck()); } @@ -311,163 +430,184 @@ public final void testArrays() { @Test public final void testBoundedSequences() { rosidl_generator_java.msg.BoundedSequences bounded_seq = new rosidl_generator_java.msg.BoundedSequences(); + ListFixtureData fixture = new ListFixtureData(); // This value should not change and is asserted at end of test bounded_seq.setAlignmentCheck(42); // Test setting/getting fixed length bounded_seq of primitive types - List boolList = Arrays.asList(true, false, true); - bounded_seq.setBoolValues(boolList); - assertEquals(boolList, bounded_seq.getBoolValues()); - List boolListShort = Arrays.asList(false); - bounded_seq.setBoolValues(boolListShort); - assertEquals(boolListShort, bounded_seq.getBoolValues()); + bounded_seq.setBoolValues(fixture.boolList); + assertArrayEquals(fixture.boolArr, bounded_seq.getBoolValues()); + assertEquals(fixture.boolList, bounded_seq.getBoolValuesAsList()); + bounded_seq.setBoolValues(fixture.boolListShort); + assertArrayEquals(fixture.boolArrShort, bounded_seq.getBoolValues()); + assertEquals(fixture.boolListShort, bounded_seq.getBoolValuesAsList()); + assertThrows(IllegalArgumentException.class, + () -> bounded_seq.setBoolValues(new boolean[]{true, false, true, false})); assertThrows(IllegalArgumentException.class, () -> bounded_seq.setBoolValues(Arrays.asList(true, false, true, false))); - List byteList = Arrays.asList((byte) 0, (byte) 1, (byte) 255); - bounded_seq.setByteValues(byteList); - assertEquals(byteList, bounded_seq.getByteValues()); - List byteListShort = Arrays.asList((byte) 1); - bounded_seq.setByteValues(byteListShort); - assertEquals(byteListShort, bounded_seq.getByteValues()); + bounded_seq.setByteValues(fixture.byteList); + assertArrayEquals(fixture.byteArr, bounded_seq.getByteValues()); + assertEquals(fixture.byteList, bounded_seq.getByteValuesAsList()); + bounded_seq.setByteValues(fixture.byteListShort); + assertArrayEquals(fixture.byteArrShort, bounded_seq.getByteValues()); + assertEquals(fixture.byteListShort, bounded_seq.getByteValuesAsList()); + assertThrows(IllegalArgumentException.class, + () -> bounded_seq.setByteValues(new byte[]{(byte) 1, (byte) 2, (byte) 3, (byte) 4})); assertThrows(IllegalArgumentException.class, () -> bounded_seq.setByteValues(Arrays.asList((byte) 1, (byte) 2, (byte) 3, (byte) 4))); - List charList = Arrays.asList(' ', 'a', 'Z'); - bounded_seq.setCharValues(charList); - assertEquals(charList, bounded_seq.getCharValues()); - List charListShort = Arrays.asList('z', 'A'); - bounded_seq.setCharValues(charListShort); - assertEquals(charListShort, bounded_seq.getCharValues()); + bounded_seq.setCharValues(fixture.charList); + assertArrayEquals(fixture.charArr, bounded_seq.getCharValues()); + assertEquals(fixture.charList, bounded_seq.getCharValuesAsList()); + bounded_seq.setCharValues(fixture.charListShort); + assertArrayEquals(fixture.charArrShort, bounded_seq.getCharValues()); + assertEquals(fixture.charListShort, bounded_seq.getCharValuesAsList()); + assertThrows(IllegalArgumentException.class, + () -> bounded_seq.setCharValues(new byte[]{(byte) 'a', (byte) 'b', (byte) 'c', (byte) 'd'})); assertThrows(IllegalArgumentException.class, () -> bounded_seq.setCharValues(Arrays.asList((byte) 'a', (byte) 'b', (byte) 'c', (byte) 'd'))); - List float32List = Arrays.asList(0.0f, -1.125f, 1.125f); - bounded_seq.setFloat32Values(float32List); - assertEquals(float32List, bounded_seq.getFloat32Values()); - List float32ListShort = Arrays.asList(1.125f, -1.125f); - bounded_seq.setFloat32Values(float32ListShort); - assertEquals(float32ListShort, bounded_seq.getFloat32Values()); + bounded_seq.setFloat32Values(fixture.float32List); + assertTrue(Arrays.equals(fixture.float32Arr, bounded_seq.getFloat32Values())); + assertEquals(fixture.float32List, bounded_seq.getFloat32ValuesAsList()); + bounded_seq.setFloat32Values(fixture.float32ListShort); + assertTrue(Arrays.equals(fixture.float32ArrShort, bounded_seq.getFloat32Values())); + assertEquals(fixture.float32ListShort, bounded_seq.getFloat32ValuesAsList()); + assertThrows(IllegalArgumentException.class, + () -> bounded_seq.setFloat32Values(new float[]{1.0f, 2.0f, 3.0f, 4.0f})); assertThrows(IllegalArgumentException.class, () -> bounded_seq.setFloat32Values(Arrays.asList(1.0f, 2.0f, 3.0f, 4.0f))); - List float64List = Arrays.asList(0.0f, -3.1415, 3.1415); - bounded_seq.setFloat64Values(float64List); - assertEquals(float64List, bounded_seq.getFloat64Values()); - List float64ListShort = Arrays.asList(3.1415, -3.1415); - bounded_seq.setFloat64Values(float64ListShort); - assertEquals(float64ListShort, bounded_seq.getFloat64Values()); + bounded_seq.setFloat64Values(fixture.float64List); + assertTrue(Arrays.equals(fixture.float64Arr, bounded_seq.getFloat64Values())); + assertEquals(fixture.float64List, bounded_seq.getFloat64ValuesAsList()); + bounded_seq.setFloat64Values(fixture.float64ListShort); + assertTrue(Arrays.equals(fixture.float64ArrShort, bounded_seq.getFloat64Values())); + assertEquals(fixture.float64ListShort, bounded_seq.getFloat64ValuesAsList()); assertThrows(IllegalArgumentException.class, () -> bounded_seq.setFloat64Values(Arrays.asList(1.0, 2.0, 3.0, 4.0))); - List int8List = Arrays.asList(0, -128, 127); - bounded_seq.setInt8Values(int8List); - assertEquals(int8List, bounded_seq.getInt8Values()); - List int8ListShort = Arrays.asList(127, -128); - bounded_seq.setInt8Values(int8ListShort); - assertEquals(int8ListShort, bounded_seq.getInt8Values()); + bounded_seq.setInt8Values(fixture.int8List); + assertArrayEquals(fixture.int8Arr, bounded_seq.getInt8Values()); + assertEquals(fixture.int8List, bounded_seq.getInt8ValuesAsList()); + bounded_seq.setInt8Values(fixture.int8ListShort); + assertArrayEquals(fixture.int8ArrShort, bounded_seq.getInt8Values()); + assertEquals(fixture.int8ListShort, bounded_seq.getInt8ValuesAsList()); + assertThrows(IllegalArgumentException.class, + () -> bounded_seq.setInt8Values(new byte[]{(byte) 1, (byte) 2, (byte) 3, (byte) 4})); assertThrows(IllegalArgumentException.class, () -> bounded_seq.setInt8Values(Arrays.asList((byte) 1, (byte) 2, (byte) 3, (byte) 4))); - List uint8List = Arrays.asList(0, 1, 255); - bounded_seq.setUint8Values(uint8List); - assertEquals(uint8List, bounded_seq.getUint8Values()); - List uint8ListShort = Arrays.asList(255, 1); - bounded_seq.setUint8Values(uint8ListShort); - assertEquals(uint8ListShort, bounded_seq.getUint8Values()); + bounded_seq.setUint8Values(fixture.uint8List); + assertArrayEquals(fixture.uint8Arr, bounded_seq.getUint8Values()); + assertEquals(fixture.uint8List, bounded_seq.getUint8ValuesAsList()); + bounded_seq.setUint8Values(fixture.uint8ListShort); + assertArrayEquals(fixture.uint8ArrShort, bounded_seq.getUint8Values()); + assertEquals(fixture.uint8ListShort, bounded_seq.getUint8ValuesAsList()); + assertThrows(IllegalArgumentException.class, + () -> bounded_seq.setUint8Values(new byte[]{(byte) 1, (byte) 2, (byte) 3, (byte) 4})); assertThrows(IllegalArgumentException.class, () -> bounded_seq.setUint8Values(Arrays.asList((byte) 1, (byte) 2, (byte) 3, (byte) 4))); - List int16List = Arrays.asList(0, -32768, 32767); - bounded_seq.setInt16Values(int16List); - assertEquals(int16List, bounded_seq.getInt16Values()); - List int16ListShort = Arrays.asList(32767, -32768); - bounded_seq.setInt16Values(int16ListShort); - assertEquals(int16ListShort, bounded_seq.getInt16Values()); + bounded_seq.setInt16Values(fixture.int16List); + assertTrue(Arrays.equals(fixture.int16Arr, bounded_seq.getInt16Values())); + assertEquals(fixture.int16List, bounded_seq.getInt16ValuesAsList()); + bounded_seq.setInt16Values(fixture.int16ListShort); + assertTrue(Arrays.equals(fixture.int16ArrShort, bounded_seq.getInt16Values())); + assertEquals(fixture.int16ListShort, bounded_seq.getInt16ValuesAsList()); + assertThrows(IllegalArgumentException.class, + () -> bounded_seq.setInt16Values(new short[]{(short) 1, (short) 2, (short) 3, (short) 4})); assertThrows(IllegalArgumentException.class, () -> bounded_seq.setInt16Values(Arrays.asList((short) 1, (short) 2, (short) 3, (short) 4))); - List uint16List = Arrays.asList(0, 1, 65535); - bounded_seq.setUint16Values(uint16List); - assertEquals(uint16List, bounded_seq.getUint16Values()); - List uint16ListShort = Arrays.asList(0, 1, 65535); - bounded_seq.setUint16Values(uint16ListShort); - assertEquals(uint16ListShort, bounded_seq.getUint16Values()); + bounded_seq.setUint16Values(fixture.uint16List); + assertTrue(Arrays.equals(fixture.uint16Arr, bounded_seq.getUint16Values())); + assertEquals(fixture.uint16List, bounded_seq.getUint16ValuesAsList()); + bounded_seq.setUint16Values(fixture.uint16ListShort); + assertTrue(Arrays.equals(fixture.uint16ArrShort, bounded_seq.getUint16Values())); + assertEquals(fixture.uint16ListShort, bounded_seq.getUint16ValuesAsList()); + assertThrows(IllegalArgumentException.class, + () -> bounded_seq.setUint16Values(new short[]{(short) 1, (short) 2, (short) 3, (short) 4})); assertThrows(IllegalArgumentException.class, () -> bounded_seq.setUint16Values(Arrays.asList((short) 1, (short) 2, (short) 3, (short) 4))); - List int32List = Arrays.asList(0, -2147483648, 2147483647); - bounded_seq.setInt32Values(int32List); - assertEquals(int32List, bounded_seq.getInt32Values()); - List int32ListShort = Arrays.asList(2147483647, -2147483648); - bounded_seq.setInt32Values(int32ListShort); - assertEquals(int32ListShort, bounded_seq.getInt32Values()); + bounded_seq.setInt32Values(fixture.int32List); + assertArrayEquals(fixture.int32Arr, bounded_seq.getInt32Values()); + assertEquals(fixture.int32List, bounded_seq.getInt32ValuesAsList()); + bounded_seq.setInt32Values(fixture.int32ListShort); + assertArrayEquals(fixture.int32ArrShort, bounded_seq.getInt32Values()); + assertEquals(fixture.int32ListShort, bounded_seq.getInt32ValuesAsList()); + assertThrows(IllegalArgumentException.class, + () -> bounded_seq.setInt32Values(new int[]{1, 2, 3, 4})); assertThrows(IllegalArgumentException.class, () -> bounded_seq.setInt32Values(Arrays.asList(1, 2, 3, 4))); - List uint32List = Arrays.asList(0, 1, 4294967295L); - bounded_seq.setUint32Values(uint32List); - assertEquals(uint32List, bounded_seq.getUint32Values()); - List uint32ListShort = Arrays.asList(4294967295L, 1); - bounded_seq.setUint32Values(uint32ListShort); - assertEquals(uint32ListShort, bounded_seq.getUint32Values()); + bounded_seq.setUint32Values(fixture.uint32List); + assertArrayEquals(fixture.uint32Arr, bounded_seq.getUint32Values()); + assertEquals(fixture.uint32List, bounded_seq.getUint32ValuesAsList()); + bounded_seq.setUint32Values(fixture.uint32ListShort); + assertArrayEquals(fixture.uint32ArrShort, bounded_seq.getUint32Values()); + assertEquals(fixture.uint32ListShort, bounded_seq.getUint32ValuesAsList()); + assertThrows(IllegalArgumentException.class, + () -> bounded_seq.setUint32Values(new int[]{1, 2, 3, 4})); assertThrows(IllegalArgumentException.class, () -> bounded_seq.setUint32Values(Arrays.asList(1, 2, 3, 4))); - List int64List = Arrays.asList(0, -9223372036854775808L, 9223372036854775807L); - bounded_seq.setInt64Values(int64List); - assertEquals(int64List, bounded_seq.getInt64Values()); - List int64ListShort = Arrays.asList(0, -9223372036854775808L, 9223372036854775807L); - bounded_seq.setInt64Values(int64ListShort); - assertEquals(int64ListShort, bounded_seq.getInt64Values()); + bounded_seq.setInt64Values(fixture.int64List); + assertArrayEquals(fixture.int64Arr, bounded_seq.getInt64Values()); + assertEquals(fixture.int64List, bounded_seq.getInt64ValuesAsList()); + bounded_seq.setInt64Values(fixture.int64ListShort); + assertArrayEquals(fixture.int64ArrShort, bounded_seq.getInt64Values()); + assertEquals(fixture.int64ListShort, bounded_seq.getInt64ValuesAsList()); + assertThrows(IllegalArgumentException.class, + () -> bounded_seq.setInt64Values(new long[]{1L, 2L, 3L, 4L})); assertThrows(IllegalArgumentException.class, () -> bounded_seq.setInt64Values(Arrays.asList(1L, 2L, 3L, 4L))); - List uint64List = Arrays.asList(0, 1, -1); - bounded_seq.setUint64Values(uint64List); - assertEquals(uint64List, bounded_seq.getUint64Values()); - List uint64ListShort = Arrays.asList(0, 1, -1); - bounded_seq.setUint64Values(uint64ListShort); - assertEquals(uint64ListShort, bounded_seq.getUint64Values()); + bounded_seq.setUint64Values(fixture.uint64List); + assertArrayEquals(fixture.uint64Arr, bounded_seq.getUint64Values()); + assertEquals(fixture.uint64List, bounded_seq.getUint64ValuesAsList()); + bounded_seq.setUint64Values(fixture.uint64ListShort); + assertArrayEquals(fixture.uint64ArrShort, bounded_seq.getUint64Values()); + assertEquals(fixture.uint64ListShort, bounded_seq.getUint64ValuesAsList()); + assertThrows(IllegalArgumentException.class, + () -> bounded_seq.setUint64Values(new long[]{1L, 2L, 3L, 4L})); assertThrows(IllegalArgumentException.class, () -> bounded_seq.setUint64Values(Arrays.asList(1L, 2L, 3L, 4L))); // Test setting/getting fixed length bounded_seq of strings - List stringList = Arrays.asList("", "min value", "max_value"); - bounded_seq.setStringValues(stringList); - assertEquals(stringList, bounded_seq.getStringValues()); - List stringListShort = Arrays.asList("max_value", ""); - bounded_seq.setStringValues(stringListShort); - assertEquals(stringListShort, bounded_seq.getStringValues()); + bounded_seq.setStringValues(fixture.stringList); + assertArrayEquals(fixture.stringArr, bounded_seq.getStringValues()); + assertEquals(fixture.stringList, bounded_seq.getStringValuesAsList()); + bounded_seq.setStringValues(fixture.stringListShort); + assertArrayEquals(fixture.stringArrShort, bounded_seq.getStringValues()); + assertEquals(fixture.stringListShort, bounded_seq.getStringValuesAsList()); + assertThrows(IllegalArgumentException.class, + () -> bounded_seq.setStringValues(new String[]{"too", "many", "values", "!"})); assertThrows(IllegalArgumentException.class, () -> bounded_seq.setStringValues(Arrays.asList("too", "many", "values", "!"))); // Test setting/getting fixed length bounded_seq of nested types - rosidl_generator_java.msg.BasicTypes basicTypes = new rosidl_generator_java.msg.BasicTypes(); - List basicTypesList = Arrays.asList( - new rosidl_generator_java.msg.BasicTypes[] {basicTypes, basicTypes, basicTypes}); - bounded_seq.setBasicTypesValues(basicTypesList); - assertEquals(basicTypesList, bounded_seq.getBasicTypesValues()); - List basicTypesListShort = Arrays.asList( - new rosidl_generator_java.msg.BasicTypes[] {basicTypes}); - bounded_seq.setBasicTypesValues(basicTypesListShort); - assertEquals(basicTypesListShort, bounded_seq.getBasicTypesValues()); - assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setBasicTypesValues( - Arrays.asList(new rosidl_generator_java.msg.BasicTypes[] {basicTypes, basicTypes, basicTypes, basicTypes}))); - rosidl_generator_java.msg.Constants constants = new rosidl_generator_java.msg.Constants(); - List constantsList = Arrays.asList( - new rosidl_generator_java.msg.Constants[] {constants, constants, constants}); - bounded_seq.setConstantsValues(constantsList); - assertEquals(constantsList, bounded_seq.getConstantsValues()); - List constantsListShort = Arrays.asList( - new rosidl_generator_java.msg.Constants[] {constants}); - bounded_seq.setConstantsValues(constantsListShort); - assertEquals(constantsListShort, bounded_seq.getConstantsValues()); - assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setConstantsValues( - Arrays.asList(new rosidl_generator_java.msg.Constants[] {constants, constants, constants, constants}))); - rosidl_generator_java.msg.Defaults defaults = new rosidl_generator_java.msg.Defaults(); - List defaultsList = Arrays.asList( - new rosidl_generator_java.msg.Defaults[] {defaults, defaults, defaults}); - bounded_seq.setDefaultsValues(defaultsList); - assertEquals(defaultsList, bounded_seq.getDefaultsValues()); - List defaultsListShort = Arrays.asList( - new rosidl_generator_java.msg.Defaults[] {defaults, defaults, defaults}); - bounded_seq.setDefaultsValues(defaultsListShort); - assertEquals(defaultsListShort, bounded_seq.getDefaultsValues()); - assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setDefaultsValues( - Arrays.asList(new rosidl_generator_java.msg.Defaults[] {defaults, defaults, defaults, defaults}))); + bounded_seq.setBasicTypesValues(fixture.basicTypesList); + assertArrayEquals(fixture.basicTypesArr, bounded_seq.getBasicTypesValues()); + assertEquals(fixture.basicTypesList, bounded_seq.getBasicTypesValuesAsList()); + bounded_seq.setBasicTypesValues(fixture.basicTypesListShort); + assertArrayEquals(fixture.basicTypesArrShort, bounded_seq.getBasicTypesValues()); + assertEquals(fixture.basicTypesListShort, bounded_seq.getBasicTypesValuesAsList()); + assertThrows(IllegalArgumentException.class, + () -> bounded_seq.setBasicTypesValues(fixture.basicTypesArrLong)); + assertThrows(IllegalArgumentException.class, + () -> bounded_seq.setBasicTypesValues(fixture.basicTypesListLong)); + bounded_seq.setConstantsValues(fixture.constantsList); + assertArrayEquals(fixture.constantsArr, bounded_seq.getConstantsValues()); + assertEquals(fixture.constantsList, bounded_seq.getConstantsValuesAsList()); + bounded_seq.setConstantsValues(fixture.constantsListShort); + assertArrayEquals(fixture.constantsArrShort, bounded_seq.getConstantsValues()); + assertEquals(fixture.constantsListShort, bounded_seq.getConstantsValuesAsList()); + assertThrows(IllegalArgumentException.class, + () -> bounded_seq.setConstantsValues(fixture.constantsArrLong)); + assertThrows(IllegalArgumentException.class, + () -> bounded_seq.setConstantsValues(fixture.constantsListLong)); + bounded_seq.setDefaultsValues(fixture.defaultsList); + assertArrayEquals(fixture.defaultsArr, bounded_seq.getDefaultsValues()); + assertEquals(fixture.defaultsList, bounded_seq.getDefaultsValuesAsList()); + bounded_seq.setDefaultsValues(fixture.defaultsListShort); + assertArrayEquals(fixture.defaultsArrShort, bounded_seq.getDefaultsValues()); + assertEquals(fixture.defaultsListShort, bounded_seq.getDefaultsValuesAsList()); + assertThrows(IllegalArgumentException.class, + () -> bounded_seq.setDefaultsValues(fixture.defaultsArrLong)); + assertThrows(IllegalArgumentException.class, + () -> bounded_seq.setDefaultsValues(fixture.defaultsListLong)); assertEquals(42, bounded_seq.getAlignmentCheck()); } @@ -475,72 +615,67 @@ public final void testBoundedSequences() { @Test public final void testUnboundedSequences() { rosidl_generator_java.msg.UnboundedSequences unbounded_seq = new rosidl_generator_java.msg.UnboundedSequences(); + ListFixtureData fixture = new ListFixtureData(); // This value should not change and is asserted at end of test unbounded_seq.setAlignmentCheck(42); // Test setting/getting fixed length unbounded_seq of primitive types - List boolList = Arrays.asList(true, false, true); - unbounded_seq.setBoolValues(boolList); - assertEquals(boolList, unbounded_seq.getBoolValues()); - List byteList = Arrays.asList((byte) 0, (byte) 1, (byte) 255); - unbounded_seq.setByteValues(byteList); - assertEquals(byteList, unbounded_seq.getByteValues()); - List charList = Arrays.asList(' ', 'a', 'Z'); - unbounded_seq.setCharValues(charList); - assertEquals(charList, unbounded_seq.getCharValues()); - List float32List = Arrays.asList(0.0f, -1.125f, 1.125f); - unbounded_seq.setFloat32Values(float32List); - assertEquals(float32List, unbounded_seq.getFloat32Values()); - List float64List = Arrays.asList(0.0f, -3.1415, 3.1415); - unbounded_seq.setFloat64Values(float64List); - assertEquals(float64List, unbounded_seq.getFloat64Values()); - List int8List = Arrays.asList(0, -128, 127); - unbounded_seq.setInt8Values(int8List); - assertEquals(int8List, unbounded_seq.getInt8Values()); - List uint8List = Arrays.asList(0, 1, 255); - unbounded_seq.setUint8Values(uint8List); - assertEquals(uint8List, unbounded_seq.getUint8Values()); - List int16List = Arrays.asList(0, -32768, 32767); - unbounded_seq.setInt16Values(int16List); - assertEquals(int16List, unbounded_seq.getInt16Values()); - List uint16List = Arrays.asList(0, 1, 65535); - unbounded_seq.setUint16Values(uint16List); - assertEquals(uint16List, unbounded_seq.getUint16Values()); - List int32List = Arrays.asList(0, -2147483648, 2147483647); - unbounded_seq.setInt32Values(int32List); - assertEquals(int32List, unbounded_seq.getInt32Values()); - List uint32List = Arrays.asList(0, 1, 4294967295L); - unbounded_seq.setUint32Values(uint32List); - assertEquals(uint32List, unbounded_seq.getUint32Values()); - List int64List = Arrays.asList(0, -9223372036854775808L, 9223372036854775807L); - unbounded_seq.setInt64Values(int64List); - assertEquals(int64List, unbounded_seq.getInt64Values()); - List uint64List = Arrays.asList(0, 1, -1); - unbounded_seq.setUint64Values(uint64List); - assertEquals(uint64List, unbounded_seq.getUint64Values()); + unbounded_seq.setBoolValues(fixture.boolList); + assertArrayEquals(fixture.boolArr, unbounded_seq.getBoolValues()); + assertEquals(fixture.boolList, unbounded_seq.getBoolValuesAsList()); + unbounded_seq.setByteValues(fixture.byteList); + assertArrayEquals(fixture.byteArr, unbounded_seq.getByteValues()); + assertEquals(fixture.byteList, unbounded_seq.getByteValuesAsList()); + unbounded_seq.setCharValues(fixture.charList); + assertArrayEquals(fixture.charArr, unbounded_seq.getCharValues()); + assertEquals(fixture.charList, unbounded_seq.getCharValuesAsList()); + unbounded_seq.setFloat32Values(fixture.float32List); + assertTrue(Arrays.equals(fixture.float32Arr, unbounded_seq.getFloat32Values())); + assertEquals(fixture.float32List, unbounded_seq.getFloat32ValuesAsList()); + unbounded_seq.setFloat64Values(fixture.float64List); + assertTrue(Arrays.equals(fixture.float64Arr, unbounded_seq.getFloat64Values())); + assertEquals(fixture.float64List, unbounded_seq.getFloat64ValuesAsList()); + unbounded_seq.setInt8Values(fixture.int8List); + assertArrayEquals(fixture.int8Arr, unbounded_seq.getInt8Values()); + assertEquals(fixture.int8List, unbounded_seq.getInt8ValuesAsList()); + unbounded_seq.setUint8Values(fixture.uint8List); + assertArrayEquals(fixture.uint8Arr, unbounded_seq.getUint8Values()); + assertEquals(fixture.uint8List, unbounded_seq.getUint8ValuesAsList()); + unbounded_seq.setInt16Values(fixture.int16List); + assertTrue(Arrays.equals(fixture.int16Arr, unbounded_seq.getInt16Values())); + assertEquals(fixture.int16List, unbounded_seq.getInt16ValuesAsList()); + unbounded_seq.setUint16Values(fixture.uint16List); + assertTrue(Arrays.equals(fixture.uint16Arr, unbounded_seq.getUint16Values())); + assertEquals(fixture.uint16List, unbounded_seq.getUint16ValuesAsList()); + unbounded_seq.setInt32Values(fixture.int32List); + assertArrayEquals(fixture.int32Arr, unbounded_seq.getInt32Values()); + assertEquals(fixture.int32List, unbounded_seq.getInt32ValuesAsList()); + unbounded_seq.setUint32Values(fixture.uint32List); + assertArrayEquals(fixture.uint32Arr, unbounded_seq.getUint32Values()); + assertEquals(fixture.uint32List, unbounded_seq.getUint32ValuesAsList()); + unbounded_seq.setInt64Values(fixture.int64List); + assertArrayEquals(fixture.int64Arr, unbounded_seq.getInt64Values()); + assertEquals(fixture.int64List, unbounded_seq.getInt64ValuesAsList()); + unbounded_seq.setUint64Values(fixture.uint64List); + assertArrayEquals(fixture.uint64Arr, unbounded_seq.getUint64Values()); + assertEquals(fixture.uint64List, unbounded_seq.getUint64ValuesAsList()); // Test setting/getting fixed length unbounded_seq of strings - List stringList = Arrays.asList("", "min value", "max_value"); - unbounded_seq.setStringValues(stringList); - assertEquals(stringList, unbounded_seq.getStringValues()); + unbounded_seq.setStringValues(fixture.stringList); + assertArrayEquals(fixture.stringArr, unbounded_seq.getStringValues()); + assertEquals(fixture.stringList, unbounded_seq.getStringValuesAsList()); // Test setting/getting fixed length unbounded_seq of nested types - rosidl_generator_java.msg.BasicTypes basicTypes = new rosidl_generator_java.msg.BasicTypes(); - List basicTypesList = Arrays.asList( - new rosidl_generator_java.msg.BasicTypes[] {basicTypes, basicTypes, basicTypes}); - unbounded_seq.setBasicTypesValues(basicTypesList); - assertEquals(basicTypesList, unbounded_seq.getBasicTypesValues()); - rosidl_generator_java.msg.Constants constants = new rosidl_generator_java.msg.Constants(); - List constantsList = Arrays.asList( - new rosidl_generator_java.msg.Constants[] {constants, constants, constants}); - unbounded_seq.setConstantsValues(constantsList); - assertEquals(constantsList, unbounded_seq.getConstantsValues()); - rosidl_generator_java.msg.Defaults defaults = new rosidl_generator_java.msg.Defaults(); - List defaultsList = Arrays.asList( - new rosidl_generator_java.msg.Defaults[] {defaults, defaults, defaults}); - unbounded_seq.setDefaultsValues(defaultsList); - assertEquals(defaultsList, unbounded_seq.getDefaultsValues()); + unbounded_seq.setBasicTypesValues(fixture.basicTypesList); + assertArrayEquals(fixture.basicTypesArr, unbounded_seq.getBasicTypesValues()); + assertEquals(fixture.basicTypesList, unbounded_seq.getBasicTypesValuesAsList()); + unbounded_seq.setConstantsValues(fixture.constantsList); + assertArrayEquals(fixture.constantsArr, unbounded_seq.getConstantsValues()); + assertEquals(fixture.constantsList, unbounded_seq.getConstantsValuesAsList()); + unbounded_seq.setDefaultsValues(fixture.defaultsList); + assertArrayEquals(fixture.defaultsArr, unbounded_seq.getDefaultsValues()); + assertEquals(fixture.defaultsList, unbounded_seq.getDefaultsValuesAsList()); assertEquals(42, unbounded_seq.getAlignmentCheck()); } From 15a15677c1e77604af3b6b5003d216d12dad7553 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Wed, 20 Jan 2021 13:02:07 -0800 Subject: [PATCH 28/51] Add action interfaces for goal request and goal response (osrf/ros2_java#52) * Add interfaces for action goal, result, and feedback Implementing these interfaces in the code generation template makes it easier to pass around these types in a generic way. Note, the 'final' modifier had to be removed from generated message types in order to extend goal, result, and feedback types in action definitions. Signed-off-by: Jacob Perron * Add new definitions for action goal response and request Signed-off-by: Jacob Perron * Add getter for UUID to SendGoalRequest Also make inner classes static. Signed-off-by: Jacob Perron * Add getStamp method to GoalResponseDefinition Signed-off-by: Jacob Perron * Partially revert "Add interfaces for action goal, result, and feedback" Partially revert commit dd046147d38c1aa844376149f62c4f3aaec04d96. I don't think we need to aliases for the message types, but I'll add them back if they turn out to be useful. * Parameterize goal request and response interfaces on action type Signed-off-by: Jacob Perron * Fix getGoalUuid implementation It should return a List, since it is hashable. Signed-off-by: Jacob Perron --- rcljava_common/CMakeLists.txt | 2 + .../rcljava/interfaces/ActionDefinition.java | 7 +++- .../interfaces/GoalRequestDefinition.java | 23 +++++++++++ .../interfaces/GoalResponseDefinition.java | 21 ++++++++++ rosidl_generator_java/resource/action.java.em | 40 +++++++++++++++++++ rosidl_generator_java/resource/msg.java.em | 2 +- 6 files changed, 93 insertions(+), 2 deletions(-) create mode 100644 rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalRequestDefinition.java create mode 100644 rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalResponseDefinition.java diff --git a/rcljava_common/CMakeLists.txt b/rcljava_common/CMakeLists.txt index 3bc04fde..46715393 100644 --- a/rcljava_common/CMakeLists.txt +++ b/rcljava_common/CMakeLists.txt @@ -38,6 +38,8 @@ set(${PROJECT_NAME}_java_sources "src/main/java/org/ros2/rcljava/exceptions/RCLReturn.java" "src/main/java/org/ros2/rcljava/interfaces/ActionDefinition.java" "src/main/java/org/ros2/rcljava/interfaces/Disposable.java" + "src/main/java/org/ros2/rcljava/interfaces/GoalRequestDefinition.java" + "src/main/java/org/ros2/rcljava/interfaces/GoalResponseDefinition.java" "src/main/java/org/ros2/rcljava/interfaces/MessageDefinition.java" "src/main/java/org/ros2/rcljava/interfaces/ServiceDefinition.java" ) diff --git a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ActionDefinition.java b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ActionDefinition.java index 9681463b..e39d65d8 100644 --- a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ActionDefinition.java +++ b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ActionDefinition.java @@ -15,4 +15,9 @@ package org.ros2.rcljava.interfaces; -public interface ActionDefinition {} +public interface ActionDefinition { + Class getSendGoalRequestType(); + Class getSendGoalResponseType(); + Class getGetResultRequestType(); + Class getGetResultResponseType(); +} diff --git a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalRequestDefinition.java b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalRequestDefinition.java new file mode 100644 index 00000000..cf6dc177 --- /dev/null +++ b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalRequestDefinition.java @@ -0,0 +1,23 @@ +/* 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.interfaces; + +import java.util.List; + +public interface GoalRequestDefinition extends MessageDefinition { + MessageDefinition getGoal(); + List getGoalUuid(); +} diff --git a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalResponseDefinition.java b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalResponseDefinition.java new file mode 100644 index 00000000..c645cf8f --- /dev/null +++ b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalResponseDefinition.java @@ -0,0 +1,21 @@ +/* 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.interfaces; + +public interface GoalResponseDefinition extends MessageDefinition { + void accept(boolean accepted); + void setStamp(int sec, int nanosec); +} diff --git a/rosidl_generator_java/resource/action.java.em b/rosidl_generator_java/resource/action.java.em index 5e7ebbe9..cb6c1dd3 100644 --- a/rosidl_generator_java/resource/action.java.em +++ b/rosidl_generator_java/resource/action.java.em @@ -63,8 +63,12 @@ expand_template( template_basepath=template_basepath) action_imports = [ + 'java.util.List', 'org.ros2.rcljava.common.JNIUtils', 'org.ros2.rcljava.interfaces.ActionDefinition', + 'org.ros2.rcljava.interfaces.GoalRequestDefinition', + 'org.ros2.rcljava.interfaces.GoalResponseDefinition', + 'org.ros2.rcljava.interfaces.MessageDefinition', 'org.slf4j.Logger', 'org.slf4j.LoggerFactory', ] @@ -76,6 +80,42 @@ import @(action_import); public class @(type_name) implements ActionDefinition { + public static class SendGoalRequest extends @(type_name)_SendGoal_Request implements GoalRequestDefinition<@(type_name)> { + public List getGoalUuid() { + // Return List since it's hash is based on the values (not the object pointer) + return super.getGoalId().getUuidAsList(); + } + } + + public static class SendGoalResponse extends @(type_name)_SendGoal_Response implements GoalResponseDefinition<@(type_name)> { + public void accept(boolean accepted) { + super.setAccepted(accepted); + } + + public void setStamp(int sec, int nanosec) { + builtin_interfaces.msg.Time msg = new builtin_interfaces.msg.Time(); + msg.setSec(sec); + msg.setNanosec(nanosec); + super.setStamp(msg); + } + } + + public Class getSendGoalRequestType() { + return SendGoalRequest.class; + } + + public Class getSendGoalResponseType() { + return SendGoalResponse.class; + } + + public Class getGetResultRequestType() { + return @(type_name)_GetResult_Request.class; + } + + public Class getGetResultResponseType() { + return @(type_name)_GetResult_Response.class; + } + private static final Logger logger = LoggerFactory.getLogger(@(type_name).class); static { diff --git a/rosidl_generator_java/resource/msg.java.em b/rosidl_generator_java/resource/msg.java.em index c8912488..9dcb2a42 100644 --- a/rosidl_generator_java/resource/msg.java.em +++ b/rosidl_generator_java/resource/msg.java.em @@ -38,7 +38,7 @@ import @('.'.join(member.type.namespaced_name())); @[ end if]@ @[end for]@ -public final class @(type_name) implements MessageDefinition { +public class @(type_name) implements MessageDefinition { private static final Logger logger = LoggerFactory.getLogger(@(type_name).class); From d2ea45d985dae4e3bb68559284a5191a24f2be37 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Thu, 21 Jan 2021 10:08:51 -0800 Subject: [PATCH 29/51] Add action server with logic for accepting and canceling goals (osrf/ros2_java#53) * Add interfaces for action goal, result, and feedback Implementing these interfaces in the code generation template makes it easier to pass around these types in a generic way. Note, the 'final' modifier had to be removed from generated message types in order to extend goal, result, and feedback types in action definitions. Signed-off-by: Jacob Perron * Add new definitions for action goal response and request Signed-off-by: Jacob Perron * Add getter for UUID to SendGoalRequest Also make inner classes static. Signed-off-by: Jacob Perron * Add getStamp method to GoalResponseDefinition Signed-off-by: Jacob Perron * Partially revert "Add interfaces for action goal, result, and feedback" Partially revert commit dd046147d38c1aa844376149f62c4f3aaec04d96. I don't think we need to aliases for the message types, but I'll add them back if they turn out to be useful. * Parameterize goal request and response interfaces on action type Signed-off-by: Jacob Perron * Fix getGoalUuid implementation It should return a List, since it is hashable. Signed-off-by: Jacob Perron * List -> byte[] Signed-off-by: Jacob Perron * Add ActionServer skeleton * Add new action module with classes/interfaces related to the ActionServer * Add methods for creating and removing ActionServers from a Node * Implement dispose method for ActionServer. Signed-off-by: Jacob Perron * Add ActionServer creation logic and unit tests Signed-off-by: Jacob Perron * Add action server logic for accepting and canceling goals This changeset includes the following: * an implementation of the goal handle for action servers * action server integration with the base executor * action server integration with ROS nodes * unit tests for the action server, receiving goal and cancel requests Signed-off-by: Jacob Perron * cleanup Signed-off-by: Jacob Perron * more cleanup Signed-off-by: Jacob Perron * Minor refactor: move response enums into callback interfaces Signed-off-by: Jacob Perron * Remove TODO The goal request already contains a getter for the goal ID. Signed-off-by: Jacob Perron * Fix accident Signed-off-by: Jacob Perron * Alphabetize Signed-off-by: Jacob Perron * Refactor: replace 'getNumberOf*()' JNI functions with single 'getNumberOfEntites()' function Signed-off-by: Jacob Perron * Avoid string copy Signed-off-by: Jacob Perron * Remove extern C Replace with 'namespace rcljava' Signed-off-by: Jacob Perron * Remove unnecessary synchronization Signed-off-by: Jacob Perron * Refactor access to goalCallback Signed-off-by: Jacob Perron * typesafe request and response definitions Signed-off-by: Jacob Perron * Be more specific about template type for GoalCallback Signed-off-by: Jacob Perron * Minor refactor Signed-off-by: Jacob Perron * Make GoalHandleImpl inner class instead of static Signed-off-by: Jacob Perron * Remove synchronized from getHandle Signed-off-by: Jacob Perron * Add TODO for Waitable interface Signed-off-by: Jacob Perron * Fix style Signed-off-by: Jacob Perron * Minor refactor Signed-off-by: Jacob Perron * Add docs for createActionServer Signed-off-by: Jacob Perron * Switch from List to array Signed-off-by: Jacob Perron --- rcljava/CMakeLists.txt | 58 +- ...org_ros2_rcljava_action_ActionServerImpl.h | 138 +++++ ...a_action_ActionServerImpl_GoalHandleImpl.h | 99 ++++ .../org_ros2_rcljava_executors_BaseExecutor.h | 9 + rcljava/package.xml | 11 +- rcljava/src/main/cpp/convert.hpp | 88 +++ ...g_ros2_rcljava_action_ActionServerImpl.cpp | 365 ++++++++++++ ...action_ActionServerImpl_GoalHandleImpl.cpp | 149 +++++ ...rg_ros2_rcljava_executors_BaseExecutor.cpp | 84 +-- .../org/ros2/rcljava/action/ActionServer.java | 66 +++ .../action/ActionServerGoalHandle.java | 63 ++ .../ros2/rcljava/action/ActionServerImpl.java | 547 ++++++++++++++++++ .../ros2/rcljava/action/CancelCallback.java | 33 ++ .../org/ros2/rcljava/action/GoalCallback.java | 33 ++ .../org/ros2/rcljava/action/GoalStatus.java | 47 ++ .../ros2/rcljava/executors/AnyExecutable.java | 2 + .../ros2/rcljava/executors/BaseExecutor.java | 52 ++ .../main/java/org/ros2/rcljava/node/Node.java | 41 ++ .../java/org/ros2/rcljava/node/NodeImpl.java | 39 ++ .../ros2/rcljava/action/ActionServerTest.java | 234 ++++++++ .../ros2/rcljava/action/MockActionClient.java | 104 ++++ 21 files changed, 2196 insertions(+), 66 deletions(-) create mode 100644 rcljava/include/org_ros2_rcljava_action_ActionServerImpl.h create mode 100644 rcljava/include/org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl.h create mode 100644 rcljava/src/main/cpp/convert.hpp create mode 100644 rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp create mode 100644 rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl.cpp create mode 100644 rcljava/src/main/java/org/ros2/rcljava/action/ActionServer.java create mode 100644 rcljava/src/main/java/org/ros2/rcljava/action/ActionServerGoalHandle.java create mode 100644 rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java create mode 100644 rcljava/src/main/java/org/ros2/rcljava/action/CancelCallback.java create mode 100644 rcljava/src/main/java/org/ros2/rcljava/action/GoalCallback.java create mode 100644 rcljava/src/main/java/org/ros2/rcljava/action/GoalStatus.java create mode 100644 rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java create mode 100644 rcljava/src/test/java/org/ros2/rcljava/action/MockActionClient.java diff --git a/rcljava/CMakeLists.txt b/rcljava/CMakeLists.txt index b357f0d4..983eabf4 100644 --- a/rcljava/CMakeLists.txt +++ b/rcljava/CMakeLists.txt @@ -2,16 +2,19 @@ cmake_minimum_required(VERSION 3.5) project(rcljava) +find_package(action_msgs REQUIRED) 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(rcl REQUIRED) +find_package(rcl_action REQUIRED) find_package(rcl_interfaces REQUIRED) find_package(rcljava_common REQUIRED) find_package(rmw REQUIRED) find_package(rmw_implementation_cmake REQUIRED) find_package(rosgraph_msgs REQUIRED) +find_package(unique_identifier_msgs REQUIRED) include(CrossCompilingExtra) @@ -57,6 +60,8 @@ endif() set(${PROJECT_NAME}_jni_sources "src/main/cpp/org_ros2_rcljava_RCLJava.cpp" "src/main/cpp/org_ros2_rcljava_Time.cpp" + "src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp" + "src/main/cpp/org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl.cpp" "src/main/cpp/org_ros2_rcljava_client_ClientImpl.cpp" "src/main/cpp/org_ros2_rcljava_contexts_ContextImpl.cpp" "src/main/cpp/org_ros2_rcljava_detail_QosIncompatibleStatus.cpp" @@ -105,11 +110,14 @@ foreach(_jni_source ${${PROJECT_NAME}_jni_sources}) endif() ament_target_dependencies(${_target_name} - "rcl" - "rcljava_common" + "action_msgs" "builtin_interfaces" + "rcl" + "rcl_action" "rcl_interfaces" + "rcljava_common" "rosgraph_msgs" + "unique_identifier_msgs" ) target_include_directories(${_target_name} @@ -129,6 +137,12 @@ endforeach() set(${PROJECT_NAME}_sources "src/main/java/org/ros2/rcljava/RCLJava.java" "src/main/java/org/ros2/rcljava/Time.java" + "src/main/java/org/ros2/rcljava/action/ActionServer.java" + "src/main/java/org/ros2/rcljava/action/ActionServerGoalHandle.java" + "src/main/java/org/ros2/rcljava/action/ActionServerImpl.java" + "src/main/java/org/ros2/rcljava/action/CancelCallback.java" + "src/main/java/org/ros2/rcljava/action/GoalCallback.java" + "src/main/java/org/ros2/rcljava/action/GoalStatus.java" "src/main/java/org/ros2/rcljava/client/Client.java" "src/main/java/org/ros2/rcljava/client/ClientImpl.java" "src/main/java/org/ros2/rcljava/concurrent/Callback.java" @@ -205,10 +219,12 @@ add_jar("${PROJECT_NAME}_jar" OUTPUT_NAME ${PROJECT_NAME} INCLUDE_JARS - ${rcljava_common_JARS} + ${action_msgs_JARS} ${builtin_interfaces_JARS} ${rcl_interfaces_JARS} + ${rcljava_common_JARS} ${rosgraph_msgs_JARS} + ${unique_identifier_msgs_JARS} ) install_jar("${PROJECT_NAME}_jar" "share/${PROJECT_NAME}/java") @@ -218,6 +234,7 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) find_package(std_msgs REQUIRED) find_package(mockito_vendor REQUIRED) + find_package(test_msgs REQUIRED) ament_lint_auto_find_test_dependencies() set(${PROJECT_NAME}_message_files @@ -245,9 +262,11 @@ if(BUILD_TESTING) ${${PROJECT_NAME}_message_files} ${${PROJECT_NAME}_service_files} DEPENDENCIES + action_msgs builtin_interfaces rcl_interfaces rosgraph_msgs + unique_identifier_msgs ${_java_type_supports} SKIP_INSTALL ) @@ -264,6 +283,8 @@ if(BUILD_TESTING) "src/test/java/org/ros2/rcljava/RCLJavaTest.java" "src/test/java/org/ros2/rcljava/SpinTest.java" "src/test/java/org/ros2/rcljava/TimeTest.java" + "src/test/java/org/ros2/rcljava/action/ActionServerTest.java" + "src/test/java/org/ros2/rcljava/action/MockActionClient.java" "src/test/java/org/ros2/rcljava/client/ClientTest.java" "src/test/java/org/ros2/rcljava/contexts/ContextTest.java" "src/test/java/org/ros2/rcljava/node/NodeOptionsTest.java" @@ -283,6 +304,7 @@ if(BUILD_TESTING) "org.ros2.rcljava.RCLJavaTest" "org.ros2.rcljava.SpinTest" "org.ros2.rcljava.TimeTest" + "org.ros2.rcljava.action.ActionServerTest" "org.ros2.rcljava.client.ClientTest" "org.ros2.rcljava.contexts.ContextTest" "org.ros2.rcljava.node.NodeOptionsTest" @@ -344,6 +366,33 @@ if(BUILD_TESTING) list_append_unique(_deps_library_dirs ${_dep_dir}) endforeach() + foreach(_dep_lib ${action_msgs_LIBRARIES}) + get_filename_component(_dep_dir "${_dep_lib}" DIRECTORY) + list_append_unique(_deps_library_dirs ${_dep_dir}) + endforeach() + foreach(_dep_lib ${action_msgs_JNI_LIBRARIES}) + get_filename_component(_dep_dir "${_dep_lib}" DIRECTORY) + list_append_unique(_deps_library_dirs ${_dep_dir}) + endforeach() + + foreach(_dep_lib ${unique_identifier_msgs_LIBRARIES}) + get_filename_component(_dep_dir "${_dep_lib}" DIRECTORY) + list_append_unique(_deps_library_dirs ${_dep_dir}) + endforeach() + foreach(_dep_lib ${unique_identifier_msgs_JNI_LIBRARIES}) + get_filename_component(_dep_dir "${_dep_lib}" DIRECTORY) + list_append_unique(_deps_library_dirs ${_dep_dir}) + endforeach() + + foreach(_dep_lib ${test_msgs_LIBRARIES}) + get_filename_component(_dep_dir "${_dep_lib}" DIRECTORY) + list_append_unique(_deps_library_dirs ${_dep_dir}) + endforeach() + foreach(_dep_lib ${test_msgs_JNI_LIBRARIES}) + get_filename_component(_dep_dir "${_dep_lib}" DIRECTORY) + list_append_unique(_deps_library_dirs ${_dep_dir}) + endforeach() + list_append_unique(_deps_library_dirs ${CMAKE_CURRENT_BINARY_DIR}) list_append_unique(_deps_library_dirs ${CMAKE_CURRENT_BINARY_DIR}/rcljava) list_append_unique(_deps_library_dirs ${CMAKE_CURRENT_BINARY_DIR}/rosidl_generator_java/rcljava/msg/) @@ -359,13 +408,16 @@ if(BUILD_TESTING) TESTS "${testsuite}" INCLUDE_JARS + "${action_msgs_JARS}" "${rcljava_common_JARS}" "${rcljava_test_msgs_JARS}" "${std_msgs_JARS}" "${builtin_interfaces_JARS}" "${rcl_interfaces_JARS}" "${rosgraph_msgs_JARS}" + "${test_msgs_JARS}" "${mockito_vendor_JARS}" + "${unique_identifier_msgs_JARS}" "${_${PROJECT_NAME}_jar_file}" "${_${PROJECT_NAME}_messages_jar_file}" APPEND_LIBRARY_DIRS diff --git a/rcljava/include/org_ros2_rcljava_action_ActionServerImpl.h b/rcljava/include/org_ros2_rcljava_action_ActionServerImpl.h new file mode 100644 index 00000000..28b65441 --- /dev/null +++ b/rcljava/include/org_ros2_rcljava_action_ActionServerImpl.h @@ -0,0 +1,138 @@ +// Copyright 2020 ros2-java contributors +// +// 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_action_ActionServerImpl */ + +#ifndef ORG_ROS2_RCLJAVA_ACTION_ACTIONSERVERIMPL_H_ +#define ORG_ROS2_RCLJAVA_ACTION_ACTIONSERVERIMPL_H_ +#ifdef __cplusplus +extern "C" { +#endif + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeGetNumberOfEntities + * Signature: (L)[I + * Returns array of numbers for each type of entity, + * [subscriptions, guard_conditions, timers, clients, services] + */ +JNIEXPORT jintArray +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeGetNumberOfEntities( + JNIEnv *, jclass, jlong); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeGetReadyEntities + * Signature: (LL)[Z + */ +JNIEXPORT jbooleanArray +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeGetReadyEntities( + JNIEnv *, jclass, jlong, jlong); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeDispose + * Signature: (JJ)V + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeDispose(JNIEnv *, jclass, jlong, jlong); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeCreateActionServer + * Signature: (JLjava/lang/Class;Ljava/lang/String;)J + */ +JNIEXPORT jlong +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeCreateActionServer( + JNIEnv *, jobject, jlong, jlong, jclass, jstring); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeTakeGoalRequest + * Signature: (JJJJLorg/ros2/rcljava/interfaces/MessageDefinition;)Lorg/ros2/rcljava/RMWRequestId; + */ +JNIEXPORT jobject +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeTakeGoalRequest( + JNIEnv *, jclass, jlong, jlong, jlong, jlong, jobject); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeTakeCancelRequest + * Signature: (JJJJLorg/ros2/rcljava/interfaces/MessageDefinition;)Lorg/ros2/rcljava/RMWRequestId; + */ +JNIEXPORT jobject +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeTakeCancelRequest( + JNIEnv *, jclass, jlong, jlong, jlong, jlong, jobject); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeTakeResultRequest + * Signature: (JJJJLorg/ros2/rcljava/interfaces/MessageDefinition;)Lorg/ros2/rcljava/RMWRequestId; + */ +JNIEXPORT jobject +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeTakeResultRequest( + JNIEnv *, jclass, jlong, jlong, jlong, jlong, jobject); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeSendGoalResponse + * Signature: (JLorg/ros2/rcljava/RMWRequestId;JJJLorg/ros2/rcljava/interfaces/MessageDefinition;) + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeSendGoalResponse( + JNIEnv *, jclass, jlong, jobject, jlong, jlong, jlong, jobject); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeSendCancelResponse + * Signature: (JLorg/ros2/rcljava/RMWRequestId;JJJLorg/ros2/rcljava/interfaces/MessageDefinition;) + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeSendCancelResponse( + JNIEnv *, jclass, jlong, jobject, jlong, jlong, jlong, jobject); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeSendResultResponse + * Signature: (JLorg/ros2/rcljava/RMWRequestId;JJJLorg/ros2/rcljava/interfaces/MessageDefinition;) + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeSendResultResponse( + JNIEnv *, jclass, jlong, jobject, jlong, jlong, jlong, jobject); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeProcessCancelRequest + * Signature: (JJJJLorg/ros2/rcljava/interfaces/MessageDefinition;Lorg/ros2/rcljava/interfaces/MessageDefinition;) + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeProcessCancelRequest( + JNIEnv *, jclass, jlong, jlong, jlong, jlong, jobject, jobject); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeCheckGoalExists + * Signature: (JLorg/ros2/rcljava/interfaces/MessageDefinition;JJ)Z + */ + +JNIEXPORT jboolean +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeCheckGoalExists( + JNIEnv * env, jclass, + jlong, jobject, jlong, jlong); + +#ifdef __cplusplus +} +#endif +#endif // ORG_ROS2_RCLJAVA_ACTION_ACTIONSERVERIMPL_H__ diff --git a/rcljava/include/org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl.h b/rcljava/include/org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl.h new file mode 100644 index 00000000..94aaab23 --- /dev/null +++ b/rcljava/include/org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl.h @@ -0,0 +1,99 @@ +// Copyright 2020 ros2-java contributors +// +// 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_action_ActionServerImpl_GoalHandleImpl */ + +#ifndef ORG_ROS2_RCLJAVA_ACTION_ACTIONSERVERIMPL_GOALHANDLEIMPL_H_ +#define ORG_ROS2_RCLJAVA_ACTION_ACTIONSERVERIMPL_GOALHANDLEIMPL_H_ +#ifdef __cplusplus +extern "C" { +#endif + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl$GoalHandleImpl + * Method: nativeAcceptNewGoal + * Signature: (JJJLorg/ros2/rcljava/interfaces/MessageDefinition;)J + */ +JNIEXPORT jlong +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeAcceptNewGoal( + JNIEnv *, jclass, jlong, jlong, jlong, jobject); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl$GoalHandleImpl + * Method: nativeGetStatus + * Signature: (J)I + */ +JNIEXPORT int +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGetStatus( + JNIEnv *, jclass, jlong); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl$GoalHandleImpl + * Method: nativeGoalEventExecute + * Signature: (J) + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventExecute( + JNIEnv * env, jclass, jlong); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl$GoalHandleImpl + * Method: nativeGoalEventCancelGoal + * Signature: (J) + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventCancelGoal( + JNIEnv * env, jclass, jlong); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl$GoalHandleImpl + * Method: nativeGoalEventSucceed + * Signature: (J) + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventSucceed( + JNIEnv * env, jclass, jlong); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl$GoalHandleImpl + * Method: nativeGoalEventAbort + * Signature: (J) + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventAbort( + JNIEnv * env, jclass, jlong); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl$GoalHandleImpl + * Method: nativeGoalEventCanceled + * Signature: (J) + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventCanceled( + JNIEnv * env, jclass, jlong); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl$GoalHandleImpl + * Method: nativeDispose + * Signature: (J) + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeDipose( + JNIEnv *, jclass, jlong); + +#ifdef __cplusplus +} +#endif +#endif // ORG_ROS2_RCLJAVA_ACTION_ACTIONSERVERIMPL_GOALHANDLEIMPL_H__ diff --git a/rcljava/include/org_ros2_rcljava_executors_BaseExecutor.h b/rcljava/include/org_ros2_rcljava_executors_BaseExecutor.h index 5522ac8d..e96b88c8 100644 --- a/rcljava/include/org_ros2_rcljava_executors_BaseExecutor.h +++ b/rcljava/include/org_ros2_rcljava_executors_BaseExecutor.h @@ -98,6 +98,15 @@ JNIEXPORT void JNICALL Java_org_ros2_rcljava_executors_BaseExecutor_nativeWaitSetAddClient( JNIEnv *, jclass, jlong, jlong); +/* + * Class: org_ros2_rcljava_executors_BaseExecutor + * Method: nativeWaitSetAddActionServer + * Signature: (JJ)V + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_executors_BaseExecutor_nativeWaitSetAddActionServer( + JNIEnv *, jclass, jlong, jlong); + /* * Class: org_ros2_rcljava_executors_BaseExecutor * Method: nativeTakeRequest diff --git a/rcljava/package.xml b/rcljava/package.xml index eb817669..fa218c0c 100644 --- a/rcljava/package.xml +++ b/rcljava/package.xml @@ -12,15 +12,18 @@ ament_cmake_export_jni_libraries rcljava_common + action_msgs builtin_interfaces - rcl_interfaces rcl + rcl_action + rcl_interfaces rcpputils rmw_implementation_cmake rmw rosgraph_msgs rosidl_generator_c rosidl_typesupport_c + unique_identifier_msgs builtin_interfaces rcl_interfaces rmw @@ -29,15 +32,18 @@ rosidl_generator_java rosidl_typesupport_c + action_msgs builtin_interfaces - rcl_interfaces rcl + rcl_action + rcl_interfaces rcpputils rmw_implementation_cmake rmw_implementation rosgraph_msgs rosidl_runtime_c rosidl_parser + unique_identifier_msgs ament_lint_auto ament_lint_common @@ -52,6 +58,7 @@ rosidl_generator_cpp rosidl_generator_java std_msgs + test_msgs ament_cmake diff --git a/rcljava/src/main/cpp/convert.hpp b/rcljava/src/main/cpp/convert.hpp new file mode 100644 index 00000000..31db9655 --- /dev/null +++ b/rcljava/src/main/cpp/convert.hpp @@ -0,0 +1,88 @@ +// Copyright 2017-2018 Esteve Fernandez +// +// 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 + +#include +#include + +#include "rmw/rmw.h" + +#ifndef MAIN__CPP__CONVERT_HPP_ +#define MAIN__CPP__CONVERT_HPP_ + +namespace rcljava +{ + +jobject +convert_rmw_request_id_to_java(JNIEnv * env, rmw_request_id_t * request_id) +{ + jclass jrequest_id_class = env->FindClass("org/ros2/rcljava/service/RMWRequestId"); + assert(jrequest_id_class != nullptr); + + jmethodID jconstructor = env->GetMethodID(jrequest_id_class, "", "()V"); + assert(jconstructor != nullptr); + + jobject jrequest_id = env->NewObject(jrequest_id_class, jconstructor); + + jfieldID jsequence_number_field_id = env->GetFieldID(jrequest_id_class, "sequenceNumber", "J"); + jfieldID jwriter_guid_field_id = env->GetFieldID(jrequest_id_class, "writerGUID", "[B"); + + assert(jsequence_number_field_id != nullptr); + assert(jwriter_guid_field_id != nullptr); + + int8_t * writer_guid = request_id->writer_guid; + int64_t sequence_number = request_id->sequence_number; + + env->SetLongField(jrequest_id, jsequence_number_field_id, sequence_number); + + jsize writer_guid_len = sizeof(request_id->writer_guid) / sizeof(request_id->writer_guid[0]); + + jbyteArray jwriter_guid = env->NewByteArray(writer_guid_len); + env->SetByteArrayRegion(jwriter_guid, 0, writer_guid_len, reinterpret_cast(writer_guid)); + env->SetObjectField(jrequest_id, jwriter_guid_field_id, jwriter_guid); + + return jrequest_id; +} + +rmw_request_id_t * +convert_rmw_request_id_from_java(JNIEnv * env, jobject jrequest_id) +{ + assert(jrequest_id != nullptr); + + jclass jrequest_id_class = env->GetObjectClass(jrequest_id); + assert(jrequest_id_class != nullptr); + + jfieldID jsequence_number_field_id = env->GetFieldID(jrequest_id_class, "sequenceNumber", "J"); + jfieldID jwriter_guid_field_id = env->GetFieldID(jrequest_id_class, "writerGUID", "[B"); + + assert(jsequence_number_field_id != nullptr); + assert(jwriter_guid_field_id != nullptr); + + rmw_request_id_t * request_id = static_cast(malloc(sizeof(rmw_request_id_t))); + + int8_t * writer_guid = request_id->writer_guid; + request_id->sequence_number = env->GetLongField(jrequest_id, jsequence_number_field_id); + + jsize writer_guid_len = sizeof(request_id->writer_guid) / sizeof(request_id->writer_guid[0]); + + jbyteArray jwriter_guid = (jbyteArray)env->GetObjectField(jrequest_id, jwriter_guid_field_id); + env->GetByteArrayRegion(jwriter_guid, 0, writer_guid_len, reinterpret_cast(writer_guid)); + + return request_id; +} + +} // namespace rcljava + +#endif // MAIN__CPP__CONVERT_HPP_ diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp new file mode 100644 index 00000000..2d48fef1 --- /dev/null +++ b/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp @@ -0,0 +1,365 @@ +// Copyright 2020 ros2-java contributors +// +// 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 + +#include +#include +#include + +#include "rcl/error_handling.h" +#include "rcl/rcl.h" +#include "rcl_action/rcl_action.h" +#include "rosidl_runtime_c/message_type_support_struct.h" + +#include "rcljava_common/exceptions.hpp" +#include "rcljava_common/signatures.hpp" + +#include "org_ros2_rcljava_action_ActionServerImpl.h" + +#include "./convert.hpp" + +using rcljava_common::exceptions::rcljava_throw_rclexception; +using rcljava_common::signatures::convert_from_java_signature; +using rcljava_common::signatures::convert_to_java_signature; +using rcljava_common::signatures::destroy_ros_message_signature; + +JNIEXPORT jintArray +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeGetNumberOfEntities( + JNIEnv * env, jclass, jlong action_server_handle) +{ + size_t num_subscriptions; + size_t num_guard_conditions; + size_t num_timers; + size_t num_clients; + size_t num_services; + rcl_action_server_t * action_server = reinterpret_cast( + action_server_handle); + rcl_ret_t ret = rcl_action_server_wait_set_get_num_entities( + action_server, + &num_subscriptions, + &num_guard_conditions, + &num_timers, + &num_clients, + &num_services); + if (ret != RCL_RET_OK) { + std::string msg = + "Failed to get number of entities for an action server: " + + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + } + jintArray result = env->NewIntArray(5); + jint temp_result[5] = { + static_cast(num_subscriptions), + static_cast(num_guard_conditions), + static_cast(num_timers), + static_cast(num_clients), + static_cast(num_services) + }; + env->SetIntArrayRegion(result, 0, 5, temp_result); + return result; +} + +JNIEXPORT jbooleanArray +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeGetReadyEntities( + JNIEnv * env, jclass, jlong action_server_handle, jlong wait_set_handle) +{ + rcl_action_server_t * action_server = reinterpret_cast( + action_server_handle); + rcl_wait_set_t * wait_set = reinterpret_cast(wait_set_handle); + + bool is_goal_request_ready = false; + bool is_cancel_request_ready = false; + bool is_result_request_ready = false; + bool is_goal_expired = false; + rcl_ret_t ret = rcl_action_server_wait_set_get_entities_ready( + wait_set, + action_server, + &is_goal_request_ready, + &is_cancel_request_ready, + &is_result_request_ready, + &is_goal_expired); + if (RCL_RET_OK != ret) { + std::string msg = "Failed to get ready entities for action server: " + + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + return NULL; + } + + jbooleanArray result = env->NewBooleanArray(4); + jboolean temp_result[4] = { + is_goal_request_ready, + is_cancel_request_ready, + is_result_request_ready, + is_goal_expired + }; + env->SetBooleanArrayRegion(result, 0, 4, temp_result); + return result; +} + +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_action_ActionServerImpl_nativeDispose( + JNIEnv * env, jclass, jlong node_handle, jlong action_server_handle) +{ + if (action_server_handle == 0) { + // everything is ok, already destroyed + return; + } + + if (node_handle == 0) { + // TODO(jacobperron): throw exception + return; + } + + rcl_node_t * node = reinterpret_cast(node_handle); + + assert(node != NULL); + + rcl_action_server_t * action_server = reinterpret_cast( + action_server_handle); + + assert(action_server != NULL); + + rcl_ret_t ret = rcl_action_server_fini(action_server, node); + + if (ret != RCL_RET_OK) { + std::string msg = "Failed to destroy action server: " + + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + } +} + +JNIEXPORT jlong JNICALL +Java_org_ros2_rcljava_action_ActionServerImpl_nativeCreateActionServer( + JNIEnv * env, + jobject, + jlong node_handle, + jlong clock_handle, + jclass jaction_class, + jstring jaction_name) +{ + jmethodID mid = env->GetStaticMethodID(jaction_class, "getActionTypeSupport", "()J"); + assert(mid != NULL); + + jlong jts = env->CallStaticLongMethod(jaction_class, mid); + assert(jts != 0); + + const char * action_name = env->GetStringUTFChars(jaction_name, 0); + + rcl_node_t * node = reinterpret_cast(node_handle); + rcl_clock_t * clock = reinterpret_cast(clock_handle); + + rosidl_action_type_support_t * ts = reinterpret_cast(jts); + + rcl_action_server_t * action_server = static_cast( + malloc(sizeof(rcl_action_server_t))); + *action_server = rcl_action_get_zero_initialized_server(); + rcl_action_server_options_t action_server_ops = rcl_action_server_get_default_options(); + + rcl_ret_t ret = rcl_action_server_init( + action_server, node, clock, ts, action_name, &action_server_ops); + env->ReleaseStringUTFChars(jaction_name, action_name); + + if (ret != RCL_RET_OK) { + std::string msg = "Failed to create action server: " + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + free(action_server); + return 0; + } + + jlong jaction_server = reinterpret_cast(action_server); + return jaction_server; +} + +#define RCLJAVA_ACTION_SERVER_TAKE_REQUEST(Type) \ + do { \ + assert(jrequest_from_java_converter_handle != 0); \ + assert(jrequest_to_java_converter_handle != 0); \ + rcl_action_server_t * action_server = reinterpret_cast( \ + action_server_handle); \ + convert_from_java_signature convert_from_java = \ + reinterpret_cast(jrequest_from_java_converter_handle); \ + convert_to_java_signature convert_to_java = \ + reinterpret_cast(jrequest_to_java_converter_handle); \ + destroy_ros_message_signature destroy_ros_message = \ + reinterpret_cast(jrequest_destructor_handle); \ + void * taken_msg = convert_from_java(jrequest_msg, nullptr); \ + rmw_request_id_t header; \ + rcl_ret_t ret = rcl_action_take_ ## Type ## _request(action_server, &header, taken_msg); \ + if (ret != RCL_RET_OK && ret != RCL_RET_ACTION_SERVER_TAKE_FAILED) { \ + destroy_ros_message(taken_msg); \ + std::string msg = \ + "Failed to take " #Type " request: " + std::string(rcl_get_error_string().str); \ + rcl_reset_error(); \ + rcljava_throw_rclexception(env, ret, msg); \ + return nullptr; \ + } \ + if (RCL_RET_OK == ret) { \ + jobject jtaken_msg = convert_to_java(taken_msg, jrequest_msg); \ + destroy_ros_message(taken_msg); \ + assert(jtaken_msg != nullptr); \ + jobject jheader = rcljava::convert_rmw_request_id_to_java(env, &header); \ + return jheader; \ + } \ + destroy_ros_message(taken_msg); \ + return nullptr; \ + } \ + while (0) + +#define RCLJAVA_ACTION_SERVER_SEND_RESPONSE(Type) \ + do { \ + assert(jresponse_from_java_converter_handle != 0); \ + assert(jresponse_to_java_converter_handle != 0); \ + assert(jresponse_destructor_handle != 0); \ + rcl_action_server_t * action_server = reinterpret_cast( \ + action_server_handle); \ + convert_from_java_signature convert_from_java = \ + reinterpret_cast(jresponse_from_java_converter_handle); \ + void * response_msg = convert_from_java(jresponse_msg, nullptr); \ + rmw_request_id_t * request_id = rcljava::convert_rmw_request_id_from_java(env, jrequest_id); \ + rcl_ret_t ret = rcl_action_send_ ## Type ## _response( \ + action_server, request_id, response_msg); \ + destroy_ros_message_signature destroy_ros_message = \ + reinterpret_cast(jresponse_destructor_handle); \ + destroy_ros_message(response_msg); \ + if (ret != RCL_RET_OK) { \ + std::string msg = \ + "Failed to send " #Type " response: " + std::string(rcl_get_error_string().str); \ + rcl_reset_error(); \ + rcljava_throw_rclexception(env, ret, msg); \ + } \ + } \ + while (0) + +JNIEXPORT jobject +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeTakeGoalRequest( + JNIEnv * env, jclass, jlong action_server_handle, jlong jrequest_from_java_converter_handle, + jlong jrequest_to_java_converter_handle, jlong jrequest_destructor_handle, jobject jrequest_msg) +{ + RCLJAVA_ACTION_SERVER_TAKE_REQUEST(goal); +} + +JNIEXPORT jobject +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeTakeCancelRequest( + JNIEnv * env, jclass, jlong action_server_handle, jlong jrequest_from_java_converter_handle, + jlong jrequest_to_java_converter_handle, jlong jrequest_destructor_handle, jobject jrequest_msg) +{ + RCLJAVA_ACTION_SERVER_TAKE_REQUEST(cancel); +} + +JNIEXPORT jobject +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeTakeResultRequest( + JNIEnv * env, jclass, jlong action_server_handle, jlong jrequest_from_java_converter_handle, + jlong jrequest_to_java_converter_handle, jlong jrequest_destructor_handle, jobject jrequest_msg) +{ + RCLJAVA_ACTION_SERVER_TAKE_REQUEST(result); +} + +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeSendGoalResponse( + JNIEnv * env, jclass, jlong action_server_handle, jobject jrequest_id, + jlong jresponse_from_java_converter_handle, jlong jresponse_to_java_converter_handle, + jlong jresponse_destructor_handle, jobject jresponse_msg) +{ + RCLJAVA_ACTION_SERVER_SEND_RESPONSE(goal); +} + +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeSendCancelResponse( + JNIEnv * env, jclass, jlong action_server_handle, jobject jrequest_id, + jlong jresponse_from_java_converter_handle, jlong jresponse_to_java_converter_handle, + jlong jresponse_destructor_handle, jobject jresponse_msg) +{ + RCLJAVA_ACTION_SERVER_SEND_RESPONSE(cancel); +} + +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeSendResultResponse( + JNIEnv * env, jclass, jlong action_server_handle, jobject jrequest_id, + jlong jresponse_from_java_converter_handle, jlong jresponse_to_java_converter_handle, + jlong jresponse_destructor_handle, jobject jresponse_msg) +{ + RCLJAVA_ACTION_SERVER_SEND_RESPONSE(result); +} + +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeProcessCancelRequest( + JNIEnv * env, jclass, + jlong action_server_handle, + jlong jrequest_from_java_converter_handle, + jlong jrequest_destructor_handle, + jlong jresponse_to_java_converter_handle, + jobject jrequest_msg, + jobject jresponse_msg) +{ + assert(jrequest_from_java_converter_handle != 0); + assert(jrequest_destructor_handle != 0); + assert(jresponse_to_java_converter_handle != 0); + + rcl_action_server_t * action_server = reinterpret_cast( + action_server_handle); + convert_from_java_signature request_convert_from_java = + reinterpret_cast(jrequest_from_java_converter_handle); + convert_to_java_signature response_convert_to_java = reinterpret_cast( + jresponse_to_java_converter_handle); + destroy_ros_message_signature request_destroy_ros_message = + reinterpret_cast(jrequest_destructor_handle); + + rcl_action_cancel_request_t * request_msg = reinterpret_cast( + request_convert_from_java(jrequest_msg, nullptr)); + rcl_action_cancel_response_t response_msg = rcl_action_get_zero_initialized_cancel_response(); + + rcl_ret_t ret = rcl_action_process_cancel_request( + action_server, request_msg, &response_msg); + request_destroy_ros_message(request_msg); + if (ret != RCL_RET_OK) { + std::string msg = \ + "Failed to process cancel request: " + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + return; + } + + response_convert_to_java(&response_msg, jresponse_msg); +} + +JNIEXPORT jboolean +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeCheckGoalExists( + JNIEnv *, jclass, + jlong jaction_server, + jobject jgoal_info, + jlong jgoal_info_from_java_converter_handle, + jlong jgoal_info_destructor_handle) +{ + assert(0 != jgoal_info_from_java_converter_handle); + assert(0 != jgoal_info_destructor_handle); + + rcl_action_server_t * action_server = reinterpret_cast( + jaction_server); + convert_from_java_signature convert_from_java = + reinterpret_cast(jgoal_info_from_java_converter_handle); + destroy_ros_message_signature destroy_ros_message = + reinterpret_cast(jgoal_info_destructor_handle); + + rcl_action_goal_info_t * goal_info = + reinterpret_cast(convert_from_java(jgoal_info, nullptr)); + bool exists = rcl_action_server_goal_exists(action_server, goal_info); + destroy_ros_message(goal_info); + + return exists; +} diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl.cpp new file mode 100644 index 00000000..de1fb4b6 --- /dev/null +++ b/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl.cpp @@ -0,0 +1,149 @@ +// Copyright 2020 ros2-java contributors +// +// 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 + +#include +#include +#include + +#include "rcl/error_handling.h" +#include "rcl_action/rcl_action.h" + +#include "rcljava_common/exceptions.hpp" +#include "rcljava_common/signatures.hpp" + +#include "org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl.h" + +using rcljava_common::exceptions::rcljava_throw_rclexception; +using rcljava_common::signatures::convert_from_java_signature; +using rcljava_common::signatures::destroy_ros_message_signature; + +JNIEXPORT jlong +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeAcceptNewGoal( + JNIEnv * env, jclass, jlong action_server_handle, + jlong jgoal_info_from_java_converter_handle, jlong jgoal_info_destructor_handle, + jobject jgoal_info_message) +{ + assert(jgoal_info_from_java_converter_handle != 0); + + rcl_action_server_t * action_server = reinterpret_cast( + action_server_handle); + convert_from_java_signature convert_from_java = + reinterpret_cast(jgoal_info_from_java_converter_handle); + destroy_ros_message_signature destroy_ros_message = + reinterpret_cast(jgoal_info_destructor_handle); + + rcl_action_goal_info_t * goal_info_message = + reinterpret_cast(convert_from_java(jgoal_info_message, nullptr)); + + rcl_action_goal_handle_t * goal_handle = rcl_action_accept_new_goal( + action_server, goal_info_message); + destroy_ros_message(goal_info_message); + if (!goal_handle) { + std::string msg = "Failed to accept new goal: " + std::string(rcl_get_error_string().str); + rcl_reset_error(); + // '1' is arbitrary since we don't have a specific return code + rcljava_throw_rclexception(env, 1, msg); + return 0; + } + + jlong jgoal_handle = reinterpret_cast(goal_handle); + return jgoal_handle; +} + +JNIEXPORT int +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGetStatus( + JNIEnv * env, jclass, jlong jgoal_handle) +{ + rcl_action_goal_handle_t * goal_handle = reinterpret_cast( + jgoal_handle); + rcl_action_goal_state_t status; + rcl_ret_t ret = rcl_action_goal_handle_get_status(goal_handle, &status); + if (RCL_RET_OK != ret) { + std::string msg = "Failed to get goal status: " + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + return static_cast(GOAL_STATE_UNKNOWN); + } + + return static_cast(status); +} + +static void update_goal_state(JNIEnv * env, jlong jgoal_handle, rcl_action_goal_event_t event) +{ + rcl_action_goal_handle_t * goal_handle = reinterpret_cast( + jgoal_handle); + rcl_ret_t ret = rcl_action_update_goal_state(goal_handle, event); + if (RCL_RET_OK != ret) { + std::string msg = "Failed to update goal state with event: " + + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + } +} + +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventExecute( + JNIEnv * env, jclass, jlong jgoal_handle) +{ + update_goal_state(env, jgoal_handle, GOAL_EVENT_EXECUTE); +} + +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventCancelGoal( + JNIEnv * env, jclass, jlong jgoal_handle) +{ + update_goal_state(env, jgoal_handle, GOAL_EVENT_CANCEL_GOAL); +} + +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventSucceed( + JNIEnv * env, jclass, jlong jgoal_handle) +{ + update_goal_state(env, jgoal_handle, GOAL_EVENT_SUCCEED); +} + +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventAbort( + JNIEnv * env, jclass, jlong jgoal_handle) +{ + update_goal_state(env, jgoal_handle, GOAL_EVENT_ABORT); +} + +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventCanceled( + JNIEnv * env, jclass, jlong jgoal_handle) +{ + update_goal_state(env, jgoal_handle, GOAL_EVENT_CANCELED); +} + +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeDipose( + JNIEnv * env, jclass, jlong jgoal_handle) +{ + rcl_action_goal_handle_t * goal_handle = reinterpret_cast( + jgoal_handle); + if (!goal_handle) { + // Nothing to dispose + return; + } + + rcl_ret_t ret = rcl_action_goal_handle_fini(goal_handle); + if (RCL_RET_OK != ret) { + std::string msg = "Failed to finalize goal handle: " + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + } +} diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_executors_BaseExecutor.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_executors_BaseExecutor.cpp index 287a175f..2a784dab 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_executors_BaseExecutor.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_executors_BaseExecutor.cpp @@ -22,6 +22,7 @@ #include "rcl/node.h" #include "rcl/rcl.h" #include "rcl/timer.h" +#include "rcl_action/rcl_action.h" #include "rmw/rmw.h" #include "rosidl_runtime_c/message_type_support_struct.h" @@ -30,69 +31,13 @@ #include "org_ros2_rcljava_executors_BaseExecutor.h" +#include "./convert.hpp" + using rcljava_common::exceptions::rcljava_throw_rclexception; using rcljava_common::signatures::convert_from_java_signature; using rcljava_common::signatures::convert_to_java_signature; using rcljava_common::signatures::destroy_ros_message_signature; -jobject -convert_rmw_request_id_to_java(JNIEnv * env, rmw_request_id_t * request_id) -{ - jclass jrequest_id_class = env->FindClass("org/ros2/rcljava/service/RMWRequestId"); - assert(jrequest_id_class != nullptr); - - jmethodID jconstructor = env->GetMethodID(jrequest_id_class, "", "()V"); - assert(jconstructor != nullptr); - - jobject jrequest_id = env->NewObject(jrequest_id_class, jconstructor); - - jfieldID jsequence_number_field_id = env->GetFieldID(jrequest_id_class, "sequenceNumber", "J"); - jfieldID jwriter_guid_field_id = env->GetFieldID(jrequest_id_class, "writerGUID", "[B"); - - assert(jsequence_number_field_id != nullptr); - assert(jwriter_guid_field_id != nullptr); - - int8_t * writer_guid = request_id->writer_guid; - int64_t sequence_number = request_id->sequence_number; - - env->SetLongField(jrequest_id, jsequence_number_field_id, sequence_number); - - jsize writer_guid_len = 16; // See rmw/rmw/include/rmw/types.h - - jbyteArray jwriter_guid = env->NewByteArray(writer_guid_len); - env->SetByteArrayRegion(jwriter_guid, 0, writer_guid_len, reinterpret_cast(writer_guid)); - env->SetObjectField(jrequest_id, jwriter_guid_field_id, jwriter_guid); - - return jrequest_id; -} - -rmw_request_id_t * -convert_rmw_request_id_from_java(JNIEnv * env, jobject jrequest_id) -{ - assert(jrequest_id != nullptr); - - jclass jrequest_id_class = env->GetObjectClass(jrequest_id); - assert(jrequest_id_class != nullptr); - - jfieldID jsequence_number_field_id = env->GetFieldID(jrequest_id_class, "sequenceNumber", "J"); - jfieldID jwriter_guid_field_id = env->GetFieldID(jrequest_id_class, "writerGUID", "[B"); - - assert(jsequence_number_field_id != nullptr); - assert(jwriter_guid_field_id != nullptr); - - rmw_request_id_t * request_id = static_cast(malloc(sizeof(rmw_request_id_t))); - - int8_t * writer_guid = request_id->writer_guid; - request_id->sequence_number = env->GetLongField(jrequest_id, jsequence_number_field_id); - - jsize writer_guid_len = 16; // See rmw/rmw/include/rmw/types.h - - jbyteArray jwriter_guid = (jbyteArray)env->GetObjectField(jrequest_id, jwriter_guid_field_id); - env->GetByteArrayRegion(jwriter_guid, 0, writer_guid_len, reinterpret_cast(writer_guid)); - - return request_id; -} - JNIEXPORT jlong JNICALL Java_org_ros2_rcljava_executors_BaseExecutor_nativeGetZeroInitializedWaitSet(JNIEnv *, jclass) { @@ -261,6 +206,23 @@ Java_org_ros2_rcljava_executors_BaseExecutor_nativeWaitSetAddClient( } } +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_executors_BaseExecutor_nativeWaitSetAddActionServer( + JNIEnv * env, jclass, jlong wait_set_handle, jlong action_server_handle) +{ + rcl_wait_set_t * wait_set = reinterpret_cast(wait_set_handle); + rcl_action_server_t * action_server = reinterpret_cast( + action_server_handle); + + rcl_ret_t ret = rcl_action_wait_set_add_action_server(wait_set, action_server, NULL); + if (ret != RCL_RET_OK) { + std::string msg = + "Failed to add action server to wait set: " + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + } +} + JNIEXPORT void JNICALL Java_org_ros2_rcljava_executors_BaseExecutor_nativeWaitSetAddTimer( JNIEnv * env, jclass, jlong wait_set_handle, jlong timer_handle) @@ -335,7 +297,7 @@ Java_org_ros2_rcljava_executors_BaseExecutor_nativeTakeRequest( assert(jtaken_msg != nullptr); - jobject jheader = convert_rmw_request_id_to_java(env, &header); + jobject jheader = rcljava::convert_rmw_request_id_to_java(env, &header); return jheader; } @@ -363,7 +325,7 @@ Java_org_ros2_rcljava_executors_BaseExecutor_nativeSendServiceResponse( void * response_msg = convert_from_java(jresponse_msg, nullptr); - rmw_request_id_t * request_id = convert_rmw_request_id_from_java(env, jrequest_id); + rmw_request_id_t * request_id = rcljava::convert_rmw_request_id_from_java(env, jrequest_id); rcl_ret_t ret = rcl_send_response(service, request_id, response_msg); @@ -425,7 +387,7 @@ Java_org_ros2_rcljava_executors_BaseExecutor_nativeTakeResponse( assert(jtaken_msg != nullptr); - jobject jheader = convert_rmw_request_id_to_java(env, &header); + jobject jheader = rcljava::convert_rmw_request_id_to_java(env, &header); return jheader; } diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServer.java b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServer.java new file mode 100644 index 00000000..4ab12f2e --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServer.java @@ -0,0 +1,66 @@ +/* Copyright 2020 ros2-java contributors + * + * 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.action; + +import java.util.Collection; + +import org.ros2.rcljava.interfaces.Disposable; +import org.ros2.rcljava.interfaces.ActionDefinition; + +public interface ActionServer extends Disposable { + // TODO(jacobperron): Move most of these methods to a new "Waitable" interface + /** + * Get the number of underlying subscriptions that the action server uses. + * + * @return The number of subscriptions. + */ + int getNumberOfSubscriptions(); + + /** + * Get the number of underlying timers that the action server uses. + * + * @return The number of timers. + */ + int getNumberOfTimers(); + + /** + * Get the number of underlying clients that the action server uses. + * + * @return The number of clients. + */ + int getNumberOfClients(); + + /** + * Get the number of underlying services that the action server uses. + * + * @return The number of services. + */ + int getNumberOfServices(); + + /** + * Check if an entity of the action server is ready in the wait set. + * + * @param waitSetHandle Handle to the rcl wait set that this action server was added to. + * + * @return true if at least one entity is ready, false otherwise. + */ + boolean isReady(long waitSetHandle); + + /** + * Execute any entities that are ready in the underlying wait set. + */ + void execute(); +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerGoalHandle.java b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerGoalHandle.java new file mode 100644 index 00000000..aedf0dbe --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerGoalHandle.java @@ -0,0 +1,63 @@ +/* Copyright 2020 ros2-java contributors + * + * 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.action; + +import org.ros2.rcljava.interfaces.ActionDefinition; +import org.ros2.rcljava.interfaces.Disposable; +import org.ros2.rcljava.interfaces.MessageDefinition; + +public interface ActionServerGoalHandle extends Disposable { + /** + * Get the message containing the timestamp and ID for the goal. + */ + public action_msgs.msg.GoalInfo getGoalInfo(); + + /** + * Get the goal message. + */ + public MessageDefinition getGoal(); + + /** + * Get the goal status. + */ + public GoalStatus getGoalStatus(); + + /** + * Returns true if the goal is in the CANCELING state. + */ + public boolean isCanceling(); + + /** + * Transition the goal to the SUCCEEDED state. + * + * Pre-condition: the goal must be in the EXECUTING or CANCELING state. + */ + public void succeed(); + + /** + * Transition the goal the the CANCELED state. + * + * Pre-condition: the goal must be in the CANCELING state. + */ + public void canceled(); + + /** + * Transition the goal the the CANCELED state. + * + * Pre-condition: the goal must be in the EXCUTING or CANCELING state. + */ + public void abort(); +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java new file mode 100644 index 00000000..8c883f25 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java @@ -0,0 +1,547 @@ +/* Copyright 2020 ros2-java contributors + * + * 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.action; + +import java.lang.ref.WeakReference; +import java.util.ArrayList; +import java.util.Collection; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +import org.ros2.rcljava.RCLJava; +import org.ros2.rcljava.common.JNIUtils; +import org.ros2.rcljava.consumers.Consumer; +import org.ros2.rcljava.interfaces.MessageDefinition; +import org.ros2.rcljava.interfaces.ActionDefinition; +import org.ros2.rcljava.interfaces.GoalRequestDefinition; +import org.ros2.rcljava.interfaces.GoalResponseDefinition; +import org.ros2.rcljava.node.Node; +import org.ros2.rcljava.service.RMWRequestId; +import org.ros2.rcljava.time.Clock; +import org.ros2.rcljava.Time; + +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +public class ActionServerImpl implements ActionServer { + private static final Logger logger = LoggerFactory.getLogger(ActionServerImpl.class); + + static { + try { + JNIUtils.loadImplementation(ActionServerImpl.class); + JNIUtils.loadImplementation(ActionServerImpl.GoalHandleImpl.class); + } catch (UnsatisfiedLinkError ule) { + logger.error("Native code library failed to load.\n" + ule); + System.exit(1); + } + JNIUtils.loadImplementation(ActionServerImpl.class); + } + + class GoalHandleImpl implements ActionServerGoalHandle { + private long handle; + private ActionServer actionServer; + private action_msgs.msg.GoalInfo goalInfo; + private MessageDefinition goal; + + private native long nativeAcceptNewGoal( + long actionServerHandle, + long goalInfoFromJavaConverterHandle, + long goalInfoDestructorHandle, + MessageDefinition goalInfo); + private native int nativeGetStatus(long goalHandle); + private native void nativeGoalEventExecute(long goalHandle); + private native void nativeGoalEventCancelGoal(long goalHandle); + private native void nativeGoalEventSucceed(long goalHandle); + private native void nativeGoalEventAbort(long goalHandle); + private native void nativeGoalEventCanceled(long goalHandle); + private native void nativeDispose(long handle); + + public GoalHandleImpl( + ActionServer actionServer, action_msgs.msg.GoalInfo goalInfo, MessageDefinition goal) + { + this.actionServer = actionServer; + this.goalInfo = goalInfo; + this.goal = goal; + long goalInfoFromJavaConverterHandle = goalInfo.getFromJavaConverterInstance(); + long goalInfoDestructorHandle = goalInfo.getDestructorInstance(); + this.handle = nativeAcceptNewGoal( + actionServer.getHandle(), + goalInfoFromJavaConverterHandle, + goalInfoDestructorHandle, + goalInfo); + } + + /** + * {@inheritDoc} + */ + public action_msgs.msg.GoalInfo getGoalInfo() { + return this.goalInfo; + } + + /** + * {@inheritDoc} + */ + public MessageDefinition getGoal() { + return this.goal; + } + + /** + * {@inheritDoc} + */ + public synchronized GoalStatus getGoalStatus() { + int status = nativeGetStatus(this.handle); + return GoalStatus.fromMessageValue((byte)status); + } + + /** + * {@inheritDoc} + */ + public synchronized boolean isCanceling() { + return this.getGoalStatus() == GoalStatus.CANCELING; + } + + /** + * Transition the goal to the EXECUTING state. + */ + public synchronized void execute() { + // It's possible that there has been a request to cancel the goal prior to executing. + // In this case we want to avoid the illegal state transition to EXECUTING + // but still call the users execute callback to let them handle canceling the goal. + if (!this.isCanceling()) { + nativeGoalEventExecute(this.handle); + } + } + + /** + * Transition the goal to the CANCELING state. + */ + public synchronized void cancelGoal() { + nativeGoalEventCancelGoal(this.handle); + } + + /** + * {@inheritDoc} + */ + public synchronized void succeed() { + nativeGoalEventSucceed(this.handle); + } + + /** + * {@inheritDoc} + */ + public synchronized void canceled() { + nativeGoalEventCanceled(this.handle); + } + + /** + * {@inheritDoc} + */ + public synchronized void abort() { + nativeGoalEventAbort(this.handle); + } + + /** + * {@inheritDoc} + */ + public synchronized final void dispose() { + nativeDispose(this.handle); + this.handle = 0; + } + + /** + * {@inheritDoc} + */ + public final long getHandle() { + return this.handle; + } + } // class GoalHandleImpl + + private final WeakReference nodeReference; + private final Clock clock; + private final T actionTypeInstance; + private final String actionName; + private long handle; + private final GoalCallback goalCallback; + private final CancelCallback cancelCallback; + private final Consumer> acceptedCallback; + + private boolean[] readyEntities; + + private Map, GoalHandleImpl> goalHandles; + + private boolean isGoalRequestReady() { + return this.readyEntities[0]; + } + + private boolean isCancelRequestReady() { + return this.readyEntities[1]; + } + + private boolean isResultRequestReady() { + return this.readyEntities[2]; + } + + private boolean isGoalExpiredReady() { + return this.readyEntities[3]; + } + + private native long nativeCreateActionServer( + long nodeHandle, long clockHandle, Class cls, String actionName); + + /** + * Create an action server. + * + * @param nodeReference A reference to the node to use to create this action server. + * @param actionType The type of the action. + * @param actionName The name of the action. + * @param goalCallback Callback triggered when a new goal request is received. + * @param cancelCallback Callback triggered when a new cancel request is received. + * @param acceptedCallback Callback triggered when a new goal is accepted. + */ + public ActionServerImpl( + final WeakReference nodeReference, + final Class actionType, + final String actionName, + final GoalCallback> goalCallback, + final CancelCallback cancelCallback, + final Consumer> acceptedCallback) throws IllegalArgumentException { + this.nodeReference = nodeReference; + try { + this.actionTypeInstance = actionType.newInstance(); + } catch (Exception ex) { + throw new IllegalArgumentException("Failed to instantiate provided action type", ex); + } + this.actionName = actionName; + this.goalCallback = goalCallback; + this.cancelCallback = cancelCallback; + this.acceptedCallback = acceptedCallback; + + this.goalHandles = new HashMap, GoalHandleImpl>(); + + Node node = nodeReference.get(); + if (node == null) { + throw new IllegalArgumentException("Node reference is null"); + } + + this.clock = node.getClock(); + + this.handle = nativeCreateActionServer( + node.getHandle(), node.getClock().getHandle(), actionType, actionName); + // TODO(jacobperron): Introduce 'Waitable' interface for entities like timers, services, etc + // node.addWaitable(this); + } + + private static native int[] nativeGetNumberOfEntities(long handle); + + /** + * {@inheritDoc} + */ + public int getNumberOfSubscriptions() { + return nativeGetNumberOfEntities(this.handle)[0]; + } + + /** + * {@inheritDoc} + */ + public int getNumberOfTimers() { + return nativeGetNumberOfEntities(this.handle)[2]; + } + + /** + * {@inheritDoc} + */ + public int getNumberOfClients() { + return nativeGetNumberOfEntities(this.handle)[3]; + } + + /** + * {@inheritDoc} + */ + public int getNumberOfServices() { + return nativeGetNumberOfEntities(this.handle)[4]; + } + + private static native boolean[] nativeGetReadyEntities( + long actionServerHandle, long waitSetHandle); + + /** + * {@inheritDoc} + */ + public boolean isReady(long waitSetHandle) { + this.readyEntities = nativeGetReadyEntities(this.handle, waitSetHandle); + for (boolean isReady : this.readyEntities) { + if (isReady) { + return true; + } + } + return false; + } + + private ActionServerGoalHandle executeGoalRequest( + RMWRequestId rmwRequestId, + GoalRequestDefinition requestMessage, + GoalResponseDefinition responseMessage) + { + builtin_interfaces.msg.Time timeRequestHandled = this.clock.now().toMsg(); + responseMessage.setStamp(timeRequestHandled.getSec(), timeRequestHandled.getNanosec()); + + // Create and populate a GoalInfo message + List goalUuid = requestMessage.getGoalUuid(); + action_msgs.msg.GoalInfo goalInfo = new action_msgs.msg.GoalInfo(); + unique_identifier_msgs.msg.UUID uuidMessage= new unique_identifier_msgs.msg.UUID(); + uuidMessage.setUuid(goalUuid); + goalInfo.setGoalId(uuidMessage); + goalInfo.setStamp(timeRequestHandled); + + long goalInfoFromJavaConverterHandle = goalInfo.getFromJavaConverterInstance(); + long goalInfoDestructorHandle = goalInfo.getDestructorInstance(); + + // Check that the goal ID isn't already being used + boolean goalExists = nativeCheckGoalExists( + this.handle, goalInfo, goalInfoFromJavaConverterHandle, goalInfoDestructorHandle); + if (goalExists) { + logger.warn("Received goal request for goal already being tracked by action server. Goal ID: " + goalUuid); + responseMessage.accept(false); + return null; + } + + // Call user callback + GoalCallback.GoalResponse response = this.goalCallback.handleGoal(requestMessage); + + boolean accepted = GoalCallback.GoalResponse.ACCEPT == response; + responseMessage.accept(accepted); + + System.out.println("Goal request handled " + accepted); + if (!accepted) { + return null; + } + + // Create a goal handle and add it to the list of goals + GoalHandleImpl goalHandle = this.new GoalHandleImpl( + this, goalInfo, requestMessage.getGoal()); + this.goalHandles.put(requestMessage.getGoalUuid(), goalHandle); + return goalHandle; + } + + private action_msgs.srv.CancelGoal_Response executeCancelRequest( + action_msgs.srv.CancelGoal_Response inputMessage) + { + action_msgs.srv.CancelGoal_Response outputMessage = new action_msgs.srv.CancelGoal_Response(); + outputMessage.setReturnCode(inputMessage.getReturnCode()); + List goalsToCancel = new ArrayList(); + + // Process user callback for each goal in cancel request + for (action_msgs.msg.GoalInfo goalInfo : inputMessage.getGoalsCanceling()) { + List goalUuid = goalInfo.getGoalId().getUuidAsList(); + // It's possible a goal may not be tracked by the user + if (!this.goalHandles.containsKey(goalUuid)) { + logger.warn("Ignoring cancel request for untracked goal handle with ID '" + goalUuid + "'"); + continue; + } + GoalHandleImpl goalHandle = this.goalHandles.get(goalUuid); + CancelCallback.CancelResponse cancelResponse = this.cancelCallback.handleCancel(goalHandle); + + if (CancelCallback.CancelResponse.ACCEPT == cancelResponse) { + // Update goal state to CANCELING + goalHandle.cancelGoal(); + + // Add to returned response + goalsToCancel.add(goalInfo); + } + } + + outputMessage.setGoalsCanceling(goalsToCancel); + return outputMessage; + } + + private static native RMWRequestId nativeTakeGoalRequest( + long actionServerHandle, + long requestFromJavaConverterHandle, + long requestToJavaConverterHandle, + long requestDestructorHandle, + MessageDefinition requestMessage); + + private static native RMWRequestId nativeTakeCancelRequest( + long actionServerHandle, + long requestFromJavaConverterHandle, + long requestToJavaConverterHandle, + long requestDestructorHandle, + MessageDefinition requestMessage); + + private static native RMWRequestId nativeTakeResultRequest( + long actionServerHandle, + long requestFromJavaConverterHandle, + long requestToJavaConverterHandle, + long requestDestructorHandle, + MessageDefinition requestMessage); + + private static native void nativeSendGoalResponse( + long actionServerHandle, + RMWRequestId header, + long responseFromJavaConverterHandle, + long responseToJavaConverterHandle, + long responseDestructorHandle, + MessageDefinition responseMessage); + + private static native void nativeSendCancelResponse( + long actionServerHandle, + RMWRequestId header, + long responseFromJavaConverterHandle, + long responseToJavaConverterHandle, + long responseDestructorHandle, + MessageDefinition responseMessage); + + private static native void nativeSendResultResponse( + long actionServerHandle, + RMWRequestId header, + long responseFromJavaConverterHandle, + long responseToJavaConverterHandle, + long responseDestructorHandle, + MessageDefinition responseMessage); + + private static native void nativeProcessCancelRequest( + long actionServerHandle, + long requestFromJavaConverterHandle, + long requestDestructorHandle, + long responseToJavaConverterHandle, + MessageDefinition requestMessage, + MessageDefinition responseMessage); + + private static native boolean nativeCheckGoalExists( + long handle, + MessageDefinition goalInfo, + long goalInfoFromJavaConverterHandle, + long goalInfoDestructorHandle); + + /** + * {@inheritDoc} + */ + public void execute() { + if (this.isGoalRequestReady()) { + Class requestType = this.actionTypeInstance.getSendGoalRequestType(); + Class responseType = this.actionTypeInstance.getSendGoalResponseType(); + + GoalRequestDefinition requestMessage = null; + GoalResponseDefinition responseMessage = null; + + try { + requestMessage = requestType.newInstance(); + responseMessage = responseType.newInstance(); + } catch (InstantiationException ie) { + ie.printStackTrace(); + } catch (IllegalAccessException iae) { + iae.printStackTrace(); + } + + if (requestMessage != null && responseMessage != null) { + long requestFromJavaConverterHandle = requestMessage.getFromJavaConverterInstance(); + long requestToJavaConverterHandle = requestMessage.getToJavaConverterInstance(); + long requestDestructorHandle = requestMessage.getDestructorInstance(); + long responseFromJavaConverterHandle = responseMessage.getFromJavaConverterInstance(); + long responseToJavaConverterHandle = responseMessage.getToJavaConverterInstance(); + long responseDestructorHandle = responseMessage.getDestructorInstance(); + + RMWRequestId rmwRequestId = + nativeTakeGoalRequest( + this.handle, + requestFromJavaConverterHandle, requestToJavaConverterHandle, requestDestructorHandle, + requestMessage); + if (rmwRequestId != null) { + ActionServerGoalHandle goalHandle = this.executeGoalRequest( + rmwRequestId, requestMessage, responseMessage); + nativeSendGoalResponse( + this.handle, rmwRequestId, + responseFromJavaConverterHandle, responseToJavaConverterHandle, + responseDestructorHandle, responseMessage); + if (goalHandle != null) { + this.acceptedCallback.accept(goalHandle); + } + } + } + } + + if (this.isCancelRequestReady()) { + action_msgs.srv.CancelGoal_Request requestMessage = new action_msgs.srv.CancelGoal_Request(); + action_msgs.srv.CancelGoal_Response responseMessage = new action_msgs.srv.CancelGoal_Response(); + + long requestFromJavaConverterHandle = requestMessage.getFromJavaConverterInstance(); + long requestToJavaConverterHandle = requestMessage.getToJavaConverterInstance(); + long requestDestructorHandle = requestMessage.getDestructorInstance(); + long responseFromJavaConverterHandle = responseMessage.getFromJavaConverterInstance(); + long responseToJavaConverterHandle = responseMessage.getToJavaConverterInstance(); + long responseDestructorHandle = responseMessage.getDestructorInstance(); + + RMWRequestId rmwRequestId = + nativeTakeCancelRequest( + this.handle, + requestFromJavaConverterHandle, requestToJavaConverterHandle, requestDestructorHandle, + requestMessage); + if (rmwRequestId != null) { + nativeProcessCancelRequest( + this.handle, + requestFromJavaConverterHandle, + requestDestructorHandle, + responseToJavaConverterHandle, + requestMessage, + responseMessage); + responseMessage = executeCancelRequest(responseMessage); + nativeSendCancelResponse( + this.handle, rmwRequestId, + responseFromJavaConverterHandle, responseToJavaConverterHandle, + responseDestructorHandle, responseMessage); + } + } + + if (this.isResultRequestReady()) { + // executeResultRequest(rmwRequestId, requestMessage, responseMessage); + // TODO + } + + if (this.isGoalExpiredReady()) { + // cleanupExpiredGoals(); + // TODO + } + } + + /** + * Destroy the underlying rcl_action_server_t. + * + * @param nodeHandle A pointer to the underlying rcl_node_t handle that + * created this action server. + * @param handle A pointer to the underlying rcl_action_server_t + */ + private static native void nativeDispose(long nodeHandle, long handle); + + /** + * {@inheritDoc} + */ + public final void dispose() { + Node node = this.nodeReference.get(); + if (node != null) { + nativeDispose(node.getHandle(), this.handle); + node.removeActionServer(this); + this.handle = 0; + } + } + + /** + * {@inheritDoc} + */ + public final long getHandle() { + return handle; + } +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/CancelCallback.java b/rcljava/src/main/java/org/ros2/rcljava/action/CancelCallback.java new file mode 100644 index 00000000..f16147e6 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/action/CancelCallback.java @@ -0,0 +1,33 @@ +/* Copyright 2020 ros2-java contributors + * + * 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.action; + +import org.ros2.rcljava.interfaces.ActionDefinition; + +public interface CancelCallback { + enum CancelResponse { + REJECT, + ACCEPT, + }; + + /** + * Called when a new cancel request is received. + * + * @param goalHandle The goal handle. + * @return Cancel response indicating if the cancel request was accepted or not. + */ + CancelResponse handleCancel(ActionServerGoalHandle goalHandle); +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/GoalCallback.java b/rcljava/src/main/java/org/ros2/rcljava/action/GoalCallback.java new file mode 100644 index 00000000..afd017b0 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/action/GoalCallback.java @@ -0,0 +1,33 @@ +/* Copyright 2020 ros2-java contributors + * + * 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.action; + +import org.ros2.rcljava.interfaces.GoalRequestDefinition; + +public interface GoalCallback { + public enum GoalResponse { + REJECT, + ACCEPT, + }; + + /** + * Called when a new goal request is received. + * + * @param goal The action goal request. + * @return Goal response indicating if the goal was accepted or not. + */ + GoalResponse handleGoal(T goal); +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/GoalStatus.java b/rcljava/src/main/java/org/ros2/rcljava/action/GoalStatus.java new file mode 100644 index 00000000..aee9e1c9 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/action/GoalStatus.java @@ -0,0 +1,47 @@ +/* Copyright 2020 ros2-java contributors + * + * 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.action; + +public enum GoalStatus { + UNKNOWN, + ACCEPTED, + EXECUTING, + CANCELING, + SUCCEEDED, + CANCELED, + ABORTED; + + public static GoalStatus fromMessageValue(byte status) { + switch (status) { + case action_msgs.msg.GoalStatus.STATUS_ACCEPTED: + return GoalStatus.ACCEPTED; + case action_msgs.msg.GoalStatus.STATUS_EXECUTING: + return GoalStatus.EXECUTING; + case action_msgs.msg.GoalStatus.STATUS_CANCELING: + return GoalStatus.CANCELING; + case action_msgs.msg.GoalStatus.STATUS_SUCCEEDED: + return GoalStatus.SUCCEEDED; + case action_msgs.msg.GoalStatus.STATUS_CANCELED: + return GoalStatus.CANCELED; + case action_msgs.msg.GoalStatus.STATUS_ABORTED: + return GoalStatus.ABORTED; + case action_msgs.msg.GoalStatus.STATUS_UNKNOWN: + default: + return GoalStatus.UNKNOWN; + } + } +} + diff --git a/rcljava/src/main/java/org/ros2/rcljava/executors/AnyExecutable.java b/rcljava/src/main/java/org/ros2/rcljava/executors/AnyExecutable.java index dd3d3039..701d551b 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/executors/AnyExecutable.java +++ b/rcljava/src/main/java/org/ros2/rcljava/executors/AnyExecutable.java @@ -15,6 +15,7 @@ package org.ros2.rcljava.executors; +import org.ros2.rcljava.action.ActionServer; import org.ros2.rcljava.client.Client; import org.ros2.rcljava.events.EventHandler; import org.ros2.rcljava.subscription.Subscription; @@ -27,4 +28,5 @@ public class AnyExecutable { public Service service; public Client client; public EventHandler eventHandler; + public ActionServer actionServer; } diff --git a/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java b/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java index 67c15fda..9ad11e52 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java +++ b/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java @@ -30,11 +30,13 @@ import org.slf4j.LoggerFactory; import org.ros2.rcljava.RCLJava; +import org.ros2.rcljava.action.ActionServer; import org.ros2.rcljava.client.Client; import org.ros2.rcljava.common.JNIUtils; import org.ros2.rcljava.events.EventHandler; import org.ros2.rcljava.executors.AnyExecutable; import org.ros2.rcljava.executors.Executor; +import org.ros2.rcljava.interfaces.ActionDefinition; import org.ros2.rcljava.interfaces.MessageDefinition; import org.ros2.rcljava.interfaces.ServiceDefinition; import org.ros2.rcljava.node.ComposableNode; @@ -69,6 +71,8 @@ public class BaseExecutor { private List> eventHandles = new ArrayList>(); + private List> actionServerHandles = new ArrayList>(); + protected void addNode(ComposableNode node) { this.nodes.add(node); } @@ -168,6 +172,11 @@ protected void executeAnyExecutable(AnyExecutable anyExecutable) { anyExecutable.eventHandler.executeCallback(); eventHandles.remove(anyExecutable.eventHandler.getHandle()); } + + if (anyExecutable.actionServer != null) { + anyExecutable.actionServer.execute(); + this.actionServerHandles.remove(anyExecutable.actionServer.getHandle()); + } } protected void waitForWork(long timeout) { @@ -176,6 +185,7 @@ protected void waitForWork(long timeout) { this.serviceHandles.clear(); this.clientHandles.clear(); this.eventHandles.clear(); + this.actionServerHandles.clear(); for (ComposableNode node : this.nodes) { for (Subscription subscription : node.getNode().getSubscriptions()) { @@ -209,6 +219,11 @@ protected void waitForWork(long timeout) { this.clientHandles.add( new AbstractMap.SimpleEntry(client.getHandle(), client)); } + + for (ActionServer actionServer : node.getNode().getActionServers()) { + this.actionServerHandles.add( + new AbstractMap.SimpleEntry(actionServer.getHandle(), actionServer)); + } } int subscriptionsSize = 0; @@ -222,6 +237,13 @@ protected void waitForWork(long timeout) { timersSize += node.getNode().getTimers().size(); clientsSize += node.getNode().getClients().size(); servicesSize += node.getNode().getServices().size(); + + for (ActionServer actionServer : node.getNode().getActionServers()) { + subscriptionsSize += actionServer.getNumberOfSubscriptions(); + timersSize += actionServer.getNumberOfTimers(); + clientsSize += actionServer.getNumberOfClients(); + servicesSize += actionServer.getNumberOfServices(); + } } if (subscriptionsSize == 0 && timersSize == 0 && clientsSize == 0 && servicesSize == 0) { @@ -256,6 +278,10 @@ protected void waitForWork(long timeout) { nativeWaitSetAddEvent(waitSetHandle, entry.getKey()); } + for (Map.Entry entry : this.actionServerHandles) { + nativeWaitSetAddActionServer(waitSetHandle, entry.getKey()); + } + nativeWait(waitSetHandle, timeout); for (int i = 0; i < this.subscriptionHandles.size(); ++i) { @@ -288,6 +314,12 @@ protected void waitForWork(long timeout) { } } + for (Map.Entry entry : this.actionServerHandles) { + if (!entry.getValue().isReady(waitSetHandle)) { + entry.setValue(null); + } + } + Iterator> subscriptionIterator = this.subscriptionHandles.iterator(); while (subscriptionIterator.hasNext()) { @@ -329,6 +361,14 @@ protected void waitForWork(long timeout) { } } + Iterator> actionServerIterator = this.actionServerHandles.iterator(); + while (actionServerIterator.hasNext()) { + Map.Entry entry = actionServerIterator.next(); + if (entry.getValue() == null) { + actionServerIterator.remove(); + } + } + nativeDisposeWaitSet(waitSetHandle); } @@ -378,6 +418,14 @@ protected AnyExecutable getNextExecutable() { } } + for (Map.Entry entry : this.actionServerHandles) { + if (entry.getValue() != null) { + anyExecutable.actionServer = entry.getValue(); + entry.setValue(null); + return anyExecutable; + } + } + return null; } @@ -461,6 +509,8 @@ private static native MessageDefinition nativeTake( private static native void nativeWaitSetAddEvent(long waitSetHandle, long eventHandle); + private static native void nativeWaitSetAddActionServer(long waitSetHandle, long actionServerHandle); + private static native RMWRequestId nativeTakeRequest(long serviceHandle, long requestFromJavaConverterHandle, long requestToJavaConverterHandle, long requestDestructorHandle, MessageDefinition requestMessage); @@ -482,4 +532,6 @@ private static native RMWRequestId nativeTakeResponse(long clientHandle, private static native boolean nativeWaitSetServiceIsReady(long waitSetHandle, long index); private static native boolean nativeWaitSetClientIsReady(long waitSetHandle, long index); + + private static native boolean nativeWaitSetActionServerIsReady(long waitSetHandle, long actionServerHandle); } 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 b6750e39..63807a81 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/Node.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/Node.java @@ -20,6 +20,10 @@ import java.util.Map; import java.util.concurrent.TimeUnit; +import org.ros2.rcljava.action.ActionServer; +import org.ros2.rcljava.action.ActionServerGoalHandle; +import org.ros2.rcljava.action.CancelCallback; +import org.ros2.rcljava.action.GoalCallback; import org.ros2.rcljava.client.Client; import org.ros2.rcljava.concurrent.Callback; import org.ros2.rcljava.consumers.Consumer; @@ -28,6 +32,8 @@ import org.ros2.rcljava.graph.NameAndTypes; import org.ros2.rcljava.graph.NodeNameInfo; import org.ros2.rcljava.interfaces.Disposable; +import org.ros2.rcljava.interfaces.ActionDefinition; +import org.ros2.rcljava.interfaces.GoalRequestDefinition; import org.ros2.rcljava.interfaces.MessageDefinition; import org.ros2.rcljava.interfaces.ServiceDefinition; import org.ros2.rcljava.parameters.ParameterCallback; @@ -77,6 +83,11 @@ public interface Node extends Disposable { */ Collection getTimers(); + /** + * @return All the @{link ActionServer}s that were created by this instance. + */ + Collection getActionServers(); + /** * Create a Subscription<T>. * @@ -133,6 +144,24 @@ Client createClient( Client createClient(final Class serviceType, final String serviceName) throws NoSuchFieldException, IllegalAccessException; + /** + * Create an ActionServer<T>. + * + * @param The type of action that will be handled by the created @{link ActionServer}. + * @param actionName The name of action that the create @{link ActionServer} will offer. + * @param goalCallback The callback that will be called when the @{link ActionServer} + * receives a new goal request. + * @param cancelCallback The callback that will be called when the @{link ActionServer} + * receives a cancle request for an active goal. + * @param acceptedCallback The callback that will be called when the @{link ActionServer} + * accepts a goal request. + */ + ActionServer createActionServer(final Class actionType, + final String actionName, + final GoalCallback> goalCallback, + final CancelCallback cancelCallback, + final Consumer> acceptedCallback); + /** * Remove a Subscription created by this Node. * @@ -181,6 +210,18 @@ Client createClient(final Class serviceType, */ boolean removeClient(final Client client); + /** + * Remove an @{link ActionServer} created by this Node. + * + * Calling this method effectively invalidates the passed @{link ActionServer}. + * If the server was not created by this Node, then nothing happens. + * + * @param actionServer The object to remove from this node. + * @return true if the server was removed, false if the server was already + * removed or was never created by this Node. + */ + boolean removeActionServer(final ActionServer actionServer); + /** * Create a wall timer. * 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 687138cb..7c6dd4b4 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java @@ -16,6 +16,11 @@ package org.ros2.rcljava.node; import org.ros2.rcljava.RCLJava; +import org.ros2.rcljava.action.ActionServer; +import org.ros2.rcljava.action.ActionServerImpl; +import org.ros2.rcljava.action.ActionServerGoalHandle; +import org.ros2.rcljava.action.CancelCallback; +import org.ros2.rcljava.action.GoalCallback; import org.ros2.rcljava.client.Client; import org.ros2.rcljava.client.ClientImpl; import org.ros2.rcljava.common.JNIUtils; @@ -27,6 +32,8 @@ import org.ros2.rcljava.graph.EndpointInfo; import org.ros2.rcljava.graph.NameAndTypes; import org.ros2.rcljava.interfaces.Disposable; +import org.ros2.rcljava.interfaces.ActionDefinition; +import org.ros2.rcljava.interfaces.GoalRequestDefinition; import org.ros2.rcljava.interfaces.MessageDefinition; import org.ros2.rcljava.interfaces.ServiceDefinition; import org.ros2.rcljava.node.NodeOptions; @@ -135,6 +142,11 @@ public class NodeImpl implements Node { */ private final Collection timers; + /** + * All the @{link ActionServer}s that have been created through this instance. + */ + private final Collection actionServers; + private Object parametersMutex; class ParameterAndDescriptor { @@ -169,6 +181,7 @@ public NodeImpl(final long handle, final NodeOptions nodeOptions) { this.services = new LinkedBlockingQueue(); this.clients = new LinkedBlockingQueue(); this.timers = new LinkedBlockingQueue(); + this.actionServers = new LinkedBlockingQueue(); this.parametersMutex = new Object(); this.parameters = new ConcurrentHashMap(); this.allowUndeclaredParameters = nodeOptions.getAllowUndeclaredParameters(); @@ -388,6 +401,18 @@ public Client createClient(final Class servi private static native long nativeCreateClientHandle( long handle, Class cls, String serviceName, long qosProfileHandle); + public ActionServer createActionServer(final Class actionType, + final String actionName, + final GoalCallback> goalCallback, + final CancelCallback cancelCallback, + final Consumer> acceptedCallback) throws IllegalArgumentException { + ActionServer actionServer = new ActionServerImpl( + new WeakReference(this), actionType, actionName, + goalCallback, cancelCallback, acceptedCallback); + this.actionServers.add(actionServer); + return actionServer; + } + /** * {@inheritDoc} */ @@ -402,6 +427,13 @@ public boolean removeClient(final Client client) { return this.clients.remove(client); } + /** + * {@inheritDoc} + */ + public boolean removeActionServer(final ActionServer actionServer) { + return this.actionServers.remove(actionServer); + } + /** * {@inheritDoc} */ @@ -479,6 +511,13 @@ public final Collection getTimers() { return this.timers; } + /** + * {@inheritDoc} + */ + public final Collection getActionServers() { + return this.actionServers; + } + /** * {@inheritDoc} */ diff --git a/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java b/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java new file mode 100644 index 00000000..36f004b9 --- /dev/null +++ b/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java @@ -0,0 +1,234 @@ +/* Copyright 2020 ros2-java contributors + * + * 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.action; + +import java.time.Duration; +import java.util.concurrent.Future; +import java.util.concurrent.TimeUnit; +import java.util.Collection; +import java.util.List; + +import static org.junit.Assert.assertEquals; +import static org.junit.Assert.assertNotEquals; + +import org.junit.After; +import org.junit.AfterClass; +import org.junit.Before; +import org.junit.BeforeClass; +import org.junit.Test; + +import org.ros2.rcljava.RCLJava; +import org.ros2.rcljava.executors.SingleThreadedExecutor; +import org.ros2.rcljava.consumers.Consumer; +import org.ros2.rcljava.node.ComposableNode; +import org.ros2.rcljava.node.Node; + +public class ActionServerTest { + class MockGoalCallback implements GoalCallback { + public test_msgs.action.Fibonacci_Goal goal; + public GoalResponse handleGoal(test_msgs.action.Fibonacci.SendGoalRequest goal) { + this.goal = goal.getGoal(); + return GoalResponse.ACCEPT; + } + } + + class MockCancelCallback implements CancelCallback { + public ActionServerGoalHandle goalHandle; + public CancelResponse handleCancel(ActionServerGoalHandle goalHandle) { + this.goalHandle = goalHandle; + return CancelResponse.ACCEPT; + } + } + + class MockAcceptedCallback implements Consumer> { + public ActionServerGoalHandle goalHandle; + public void accept(final ActionServerGoalHandle goalHandle) { + this.goalHandle = goalHandle; + } + } + + private SingleThreadedExecutor executor; + private Node node; + private ComposableNode composableNode; + private ActionServer actionServer; + private MockGoalCallback goalCallback; + private MockCancelCallback cancelCallback; + private MockAcceptedCallback acceptedCallback; + private MockActionClient mockActionClient; + + @BeforeClass + public static void setupOnce() { + RCLJava.rclJavaInit(); + org.apache.log4j.BasicConfigurator.configure(); + } + + @AfterClass + public static void tearDownOnce() { + RCLJava.shutdown(); + } + + @Before + public void setUp() throws Exception { + // Create a node + node = RCLJava.createNode("test_action_server_node"); + + assertNotEquals(null, node); + + // Executor requires a ComposableNode type + composableNode = new ComposableNode() { + public Node getNode() { + return node; + } + }; + executor = new SingleThreadedExecutor(); + executor.addNode(composableNode); + + // Create action server callbacks + goalCallback = new MockGoalCallback(); + cancelCallback = new MockCancelCallback(); + acceptedCallback = new MockAcceptedCallback(); + + // Create an action server + actionServer = node.createActionServer( + test_msgs.action.Fibonacci.class, "test_action", + goalCallback, cancelCallback, acceptedCallback); + + // Create mock client + mockActionClient = new MockActionClient(node, "test_action"); + + // Wait for mock client to discover action sever + // TODO(jacobperron): also wait for action server to discover the client + assertEquals(true, mockActionClient.waitForActionServer(Duration.ofSeconds(5))); + } + + @After + public void tearDown() { + // We expect that calling dispose should result in a zero handle + // and the reference is dropped from the Node + actionServer.dispose(); + assertEquals(0, actionServer.getHandle()); + assertEquals(0, this.node.getActionServers().size()); + + mockActionClient.dispose(); + + executor.removeNode(composableNode); + + node.dispose(); + } + + public test_msgs.action.Fibonacci_SendGoal_Response sendGoal(int order) throws Exception { + test_msgs.action.Fibonacci_SendGoal_Request request = + new test_msgs.action.Fibonacci_SendGoal_Request(); + test_msgs.action.Fibonacci_Goal goal = new test_msgs.action.Fibonacci_Goal(); + goal.setOrder(order); + request.setGoal(goal); + + Future future = + this.mockActionClient.sendGoalClient.asyncSendRequest(request); + + test_msgs.action.Fibonacci_SendGoal_Response response = null; + long startTime = System.nanoTime(); + while (RCLJava.ok() && !future.isDone()) { + this.executor.spinOnce(1); + response = future.get(100, TimeUnit.MILLISECONDS); + + // Check for timeout + long duration = System.nanoTime() - startTime; + if (TimeUnit.NANOSECONDS.toSeconds(duration) >= 5) { + break; + } + } + return response; + } + + @Test + public final void testCreateAndDispose() { + assertNotEquals(0, this.actionServer.getHandle()); + assertEquals(1, this.node.getActionServers().size()); + + // Assert no callbacks triggered + assertEquals(null, this.goalCallback.goal); + assertEquals(null, this.cancelCallback.goalHandle); + assertEquals(null, this.acceptedCallback.goalHandle); + } + + @Test + public final void testAcceptGoal() throws Exception { + assertNotEquals(0, this.actionServer.getHandle()); + assertEquals(1, this.node.getActionServers().size()); + + // Send a goal + test_msgs.action.Fibonacci_SendGoal_Response response = sendGoal(42); + + assertNotEquals(null, response); + + // Assert goal callback and accepted callback triggered + assertNotEquals(null, this.goalCallback.goal); + assertNotEquals(null, this.acceptedCallback.goalHandle); + + assertEquals(42, this.goalCallback.goal.getOrder()); + test_msgs.action.Fibonacci_Goal acceptedGoal = (test_msgs.action.Fibonacci_Goal)this.acceptedCallback.goalHandle.getGoal(); + assertEquals(42, acceptedGoal.getOrder()); + + // Assert cancel callback not triggered + assertEquals(null, this.cancelCallback.goalHandle); + } + + @Test + public final void testCancelGoal() throws Exception { + assertNotEquals(0, this.actionServer.getHandle()); + assertEquals(1, this.node.getActionServers().size()); + + // Send a goal + test_msgs.action.Fibonacci_SendGoal_Response response = sendGoal(42); + + assertNotEquals(null, response); + + // Cancel the goal + action_msgs.srv.CancelGoal_Request cancelRequest = new action_msgs.srv.CancelGoal_Request(); + // A zero GoalInfo means cancel all goals + action_msgs.msg.GoalInfo goalInfo = new action_msgs.msg.GoalInfo(); + unique_identifier_msgs.msg.UUID zeroGoalId = new unique_identifier_msgs.msg.UUID(); + // TODO(jacobperron): code generator should zero initialize to 16 elements + zeroGoalId.setUuid(new byte[16]); + goalInfo.setGoalId(zeroGoalId); + cancelRequest.setGoalInfo(goalInfo); + Future cancelResponseFuture = + this.mockActionClient.cancelGoalClient.asyncSendRequest(cancelRequest); + + // Wait for cancel response + long startTime = System.nanoTime(); + while (RCLJava.ok() && !cancelResponseFuture.isDone()) { + this.executor.spinOnce(100000000); // timeout of 100 milliseconds + + // Check for timeout + long duration = System.nanoTime() - startTime; + if (TimeUnit.NANOSECONDS.toSeconds(duration) >= 5) { + break; + } + } + + assertEquals(true, cancelResponseFuture.isDone()); + action_msgs.srv.CancelGoal_Response cancelResponse = cancelResponseFuture.get(); + action_msgs.msg.GoalInfo[] goalsCanceling = cancelResponse.getGoalsCanceling(); + assertEquals(1, goalsCanceling.length); + + // Assert cancel callback was triggered + assertNotEquals(null, this.cancelCallback.goalHandle); + test_msgs.action.Fibonacci_Goal cancelingGoal = (test_msgs.action.Fibonacci_Goal)this.cancelCallback.goalHandle.getGoal(); + assertEquals(42, cancelingGoal.getOrder()); + } +} diff --git a/rcljava/src/test/java/org/ros2/rcljava/action/MockActionClient.java b/rcljava/src/test/java/org/ros2/rcljava/action/MockActionClient.java new file mode 100644 index 00000000..72f0e7e4 --- /dev/null +++ b/rcljava/src/test/java/org/ros2/rcljava/action/MockActionClient.java @@ -0,0 +1,104 @@ +/* Copyright 2020 ros2-java contributors + * + * 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.action; + +import java.time.Duration; +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; + +import org.ros2.rcljava.client.Client; +import org.ros2.rcljava.consumers.Consumer; +import org.ros2.rcljava.node.Node; +import org.ros2.rcljava.subscription.Subscription; + +public class MockActionClient { + class FeedbackCallback implements Consumer { + public List feedbackReceived; + + public FeedbackCallback() { + feedbackReceived = Collections.synchronizedList( + new ArrayList()); + } + + public void accept(final test_msgs.action.Fibonacci_Feedback feedback) { + this.feedbackReceived.add(feedback); + } + } + + class StatusCallback implements Consumer { + private action_msgs.msg.GoalStatusArray statusArray; + + public synchronized action_msgs.msg.GoalStatusArray getStatusArrayMessage() { + return this.statusArray; + } + + public synchronized void accept(final action_msgs.msg.GoalStatusArray statusArray) { + this.statusArray = statusArray; + } + } + + public Client sendGoalClient; + public Client getResultClient; + public Client cancelGoalClient; + public FeedbackCallback feedbackCallback; + public StatusCallback statusCallback; + public Subscription feedbackSubscription; + public Subscription statusSubscription; + + public MockActionClient(Node node, String actionName) throws IllegalAccessException, NoSuchFieldException { + // Create mock service clients that make up an action client + sendGoalClient = node.createClient( + test_msgs.action.Fibonacci_SendGoal.class, actionName + "/_action/send_goal"); + getResultClient = node.createClient( + test_msgs.action.Fibonacci_GetResult.class, actionName + "/_action/get_result"); + cancelGoalClient = node.createClient( + action_msgs.srv.CancelGoal.class, actionName + "/_action/cancel_goal"); + // Create mock subscriptions that make up an action client + feedbackCallback = new FeedbackCallback(); + statusCallback = new StatusCallback(); + feedbackSubscription = node.createSubscription( + test_msgs.action.Fibonacci_Feedback.class, + actionName + "/_action/feedback", + feedbackCallback); + statusSubscription = node.createSubscription( + action_msgs.msg.GoalStatusArray.class, + actionName + "/_action/status", + statusCallback); + } + + public void dispose() { + this.sendGoalClient.dispose(); + this.getResultClient.dispose(); + this.cancelGoalClient.dispose(); + this.feedbackSubscription.dispose(); + this.statusSubscription.dispose(); + } + + public boolean waitForActionServer(Duration timeout) { + if (!this.sendGoalClient.waitForService(timeout)) { + return false; + } + if (!this.getResultClient.waitForService(timeout)) { + return false; + } + if (!this.cancelGoalClient.waitForService(timeout)) { + return false; + } + //TODO(jacobperron): wait for feedback and status subscriptions to match publishers, when API is available + return true; + } +} From 5374f3187d6d0b3bdfdb612c4a2add60485785e5 Mon Sep 17 00:00:00 2001 From: Gonzo <42421541+gonzodepedro@users.noreply.github.com> Date: Tue, 30 Mar 2021 15:28:32 -0300 Subject: [PATCH 30/51] Build sources jar for rcljava (osrf/ros2_java#56) * Add cmake function for bundling source files into a jar * Create and install source jar for rcljava Signed-off-by: Gonzalo de Pedro Co-authored-by: Jacob Perron --- rcljava/CMakeLists.txt | 10 +++++ rcljava_common/cmake/Modules/JavaExtra.cmake | 43 ++++++++++++++++++++ 2 files changed, 53 insertions(+) diff --git a/rcljava/CMakeLists.txt b/rcljava/CMakeLists.txt index 983eabf4..85cba623 100644 --- a/rcljava/CMakeLists.txt +++ b/rcljava/CMakeLists.txt @@ -230,6 +230,16 @@ add_jar("${PROJECT_NAME}_jar" install_jar("${PROJECT_NAME}_jar" "share/${PROJECT_NAME}/java") ament_export_jars("share/${PROJECT_NAME}/java/${PROJECT_NAME}.jar") +add_source_jar("${PROJECT_NAME}-source_jar" + ${${PROJECT_NAME}_sources} + OUTPUT_NAME + ${PROJECT_NAME}-source +) + +install_jar("${PROJECT_NAME}-source_jar" "share/${PROJECT_NAME}/java") +ament_export_jars("share/${PROJECT_NAME}/java/${PROJECT_NAME}-source.jar") + + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) find_package(std_msgs REQUIRED) diff --git a/rcljava_common/cmake/Modules/JavaExtra.cmake b/rcljava_common/cmake/Modules/JavaExtra.cmake index 0ab52441..259345f7 100644 --- a/rcljava_common/cmake/Modules/JavaExtra.cmake +++ b/rcljava_common/cmake/Modules/JavaExtra.cmake @@ -165,3 +165,46 @@ function(ament_add_junit_tests TARGET_NAME) add_dependencies(${TARGET_NAME} "${TARGET_NAME}_jar") endfunction() + +# +# Creates a sources jar from list of source files +# +# :param OUTPUT_NAME: Name of Jar +# :type OUTPUT_NAME: string +# :param SOURCES: Sources files +# :type SOURCES: String list with file names +# +# @public +# +function(add_source_jar _TARGET_NAME) + + cmake_parse_arguments(_add_source_jar + "" + "OUTPUT_NAME" + "SOURCES" + ${ARGN} + ) + + if(NOT _add_source_jar_OUTPUT_NAME) + set(_add_source_jar_OUTPUT_NAME "${_TARGET_NAME}.jar") + endif() + + set(_SOURCE_FILES ${_add_source_jar_SOURCES} ${_add_source_jar_UNPARSED_ARGUMENTS}) + set(_JAVA_JAR_SOURCES_OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/${_add_source_jar_OUTPUT_NAME}.jar) + + add_custom_command( + OUTPUT ${_add_source_jar_OUTPUT_NAME} + COMMAND ${Java_JAR_EXECUTABLE} -cf ${_JAVA_JAR_SOURCES_OUTPUT} ${_SOURCE_FILES} + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + ) + add_custom_target(${_TARGET_NAME} ALL DEPENDS ${_add_source_jar_OUTPUT_NAME}) + + # Set install property (for 'install_jar()'). + set_property( + TARGET ${_TARGET_NAME} + PROPERTY + INSTALL_FILES + ${_JAVA_JAR_SOURCES_OUTPUT} + ) + +endfunction() From 1b00e9e33a861794513d95e9198bb2854e9f5955 Mon Sep 17 00:00:00 2001 From: Gonzo <42421541+gonzodepedro@users.noreply.github.com> Date: Wed, 7 Apr 2021 13:56:31 -0300 Subject: [PATCH 31/51] Add source jars generation for rosidl_generator_java (osrf/ros2_java#57) Signed-off-by: Gonzalo de Pedro --- rosidl_generator_java/cmake/custom_command.cmake | 7 +++++++ .../cmake/rosidl_generator_java_generate_interfaces.cmake | 3 +++ 2 files changed, 10 insertions(+) diff --git a/rosidl_generator_java/cmake/custom_command.cmake b/rosidl_generator_java/cmake/custom_command.cmake index db7d1cc1..9cdaaa79 100644 --- a/rosidl_generator_java/cmake/custom_command.cmake +++ b/rosidl_generator_java/cmake/custom_command.cmake @@ -43,4 +43,11 @@ add_jar("${PROJECT_NAME}_messages_jar" ${_jar_deps} ) +add_source_jar("${PROJECT_NAME}_messages_source_jar" + ${_generated_java_files} + OUTPUT_NAME + ${PROJECT_NAME}_messages-source +) + add_dependencies("${PROJECT_NAME}_messages_jar" "${rosidl_generate_interfaces_TARGET}${_target_suffix}") +add_dependencies("${PROJECT_NAME}_messages_source_jar" "${rosidl_generate_interfaces_TARGET}${_target_suffix}") diff --git a/rosidl_generator_java/cmake/rosidl_generator_java_generate_interfaces.cmake b/rosidl_generator_java/cmake/rosidl_generator_java_generate_interfaces.cmake index 0093329c..4ace4bfd 100644 --- a/rosidl_generator_java/cmake/rosidl_generator_java_generate_interfaces.cmake +++ b/rosidl_generator_java/cmake/rosidl_generator_java_generate_interfaces.cmake @@ -31,6 +31,7 @@ if(NOT ANDROID) find_package(JNI REQUIRED) endif() include(UseJava) +include(JavaExtra) if(NOT WIN32) if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") @@ -306,6 +307,8 @@ if(NOT rosidl_generate_interfaces_SKIP_INSTALL) if(NOT _generated_java_files STREQUAL "") install_jar("${PROJECT_NAME}_messages_jar" "share/${PROJECT_NAME}/java") ament_export_jars("share/${PROJECT_NAME}/java/${PROJECT_NAME}_messages.jar") + install_jar("${PROJECT_NAME}_messages_source_jar" "share/${PROJECT_NAME}/java") + ament_export_jars("share/${PROJECT_NAME}/java/${PROJECT_NAME}_messages-source.jar") endif() endif() From 6819ecc31d41da10df6853a1e4faf51a255d008b Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 13 Apr 2021 16:55:23 -0300 Subject: [PATCH 32/51] Add transition from accepted to executing in the action server (osrf/ros2_java#59) * Add ACCEPT_AND_EXECUTE/ACCEPT_AND_DEFER to the result of an action goal callback. Signed-off-by: Ivan Santiago Paunovic * Automatically transition from accepted to executing if ACCEPT_AND_EXECUTE was returned. Signed-off-by: Ivan Santiago Paunovic * Expose an "execute()" method in goal callback. Signed-off-by: Ivan Santiago Paunovic --- .../ros2/rcljava/action/ActionServerGoalHandle.java | 7 +++++++ .../java/org/ros2/rcljava/action/ActionServerImpl.java | 10 ++++++---- .../java/org/ros2/rcljava/action/GoalCallback.java | 3 ++- rcljava/src/main/java/org/ros2/rcljava/node/Node.java | 2 +- .../java/org/ros2/rcljava/action/ActionServerTest.java | 2 +- 5 files changed, 17 insertions(+), 7 deletions(-) diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerGoalHandle.java b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerGoalHandle.java index aedf0dbe..11117437 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerGoalHandle.java +++ b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerGoalHandle.java @@ -40,6 +40,13 @@ public interface ActionServerGoalHandle extends Disp */ public boolean isCanceling(); + /** + * Transition the goal to the EXECUTING state. + * + * Pre-condition: the goal must be in the ACCEPTED state. + */ + public void execute(); + /** * Transition the goal to the SUCCEEDED state. * diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java index 8c883f25..b7e740c8 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java @@ -322,7 +322,8 @@ private ActionServerGoalHandle executeGoalRequest( // Call user callback GoalCallback.GoalResponse response = this.goalCallback.handleGoal(requestMessage); - boolean accepted = GoalCallback.GoalResponse.ACCEPT == response; + boolean accepted = GoalCallback.GoalResponse.ACCEPT_AND_DEFER == response + || GoalCallback.GoalResponse.ACCEPT_AND_EXECUTE == response; responseMessage.accept(accepted); System.out.println("Goal request handled " + accepted); @@ -334,6 +335,10 @@ private ActionServerGoalHandle executeGoalRequest( GoalHandleImpl goalHandle = this.new GoalHandleImpl( this, goalInfo, requestMessage.getGoal()); this.goalHandles.put(requestMessage.getGoalUuid(), goalHandle); + if (GoalCallback.GoalResponse.ACCEPT_AND_EXECUTE == response) { + goalHandle.execute(); + this.acceptedCallback.accept(goalHandle); + } return goalHandle; } @@ -467,9 +472,6 @@ public void execute() { this.handle, rmwRequestId, responseFromJavaConverterHandle, responseToJavaConverterHandle, responseDestructorHandle, responseMessage); - if (goalHandle != null) { - this.acceptedCallback.accept(goalHandle); - } } } } diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/GoalCallback.java b/rcljava/src/main/java/org/ros2/rcljava/action/GoalCallback.java index afd017b0..a448ac82 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/action/GoalCallback.java +++ b/rcljava/src/main/java/org/ros2/rcljava/action/GoalCallback.java @@ -20,7 +20,8 @@ public interface GoalCallback { public enum GoalResponse { REJECT, - ACCEPT, + ACCEPT_AND_EXECUTE, + ACCEPT_AND_DEFER, }; /** 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 63807a81..d1ba5d03 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/Node.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/Node.java @@ -152,7 +152,7 @@ Client createClient(final Class serviceType, * @param goalCallback The callback that will be called when the @{link ActionServer} * receives a new goal request. * @param cancelCallback The callback that will be called when the @{link ActionServer} - * receives a cancle request for an active goal. + * receives a cancel request for an active goal. * @param acceptedCallback The callback that will be called when the @{link ActionServer} * accepts a goal request. */ diff --git a/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java b/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java index 36f004b9..a9f5d07f 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java @@ -41,7 +41,7 @@ class MockGoalCallback implements GoalCallback Date: Wed, 14 Apr 2021 10:24:50 -0300 Subject: [PATCH 33/51] Do not use multiple message types in the same topic (osrf/ros2_java#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 f810abf7..48a83d1a 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java @@ -953,10 +953,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", @@ -974,7 +974,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 e6ca69b2ce9e8cc30c7a1fa2ba899c1bedb223a3 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 14 Apr 2021 13:39:52 -0300 Subject: [PATCH 34/51] Use action FeedbackMessage and not Feedback message type to subscribe to the feedback topic (osrf/ros2_java#60) Signed-off-by: Ivan Santiago Paunovic --- .../org/ros2/rcljava/action/MockActionClient.java | 14 +++++++------- ...rosidl_generator_java_generate_interfaces.cmake | 2 ++ rosidl_generator_java/resource/action.java.em | 11 +++++++++++ rosidl_generator_java/resource/msg.cpp.em | 4 ++++ 4 files changed, 24 insertions(+), 7 deletions(-) diff --git a/rcljava/src/test/java/org/ros2/rcljava/action/MockActionClient.java b/rcljava/src/test/java/org/ros2/rcljava/action/MockActionClient.java index 72f0e7e4..3ff19de4 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/action/MockActionClient.java +++ b/rcljava/src/test/java/org/ros2/rcljava/action/MockActionClient.java @@ -26,15 +26,15 @@ import org.ros2.rcljava.subscription.Subscription; public class MockActionClient { - class FeedbackCallback implements Consumer { - public List feedbackReceived; + class FeedbackCallback implements Consumer { + public List feedbackReceived; public FeedbackCallback() { feedbackReceived = Collections.synchronizedList( - new ArrayList()); + new ArrayList()); } - public void accept(final test_msgs.action.Fibonacci_Feedback feedback) { + public void accept(final test_msgs.action.Fibonacci_FeedbackMessage feedback) { this.feedbackReceived.add(feedback); } } @@ -56,7 +56,7 @@ public synchronized void accept(final action_msgs.msg.GoalStatusArray statusArra public Client cancelGoalClient; public FeedbackCallback feedbackCallback; public StatusCallback statusCallback; - public Subscription feedbackSubscription; + public Subscription feedbackSubscription; public Subscription statusSubscription; public MockActionClient(Node node, String actionName) throws IllegalAccessException, NoSuchFieldException { @@ -70,8 +70,8 @@ public MockActionClient(Node node, String actionName) throws IllegalAccessExcept // Create mock subscriptions that make up an action client feedbackCallback = new FeedbackCallback(); statusCallback = new StatusCallback(); - feedbackSubscription = node.createSubscription( - test_msgs.action.Fibonacci_Feedback.class, + feedbackSubscription = node.createSubscription( + test_msgs.action.Fibonacci_FeedbackMessage.class, actionName + "/_action/feedback", feedbackCallback); statusSubscription = node.createSubscription( diff --git a/rosidl_generator_java/cmake/rosidl_generator_java_generate_interfaces.cmake b/rosidl_generator_java/cmake/rosidl_generator_java_generate_interfaces.cmake index 4ace4bfd..5ebdba1a 100644 --- a/rosidl_generator_java/cmake/rosidl_generator_java_generate_interfaces.cmake +++ b/rosidl_generator_java/cmake/rosidl_generator_java_generate_interfaces.cmake @@ -87,6 +87,7 @@ foreach(_abs_idl_file ${rosidl_generate_interfaces_ABS_IDL_FILES}) "${_output_path}/${_parent_folder}/${_idl_name}_Goal.java" "${_output_path}/${_parent_folder}/${_idl_name}_Result.java" "${_output_path}/${_parent_folder}/${_idl_name}_Feedback.java" + "${_output_path}/${_parent_folder}/${_idl_name}_FeedbackMessage.java" "${_output_path}/${_parent_folder}/${_idl_name}_SendGoal.java" "${_output_path}/${_parent_folder}/${_idl_name}_SendGoal_Request.java" "${_output_path}/${_parent_folder}/${_idl_name}_SendGoal_Response.java" @@ -99,6 +100,7 @@ foreach(_abs_idl_file ${rosidl_generate_interfaces_ABS_IDL_FILES}) "${_output_path}/${_parent_folder}/${_idl_name}_Goal.ep.${_typesupport_impl}.cpp" "${_output_path}/${_parent_folder}/${_idl_name}_Result.ep.${_typesupport_impl}.cpp" "${_output_path}/${_parent_folder}/${_idl_name}_Feedback.ep.${_typesupport_impl}.cpp" + "${_output_path}/${_parent_folder}/${_idl_name}_FeedbackMessage.ep.${_typesupport_impl}.cpp" "${_output_path}/${_parent_folder}/${_idl_name}_SendGoal.ep.${_typesupport_impl}.cpp" "${_output_path}/${_parent_folder}/${_idl_name}_SendGoal_Request.ep.${_typesupport_impl}.cpp" "${_output_path}/${_parent_folder}/${_idl_name}_SendGoal_Response.ep.${_typesupport_impl}.cpp" diff --git a/rosidl_generator_java/resource/action.java.em b/rosidl_generator_java/resource/action.java.em index cb6c1dd3..2656f192 100644 --- a/rosidl_generator_java/resource/action.java.em +++ b/rosidl_generator_java/resource/action.java.em @@ -13,6 +13,7 @@ type_name = action.namespaced_type.name goal_type_name = action.goal.structure.namespaced_type.name result_type_name = action.result.structure.namespaced_type.name feedback_type_name = action.feedback.structure.namespaced_type.name +feedback_message_type_name = action.feedback_message.structure.namespaced_type.name send_goal_type_name = action.send_goal_service.namespaced_type.name get_result_type_name = action.get_result_service.namespaced_type.name @@ -46,6 +47,14 @@ expand_template( output_file, template_basepath=template_basepath) +data.update({'message': action.feedback_message}) +output_file = os.path.join(output_dir, *namespaces[1:], feedback_message_type_name + '.java') +expand_template( + 'msg.java.em', + data, + output_file, + template_basepath=template_basepath) + data.update({'service': action.send_goal_service}) output_file = os.path.join(output_dir, *namespaces[1:], send_goal_type_name + '.java') expand_template( @@ -134,4 +143,6 @@ public class @(type_name) implements ActionDefinition { public static final Class<@(type_name)_Result> ResultType = @(type_name)_Result.class; public static final Class<@(type_name)_Feedback> FeedbackType = @(type_name)_Feedback.class; + + public static final Class<@(type_name)_FeedbackMessage> FeedbackMessageType = @(type_name)_FeedbackMessage.class; } diff --git a/rosidl_generator_java/resource/msg.cpp.em b/rosidl_generator_java/resource/msg.cpp.em index 733b0513..8553bfcb 100644 --- a/rosidl_generator_java/resource/msg.cpp.em +++ b/rosidl_generator_java/resource/msg.cpp.em @@ -66,6 +66,8 @@ for member in message.structure.members: include_prefix = include_prefix[:-8] elif include_prefix.endswith('__feedback'): include_prefix = include_prefix[:-10] + elif include_prefix.endswith('__feedback_message'): + include_prefix = include_prefix[:-18] elif include_prefix.endswith('__send_goal'): include_prefix = include_prefix[:-11] elif include_prefix.endswith('__get_result'): @@ -87,6 +89,8 @@ elif message_c_include_prefix.endswith('__result'): message_c_include_prefix = message_c_include_prefix[:-8] elif message_c_include_prefix.endswith('__feedback'): message_c_include_prefix = message_c_include_prefix[:-10] +elif message_c_include_prefix.endswith('__feedback_message'): + message_c_include_prefix = message_c_include_prefix[:-18] elif message_c_include_prefix.endswith('__send_goal'): message_c_include_prefix = message_c_include_prefix[:-11] elif message_c_include_prefix.endswith('__get_result'): From 8747a58176d3b943ab90353e6f466717b9c0ba5a Mon Sep 17 00:00:00 2001 From: jgyorfi <35983303+jgyorfi@users.noreply.github.com> Date: Wed, 14 Apr 2021 14:02:58 -0700 Subject: [PATCH 35/51] Added namespace constructor to BaseComposableNode (osrf/ros2_java#62) Co-authored-by: Julius Gyorfi --- .../main/java/org/ros2/rcljava/node/BaseComposableNode.java | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/rcljava/src/main/java/org/ros2/rcljava/node/BaseComposableNode.java b/rcljava/src/main/java/org/ros2/rcljava/node/BaseComposableNode.java index 271a746c..4c328aaa 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/BaseComposableNode.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/BaseComposableNode.java @@ -27,6 +27,11 @@ public BaseComposableNode(String name) { node = RCLJava.createNode(this.name); } + public BaseComposableNode(String name, String namespace) { + this.name = name; + node = RCLJava.createNode(name, namespace); + } + public Node getNode() { return node; } From b191b8b3a91da865aa49f5242980fd0c435297c3 Mon Sep 17 00:00:00 2001 From: Gonzo <42421541+gonzodepedro@users.noreply.github.com> Date: Tue, 4 May 2021 17:10:27 -0300 Subject: [PATCH 36/51] Added sources for rcljava_common (osrf/ros2_java#64) Signed-off-by: Gonzalo de Pedro --- rcljava_common/CMakeLists.txt | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/rcljava_common/CMakeLists.txt b/rcljava_common/CMakeLists.txt index 46715393..09c040ea 100644 --- a/rcljava_common/CMakeLists.txt +++ b/rcljava_common/CMakeLists.txt @@ -20,6 +20,7 @@ if(NOT ANDROID) find_package(JNI REQUIRED) endif() include(UseJava) +include(JavaExtra) # Default to C++14 if(NOT CMAKE_CXX_STANDARD) @@ -148,6 +149,14 @@ list(APPEND ${PROJECT_NAME}_exported_jars "share/${PROJECT_NAME}/java/${PROJECT_ ament_export_jars(${${PROJECT_NAME}_exported_jars}) +add_source_jar("${PROJECT_NAME}-source_jar" + ${${PROJECT_NAME}_java_sources} + OUTPUT_NAME + ${PROJECT_NAME}-source +) + +install_jar("${PROJECT_NAME}-source_jar" "share/${PROJECT_NAME}/java") + include_directories(include) add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_cpp_sources}) From 9577ed8ce9fa852fe47d242350da874172214eeb Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 7 May 2021 11:41:35 -0300 Subject: [PATCH 37/51] Complete action server implementation (osrf/ros2_java#65) * GoalHandle transitions should be able to set a result. * Refactor GoalStatus enumeration. * Add interface for ResultRequestDefinition, ResultResponseDefinition, FeedbackDefinition, GoalDefinition, ResultDefinition. * Modify rcljava rosidl generator to implement those interfaces. * Handle result requests. * Handle goal handle terminal transitions. * Cleanup goal handle status transition bindings. * Take advantage of nested class. * Publish status after terminal state transition. * Handle expire goals. * Add support for publishing feedback. Signed-off-by: Ivan Santiago Paunovic Co-authored-by: Jacob Perron --- ...org_ros2_rcljava_action_ActionServerImpl.h | 37 ++- ...a_action_ActionServerImpl_GoalHandleImpl.h | 44 +-- ...g_ros2_rcljava_action_ActionServerImpl.cpp | 104 +++++++ ...action_ActionServerImpl_GoalHandleImpl.cpp | 72 +++-- .../action/ActionServerGoalHandle.java | 29 +- .../ros2/rcljava/action/ActionServerImpl.java | 257 +++++++++++++++--- .../org/ros2/rcljava/action/GoalStatus.java | 47 ++-- rcljava_common/CMakeLists.txt | 8 +- .../rcljava/interfaces/ActionDefinition.java | 7 +- .../interfaces/FeedbackDefinition.java | 25 ++ .../interfaces/FeedbackMessageDefinition.java | 29 ++ .../rcljava/interfaces/GoalDefinition.java | 25 ++ .../interfaces/GoalRequestDefinition.java | 2 +- .../rcljava/interfaces/ResultDefinition.java | 25 ++ .../interfaces/ResultRequestDefinition.java | 22 ++ .../interfaces/ResultResponseDefinition.java | 23 ++ rosidl_generator_java/resource/action.java.em | 79 +++++- rosidl_generator_java/resource/idl.java.em | 1 + rosidl_generator_java/resource/msg.java.em | 6 +- rosidl_generator_java/resource/srv.java.em | 1 + 20 files changed, 682 insertions(+), 161 deletions(-) create mode 100644 rcljava_common/src/main/java/org/ros2/rcljava/interfaces/FeedbackDefinition.java create mode 100644 rcljava_common/src/main/java/org/ros2/rcljava/interfaces/FeedbackMessageDefinition.java create mode 100644 rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalDefinition.java create mode 100644 rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ResultDefinition.java create mode 100644 rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ResultRequestDefinition.java create mode 100644 rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ResultResponseDefinition.java diff --git a/rcljava/include/org_ros2_rcljava_action_ActionServerImpl.h b/rcljava/include/org_ros2_rcljava_action_ActionServerImpl.h index 28b65441..fe3097bb 100644 --- a/rcljava/include/org_ros2_rcljava_action_ActionServerImpl.h +++ b/rcljava/include/org_ros2_rcljava_action_ActionServerImpl.h @@ -126,12 +126,47 @@ JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeProcessCancelRequest * Method: nativeCheckGoalExists * Signature: (JLorg/ros2/rcljava/interfaces/MessageDefinition;JJ)Z */ - JNIEXPORT jboolean JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeCheckGoalExists( JNIEnv * env, jclass, jlong, jobject, jlong, jlong); +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativePublishStatus + * Signature: (J) + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativePublishStatus( + JNIEnv * env, jclass, jlong); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativePublishFeedbackMessage + * Signature: (JLorg/ros2/rcljava/interfaces/FeedbackMessageDefinition;JJ) + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativePublishFeedbackMessage( + JNIEnv * env, jclass, jlong, jobject, jlong, jlong); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeNotifyGoalDone + * Signature: (J) + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeNotifyGoalDone( + JNIEnv * env, jclass, jlong); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeExpireGoals + * Signature: (JLaction_msgs/msg/GoalInfo;JLorg/ros2/rcljava/consumers/Consumer;) + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeExpireGoals( + JNIEnv * env, jclass, jlong, jobject, jlong, jobject); + #ifdef __cplusplus } #endif diff --git a/rcljava/include/org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl.h b/rcljava/include/org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl.h index 94aaab23..6f6f98ce 100644 --- a/rcljava/include/org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl.h +++ b/rcljava/include/org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl.h @@ -41,48 +41,12 @@ JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_native /* * Class: org_ros2_rcljava_action_ActionServerImpl$GoalHandleImpl - * Method: nativeGoalEventExecute - * Signature: (J) - */ -JNIEXPORT void -JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventExecute( - JNIEnv * env, jclass, jlong); - -/* - * Class: org_ros2_rcljava_action_ActionServerImpl$GoalHandleImpl - * Method: nativeGoalEventCancelGoal - * Signature: (J) - */ -JNIEXPORT void -JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventCancelGoal( - JNIEnv * env, jclass, jlong); - -/* - * Class: org_ros2_rcljava_action_ActionServerImpl$GoalHandleImpl - * Method: nativeGoalEventSucceed - * Signature: (J) - */ -JNIEXPORT void -JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventSucceed( - JNIEnv * env, jclass, jlong); - -/* - * Class: org_ros2_rcljava_action_ActionServerImpl$GoalHandleImpl - * Method: nativeGoalEventAbort - * Signature: (J) - */ -JNIEXPORT void -JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventAbort( - JNIEnv * env, jclass, jlong); - -/* - * Class: org_ros2_rcljava_action_ActionServerImpl$GoalHandleImpl - * Method: nativeGoalEventCanceled - * Signature: (J) + * Method: nativeUpdateGoalState + * Signature: (JJ) */ JNIEXPORT void -JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventCanceled( - JNIEnv * env, jclass, jlong); +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeUpdateGoalState( + JNIEnv * env, jclass, jlong jgoal_handle, jlong jevent); /* * Class: org_ros2_rcljava_action_ActionServerImpl$GoalHandleImpl diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp index 2d48fef1..47a89440 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp @@ -21,6 +21,7 @@ #include "rcl/error_handling.h" #include "rcl/rcl.h" #include "rcl_action/rcl_action.h" +#include "rcpputils/scope_exit.hpp" #include "rosidl_runtime_c/message_type_support_struct.h" #include "rcljava_common/exceptions.hpp" @@ -30,6 +31,7 @@ #include "./convert.hpp" +using rcljava_common::exceptions::rcljava_throw_exception; using rcljava_common::exceptions::rcljava_throw_rclexception; using rcljava_common::signatures::convert_from_java_signature; using rcljava_common::signatures::convert_to_java_signature; @@ -363,3 +365,105 @@ JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeCheckGoalExists( return exists; } + +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativePublishStatus( + JNIEnv * env, jclass, jlong jaction_server) +{ + assert(0 != jaction_server); + auto * action_server = reinterpret_cast(jaction_server); + rcl_action_goal_status_array_t status_message = + rcl_action_get_zero_initialized_goal_status_array(); + rcl_ret_t ret = rcl_action_get_goal_status_array(action_server, &status_message); + if (ret != RCL_RET_OK) { + std::string msg = \ + "Failed to get goal status array: " + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + return; + } + + ret = rcl_action_publish_status(action_server, &status_message); + + if (ret != RCL_RET_OK) { + std::string msg = \ + "Failed to publish status array: " + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + return; + } +} + +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativePublishFeedbackMessage( + JNIEnv * env, jclass, jlong jaction_server, jobject jfeedback_msg, + jlong jfeedback_from_java, jlong jfeedback_destroy) +{ + assert(0 != jaction_server); + assert(nullptr != jfeedback_msg); + assert(0 != jfeedback_from_java); + assert(0 != jfeedback_destroy); + + auto * action_server = reinterpret_cast(jaction_server); + auto destroy_feedback = reinterpret_cast(jfeedback_destroy); + auto from_java_feedback = reinterpret_cast(jfeedback_from_java); + void * feedback = from_java_feedback(jfeedback_msg, nullptr); + auto destroy_feedback_scope_exit = rcpputils::make_scope_exit( + [feedback, destroy_feedback]() {destroy_feedback(feedback);}); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + if (!feedback) { + return; + } + rcl_ret_t ret = rcl_action_publish_feedback(action_server, feedback); + RCLJAVA_COMMON_THROW_FROM_RCL(env, ret, "Failed to publish feedback"); +} + +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeNotifyGoalDone( + JNIEnv * env, jclass, jlong jaction_server) +{ + assert(0 != jaction_server); + auto * action_server = reinterpret_cast(jaction_server); + rcl_ret_t ret = rcl_action_notify_goal_done(action_server); + RCLJAVA_COMMON_THROW_FROM_RCL(env, ret, "Failed to notify goal done"); +} + +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeExpireGoals( + JNIEnv * env, jclass, jlong jaction_server, jobject jgoal_info, + jlong jgoal_info_to_java, jobject jaccept) +{ + assert(0 != jaction_server); + auto * action_server = reinterpret_cast(jaction_server); + + rcl_action_goal_info_t expired_goal; + size_t num_expired = 1; + + // Loop in case more than 1 goal expired + jclass jaccept_class = env->GetObjectClass(jaccept); + if (!jaccept_class) { + rcljava_throw_exception( + env, "java/lang/IllegalStateException", + "nativeExpireGoals(): could not find jaccept class"); + return; + } + jmethodID jaccept_mid = env->GetMethodID( + jaccept_class, "accept", "(Laction_msgs/msg/GoalInfo;)V"); + if (!jaccept_mid) { + rcljava_throw_exception( + env, "java/lang/IllegalStateException", + "nativeExpireGoals(): could not find jaccept accept method"); + return; + } + while (num_expired > 0u) { + rcl_ret_t ret; + ret = rcl_action_expire_goals(action_server, &expired_goal, 1, &num_expired); + RCLJAVA_COMMON_THROW_FROM_RCL(env, ret, "Failed to expire goals"); + if (num_expired) { + auto goal_info_to_java = reinterpret_cast(jgoal_info_to_java); + goal_info_to_java(&expired_goal, jgoal_info); + env->CallVoidMethod(jaccept, jaccept_mid, jgoal_info); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + } + } +} diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl.cpp index de1fb4b6..85185eaa 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl.cpp @@ -26,6 +26,7 @@ #include "org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl.h" +using rcljava_common::exceptions::rcljava_throw_exception; using rcljava_common::exceptions::rcljava_throw_rclexception; using rcljava_common::signatures::convert_from_java_signature; using rcljava_common::signatures::destroy_ros_message_signature; @@ -81,10 +82,40 @@ JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_native return static_cast(status); } -static void update_goal_state(JNIEnv * env, jlong jgoal_handle, rcl_action_goal_event_t event) +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeUpdateGoalState( + JNIEnv * env, jclass, jlong jgoal_handle, jlong jto_status) { - rcl_action_goal_handle_t * goal_handle = reinterpret_cast( - jgoal_handle); + assert(0 != jgoal_handle); + rcl_action_goal_event_t event = GOAL_EVENT_NUM_EVENTS; + switch (jto_status) { + case GOAL_STATE_EXECUTING: + event = GOAL_EVENT_EXECUTE; + break; + case GOAL_STATE_CANCELING: + event = GOAL_EVENT_CANCEL_GOAL; + break; + case GOAL_STATE_SUCCEEDED: + event = GOAL_EVENT_SUCCEED; + break; + case GOAL_STATE_CANCELED: + event = GOAL_EVENT_CANCELED; + break; + case GOAL_STATE_ABORTED: + event = GOAL_EVENT_ABORT; + break; + case GOAL_STATE_ACCEPTED: // fallthrough + case GOAL_STATE_UNKNOWN: // fallthrough + default: + break; + } + if (event >= GOAL_EVENT_NUM_EVENTS) { + rcljava_throw_exception( + env, "java/lang/IllegalStateException", "nativeUpdateGoalState(): Unknown event"); + return; + } + + auto * goal_handle = reinterpret_cast(jgoal_handle); rcl_ret_t ret = rcl_action_update_goal_state(goal_handle, event); if (RCL_RET_OK != ret) { std::string msg = "Failed to update goal state with event: " + @@ -94,41 +125,6 @@ static void update_goal_state(JNIEnv * env, jlong jgoal_handle, rcl_action_goal_ } } -JNIEXPORT void -JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventExecute( - JNIEnv * env, jclass, jlong jgoal_handle) -{ - update_goal_state(env, jgoal_handle, GOAL_EVENT_EXECUTE); -} - -JNIEXPORT void -JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventCancelGoal( - JNIEnv * env, jclass, jlong jgoal_handle) -{ - update_goal_state(env, jgoal_handle, GOAL_EVENT_CANCEL_GOAL); -} - -JNIEXPORT void -JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventSucceed( - JNIEnv * env, jclass, jlong jgoal_handle) -{ - update_goal_state(env, jgoal_handle, GOAL_EVENT_SUCCEED); -} - -JNIEXPORT void -JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventAbort( - JNIEnv * env, jclass, jlong jgoal_handle) -{ - update_goal_state(env, jgoal_handle, GOAL_EVENT_ABORT); -} - -JNIEXPORT void -JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventCanceled( - JNIEnv * env, jclass, jlong jgoal_handle) -{ - update_goal_state(env, jgoal_handle, GOAL_EVENT_CANCELED); -} - JNIEXPORT void JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeDipose( JNIEnv * env, jclass, jlong jgoal_handle) diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerGoalHandle.java b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerGoalHandle.java index 11117437..6f207319 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerGoalHandle.java +++ b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerGoalHandle.java @@ -17,9 +17,21 @@ import org.ros2.rcljava.interfaces.ActionDefinition; import org.ros2.rcljava.interfaces.Disposable; -import org.ros2.rcljava.interfaces.MessageDefinition; +import org.ros2.rcljava.interfaces.GoalDefinition; +import org.ros2.rcljava.interfaces.FeedbackDefinition; +import org.ros2.rcljava.interfaces.ResultDefinition; public interface ActionServerGoalHandle extends Disposable { + /** + * Get the action result type. + */ + public Class getResultType(); + + /** + * Get the action feedback type. + */ + public Class getFeedbackType(); + /** * Get the message containing the timestamp and ID for the goal. */ @@ -28,7 +40,7 @@ public interface ActionServerGoalHandle extends Disp /** * Get the goal message. */ - public MessageDefinition getGoal(); + public GoalDefinition getGoal(); /** * Get the goal status. @@ -52,19 +64,26 @@ public interface ActionServerGoalHandle extends Disp * * Pre-condition: the goal must be in the EXECUTING or CANCELING state. */ - public void succeed(); + public void succeed(ResultDefinition result); /** * Transition the goal the the CANCELED state. * * Pre-condition: the goal must be in the CANCELING state. */ - public void canceled(); + public void canceled(ResultDefinition result); /** * Transition the goal the the CANCELED state. * * Pre-condition: the goal must be in the EXCUTING or CANCELING state. */ - public void abort(); + public void abort(ResultDefinition result); + + /** + * Send an update about the progress of a goal. + * + * Pre-condition: the goal must be in the EXCUTING state. + */ + public void publishFeedback(FeedbackDefinition result); } diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java index b7e740c8..0a2aee98 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java @@ -27,8 +27,14 @@ import org.ros2.rcljava.consumers.Consumer; import org.ros2.rcljava.interfaces.MessageDefinition; import org.ros2.rcljava.interfaces.ActionDefinition; +import org.ros2.rcljava.interfaces.GoalDefinition; import org.ros2.rcljava.interfaces.GoalRequestDefinition; import org.ros2.rcljava.interfaces.GoalResponseDefinition; +import org.ros2.rcljava.interfaces.FeedbackDefinition; +import org.ros2.rcljava.interfaces.FeedbackMessageDefinition; +import org.ros2.rcljava.interfaces.ResultDefinition; +import org.ros2.rcljava.interfaces.ResultRequestDefinition; +import org.ros2.rcljava.interfaces.ResultResponseDefinition; import org.ros2.rcljava.node.Node; import org.ros2.rcljava.service.RMWRequestId; import org.ros2.rcljava.time.Clock; @@ -53,9 +59,8 @@ public class ActionServerImpl implements ActionServe class GoalHandleImpl implements ActionServerGoalHandle { private long handle; - private ActionServer actionServer; private action_msgs.msg.GoalInfo goalInfo; - private MessageDefinition goal; + private GoalDefinition goal; private native long nativeAcceptNewGoal( long actionServerHandle, @@ -63,28 +68,37 @@ private native long nativeAcceptNewGoal( long goalInfoDestructorHandle, MessageDefinition goalInfo); private native int nativeGetStatus(long goalHandle); - private native void nativeGoalEventExecute(long goalHandle); - private native void nativeGoalEventCancelGoal(long goalHandle); - private native void nativeGoalEventSucceed(long goalHandle); - private native void nativeGoalEventAbort(long goalHandle); - private native void nativeGoalEventCanceled(long goalHandle); + private native void nativeUpdateGoalState(long goalHandle, long event); private native void nativeDispose(long handle); public GoalHandleImpl( - ActionServer actionServer, action_msgs.msg.GoalInfo goalInfo, MessageDefinition goal) + action_msgs.msg.GoalInfo goalInfo, GoalDefinition goal) { - this.actionServer = actionServer; this.goalInfo = goalInfo; this.goal = goal; long goalInfoFromJavaConverterHandle = goalInfo.getFromJavaConverterInstance(); long goalInfoDestructorHandle = goalInfo.getDestructorInstance(); this.handle = nativeAcceptNewGoal( - actionServer.getHandle(), + ActionServerImpl.this.getHandle(), goalInfoFromJavaConverterHandle, goalInfoDestructorHandle, goalInfo); } + /** + * {@inheritDoc} + */ + public Class getResultType() { + return ActionServerImpl.this.actionTypeInstance.getResultType(); + } + + /** + * {@inheritDoc} + */ + public Class getFeedbackType() { + return ActionServerImpl.this.actionTypeInstance.getFeedbackType(); + } + /** * {@inheritDoc} */ @@ -95,7 +109,7 @@ public action_msgs.msg.GoalInfo getGoalInfo() { /** * {@inheritDoc} */ - public MessageDefinition getGoal() { + public GoalDefinition getGoal() { return this.goal; } @@ -122,7 +136,7 @@ public synchronized void execute() { // In this case we want to avoid the illegal state transition to EXECUTING // but still call the users execute callback to let them handle canceling the goal. if (!this.isCanceling()) { - nativeGoalEventExecute(this.handle); + nativeUpdateGoalState(this.handle, action_msgs.msg.GoalStatus.STATUS_EXECUTING); } } @@ -130,28 +144,45 @@ public synchronized void execute() { * Transition the goal to the CANCELING state. */ public synchronized void cancelGoal() { - nativeGoalEventCancelGoal(this.handle); + nativeUpdateGoalState(this.handle, action_msgs.msg.GoalStatus.STATUS_CANCELING); } /** * {@inheritDoc} */ - public synchronized void succeed() { - nativeGoalEventSucceed(this.handle); + public synchronized void succeed(ResultDefinition result) { + this.toTerminalState(action_msgs.msg.GoalStatus.STATUS_SUCCEEDED, result); } /** * {@inheritDoc} */ - public synchronized void canceled() { - nativeGoalEventCanceled(this.handle); + public synchronized void canceled(ResultDefinition result) { + this.toTerminalState(action_msgs.msg.GoalStatus.STATUS_CANCELED, result); } /** * {@inheritDoc} */ - public synchronized void abort() { - nativeGoalEventAbort(this.handle); + public synchronized void abort(ResultDefinition result) { + this.toTerminalState(action_msgs.msg.GoalStatus.STATUS_ABORTED, result); + } + + /** + * {@inheritDoc} + */ + public synchronized void publishFeedback(FeedbackDefinition feedback) { + Class feedbackMessageType = + ActionServerImpl.this.actionTypeInstance.getFeedbackMessageType(); + FeedbackMessageDefinition feedbackMessage = null; + try { + feedbackMessage = feedbackMessageType.newInstance(); + } catch (Exception ex) { + throw new IllegalArgumentException("Failed to instantiate feedback message", ex); + } + feedbackMessage.setFeedback(feedback); + feedbackMessage.setGoalUuid(this.goalInfo.getGoalId().getUuidAsList()); + ActionServerImpl.this.publishFeedbackMessage(feedbackMessage); } /** @@ -168,6 +199,17 @@ public synchronized final void dispose() { public final long getHandle() { return this.handle; } + + private synchronized final void toTerminalState(byte status, ResultDefinition result) { + nativeUpdateGoalState(this.handle, status); + ResultResponseDefinition resultResponse = ActionServerImpl.this.createResultResponse(); + resultResponse.setGoalStatus(status); + resultResponse.setResult(result); + ActionServerImpl.this.sendResult(goalInfo.getGoalId().getUuidAsList(), resultResponse); + ActionServerImpl.this.publishStatus(); + ActionServerImpl.this.notifyGoalDone(); + ActionServerImpl.this.goalHandles.remove(this.goalInfo.getGoalId().getUuidAsList()); + } } // class GoalHandleImpl private final WeakReference nodeReference; @@ -182,6 +224,8 @@ public final long getHandle() { private boolean[] readyEntities; private Map, GoalHandleImpl> goalHandles; + private Map, List> goalRequests; + private Map, ResultResponseDefinition> goalResults; private boolean isGoalRequestReady() { return this.readyEntities[0]; @@ -231,6 +275,8 @@ public ActionServerImpl( this.acceptedCallback = acceptedCallback; this.goalHandles = new HashMap, GoalHandleImpl>(); + this.goalRequests = new HashMap, List>(); + this.goalResults = new HashMap, ResultResponseDefinition>(); Node node = nodeReference.get(); if (node == null) { @@ -301,18 +347,10 @@ private ActionServerGoalHandle executeGoalRequest( // Create and populate a GoalInfo message List goalUuid = requestMessage.getGoalUuid(); - action_msgs.msg.GoalInfo goalInfo = new action_msgs.msg.GoalInfo(); - unique_identifier_msgs.msg.UUID uuidMessage= new unique_identifier_msgs.msg.UUID(); - uuidMessage.setUuid(goalUuid); - goalInfo.setGoalId(uuidMessage); - goalInfo.setStamp(timeRequestHandled); - - long goalInfoFromJavaConverterHandle = goalInfo.getFromJavaConverterInstance(); - long goalInfoDestructorHandle = goalInfo.getDestructorInstance(); + action_msgs.msg.GoalInfo goalInfo = this.createGoalInfo(goalUuid); // Check that the goal ID isn't already being used - boolean goalExists = nativeCheckGoalExists( - this.handle, goalInfo, goalInfoFromJavaConverterHandle, goalInfoDestructorHandle); + boolean goalExists = this.goalExists(goalInfo); if (goalExists) { logger.warn("Received goal request for goal already being tracked by action server. Goal ID: " + goalUuid); responseMessage.accept(false); @@ -326,14 +364,13 @@ private ActionServerGoalHandle executeGoalRequest( || GoalCallback.GoalResponse.ACCEPT_AND_EXECUTE == response; responseMessage.accept(accepted); - System.out.println("Goal request handled " + accepted); if (!accepted) { return null; } // Create a goal handle and add it to the list of goals GoalHandleImpl goalHandle = this.new GoalHandleImpl( - this, goalInfo, requestMessage.getGoal()); + goalInfo, requestMessage.getGoal()); this.goalHandles.put(requestMessage.getGoalUuid(), goalHandle); if (GoalCallback.GoalResponse.ACCEPT_AND_EXECUTE == response) { goalHandle.execute(); @@ -373,6 +410,23 @@ private action_msgs.srv.CancelGoal_Response executeCancelRequest( return outputMessage; } + private void sendResultResponse( + RMWRequestId rmwRequestId, + ResultResponseDefinition resultResponse) + { + long resultFromJavaConverterHandle = resultResponse.getFromJavaConverterInstance(); + long resultToJavaConverterHandle = resultResponse.getToJavaConverterInstance(); + long resultDestructorHandle = resultResponse.getDestructorInstance(); + + nativeSendResultResponse( + this.handle, + rmwRequestId, + resultFromJavaConverterHandle, + resultToJavaConverterHandle, + resultDestructorHandle, + resultResponse); + } + private static native RMWRequestId nativeTakeGoalRequest( long actionServerHandle, long requestFromJavaConverterHandle, @@ -432,6 +486,95 @@ private static native boolean nativeCheckGoalExists( long goalInfoFromJavaConverterHandle, long goalInfoDestructorHandle); + private boolean goalExists(action_msgs.msg.GoalInfo goalInfo) { + long goalInfoFromJavaConverterHandle = goalInfo.getFromJavaConverterInstance(); + long goalInfoDestructorHandle = goalInfo.getDestructorInstance(); + + return nativeCheckGoalExists( + this.handle, goalInfo, goalInfoFromJavaConverterHandle, goalInfoDestructorHandle); + } + + private static native void nativePublishStatus(long handle); + + private void publishStatus() { + this.nativePublishStatus(this.handle); + } + + private static native void nativePublishFeedbackMessage( + long handle, + FeedbackMessageDefinition feedbackMessage, + long feedbackMessageFromJavaConverterHandle, + long feedbackMessageDestructorHandle); + + private void publishFeedbackMessage(FeedbackMessageDefinition feedbackMessage) { + this.nativePublishFeedbackMessage( + this.handle, + feedbackMessage, + feedbackMessage.getFromJavaConverterInstance(), + feedbackMessage.getDestructorInstance()); + } + + private static native void nativeNotifyGoalDone(long handle); + + private void notifyGoalDone() { + this.nativeNotifyGoalDone(this.handle); + } + + private static native void nativeExpireGoals( + long handle, action_msgs.msg.GoalInfo goalInfo, + long goalInfoToJavaConverterHandle, Consumer onExpiredGoal); + + private void expireGoals() { + action_msgs.msg.GoalInfo goalInfo = new action_msgs.msg.GoalInfo(); + long goalInfoToJavaConverterHandle = goalInfo.getFromJavaConverterInstance(); + nativeExpireGoals( + this.handle, goalInfo, goalInfoToJavaConverterHandle, + new Consumer() { + public void accept(action_msgs.msg.GoalInfo goalInfo) { + List goalUuid = goalInfo.getGoalId().getUuidAsList(); + ActionServerImpl.this.goalResults.remove(goalUuid); + ActionServerImpl.this.goalRequests.remove(goalUuid); + ActionServerImpl.this.goalHandles.remove(goalUuid); + } + }); + } + + private action_msgs.msg.GoalInfo createGoalInfo(List goalUuid) { + action_msgs.msg.GoalInfo goalInfo = new action_msgs.msg.GoalInfo(); + unique_identifier_msgs.msg.UUID uuidMessage= new unique_identifier_msgs.msg.UUID(); + uuidMessage.setUuid(goalUuid); + goalInfo.setGoalId(uuidMessage); + return goalInfo; + } + + private ResultResponseDefinition createResultResponse() { + ResultResponseDefinition resultResponse; + try { + resultResponse = this.actionTypeInstance.getGetResultResponseType().newInstance(); + } catch (Exception ex) { + throw new IllegalArgumentException("Failed to instantiate provided action type", ex); + } + return resultResponse; + } + + // This will store the result, so it can be sent to future result requests, and + // will also send a result response to all requests that were already made. + private void sendResult(List goalUuid, ResultResponseDefinition resultResponse) { + boolean goalExists = this.goalExists(this.createGoalInfo(goalUuid)); + if (!goalExists) { + throw new IllegalStateException("Asked to publish result for goal that does not exist"); + } + this.goalResults.put(goalUuid, resultResponse); + + // if there are clients who already asked for the result, send it to them + List requests = this.goalRequests.get(goalUuid); + if (requests != null) { + for (RMWRequestId request : requests) { + this.sendResultResponse(request, resultResponse); + } + } + } + /** * {@inheritDoc} */ @@ -509,13 +652,57 @@ public void execute() { } if (this.isResultRequestReady()) { - // executeResultRequest(rmwRequestId, requestMessage, responseMessage); - // TODO + Class requestType = this.actionTypeInstance.getGetResultRequestType(); + + ResultRequestDefinition requestMessage = null; + try { + requestMessage = requestType.newInstance(); + } catch (Exception ex) { + throw new IllegalArgumentException("Failed to instantiate action result request", ex); + } + + if (requestMessage != null) { + long requestFromJavaConverterHandle = requestMessage.getFromJavaConverterInstance(); + long requestToJavaConverterHandle = requestMessage.getToJavaConverterInstance(); + long requestDestructorHandle = requestMessage.getDestructorInstance(); + + RMWRequestId rmwRequestId = + nativeTakeResultRequest( + this.handle, + requestFromJavaConverterHandle, requestToJavaConverterHandle, requestDestructorHandle, + requestMessage); + + if (rmwRequestId == null) { + return; + } + + List goalUuid = requestMessage.getGoalUuid(); + boolean goalExists = this.goalExists(this.createGoalInfo(goalUuid)); + + ResultResponseDefinition resultResponse = null; + if (!goalExists) { + resultResponse = this.createResultResponse(); + resultResponse.setGoalStatus(action_msgs.msg.GoalStatus.STATUS_UNKNOWN); + } else { + resultResponse = this.goalResults.get(goalUuid); + } + + if (null == resultResponse) { + List requestIds = null; + requestIds = this.goalRequests.get(goalUuid); + if (requestIds == null) { + requestIds = new ArrayList(); + this.goalRequests.put(goalUuid, requestIds); + } + requestIds.add(rmwRequestId); + } else { + this.sendResultResponse(rmwRequestId, resultResponse); + } + } } if (this.isGoalExpiredReady()) { - // cleanupExpiredGoals(); - // TODO + this.expireGoals(); } } diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/GoalStatus.java b/rcljava/src/main/java/org/ros2/rcljava/action/GoalStatus.java index aee9e1c9..abacc6df 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/action/GoalStatus.java +++ b/rcljava/src/main/java/org/ros2/rcljava/action/GoalStatus.java @@ -16,32 +16,31 @@ package org.ros2.rcljava.action; public enum GoalStatus { - UNKNOWN, - ACCEPTED, - EXECUTING, - CANCELING, - SUCCEEDED, - CANCELED, - ABORTED; + UNKNOWN(action_msgs.msg.GoalStatus.STATUS_UNKNOWN), + ACCEPTED(action_msgs.msg.GoalStatus.STATUS_ACCEPTED), + EXECUTING(action_msgs.msg.GoalStatus.STATUS_EXECUTING), + CANCELING(action_msgs.msg.GoalStatus.STATUS_CANCELING), + SUCCEEDED(action_msgs.msg.GoalStatus.STATUS_SUCCEEDED), + CANCELED(action_msgs.msg.GoalStatus.STATUS_CANCELED), + ABORTED(action_msgs.msg.GoalStatus.STATUS_ABORTED); - public static GoalStatus fromMessageValue(byte status) { - switch (status) { - case action_msgs.msg.GoalStatus.STATUS_ACCEPTED: - return GoalStatus.ACCEPTED; - case action_msgs.msg.GoalStatus.STATUS_EXECUTING: - return GoalStatus.EXECUTING; - case action_msgs.msg.GoalStatus.STATUS_CANCELING: - return GoalStatus.CANCELING; - case action_msgs.msg.GoalStatus.STATUS_SUCCEEDED: - return GoalStatus.SUCCEEDED; - case action_msgs.msg.GoalStatus.STATUS_CANCELED: - return GoalStatus.CANCELED; - case action_msgs.msg.GoalStatus.STATUS_ABORTED: - return GoalStatus.ABORTED; - case action_msgs.msg.GoalStatus.STATUS_UNKNOWN: - default: - return GoalStatus.UNKNOWN; + public static GoalStatus fromMessageValue(byte value) { + for (GoalStatus status: GoalStatus.values()) { + if (status.status == value) { + return status; + } } + return GoalStatus.ABORTED; } + + public byte toMessageValue() { + return this.status; + } + + private GoalStatus(byte status) { + this.status = status; + } + + private byte status; } diff --git a/rcljava_common/CMakeLists.txt b/rcljava_common/CMakeLists.txt index 09c040ea..228f24ea 100644 --- a/rcljava_common/CMakeLists.txt +++ b/rcljava_common/CMakeLists.txt @@ -39,9 +39,15 @@ set(${PROJECT_NAME}_java_sources "src/main/java/org/ros2/rcljava/exceptions/RCLReturn.java" "src/main/java/org/ros2/rcljava/interfaces/ActionDefinition.java" "src/main/java/org/ros2/rcljava/interfaces/Disposable.java" - "src/main/java/org/ros2/rcljava/interfaces/GoalRequestDefinition.java" + "src/main/java/org/ros2/rcljava/interfaces/FeedbackDefinition.java" + "src/main/java/org/ros2/rcljava/interfaces/FeedbackMessageDefinition.java" + "src/main/java/org/ros2/rcljava/interfaces/GoalDefinition.java" "src/main/java/org/ros2/rcljava/interfaces/GoalResponseDefinition.java" + "src/main/java/org/ros2/rcljava/interfaces/GoalRequestDefinition.java" "src/main/java/org/ros2/rcljava/interfaces/MessageDefinition.java" + "src/main/java/org/ros2/rcljava/interfaces/ResultDefinition.java" + "src/main/java/org/ros2/rcljava/interfaces/ResultRequestDefinition.java" + "src/main/java/org/ros2/rcljava/interfaces/ResultResponseDefinition.java" "src/main/java/org/ros2/rcljava/interfaces/ServiceDefinition.java" ) diff --git a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ActionDefinition.java b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ActionDefinition.java index e39d65d8..d225901e 100644 --- a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ActionDefinition.java +++ b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ActionDefinition.java @@ -18,6 +18,9 @@ public interface ActionDefinition { Class getSendGoalRequestType(); Class getSendGoalResponseType(); - Class getGetResultRequestType(); - Class getGetResultResponseType(); + Class getGetResultRequestType(); + Class getGetResultResponseType(); + Class getResultType(); + Class getFeedbackType(); + Class getFeedbackMessageType(); } diff --git a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/FeedbackDefinition.java b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/FeedbackDefinition.java new file mode 100644 index 00000000..daff4162 --- /dev/null +++ b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/FeedbackDefinition.java @@ -0,0 +1,25 @@ +/* Copyright 2021 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.interfaces; + +/** +* Definition of an action feedback. +* Implementation of this interface for an Action is automatically generated. +* +* Don't extend this interface yourself!! +*/ +public interface FeedbackDefinition extends MessageDefinition { +} diff --git a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/FeedbackMessageDefinition.java b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/FeedbackMessageDefinition.java new file mode 100644 index 00000000..8ef39114 --- /dev/null +++ b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/FeedbackMessageDefinition.java @@ -0,0 +1,29 @@ +/* Copyright 2021 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.interfaces; + +import java.util.List; + +/** +* Definition of an action feedback message. +* Implementation of this interface for an Action is automatically generated. +* +* Don't extend this interface yourself!! +*/ +public interface FeedbackMessageDefinition extends MessageDefinition { + void setFeedback(FeedbackDefinition feedback); + void setGoalUuid(List goalUuid); +} diff --git a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalDefinition.java b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalDefinition.java new file mode 100644 index 00000000..3aeea1af --- /dev/null +++ b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalDefinition.java @@ -0,0 +1,25 @@ +/* Copyright 2021 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.interfaces; + +/** +* Definition of an action goal. +* Implementation of this interface for an Action is automatically generated. +* +* Don't extend this interface yourself!! +*/ +public interface GoalDefinition extends MessageDefinition { +} diff --git a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalRequestDefinition.java b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalRequestDefinition.java index cf6dc177..5191a4ab 100644 --- a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalRequestDefinition.java +++ b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalRequestDefinition.java @@ -18,6 +18,6 @@ import java.util.List; public interface GoalRequestDefinition extends MessageDefinition { - MessageDefinition getGoal(); + GoalDefinition getGoal(); List getGoalUuid(); } diff --git a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ResultDefinition.java b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ResultDefinition.java new file mode 100644 index 00000000..daec4304 --- /dev/null +++ b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ResultDefinition.java @@ -0,0 +1,25 @@ +/* Copyright 2021 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.interfaces; + +/** +* Definition of an action result. +* Implementation of this interface for an Action is automatically generated. +* +* Don't extend this interface yourself!! +*/ +public interface ResultDefinition extends MessageDefinition { +} diff --git a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ResultRequestDefinition.java b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ResultRequestDefinition.java new file mode 100644 index 00000000..f259e1e7 --- /dev/null +++ b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ResultRequestDefinition.java @@ -0,0 +1,22 @@ +/* Copyright 2021 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.interfaces; + +import java.util.List; + +public interface ResultRequestDefinition extends MessageDefinition { + List getGoalUuid(); +} diff --git a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ResultResponseDefinition.java b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ResultResponseDefinition.java new file mode 100644 index 00000000..86a927e7 --- /dev/null +++ b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ResultResponseDefinition.java @@ -0,0 +1,23 @@ +/* Copyright 2021 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.interfaces; + +import java.util.List; + +public interface ResultResponseDefinition extends MessageDefinition { + void setResult(ResultDefinition result); + void setGoalStatus(byte status); +} diff --git a/rosidl_generator_java/resource/action.java.em b/rosidl_generator_java/resource/action.java.em index 2656f192..f5e33b2b 100644 --- a/rosidl_generator_java/resource/action.java.em +++ b/rosidl_generator_java/resource/action.java.em @@ -23,7 +23,10 @@ data = { 'output_dir': output_dir, 'template_basepath': template_basepath, } -data.update({'message': action.goal}) +data.update({ + 'message': action.goal, + 'marker_interfaces': [f'org.ros2.rcljava.interfaces.GoalDefinition<{type_name}>'], +}) output_file = os.path.join(output_dir, *namespaces[1:], goal_type_name + '.java') expand_template( 'msg.java.em', @@ -31,7 +34,10 @@ expand_template( output_file, template_basepath=template_basepath) -data.update({'message': action.result}) +data.update({ + 'message': action.result, + 'marker_interfaces': [f'org.ros2.rcljava.interfaces.ResultDefinition<{type_name}>'], +}) output_file = os.path.join(output_dir, *namespaces[1:], result_type_name + '.java') expand_template( 'msg.java.em', @@ -39,7 +45,10 @@ expand_template( output_file, template_basepath=template_basepath) -data.update({'message': action.feedback}) +data.update({ + 'message': action.feedback, + 'marker_interfaces': [f'org.ros2.rcljava.interfaces.FeedbackDefinition<{type_name}>'], +}) output_file = os.path.join(output_dir, *namespaces[1:], feedback_type_name + '.java') expand_template( 'msg.java.em', @@ -47,7 +56,10 @@ expand_template( output_file, template_basepath=template_basepath) -data.update({'message': action.feedback_message}) +data.update({ + 'message': action.feedback_message, + 'marker_interfaces': [], +}) output_file = os.path.join(output_dir, *namespaces[1:], feedback_message_type_name + '.java') expand_template( 'msg.java.em', @@ -55,6 +67,8 @@ expand_template( output_file, template_basepath=template_basepath) +del data['marker_interfaces'] +del data['message'] data.update({'service': action.send_goal_service}) output_file = os.path.join(output_dir, *namespaces[1:], send_goal_type_name + '.java') expand_template( @@ -75,9 +89,15 @@ action_imports = [ 'java.util.List', 'org.ros2.rcljava.common.JNIUtils', 'org.ros2.rcljava.interfaces.ActionDefinition', + 'org.ros2.rcljava.interfaces.FeedbackDefinition', + 'org.ros2.rcljava.interfaces.FeedbackMessageDefinition', + 'org.ros2.rcljava.interfaces.GoalDefinition', 'org.ros2.rcljava.interfaces.GoalRequestDefinition', 'org.ros2.rcljava.interfaces.GoalResponseDefinition', 'org.ros2.rcljava.interfaces.MessageDefinition', + 'org.ros2.rcljava.interfaces.ResultDefinition', + 'org.ros2.rcljava.interfaces.ResultRequestDefinition', + 'org.ros2.rcljava.interfaces.ResultResponseDefinition', 'org.slf4j.Logger', 'org.slf4j.LoggerFactory', ] @@ -91,7 +111,6 @@ public class @(type_name) implements ActionDefinition { public static class SendGoalRequest extends @(type_name)_SendGoal_Request implements GoalRequestDefinition<@(type_name)> { public List getGoalUuid() { - // Return List since it's hash is based on the values (not the object pointer) return super.getGoalId().getUuidAsList(); } } @@ -109,20 +128,56 @@ public class @(type_name) implements ActionDefinition { } } - public Class getSendGoalRequestType() { + public static class GetResultRequest extends @(type_name)_GetResult_Request implements ResultRequestDefinition<@(type_name)> { + public List getGoalUuid() { + return super.getGoalId().getUuidAsList(); + } + } + + public static class GetResultResponse extends @(type_name)_GetResult_Response implements ResultResponseDefinition<@(type_name)> { + public void setResult(ResultDefinition<@(type_name)> result) { + super.setResult((@(type_name)_Result)result); + } + public void setGoalStatus(byte status) { + super.setStatus(status); + } + } + + public static class FeedbackMessage extends @(type_name)_FeedbackMessage implements FeedbackMessageDefinition<@(type_name)> { + public void setFeedback(FeedbackDefinition<@(type_name)> feedback) { + super.setFeedback((@(type_name)_Feedback) feedback); + } + public void setGoalUuid(List goalUuid) { + super.getGoalId().setUuid(goalUuid); + } + } + + public Class> getSendGoalRequestType() { return SendGoalRequest.class; } - public Class getSendGoalResponseType() { + public Class> getSendGoalResponseType() { return SendGoalResponse.class; } - public Class getGetResultRequestType() { - return @(type_name)_GetResult_Request.class; + public Class> getGetResultRequestType() { + return GetResultRequest.class; + } + + public Class> getGetResultResponseType() { + return GetResultResponse.class; } - public Class getGetResultResponseType() { - return @(type_name)_GetResult_Response.class; + public Class> getResultType() { + return @(type_name)_Result.class; + } + + public Class> getFeedbackType() { + return @(type_name)_Feedback.class; + } + + public Class> getFeedbackMessageType() { + return FeedbackMessage.class; } private static final Logger logger = LoggerFactory.getLogger(@(type_name).class); @@ -143,6 +198,4 @@ public class @(type_name) implements ActionDefinition { public static final Class<@(type_name)_Result> ResultType = @(type_name)_Result.class; public static final Class<@(type_name)_Feedback> FeedbackType = @(type_name)_Feedback.class; - - public static final Class<@(type_name)_FeedbackMessage> FeedbackMessageType = @(type_name)_FeedbackMessage.class; } diff --git a/rosidl_generator_java/resource/idl.java.em b/rosidl_generator_java/resource/idl.java.em index f34d75c7..9c90c354 100644 --- a/rosidl_generator_java/resource/idl.java.em +++ b/rosidl_generator_java/resource/idl.java.em @@ -26,6 +26,7 @@ data = { 'interface_path': interface_path, 'output_dir': output_dir, 'template_basepath': template_basepath, + 'marker_interfaces': [], } for message in content.get_elements_of_type(Message): diff --git a/rosidl_generator_java/resource/msg.java.em b/rosidl_generator_java/resource/msg.java.em index 9dcb2a42..f5b62f2e 100644 --- a/rosidl_generator_java/resource/msg.java.em +++ b/rosidl_generator_java/resource/msg.java.em @@ -17,6 +17,9 @@ from rosidl_parser.definition import BoundedSequence from rosidl_parser.definition import NamespacedType type_name = message.structure.namespaced_type.name +interfaces_implemented = ['MessageDefinition'] +interfaces_implemented.extend(f"{t.rsplit('.', 1)[1]}" for t in marker_interfaces) +interfaces_implemented = ', '.join(interfaces_implemented) message_imports = [ 'org.apache.commons.lang3.builder.EqualsBuilder', @@ -26,6 +29,7 @@ message_imports = [ 'org.slf4j.Logger', 'org.slf4j.LoggerFactory', ] +message_imports.extend(f"{t.split('<', 1)[0]}" for t in marker_interfaces) }@ @[for message_import in message_imports]@ import @(message_import); @@ -38,7 +42,7 @@ import @('.'.join(member.type.namespaced_name())); @[ end if]@ @[end for]@ -public class @(type_name) implements MessageDefinition { +public class @(type_name) implements @(interfaces_implemented) { private static final Logger logger = LoggerFactory.getLogger(@(type_name).class); diff --git a/rosidl_generator_java/resource/srv.java.em b/rosidl_generator_java/resource/srv.java.em index 9be7a6a7..8bfb887c 100644 --- a/rosidl_generator_java/resource/srv.java.em +++ b/rosidl_generator_java/resource/srv.java.em @@ -18,6 +18,7 @@ data = { 'interface_path': interface_path, 'output_dir': output_dir, 'template_basepath': template_basepath, + 'marker_interfaces': [], } data.update({'message': service.request_message}) output_file = os.path.join(output_dir, *namespaces[1:], request_type_name + '.java') From 2775f666fc66a54a4f1bcf5b23e0572f1f9cde6c Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 13 May 2021 10:16:20 -0300 Subject: [PATCH 38/51] Ignore unused vm argument in JNI_OnUnload (osrf/ros2_java#67) Signed-off-by: Ivan Santiago Paunovic --- rosidl_generator_java/resource/msg.cpp.em | 1 + 1 file changed, 1 insertion(+) diff --git a/rosidl_generator_java/resource/msg.cpp.em b/rosidl_generator_java/resource/msg.cpp.em index 8553bfcb..84c4bb64 100644 --- a/rosidl_generator_java/resource/msg.cpp.em +++ b/rosidl_generator_java/resource/msg.cpp.em @@ -519,6 +519,7 @@ if value_method: JNIEXPORT void JNICALL JNI_OnUnload(JavaVM * vm, void *) { + (void)vm; assert(g_vm != nullptr); assert(g_vm == vm); From 822f613a3d597b258aace14303d83171fe1c3ed9 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 13 May 2021 17:08:20 -0300 Subject: [PATCH 39/51] Log debug message when jni library is loaded, not info message (osrf/ros2_java#69) Signed-off-by: Ivan Santiago Paunovic --- .../src/main/java/org/ros2/rcljava/common/JNIUtils.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rcljava_common/src/main/java/org/ros2/rcljava/common/JNIUtils.java b/rcljava_common/src/main/java/org/ros2/rcljava/common/JNIUtils.java index 58661a7f..c335be1c 100644 --- a/rcljava_common/src/main/java/org/ros2/rcljava/common/JNIUtils.java +++ b/rcljava_common/src/main/java/org/ros2/rcljava/common/JNIUtils.java @@ -43,14 +43,14 @@ private static String normalizeClassName(Class cls) { public static void loadImplementation(Class cls) { String libraryName = normalizeClassName(cls); libraryName = libraryName + "__jni"; - logger.info("Loading implementation: " + libraryName); + logger.debug("Loading implementation: " + libraryName); System.loadLibrary(libraryName); } public static void loadTypesupport(Class cls) { String libraryName = normalizeClassName(cls); libraryName = libraryName + "__jni__" + JNIUtils.TYPESUPPORT; - logger.info("Loading typesupport: " + libraryName); + logger.debug("Loading typesupport: " + libraryName); System.loadLibrary(libraryName); } } From 3c0e575f83c5cc832078c2b9f5457b4d15594a25 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 13 May 2021 19:59:10 -0300 Subject: [PATCH 40/51] Fix MessageLost native methods names causing loading issues (osrf/ros2_java#70) Signed-off-by: Ivan Santiago Paunovic --- ...ljava_subscription_statuses_MessageLost.cpp | 8 ++++---- .../subscription/statuses/MessageLost.java | 18 +++++++++--------- 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_subscription_statuses_MessageLost.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_subscription_statuses_MessageLost.cpp index d2f935d5..9fdee7ab 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_subscription_statuses_MessageLost.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_subscription_statuses_MessageLost.cpp @@ -24,7 +24,7 @@ using rcljava_common::exceptions::rcljava_throw_exception; JNIEXPORT jlong JNICALL -Java_org_ros2_rcljava_subscription_statuses_MessageLost_nativeAllocateRCL( +Java_org_ros2_rcljava_subscription_statuses_MessageLost_nativeAllocateRCLStatusEvent( JNIEnv * env, jclass) { void * p = malloc(sizeof(rmw_message_lost_status_t)); @@ -36,14 +36,14 @@ Java_org_ros2_rcljava_subscription_statuses_MessageLost_nativeAllocateRCL( } JNIEXPORT void JNICALL -Java_org_ros2_rcljava_subscription_statuses_MessageLost_nativeDeallocateRCL( +Java_org_ros2_rcljava_subscription_statuses_MessageLost_nativeDeallocateRCLStatusEvent( JNIEnv *, jclass, jlong handle) { free(reinterpret_cast(handle)); } JNIEXPORT void JNICALL -Java_org_ros2_rcljava_subscription_statuses_MessageLost_nativeFromRCL( +Java_org_ros2_rcljava_subscription_statuses_MessageLost_nativeFromRCLEvent( JNIEnv * env, jobject self, jlong handle) { auto * p = reinterpret_cast(handle); @@ -67,7 +67,7 @@ Java_org_ros2_rcljava_subscription_statuses_MessageLost_nativeFromRCL( } JNIEXPORT jint JNICALL -Java_org_ros2_rcljava_subscription_statuses_MessageLost_nativeGetEventType( +Java_org_ros2_rcljava_subscription_statuses_MessageLost_nativeGetSubscriptionEventType( JNIEnv *, jclass) { return RCL_SUBSCRIPTION_MESSAGE_LOST; diff --git a/rcljava/src/main/java/org/ros2/rcljava/subscription/statuses/MessageLost.java b/rcljava/src/main/java/org/ros2/rcljava/subscription/statuses/MessageLost.java index d136b4df..eb789b4c 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/subscription/statuses/MessageLost.java +++ b/rcljava/src/main/java/org/ros2/rcljava/subscription/statuses/MessageLost.java @@ -30,16 +30,16 @@ public class MessageLost implements SubscriptionEventStatus { public int totalCountChange; public final long allocateRCLStatusEvent() { - return nativeAllocateRCL(); + return nativeAllocateRCLStatusEvent(); } public final void deallocateRCLStatusEvent(long handle) { - nativeDeallocateRCL(handle); + nativeDeallocateRCLStatusEvent(handle); } public final void fromRCLEvent(long handle) { - nativeFromRCL(handle); + nativeFromRCLEvent(handle); } public final int getSubscriptionEventType() { - return nativeGetEventType(); + return nativeGetSubscriptionEventType(); } // TODO(ivanpauno): Remove this when -source 8 can be used (method references for the win) public static final Supplier factory = new Supplier() { @@ -48,6 +48,11 @@ public MessageLost get() { } }; + private static native long nativeAllocateRCLStatusEvent(); + private static native void nativeDeallocateRCLStatusEvent(long handle); + private native void nativeFromRCLEvent(long handle); + private static native int nativeGetSubscriptionEventType(); + private static final Logger logger = LoggerFactory.getLogger(MessageLost.class); static { try { @@ -57,9 +62,4 @@ public MessageLost get() { System.exit(1); } } - - private static native long nativeAllocateRCL(); - private static native void nativeDeallocateRCL(long handle); - private native void nativeFromRCL(long handle); - private static native int nativeGetEventType(); } From 51d6fb884882ba78a1a4cb576fa43631ce78ccba Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 14 May 2021 15:26:12 -0300 Subject: [PATCH 41/51] Fix cpp compiler warnings (osrf/ros2_java#71) Signed-off-by: Ivan Santiago Paunovic --- .../org_ros2_rcljava_action_ActionServerImpl.h | 6 +++--- .../org_ros2_rcljava_client_ClientImpl.h | 2 +- .../org_ros2_rcljava_executors_BaseExecutor.h | 2 +- ...org_ros2_rcljava_action_ActionServerImpl.cpp | 16 +++++++--------- .../cpp/org_ros2_rcljava_client_ClientImpl.cpp | 4 +--- ...os2_rcljava_detail_QosIncompatibleStatus.cpp | 2 +- .../org_ros2_rcljava_executors_BaseExecutor.cpp | 17 ++++------------- .../cpp/org_ros2_rcljava_graph_EndpointInfo.cpp | 2 +- .../cpp/org_ros2_rcljava_qos_QoSProfile.cpp | 8 ++++---- .../ros2/rcljava/action/ActionServerImpl.java | 12 ++---------- .../org/ros2/rcljava/client/ClientImpl.java | 6 +++--- .../ros2/rcljava/executors/BaseExecutor.java | 7 +++---- 12 files changed, 31 insertions(+), 53 deletions(-) diff --git a/rcljava/include/org_ros2_rcljava_action_ActionServerImpl.h b/rcljava/include/org_ros2_rcljava_action_ActionServerImpl.h index fe3097bb..ae42dd5e 100644 --- a/rcljava/include/org_ros2_rcljava_action_ActionServerImpl.h +++ b/rcljava/include/org_ros2_rcljava_action_ActionServerImpl.h @@ -92,7 +92,7 @@ JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeTakeResultRequest( */ JNIEXPORT void JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeSendGoalResponse( - JNIEnv *, jclass, jlong, jobject, jlong, jlong, jlong, jobject); + JNIEnv *, jclass, jlong, jobject, jlong, jlong, jobject); /* * Class: org_ros2_rcljava_action_ActionServerImpl @@ -101,7 +101,7 @@ JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeSendGoalResponse( */ JNIEXPORT void JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeSendCancelResponse( - JNIEnv *, jclass, jlong, jobject, jlong, jlong, jlong, jobject); + JNIEnv *, jclass, jlong, jobject, jlong, jlong, jobject); /* * Class: org_ros2_rcljava_action_ActionServerImpl @@ -110,7 +110,7 @@ JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeSendCancelResponse( */ JNIEXPORT void JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeSendResultResponse( - JNIEnv *, jclass, jlong, jobject, jlong, jlong, jlong, jobject); + JNIEnv *, jclass, jlong, jobject, jlong, jlong, jobject); /* * Class: org_ros2_rcljava_action_ActionServerImpl diff --git a/rcljava/include/org_ros2_rcljava_client_ClientImpl.h b/rcljava/include/org_ros2_rcljava_client_ClientImpl.h index 9a3b20e5..56bc2082 100644 --- a/rcljava/include/org_ros2_rcljava_client_ClientImpl.h +++ b/rcljava/include/org_ros2_rcljava_client_ClientImpl.h @@ -27,7 +27,7 @@ extern "C" { */ JNIEXPORT jlong JNICALL Java_org_ros2_rcljava_client_ClientImpl_nativeSendClientRequest( - JNIEnv *, jclass, jlong, jlong, jlong, jlong, jobject); + JNIEnv *, jclass, jlong, jlong, jlong, jobject); /* * Class: org_ros2_rcljava_client_ClientImpl diff --git a/rcljava/include/org_ros2_rcljava_executors_BaseExecutor.h b/rcljava/include/org_ros2_rcljava_executors_BaseExecutor.h index e96b88c8..08b95c08 100644 --- a/rcljava/include/org_ros2_rcljava_executors_BaseExecutor.h +++ b/rcljava/include/org_ros2_rcljava_executors_BaseExecutor.h @@ -123,7 +123,7 @@ JNICALL Java_org_ros2_rcljava_executors_BaseExecutor_nativeTakeRequest( */ JNIEXPORT void JNICALL Java_org_ros2_rcljava_executors_BaseExecutor_nativeSendServiceResponse( - JNIEnv *, jclass, jlong, jobject, jlong, jlong, jlong, jobject); + JNIEnv *, jclass, jlong, jobject, jlong, jlong, jobject); /* * Class: org_ros2_rcljava_executors_BaseExecutor diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp index 47a89440..7ed6d699 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp @@ -212,9 +212,8 @@ Java_org_ros2_rcljava_action_ActionServerImpl_nativeCreateActionServer( return nullptr; \ } \ if (RCL_RET_OK == ret) { \ - jobject jtaken_msg = convert_to_java(taken_msg, jrequest_msg); \ + convert_to_java(taken_msg, jrequest_msg); \ destroy_ros_message(taken_msg); \ - assert(jtaken_msg != nullptr); \ jobject jheader = rcljava::convert_rmw_request_id_to_java(env, &header); \ return jheader; \ } \ @@ -226,7 +225,6 @@ Java_org_ros2_rcljava_action_ActionServerImpl_nativeCreateActionServer( #define RCLJAVA_ACTION_SERVER_SEND_RESPONSE(Type) \ do { \ assert(jresponse_from_java_converter_handle != 0); \ - assert(jresponse_to_java_converter_handle != 0); \ assert(jresponse_destructor_handle != 0); \ rcl_action_server_t * action_server = reinterpret_cast( \ action_server_handle); \ @@ -275,8 +273,8 @@ JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeTakeResultRequest( JNIEXPORT void JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeSendGoalResponse( JNIEnv * env, jclass, jlong action_server_handle, jobject jrequest_id, - jlong jresponse_from_java_converter_handle, jlong jresponse_to_java_converter_handle, - jlong jresponse_destructor_handle, jobject jresponse_msg) + jlong jresponse_from_java_converter_handle, jlong jresponse_destructor_handle, + jobject jresponse_msg) { RCLJAVA_ACTION_SERVER_SEND_RESPONSE(goal); } @@ -284,8 +282,8 @@ JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeSendGoalResponse( JNIEXPORT void JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeSendCancelResponse( JNIEnv * env, jclass, jlong action_server_handle, jobject jrequest_id, - jlong jresponse_from_java_converter_handle, jlong jresponse_to_java_converter_handle, - jlong jresponse_destructor_handle, jobject jresponse_msg) + jlong jresponse_from_java_converter_handle, jlong jresponse_destructor_handle, + jobject jresponse_msg) { RCLJAVA_ACTION_SERVER_SEND_RESPONSE(cancel); } @@ -293,8 +291,8 @@ JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeSendCancelResponse( JNIEXPORT void JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeSendResultResponse( JNIEnv * env, jclass, jlong action_server_handle, jobject jrequest_id, - jlong jresponse_from_java_converter_handle, jlong jresponse_to_java_converter_handle, - jlong jresponse_destructor_handle, jobject jresponse_msg) + jlong jresponse_from_java_converter_handle, jlong jresponse_destructor_handle, + jobject jresponse_msg) { RCLJAVA_ACTION_SERVER_SEND_RESPONSE(result); } diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_client_ClientImpl.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_client_ClientImpl.cpp index 842b7d99..b4fe8e09 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_client_ClientImpl.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_client_ClientImpl.cpp @@ -36,13 +36,11 @@ using rcljava_common::signatures::destroy_ros_message_signature; JNIEXPORT jlong JNICALL Java_org_ros2_rcljava_client_ClientImpl_nativeSendClientRequest( - JNIEnv * env, jclass, jlong client_handle, - jlong jrequest_from_java_converter_handle, jlong jrequest_to_java_converter_handle, + JNIEnv * env, jclass, jlong client_handle, jlong jrequest_from_java_converter_handle, jlong jrequest_destructor_handle, jobject jrequest_msg) { assert(client_handle != 0); assert(jrequest_from_java_converter_handle != 0); - assert(jrequest_to_java_converter_handle != 0); assert(jrequest_destructor_handle != 0); assert(jrequest_msg != nullptr); diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_detail_QosIncompatibleStatus.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_detail_QosIncompatibleStatus.cpp index 149b90c6..a09d7867 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_detail_QosIncompatibleStatus.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_detail_QosIncompatibleStatus.cpp @@ -75,7 +75,7 @@ Java_org_ros2_rcljava_detail_QosIncompatibleStatus_nativeFromRCLEvent( return; } - jfieldID enum_value_fid; + jfieldID enum_value_fid = nullptr; switch (p->last_policy_kind) { case RMW_QOS_POLICY_INVALID: enum_value_fid = env->GetStaticFieldID(qos_kind_clazz, "INVALID", enum_class_path); diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_executors_BaseExecutor.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_executors_BaseExecutor.cpp index 2a784dab..7cbdb6cc 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_executors_BaseExecutor.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_executors_BaseExecutor.cpp @@ -165,9 +165,7 @@ Java_org_ros2_rcljava_executors_BaseExecutor_nativeTake( reinterpret_cast(jto_java_converter); jobject jtaken_msg = convert_to_java(taken_msg, nullptr); - destroy_ros_message(taken_msg); - return jtaken_msg; } @@ -291,12 +289,9 @@ Java_org_ros2_rcljava_executors_BaseExecutor_nativeTakeRequest( } if (ret != RCL_RET_SERVICE_TAKE_FAILED) { - jobject jtaken_msg = convert_to_java(taken_msg, jrequest_msg); - + convert_to_java(taken_msg, jrequest_msg); destroy_ros_message(taken_msg); - assert(jtaken_msg != nullptr); - jobject jheader = rcljava::convert_rmw_request_id_to_java(env, &header); return jheader; } @@ -309,12 +304,11 @@ Java_org_ros2_rcljava_executors_BaseExecutor_nativeTakeRequest( JNIEXPORT void JNICALL Java_org_ros2_rcljava_executors_BaseExecutor_nativeSendServiceResponse( JNIEnv * env, jclass, jlong service_handle, jobject jrequest_id, - jlong jresponse_from_java_converter_handle, jlong jresponse_to_java_converter_handle, - jlong jresponse_destructor_handle, jobject jresponse_msg) + jlong jresponse_from_java_converter_handle, jlong jresponse_destructor_handle, + jobject jresponse_msg) { assert(service_handle != 0); assert(jresponse_from_java_converter_handle != 0); - assert(jresponse_to_java_converter_handle != 0); assert(jresponse_destructor_handle != 0); assert(jresponse_msg != nullptr); @@ -381,12 +375,9 @@ Java_org_ros2_rcljava_executors_BaseExecutor_nativeTakeResponse( } if (ret != RCL_RET_CLIENT_TAKE_FAILED) { - jobject jtaken_msg = convert_to_java(taken_msg, jresponse_msg); - + convert_to_java(taken_msg, jresponse_msg); destroy_ros_message(taken_msg); - assert(jtaken_msg != nullptr); - jobject jheader = rcljava::convert_rmw_request_id_to_java(env, &header); return jheader; } diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_graph_EndpointInfo.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_graph_EndpointInfo.cpp index 1e7763d4..db0f48dc 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_graph_EndpointInfo.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_graph_EndpointInfo.cpp @@ -59,7 +59,7 @@ Java_org_ros2_rcljava_graph_EndpointInfo_nativeFromRCL(JNIEnv * env, jobject sel 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; + jfieldID enum_value_fid = nullptr; switch (p->endpoint_type) { case RMW_ENDPOINT_INVALID: enum_value_fid = env->GetStaticFieldID( diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_qos_QoSProfile.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_qos_QoSProfile.cpp index 16239f7b..e082dca5 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_qos_QoSProfile.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_qos_QoSProfile.cpp @@ -78,7 +78,7 @@ qos_from_rcl(JNIEnv * env, const rmw_qos_profile_t & qos, jobject jqos) RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); jclass history_clazz = env->FindClass("org/ros2/rcljava/qos/policies/History"); - jfieldID history_value_fid; + jfieldID history_value_fid = nullptr; switch (qos.history) { case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT: history_value_fid = env->GetStaticFieldID( @@ -108,7 +108,7 @@ qos_from_rcl(JNIEnv * env, const rmw_qos_profile_t & qos, jobject jqos) env->SetIntField(jqos, depth_fid, qos.depth); jclass reliability_clazz = env->FindClass("org/ros2/rcljava/qos/policies/Reliability"); - jfieldID reliability_value_fid; + jfieldID reliability_value_fid = nullptr; switch (qos.reliability) { case RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT: reliability_value_fid = env->GetStaticFieldID( @@ -136,7 +136,7 @@ qos_from_rcl(JNIEnv * env, const rmw_qos_profile_t & qos, jobject jqos) env->SetObjectField(jqos, reliability_fid, reliability_value); jclass durability_clazz = env->FindClass("org/ros2/rcljava/qos/policies/Durability"); - jfieldID durability_value_fid; + jfieldID durability_value_fid = nullptr; switch (qos.durability) { case RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT: durability_value_fid = env->GetStaticFieldID( @@ -173,7 +173,7 @@ qos_from_rcl(JNIEnv * env, const rmw_qos_profile_t & qos, jobject jqos) RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); jclass liveliness_clazz = env->FindClass("org/ros2/rcljava/qos/policies/Liveliness"); - jfieldID liveliness_value_fid; + jfieldID liveliness_value_fid = nullptr; switch (qos.liveliness) { case RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT: liveliness_value_fid = env->GetStaticFieldID( diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java index 0a2aee98..31369ede 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java @@ -415,14 +415,12 @@ private void sendResultResponse( ResultResponseDefinition resultResponse) { long resultFromJavaConverterHandle = resultResponse.getFromJavaConverterInstance(); - long resultToJavaConverterHandle = resultResponse.getToJavaConverterInstance(); long resultDestructorHandle = resultResponse.getDestructorInstance(); nativeSendResultResponse( this.handle, rmwRequestId, resultFromJavaConverterHandle, - resultToJavaConverterHandle, resultDestructorHandle, resultResponse); } @@ -452,7 +450,6 @@ private static native void nativeSendGoalResponse( long actionServerHandle, RMWRequestId header, long responseFromJavaConverterHandle, - long responseToJavaConverterHandle, long responseDestructorHandle, MessageDefinition responseMessage); @@ -460,7 +457,6 @@ private static native void nativeSendCancelResponse( long actionServerHandle, RMWRequestId header, long responseFromJavaConverterHandle, - long responseToJavaConverterHandle, long responseDestructorHandle, MessageDefinition responseMessage); @@ -468,7 +464,6 @@ private static native void nativeSendResultResponse( long actionServerHandle, RMWRequestId header, long responseFromJavaConverterHandle, - long responseToJavaConverterHandle, long responseDestructorHandle, MessageDefinition responseMessage); @@ -600,7 +595,6 @@ public void execute() { long requestToJavaConverterHandle = requestMessage.getToJavaConverterInstance(); long requestDestructorHandle = requestMessage.getDestructorInstance(); long responseFromJavaConverterHandle = responseMessage.getFromJavaConverterInstance(); - long responseToJavaConverterHandle = responseMessage.getToJavaConverterInstance(); long responseDestructorHandle = responseMessage.getDestructorInstance(); RMWRequestId rmwRequestId = @@ -612,8 +606,7 @@ public void execute() { ActionServerGoalHandle goalHandle = this.executeGoalRequest( rmwRequestId, requestMessage, responseMessage); nativeSendGoalResponse( - this.handle, rmwRequestId, - responseFromJavaConverterHandle, responseToJavaConverterHandle, + this.handle, rmwRequestId, responseFromJavaConverterHandle, responseDestructorHandle, responseMessage); } } @@ -646,8 +639,7 @@ public void execute() { responseMessage = executeCancelRequest(responseMessage); nativeSendCancelResponse( this.handle, rmwRequestId, - responseFromJavaConverterHandle, responseToJavaConverterHandle, - responseDestructorHandle, responseMessage); + responseFromJavaConverterHandle, responseDestructorHandle, responseMessage); } } diff --git a/rcljava/src/main/java/org/ros2/rcljava/client/ClientImpl.java b/rcljava/src/main/java/org/ros2/rcljava/client/ClientImpl.java index c5189b49..3c3187e3 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/client/ClientImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/client/ClientImpl.java @@ -81,7 +81,7 @@ public void accept(Future input) {} synchronized (pendingRequests) { long sequenceNumber = nativeSendClientRequest( handle, request.getFromJavaConverterInstance(), - request.getToJavaConverterInstance(), request.getDestructorInstance(), request); + request.getDestructorInstance(), request); RCLFuture future = new RCLFuture(this.nodeReference); Map.Entry entry = @@ -109,8 +109,8 @@ public final void handleResponse( } private static native long nativeSendClientRequest( - long handle, long requestFromJavaConverterHandle, long requestToJavaConverterHandle, - long requestDestructorHandle, MessageDefinition requestMessage); + long handle, long requestFromJavaConverterHandle, long requestDestructorHandle, + MessageDefinition requestMessage); public final Class getRequestType() { return this.requestType; diff --git a/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java b/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java index 9ad11e52..f6ed8c02 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java +++ b/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java @@ -127,8 +127,7 @@ protected void executeAnyExecutable(AnyExecutable anyExecutable) { if (rmwRequestId != null) { anyExecutable.service.executeCallback(rmwRequestId, requestMessage, responseMessage); nativeSendServiceResponse(anyExecutable.service.getHandle(), rmwRequestId, - responseFromJavaConverterHandle, responseToJavaConverterHandle, - responseDestructorHandle, responseMessage); + responseFromJavaConverterHandle, responseDestructorHandle, responseMessage); } } serviceHandles.remove(anyExecutable.service.getHandle()); @@ -515,8 +514,8 @@ private static native RMWRequestId nativeTakeRequest(long serviceHandle, long requestFromJavaConverterHandle, long requestToJavaConverterHandle, long requestDestructorHandle, MessageDefinition requestMessage); - private static native void nativeSendServiceResponse(long serviceHandle, RMWRequestId header, - long responseFromJavaConverterHandle, long responseToJavaConverterHandle, + private static native void nativeSendServiceResponse( + long serviceHandle, RMWRequestId header, long responseFromJavaConverterHandle, long responseDestructorHandle, MessageDefinition responseMessage); private static native RMWRequestId nativeTakeResponse(long clientHandle, From 8753c5bb7b9774311f755bcd8ee8e7c0eeb6ca3c Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Mon, 17 May 2021 16:06:04 -0300 Subject: [PATCH 42/51] Fix some java compiler warnings (osrf/ros2_java#72) Signed-off-by: Ivan Santiago Paunovic --- .../ros2/rcljava/action/ActionServerImpl.java | 37 +++++----- .../ros2/rcljava/executors/BaseExecutor.java | 70 +++++++++++-------- .../org/ros2/rcljava/graph/NameAndTypes.java | 4 +- .../main/java/org/ros2/rcljava/node/Node.java | 1 + .../java/org/ros2/rcljava/node/NodeImpl.java | 4 +- .../parameters/SyncParametersClientTest.java | 2 +- 6 files changed, 67 insertions(+), 51 deletions(-) diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java index 31369ede..ed379c6b 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java @@ -176,9 +176,9 @@ public synchronized void publishFeedback(FeedbackDefinition feedback) { ActionServerImpl.this.actionTypeInstance.getFeedbackMessageType(); FeedbackMessageDefinition feedbackMessage = null; try { - feedbackMessage = feedbackMessageType.newInstance(); - } catch (Exception ex) { - throw new IllegalArgumentException("Failed to instantiate feedback message", ex); + feedbackMessage = feedbackMessageType.getDeclaredConstructor().newInstance(); + } catch (ReflectiveOperationException ex) { + throw new IllegalArgumentException("Failed to instantiate feedback message: ", ex); } feedbackMessage.setFeedback(feedback); feedbackMessage.setGoalUuid(this.goalInfo.getGoalId().getUuidAsList()); @@ -202,7 +202,7 @@ public final long getHandle() { private synchronized final void toTerminalState(byte status, ResultDefinition result) { nativeUpdateGoalState(this.handle, status); - ResultResponseDefinition resultResponse = ActionServerImpl.this.createResultResponse(); + ResultResponseDefinition resultResponse = ActionServerImpl.this.createResultResponse(); resultResponse.setGoalStatus(status); resultResponse.setResult(result); ActionServerImpl.this.sendResult(goalInfo.getGoalId().getUuidAsList(), resultResponse); @@ -265,9 +265,9 @@ public ActionServerImpl( final Consumer> acceptedCallback) throws IllegalArgumentException { this.nodeReference = nodeReference; try { - this.actionTypeInstance = actionType.newInstance(); - } catch (Exception ex) { - throw new IllegalArgumentException("Failed to instantiate provided action type", ex); + this.actionTypeInstance = actionType.getDeclaredConstructor().newInstance(); + } catch (ReflectiveOperationException ex) { + throw new IllegalArgumentException("Failed to instantiate provided action type: ", ex); } this.actionName = actionName; this.goalCallback = goalCallback; @@ -545,9 +545,10 @@ private action_msgs.msg.GoalInfo createGoalInfo(List goalUuid) { private ResultResponseDefinition createResultResponse() { ResultResponseDefinition resultResponse; try { - resultResponse = this.actionTypeInstance.getGetResultResponseType().newInstance(); - } catch (Exception ex) { - throw new IllegalArgumentException("Failed to instantiate provided action type", ex); + resultResponse = + this.actionTypeInstance.getGetResultResponseType().getDeclaredConstructor().newInstance(); + } catch (ReflectiveOperationException ex) { + throw new IllegalStateException("Failed to instantiate provided action type: ", ex); } return resultResponse; } @@ -582,12 +583,10 @@ public void execute() { GoalResponseDefinition responseMessage = null; try { - requestMessage = requestType.newInstance(); - responseMessage = responseType.newInstance(); - } catch (InstantiationException ie) { - ie.printStackTrace(); - } catch (IllegalAccessException iae) { - iae.printStackTrace(); + requestMessage = requestType.getDeclaredConstructor().newInstance(); + responseMessage = responseType.getDeclaredConstructor().newInstance(); + } catch (ReflectiveOperationException ex) { + throw new IllegalStateException("Failed to instantiate request or responce: ", ex); } if (requestMessage != null && responseMessage != null) { @@ -648,9 +647,9 @@ public void execute() { ResultRequestDefinition requestMessage = null; try { - requestMessage = requestType.newInstance(); - } catch (Exception ex) { - throw new IllegalArgumentException("Failed to instantiate action result request", ex); + requestMessage = requestType.getDeclaredConstructor().newInstance(); + } catch (ReflectiveOperationException ex) { + throw new IllegalArgumentException("Failed to instantiate action result request: ", ex); } if (requestMessage != null) { diff --git a/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java b/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java index f6ed8c02..bd86e63a 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java +++ b/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java @@ -15,17 +15,18 @@ package org.ros2.rcljava.executors; +import java.lang.SuppressWarnings; import java.util.AbstractMap; import java.util.ArrayList; import java.util.Iterator; import java.util.List; import java.util.Map; - import java.util.Collection; import java.util.concurrent.BlockingQueue; import java.util.concurrent.ConcurrentHashMap; import java.util.concurrent.LinkedBlockingQueue; + import org.slf4j.Logger; import org.slf4j.LoggerFactory; @@ -81,6 +82,23 @@ protected void removeNode(ComposableNode node) { this.nodes.remove(node); } + @SuppressWarnings("unchecked") + protected static void executeSubscriptionCallbackUnchecked( + Subscription subscription, + MessageDefinition message) + { + subscription.executeCallback(message); + } + + @SuppressWarnings("unchecked") + protected static void clientHandleResponseUnchecked( + Client client, + RMWRequestId rmwRequestId, + MessageDefinition response) + { + client.handleResponse(rmwRequestId, response); + } + protected void executeAnyExecutable(AnyExecutable anyExecutable) { if (anyExecutable.timer != null) { anyExecutable.timer.callTimer(); @@ -92,25 +110,25 @@ protected void executeAnyExecutable(AnyExecutable anyExecutable) { MessageDefinition message = nativeTake( anyExecutable.subscription.getHandle(), anyExecutable.subscription.getMessageType()); if (message != null) { - anyExecutable.subscription.executeCallback(message); + // Safety: nativeTake() will return the correct type here. + // We can't do much better here, as subscriptions are type erased. + executeSubscriptionCallbackUnchecked(anyExecutable.subscription, message); } subscriptionHandles.remove(anyExecutable.subscription.getHandle()); } if (anyExecutable.service != null) { - Class requestType = anyExecutable.service.getRequestType(); - Class responseType = anyExecutable.service.getResponseType(); + Class requestType = anyExecutable.service.getRequestType(); + Class responseType = anyExecutable.service.getResponseType(); MessageDefinition requestMessage = null; MessageDefinition responseMessage = null; try { - requestMessage = requestType.newInstance(); - responseMessage = responseType.newInstance(); - } catch (InstantiationException ie) { - ie.printStackTrace(); - } catch (IllegalAccessException iae) { - iae.printStackTrace(); + requestMessage = requestType.getDeclaredConstructor().newInstance(); + responseMessage = responseType.getDeclaredConstructor().newInstance(); + } catch (ReflectiveOperationException e) { + throw new IllegalStateException("Failed to instantiate service request/response"); } if (requestMessage != null && responseMessage != null) { @@ -134,24 +152,18 @@ protected void executeAnyExecutable(AnyExecutable anyExecutable) { } if (anyExecutable.client != null) { - Class requestType = anyExecutable.client.getRequestType(); - Class responseType = anyExecutable.client.getResponseType(); + Class requestType = anyExecutable.client.getRequestType(); + Class responseType = anyExecutable.client.getResponseType(); - MessageDefinition requestMessage = null; MessageDefinition responseMessage = null; try { - requestMessage = requestType.newInstance(); - responseMessage = responseType.newInstance(); - } catch (InstantiationException ie) { - ie.printStackTrace(); - } catch (IllegalAccessException iae) { - iae.printStackTrace(); + responseMessage = responseType.getDeclaredConstructor().newInstance(); + } catch (ReflectiveOperationException ie) { + throw new IllegalStateException("Failed to instantiate service response"); } - if (requestMessage != null && responseMessage != null) { - long requestFromJavaConverterHandle = requestMessage.getFromJavaConverterInstance(); - long requestToJavaConverterHandle = requestMessage.getToJavaConverterInstance(); + if (responseMessage != null) { long responseFromJavaConverterHandle = responseMessage.getFromJavaConverterInstance(); long responseToJavaConverterHandle = responseMessage.getToJavaConverterInstance(); long responseDestructorHandle = responseMessage.getDestructorInstance(); @@ -161,7 +173,9 @@ protected void executeAnyExecutable(AnyExecutable anyExecutable) { responseToJavaConverterHandle, responseDestructorHandle, responseMessage); if (rmwRequestId != null) { - anyExecutable.client.handleResponse(rmwRequestId, responseMessage); + // Safety: nativeTakeResponse() will return the correct type here. + // We can't do much better here, as subscriptions are type erased. + clientHandleResponseUnchecked(anyExecutable.client, rmwRequestId, responseMessage); } } clientHandles.remove(anyExecutable.client.getHandle()); @@ -187,7 +201,7 @@ protected void waitForWork(long timeout) { this.actionServerHandles.clear(); for (ComposableNode node : this.nodes) { - for (Subscription subscription : node.getNode().getSubscriptions()) { + for (Subscription subscription : node.getNode().getSubscriptions()) { this.subscriptionHandles.add(new AbstractMap.SimpleEntry( subscription.getHandle(), subscription)); Collection eventHandlers = subscription.getEventHandlers(); @@ -209,17 +223,17 @@ protected void waitForWork(long timeout) { this.timerHandles.add(new AbstractMap.SimpleEntry(timer.getHandle(), timer)); } - for (Service service : node.getNode().getServices()) { + for (Service service : node.getNode().getServices()) { this.serviceHandles.add( new AbstractMap.SimpleEntry(service.getHandle(), service)); } - for (Client client : node.getNode().getClients()) { + for (Client client : node.getNode().getClients()) { this.clientHandles.add( new AbstractMap.SimpleEntry(client.getHandle(), client)); } - for (ActionServer actionServer : node.getNode().getActionServers()) { + for (ActionServer actionServer : node.getNode().getActionServers()) { this.actionServerHandles.add( new AbstractMap.SimpleEntry(actionServer.getHandle(), actionServer)); } @@ -498,7 +512,7 @@ private static native void nativeWaitSetAddSubscription( private static native void nativeWait(long waitSetHandle, long timeout); private static native MessageDefinition nativeTake( - long subscriptionHandle, Class messageType); + long subscriptionHandle, Class messageType); private static native void nativeWaitSetAddService(long waitSetHandle, long serviceHandle); diff --git a/rcljava/src/main/java/org/ros2/rcljava/graph/NameAndTypes.java b/rcljava/src/main/java/org/ros2/rcljava/graph/NameAndTypes.java index 580ad329..12ec3d78 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/graph/NameAndTypes.java +++ b/rcljava/src/main/java/org/ros2/rcljava/graph/NameAndTypes.java @@ -45,12 +45,12 @@ public class NameAndTypes { */ public NameAndTypes(final String name, final Collection types) { this.name = name; - this.types = new ArrayList(types); + this.types = new ArrayList(types); } /// @internal Default constructor, only used from jni code. private NameAndTypes() { - this.types = new ArrayList(); + this.types = new ArrayList(); } @Override 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 d1ba5d03..63dc6b24 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/Node.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/Node.java @@ -233,6 +233,7 @@ ActionServer createActionServer(final Class a * @param callback Function that is called when the timer expires. * @return The created timer. */ + @SuppressWarnings("deprecation") WallTimer createWallTimer(final long period, final TimeUnit unit, final Callback callback); /** 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 7c6dd4b4..3bfb0e4b 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java @@ -482,10 +482,11 @@ public final long getHandle() { private static native long nativeCreateTimerHandle(long clockHandle, long contextHandle, long timerPeriod); + @SuppressWarnings("deprecation") private Timer createTimer(Clock clock, final long period, final TimeUnit unit, final Callback callback) { long timerPeriodNS = TimeUnit.NANOSECONDS.convert(period, unit); long timerHandle = nativeCreateTimerHandle(clock.getHandle(), this.context.getHandle(), timerPeriodNS); - WallTimer timer = new WallTimerImpl(new WeakReference(this), timerHandle, callback, timerPeriodNS); + Timer timer = new WallTimerImpl(new WeakReference(this), timerHandle, callback, timerPeriodNS); this.timers.add(timer); return timer; } @@ -493,6 +494,7 @@ private Timer createTimer(Clock clock, final long period, final TimeUnit unit, f /** * {@inheritDoc} */ + @SuppressWarnings("deprecation") public WallTimer createWallTimer(final long period, final TimeUnit unit, final Callback callback) { return (WallTimer) this.createTimer(this.wall_clock, period, unit, callback); } diff --git a/rcljava/src/test/java/org/ros2/rcljava/parameters/SyncParametersClientTest.java b/rcljava/src/test/java/org/ros2/rcljava/parameters/SyncParametersClientTest.java index 818314f8..21d91688 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/parameters/SyncParametersClientTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/parameters/SyncParametersClientTest.java @@ -136,7 +136,7 @@ public final void testListParameters() throws Exception { assertArrayEquals( parametersAndPrefixes.getNames(), new String[] {"foo.first", "foo.second"}); - assertEquals(parametersAndPrefixes.getPrefixes(), new String[] {"foo"}); + assertArrayEquals(parametersAndPrefixes.getPrefixes(), new String[] {"foo"}); } @Test From 141e5d402aff4a58cc3e50913e09edadab9c9711 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 18 May 2021 10:22:12 -0300 Subject: [PATCH 43/51] Fix linter issues in rosidl_generator_java and in its generated code (osrf/ros2_java#73) Signed-off-by: Ivan Santiago Paunovic --- rosidl_generator_java/resource/action.cpp.em | 5 +++++ rosidl_generator_java/resource/idl.cpp.em | 1 + rosidl_generator_java/resource/idl.java.em | 3 +++ rosidl_generator_java/resource/msg.cpp.em | 17 ++++++++++------- rosidl_generator_java/resource/srv.cpp.em | 6 +++++- 5 files changed, 24 insertions(+), 8 deletions(-) diff --git a/rosidl_generator_java/resource/action.cpp.em b/rosidl_generator_java/resource/action.cpp.em index 0d3492e4..1cae4e8c 100644 --- a/rosidl_generator_java/resource/action.cpp.em +++ b/rosidl_generator_java/resource/action.cpp.em @@ -1,4 +1,7 @@ @# Included from rosidl_generator_java/resource/idl.cpp.em +// generated from rosidl_generator_java/resource/action.cpp.em +// with input from @(package_name):@(interface_path) +// generated code does not contain a copyright notice @{ import os @@ -16,6 +19,7 @@ get_result_type_name = action.get_result_service.namespaced_type.name data = { 'package_name': package_name, + 'interface_path': interface_path, 'output_dir': output_dir, 'template_basepath': template_basepath, } @@ -60,6 +64,7 @@ expand_template( data = { 'package_name': package_name, + 'interface_path': interface_path, 'output_dir': output_dir, 'template_basepath': template_basepath, 'typesupport_impl': typesupport_impl, diff --git a/rosidl_generator_java/resource/idl.cpp.em b/rosidl_generator_java/resource/idl.cpp.em index c909006a..3f355186 100644 --- a/rosidl_generator_java/resource/idl.cpp.em +++ b/rosidl_generator_java/resource/idl.cpp.em @@ -29,6 +29,7 @@ from rosidl_parser.definition import Service @{ data = { 'package_name': package_name, + 'interface_path': interface_path, 'output_dir': output_dir, 'template_basepath': template_basepath, } diff --git a/rosidl_generator_java/resource/idl.java.em b/rosidl_generator_java/resource/idl.java.em index 9c90c354..4714151d 100644 --- a/rosidl_generator_java/resource/idl.java.em +++ b/rosidl_generator_java/resource/idl.java.em @@ -13,6 +13,9 @@ @####################################################################### @# Handle messages @####################################################################### +// generated from rosidl_generator_java/resource/idl.java.em +// with input from @(package_name):@(interface_path) +// generated code does not contain a copyright notice @{ import os diff --git a/rosidl_generator_java/resource/msg.cpp.em b/rosidl_generator_java/resource/msg.cpp.em index 84c4bb64..fade6a71 100644 --- a/rosidl_generator_java/resource/msg.cpp.em +++ b/rosidl_generator_java/resource/msg.cpp.em @@ -1,4 +1,7 @@ @# Included from rosidl_generator_java/resource/idl.cpp.em +// generated from rosidl_generator_java/resource/msg.cpp.em +// with input from @(package_name):@(interface_path) +// generated code does not contain a copyright notice @{ from collections import defaultdict @@ -95,10 +98,12 @@ elif message_c_include_prefix.endswith('__send_goal'): message_c_include_prefix = message_c_include_prefix[:-11] elif message_c_include_prefix.endswith('__get_result'): message_c_include_prefix = message_c_include_prefix[:-12] +member_includes.add(f'{message_c_include_prefix}.h') }@ - +@ #include +#include #include #include #include @@ -112,8 +117,6 @@ elif message_c_include_prefix.endswith('__get_result'): #include "@(include)" @[end for]@ -#include "@(message_c_include_prefix).h" - // Ensure that a jlong is big enough to store raw pointers static_assert(sizeof(jlong) >= sizeof(std::intptr_t), "jlong must be able to store pointers"); @@ -265,7 +268,7 @@ jni_signature = get_jni_signature(base_type) auto _dest_@(member.name) = ros_message->@(member.name); @[ end if]@ @[ if isinstance(member.type.value_type, BasicType)]@ - j@(get_java_name) *_jarray_@(member.name)_ptr = env->Get@(get_method_name)ArrayElements(_jarray_@(member.name)_obj, nullptr); + j@(get_java_name) * _jarray_@(member.name)_ptr = env->Get@(get_method_name)ArrayElements(_jarray_@(member.name)_obj, nullptr); std::copy(_jarray_@(member.name)_ptr, _jarray_@(member.name)_ptr + _jarray_@(member.name)_size, _dest_@(member.name)); env->Release@(get_method_name)ArrayElements(_jarray_@(member.name)_obj, _jarray_@(member.name)_ptr, 0); @[ else]@ @@ -378,12 +381,12 @@ jni_signature = get_jni_signature(base_type) @[ if isinstance(member.type.value_type, BasicType)]@ @[ if isinstance(member.type, Array)]@ j@(get_java_name)Array _jarray_@(member.name)_obj = env->New@(get_method_name)Array(@(member.type.size)); - j@(get_java_name) *_j@(get_java_name)_@(member.name)_buf = (j@(get_java_name) *)malloc(sizeof(j@(get_java_name)) * @(member.type.size)); + auto * _j@(get_java_name)_@(member.name)_buf = static_cast(malloc(sizeof(j@(get_java_name)) * @(member.type.size))); std::copy(_ros_message->@(member.name), _ros_message->@(member.name) + @(member.type.size), _j@(get_java_name)_@(member.name)_buf); env->Set@(get_method_name)ArrayRegion(_jarray_@(member.name)_obj, 0, @(member.type.size), (const j@(get_java_name) *)_j@(get_java_name)_@(member.name)_buf); @[ else]@ j@(get_java_name)Array _jarray_@(member.name)_obj = env->New@(get_method_name)Array(_ros_message->@(member.name).size); - j@(get_java_name) *_j@(get_java_name)_@(member.name)_buf = (j@(get_java_name) *)malloc(sizeof(j@(get_java_name)) * _ros_message->@(member.name).size); + auto * _j@(get_java_name)_@(member.name)_buf = static_cast(malloc(sizeof(j@(get_java_name)) * _ros_message->@(member.name).size)); std::copy(_ros_message->@(member.name).data, _ros_message->@(member.name).data + _ros_message->@(member.name).size, _j@(get_java_name)_@(member.name)_buf); env->Set@(get_method_name)ArrayRegion(_jarray_@(member.name)_obj, 0, _ros_message->@(member.name).size, (const j@(get_java_name) *)_j@(get_java_name)_@(member.name)_buf); @[ end if]@ @@ -563,7 +566,7 @@ JNIEXPORT jlong JNICALL Java_@(underscore_separated_jni_type_name)_getToJavaConv JNIEXPORT jlong JNICALL Java_@(underscore_separated_jni_type_name)_getTypeSupport(JNIEnv *, jclass) { - jlong ptr = reinterpret_cast(ROSIDL_GET_MSG_TYPE_SUPPORT(@(','.join(message.structure.namespaced_type.namespaced_name())))); + jlong ptr = reinterpret_cast(ROSIDL_GET_MSG_TYPE_SUPPORT(@(', '.join(message.structure.namespaced_type.namespaced_name())))); return ptr; } diff --git a/rosidl_generator_java/resource/srv.cpp.em b/rosidl_generator_java/resource/srv.cpp.em index 4c13f6b5..d5357025 100644 --- a/rosidl_generator_java/resource/srv.cpp.em +++ b/rosidl_generator_java/resource/srv.cpp.em @@ -1,4 +1,7 @@ @# Included from rosidl_generator_java/resource/idl.cpp.em +// generated from rosidl_generator_java/resource/srv.cpp.em +// with input from @(package_name):@(interface_path) +// generated code does not contain a copyright notice @{ import os from rosidl_cmake import expand_template @@ -11,6 +14,7 @@ response_type_name = service.response_message.structure.namespaced_type.name data = { 'package_name': package_name, + 'interface_path': interface_path, 'output_dir': output_dir, 'template_basepath': template_basepath, } @@ -83,6 +87,6 @@ JNIEXPORT jlong JNICALL Java_@(underscore_separated_jni_type_name)_getServiceTyp JNIEXPORT jlong JNICALL Java_@(underscore_separated_jni_type_name)_getServiceTypeSupport(JNIEnv *, jclass) { const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( - @(','.join(service_fqn))); + @(', '.join(service_fqn))); return reinterpret_cast(ts); } From 8b88b39f62cf8d65708858f8d6c6895c84e0b519 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 21 May 2021 10:21:11 -0300 Subject: [PATCH 44/51] Cleanup service/client creation code (osrf/ros2_java#74) Signed-off-by: Ivan Santiago Paunovic --- .../java/org/ros2/rcljava/client/Client.java | 4 +- .../org/ros2/rcljava/client/ClientImpl.java | 29 ++++---- .../ros2/rcljava/executors/BaseExecutor.java | 36 +++------- .../main/java/org/ros2/rcljava/node/Node.java | 17 ++--- .../java/org/ros2/rcljava/node/NodeImpl.java | 70 ++++++++++--------- .../client/AsyncParametersClientImpl.java | 8 +-- .../client/SyncParametersClientImpl.java | 35 ++++++---- .../service/ParameterServiceImpl.java | 4 +- .../org/ros2/rcljava/service/Service.java | 4 +- .../org/ros2/rcljava/service/ServiceImpl.java | 29 ++++---- .../rcljava/interfaces/ServiceDefinition.java | 5 +- rosidl_generator_java/resource/srv.java.em | 8 +++ 12 files changed, 124 insertions(+), 125 deletions(-) diff --git a/rcljava/src/main/java/org/ros2/rcljava/client/Client.java b/rcljava/src/main/java/org/ros2/rcljava/client/Client.java index 774ecec6..811aa2c5 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/client/Client.java +++ b/rcljava/src/main/java/org/ros2/rcljava/client/Client.java @@ -26,9 +26,7 @@ import org.ros2.rcljava.service.RMWRequestId; public interface Client extends Disposable { - Class getRequestType(); - - Class getResponseType(); + ServiceDefinition getServiceDefinition(); void handleResponse(RMWRequestId header, U response); diff --git a/rcljava/src/main/java/org/ros2/rcljava/client/ClientImpl.java b/rcljava/src/main/java/org/ros2/rcljava/client/ClientImpl.java index 3c3187e3..442aa8ec 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/client/ClientImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/client/ClientImpl.java @@ -55,20 +55,25 @@ public class ClientImpl implements Client { private final String serviceName; private Map> pendingRequests; - private final Class requestType; - private final Class responseType; - - public ClientImpl(final WeakReference nodeReference, final long handle, - final String serviceName, final Class requestType, - final Class responseType) { + private final ServiceDefinition serviceDefinition; + + public ClientImpl( + final ServiceDefinition serviceDefinition, + final WeakReference nodeReference, + final long handle, + final String serviceName) + { this.nodeReference = nodeReference; this.handle = handle; this.serviceName = serviceName; - this.requestType = requestType; - this.responseType = responseType; + this.serviceDefinition = serviceDefinition; this.pendingRequests = new HashMap>(); } + public ServiceDefinition getServiceDefinition() { + return this.serviceDefinition; + } + public final Future asyncSendRequest(final U request) { return asyncSendRequest(request, new Consumer>() { @@ -112,14 +117,6 @@ private static native long nativeSendClientRequest( long handle, long requestFromJavaConverterHandle, long requestDestructorHandle, MessageDefinition requestMessage); - public final Class getRequestType() { - return this.requestType; - } - - public final Class getResponseType() { - return this.responseType; - } - /** * Destroy a ROS2 client (rcl_client_t). * diff --git a/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java b/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java index bd86e63a..5c43b6c4 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java +++ b/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java @@ -118,18 +118,9 @@ protected void executeAnyExecutable(AnyExecutable anyExecutable) { } if (anyExecutable.service != null) { - Class requestType = anyExecutable.service.getRequestType(); - Class responseType = anyExecutable.service.getResponseType(); - - MessageDefinition requestMessage = null; - MessageDefinition responseMessage = null; - - try { - requestMessage = requestType.getDeclaredConstructor().newInstance(); - responseMessage = responseType.getDeclaredConstructor().newInstance(); - } catch (ReflectiveOperationException e) { - throw new IllegalStateException("Failed to instantiate service request/response"); - } + ServiceDefinition serviceDefinition = anyExecutable.service.getServiceDefinition(); + MessageDefinition requestMessage = serviceDefinition.newRequestInstance(); + MessageDefinition responseMessage = serviceDefinition.newResponseInstance(); if (requestMessage != null && responseMessage != null) { long requestFromJavaConverterHandle = requestMessage.getFromJavaConverterInstance(); @@ -140,28 +131,21 @@ protected void executeAnyExecutable(AnyExecutable anyExecutable) { long responseDestructorHandle = responseMessage.getDestructorInstance(); RMWRequestId rmwRequestId = - nativeTakeRequest(anyExecutable.service.getHandle(), requestFromJavaConverterHandle, - requestToJavaConverterHandle, requestDestructorHandle, requestMessage); + nativeTakeRequest(anyExecutable.service.getHandle(), requestFromJavaConverterHandle, + requestToJavaConverterHandle, requestDestructorHandle, requestMessage); if (rmwRequestId != null) { anyExecutable.service.executeCallback(rmwRequestId, requestMessage, responseMessage); - nativeSendServiceResponse(anyExecutable.service.getHandle(), rmwRequestId, - responseFromJavaConverterHandle, responseDestructorHandle, responseMessage); + nativeSendServiceResponse( + anyExecutable.service.getHandle(), rmwRequestId, + responseFromJavaConverterHandle, responseDestructorHandle, responseMessage); } } serviceHandles.remove(anyExecutable.service.getHandle()); } if (anyExecutable.client != null) { - Class requestType = anyExecutable.client.getRequestType(); - Class responseType = anyExecutable.client.getResponseType(); - - MessageDefinition responseMessage = null; - - try { - responseMessage = responseType.getDeclaredConstructor().newInstance(); - } catch (ReflectiveOperationException ie) { - throw new IllegalStateException("Failed to instantiate service response"); - } + ServiceDefinition serviceDefinition = anyExecutable.client.getServiceDefinition(); + MessageDefinition responseMessage = serviceDefinition.newResponseInstance(); if (responseMessage != null) { long responseFromJavaConverterHandle = responseMessage.getFromJavaConverterInstance(); 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 63dc6b24..b3b289b5 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/Node.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/Node.java @@ -126,23 +126,24 @@ Publisher createPublisher( Publisher createPublisher( final Class messageType, final String topic); - Service createService(final Class serviceType, + Service createService( + final Class serviceType, final String serviceName, final TriConsumer callback, - final QoSProfile qosProfile) throws NoSuchFieldException, IllegalAccessException; + final QoSProfile qosProfile); - Service createService(final Class serviceType, + Service createService( + final Class serviceType, final String serviceName, final TriConsumer - callback) throws NoSuchFieldException, IllegalAccessException; + callback); Client createClient( - final Class serviceType, final String serviceName, final QoSProfile qosProfile) - throws NoSuchFieldException, IllegalAccessException; + final Class serviceType, final String serviceName, final QoSProfile qosProfile); - Client createClient(final Class serviceType, - final String serviceName) throws NoSuchFieldException, IllegalAccessException; + Client createClient( + final Class serviceType, final String serviceName); /** * Create an ActionServer<T>. 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 3bfb0e4b..c716c528 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java @@ -191,18 +191,8 @@ public NodeImpl(final long handle, final NodeOptions nodeOptions) { this.wall_clock = new Clock(ClockType.STEADY_TIME); this.timeSource = new TimeSource(this); this.timeSource.attachClock(this.clock); - try { - this.parameterService = nodeOptions.getStartParameterServices() ? - new ParameterServiceImpl(this) : null; - // TODO(ivanpauno): Modify createService, createClient so they don't declare - // NoSuchFieldException and IllegalAccessException checked exceptions. - // That only happens if the user passed the wrong Class object, and the exception should - // be rethrown as an unchecked IllegalArgumentException. - } catch (NoSuchFieldException ex) { - throw new IllegalArgumentException(ex.getMessage()); - } catch (IllegalAccessException ex) { - throw new IllegalArgumentException(ex.getMessage()); - } + this.parameterService = nodeOptions.getStartParameterServices() ? + new ParameterServiceImpl(this) : null; } /** @@ -340,30 +330,39 @@ private static native long nativeCreateServiceHand long handle, Class cls, String serviceName, long qosProfileHandle); public final Service createService(final Class serviceType, - final String serviceName, - final TriConsumer - callback, - final QoSProfile qosProfile) throws NoSuchFieldException, IllegalAccessException { - Class requestType = (Class) serviceType.getField("RequestType").get(null); - - Class responseType = (Class) serviceType.getField("ResponseType").get(null); - + final String serviceName, + final TriConsumer + callback, + final QoSProfile qosProfile) + { + T serviceDefinition; + try { + serviceDefinition = serviceType.getDeclaredConstructor().newInstance(); + } catch (ReflectiveOperationException e) { + throw new IllegalStateException("Failed to instantiate service definition"); + } long qosProfileHandle = RCLJava.convertQoSProfileToHandle(qosProfile); long serviceHandle = nativeCreateServiceHandle(this.handle, serviceType, serviceName, qosProfileHandle); RCLJava.disposeQoSProfile(qosProfileHandle); - Service service = new ServiceImpl(new WeakReference(this), serviceHandle, - serviceName, callback, requestType, responseType); + Service service = new ServiceImpl( + serviceDefinition, + new WeakReference(this), + serviceHandle, + serviceName, + callback); this.services.add(service); return service; } - public Service createService(final Class serviceType, - final String serviceName, - final TriConsumer - callback) throws NoSuchFieldException, IllegalAccessException { + public Service createService( + final Class serviceType, + final String serviceName, + final TriConsumer + callback) + { return this.createService(serviceType, serviceName, callback, QoSProfile.SERVICES_DEFAULT); } @@ -375,26 +374,29 @@ public final Collection getServices() { } public final Client createClient( - final Class serviceType, final String serviceName, final QoSProfile qosProfile) - throws NoSuchFieldException, IllegalAccessException { - Class requestType = (Class) serviceType.getField("RequestType").get(null); - - Class responseType = (Class) serviceType.getField("ResponseType").get(null); - + final Class serviceType, final String serviceName, final QoSProfile qosProfile) + { long qosProfileHandle = RCLJava.convertQoSProfileToHandle(qosProfile); long clientHandle = nativeCreateClientHandle(this.handle, serviceType, serviceName, qosProfileHandle); RCLJava.disposeQoSProfile(qosProfileHandle); + T serviceDefinition; + try { + serviceDefinition = serviceType.getDeclaredConstructor().newInstance(); + } catch (ReflectiveOperationException e) { + throw new IllegalStateException("Failed to instantiate service definition"); + } + Client client = new ClientImpl( - new WeakReference(this), clientHandle, serviceName, requestType, responseType); + serviceDefinition, new WeakReference(this), clientHandle, serviceName); this.clients.add(client); return client; } public Client createClient(final Class serviceType, - final String serviceName) throws NoSuchFieldException, IllegalAccessException { + final String serviceName) { return this.createClient(serviceType, serviceName, QoSProfile.SERVICES_DEFAULT); } diff --git a/rcljava/src/main/java/org/ros2/rcljava/parameters/client/AsyncParametersClientImpl.java b/rcljava/src/main/java/org/ros2/rcljava/parameters/client/AsyncParametersClientImpl.java index bfd01708..5de84b4f 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/parameters/client/AsyncParametersClientImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/parameters/client/AsyncParametersClientImpl.java @@ -49,7 +49,7 @@ public class AsyncParametersClientImpl implements AsyncParametersClient { private final Client describeParametersClient; public AsyncParametersClientImpl(final Node node, final String remoteName, - final QoSProfile qosProfile) throws NoSuchFieldException, IllegalAccessException { + final QoSProfile qosProfile) { this.node = node; if (remoteName != "") { this.remoteName = remoteName; @@ -84,17 +84,17 @@ public AsyncParametersClientImpl(final Node node, final String remoteName, } public AsyncParametersClientImpl(final Node node, final QoSProfile qosProfile) - throws NoSuchFieldException, IllegalAccessException { + { this(node, "", qosProfile); } public AsyncParametersClientImpl(final Node node, final String remoteName) - throws NoSuchFieldException, IllegalAccessException { + { this(node, remoteName, QoSProfile.PARAMETERS); } public AsyncParametersClientImpl(final Node node) - throws NoSuchFieldException, IllegalAccessException { + { this(node, "", QoSProfile.PARAMETERS); } diff --git a/rcljava/src/main/java/org/ros2/rcljava/parameters/client/SyncParametersClientImpl.java b/rcljava/src/main/java/org/ros2/rcljava/parameters/client/SyncParametersClientImpl.java index d2b4a78f..b8a0a323 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/parameters/client/SyncParametersClientImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/parameters/client/SyncParametersClientImpl.java @@ -55,44 +55,55 @@ public ConsumerHelper(RCLFuture resultFuture) { public AsyncParametersClient asyncParametersClient; - public SyncParametersClientImpl(final Node node, final String remoteName, - final QoSProfile qosProfile) throws NoSuchFieldException, IllegalAccessException { + public SyncParametersClientImpl( + final Node node, + final String remoteName, + final QoSProfile qosProfile) + { this.asyncParametersClient = new AsyncParametersClientImpl(node, remoteName, qosProfile); } public SyncParametersClientImpl(final Node node, final QoSProfile qosProfile) - throws NoSuchFieldException, IllegalAccessException { + { this(node, "", qosProfile); } public SyncParametersClientImpl(final Node node, final String remoteName) - throws NoSuchFieldException, IllegalAccessException { + { this(node, remoteName, QoSProfile.PARAMETERS); } public SyncParametersClientImpl(final Node node) - throws NoSuchFieldException, IllegalAccessException { + { this(node, "", QoSProfile.PARAMETERS); } - public SyncParametersClientImpl(final Executor executor, final Node node, final String remoteName, - final QoSProfile qosProfile) throws NoSuchFieldException, IllegalAccessException { + public SyncParametersClientImpl( + final Executor executor, + final Node node, + final String remoteName, + final QoSProfile qosProfile) + { this.executor = executor; this.asyncParametersClient = new AsyncParametersClientImpl(node, remoteName, qosProfile); } - public SyncParametersClientImpl(final Executor executor, final Node node, - final QoSProfile qosProfile) throws NoSuchFieldException, IllegalAccessException { + public SyncParametersClientImpl( + final Executor executor, + final Node node, + final QoSProfile qosProfile) + { this(executor, node, "", qosProfile); } - public SyncParametersClientImpl(final Executor executor, final Node node, final String remoteName) - throws NoSuchFieldException, IllegalAccessException { + public SyncParametersClientImpl( + final Executor executor, final Node node, final String remoteName) + { this(executor, node, remoteName, QoSProfile.PARAMETERS); } public SyncParametersClientImpl(final Executor executor, final Node node) - throws NoSuchFieldException, IllegalAccessException { + { this(executor, node, "", QoSProfile.PARAMETERS); } diff --git a/rcljava/src/main/java/org/ros2/rcljava/parameters/service/ParameterServiceImpl.java b/rcljava/src/main/java/org/ros2/rcljava/parameters/service/ParameterServiceImpl.java index 9d65ddd7..bc03b121 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/parameters/service/ParameterServiceImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/parameters/service/ParameterServiceImpl.java @@ -38,7 +38,7 @@ public class ParameterServiceImpl implements ParameterService { Service listParametersService; public ParameterServiceImpl(final Node node, final QoSProfile qosProfile) - throws NoSuchFieldException, IllegalAccessException { + { this.node = node; this.getParametersService = node.createService( rcl_interfaces.srv.GetParameters.class, @@ -146,7 +146,7 @@ public void accept(RMWRequestId rmwRequestId, qosProfile); } - public ParameterServiceImpl(final Node node) throws NoSuchFieldException, IllegalAccessException { + public ParameterServiceImpl(final Node node) { this(node, QoSProfile.PARAMETERS); } } diff --git a/rcljava/src/main/java/org/ros2/rcljava/service/Service.java b/rcljava/src/main/java/org/ros2/rcljava/service/Service.java index dfde9a09..289bad2b 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/service/Service.java +++ b/rcljava/src/main/java/org/ros2/rcljava/service/Service.java @@ -21,9 +21,7 @@ import org.ros2.rcljava.interfaces.ServiceDefinition; public interface Service extends Disposable { - Class getRequestType(); - - Class getResponseType(); + ServiceDefinition getServiceDefinition(); void executeCallback(RMWRequestId rmwRequestId, MessageDefinition request, MessageDefinition response); diff --git a/rcljava/src/main/java/org/ros2/rcljava/service/ServiceImpl.java b/rcljava/src/main/java/org/ros2/rcljava/service/ServiceImpl.java index a02896b1..e25d11e7 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/service/ServiceImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/service/ServiceImpl.java @@ -45,28 +45,25 @@ public class ServiceImpl implements Service { private final TriConsumer callback; - private final Class requestType; - private final Class responseType; - - public ServiceImpl(final WeakReference nodeReference, final long handle, - final String serviceName, - final TriConsumer - callback, - final Class requestType, final Class responseType) { + private final ServiceDefinition serviceDefinition; + + public ServiceImpl( + final ServiceDefinition serviceDefinition, + final WeakReference nodeReference, + final long handle, + final String serviceName, + final TriConsumer + callback) + { this.nodeReference = nodeReference; this.handle = handle; this.serviceName = serviceName; this.callback = callback; - this.requestType = requestType; - this.responseType = responseType; - } - - public final Class getRequestType() { - return this.requestType; + this.serviceDefinition = serviceDefinition; } - public final Class getResponseType() { - return this.responseType; + public final ServiceDefinition getServiceDefinition() { + return this.serviceDefinition; } /** diff --git a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ServiceDefinition.java b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ServiceDefinition.java index 8e3bf79e..cade9a22 100644 --- a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ServiceDefinition.java +++ b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ServiceDefinition.java @@ -15,4 +15,7 @@ package org.ros2.rcljava.interfaces; -public interface ServiceDefinition {} +public interface ServiceDefinition { + MessageDefinition newRequestInstance(); + MessageDefinition newResponseInstance(); +} diff --git a/rosidl_generator_java/resource/srv.java.em b/rosidl_generator_java/resource/srv.java.em index 8bfb887c..a238ecfd 100644 --- a/rosidl_generator_java/resource/srv.java.em +++ b/rosidl_generator_java/resource/srv.java.em @@ -61,6 +61,14 @@ public class @(type_name) implements ServiceDefinition { } } + public @(type_name)_Request newRequestInstance() { + return new @(type_name)_Request(); + } + + public @(type_name)_Response newResponseInstance() { + return new @(type_name)_Response(); + } + public static native long getServiceTypeSupport(); public static final Class<@(type_name)_Request> RequestType = @(type_name)_Request.class; From 616c22e700c7d22c37f0cc2a30f61542249ba129 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 1 Jun 2021 14:44:44 -0300 Subject: [PATCH 45/51] Add methods to create a parameter client in Node (osrf/ros2_java#75) * Cleanup service/client creation code Signed-off-by: Ivan Santiago Paunovic * Add methods to create a parameter client in Node Signed-off-by: Ivan Santiago Paunovic --- .../src/main/java/org/ros2/rcljava/node/Node.java | 12 ++++++++++++ .../main/java/org/ros2/rcljava/node/NodeImpl.java | 12 ++++++++++++ 2 files changed, 24 insertions(+) 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 b3b289b5..43d470d6 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/Node.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/Node.java @@ -39,6 +39,8 @@ import org.ros2.rcljava.parameters.ParameterCallback; import org.ros2.rcljava.parameters.ParameterType; import org.ros2.rcljava.parameters.ParameterVariant; +import org.ros2.rcljava.parameters.client.AsyncParametersClient; +import org.ros2.rcljava.parameters.client.SyncParametersClient; import org.ros2.rcljava.publisher.Publisher; import org.ros2.rcljava.qos.QoSProfile; import org.ros2.rcljava.service.RMWRequestId; @@ -264,6 +266,16 @@ ActionServer createActionServer(final Class a */ String getNamespace(); + /** + * Create an asynchronous parameter client. + */ + AsyncParametersClient createAsyncParametersClient(String nodeName); + + /** + * Create an synchronous parameter client. + */ + SyncParametersClient createSyncParametersClient(String nodeName); + /** * Declare and initialize a parameter, return the effective 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 c716c528..3ca6cfb6 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java @@ -44,6 +44,10 @@ import org.ros2.rcljava.parameters.ParameterNotDeclaredException; import org.ros2.rcljava.parameters.ParameterType; import org.ros2.rcljava.parameters.ParameterVariant; +import org.ros2.rcljava.parameters.client.AsyncParametersClient; +import org.ros2.rcljava.parameters.client.AsyncParametersClientImpl; +import org.ros2.rcljava.parameters.client.SyncParametersClient; +import org.ros2.rcljava.parameters.client.SyncParametersClientImpl; import org.ros2.rcljava.parameters.service.ParameterService; import org.ros2.rcljava.parameters.service.ParameterServiceImpl; import org.ros2.rcljava.publisher.Publisher; @@ -533,6 +537,14 @@ public final String getNamespace() { return nativeGetNamespace(this.handle); } + public AsyncParametersClient createAsyncParametersClient(String nodeName) { + return new AsyncParametersClientImpl(this, nodeName); + } + + public SyncParametersClient createSyncParametersClient(String nodeName) { + return new SyncParametersClientImpl(this, nodeName); + } + public ParameterVariant declareParameter(ParameterVariant parameter) { return declareParameter(parameter, new rcl_interfaces.msg.ParameterDescriptor().setName(parameter.getName())); } From 3b01f345655dc86964b13c46a9f176249878ee0c Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 9 Jul 2021 09:37:38 -0300 Subject: [PATCH 46/51] Fix rosidl_generator_java_warnings (osrf/ros2_java#77) Signed-off-by: Ivan Santiago Paunovic --- rosidl_generator_java/CMakeLists.txt | 2 +- .../org/ros2/generator/InterfacesTest.java | 565 ++++++++++++++---- 2 files changed, 451 insertions(+), 116 deletions(-) diff --git a/rosidl_generator_java/CMakeLists.txt b/rosidl_generator_java/CMakeLists.txt index d0c5d304..777cac9a 100644 --- a/rosidl_generator_java/CMakeLists.txt +++ b/rosidl_generator_java/CMakeLists.txt @@ -40,7 +40,7 @@ if(BUILD_TESTING) # TODO(jacobperron): This fixes generating messages with wstrings. # Figure out why this is needed as of Foxy, and how we can remove this workaround - set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "ISO-8859-1") + list(APPEND CMAKE_JAVA_COMPILE_FLAGS "-encoding" "ISO-8859-1") include(cmake/register_java.cmake) include(cmake/rosidl_generator_java_get_typesupports.cmake) diff --git a/rosidl_generator_java/src/test/java/org/ros2/generator/InterfacesTest.java b/rosidl_generator_java/src/test/java/org/ros2/generator/InterfacesTest.java index 0403d00a..8630148a 100644 --- a/rosidl_generator_java/src/test/java/org/ros2/generator/InterfacesTest.java +++ b/rosidl_generator_java/src/test/java/org/ros2/generator/InterfacesTest.java @@ -21,7 +21,7 @@ import static org.junit.Assert.assertTrue; import java.lang.reflect.Method; -import java.util.concurrent.Callable; +import java.lang.Runnable; import java.util.Arrays; import java.util.List; import org.junit.BeforeClass; @@ -40,9 +40,9 @@ public class ListFixtureData { public byte[] byteArrShort = new byte[]{1}; public List byteListShort = Arrays.asList((byte) 1); public byte[] charArr = new byte[]{(byte) ' ', (byte) 'a', (byte) 'Z'}; - public List charList = Arrays.asList(new Byte((byte) ' '), new Byte((byte) 'a'), new Byte((byte) 'Z')); + public List charList = Arrays.asList(Byte.valueOf((byte) ' '), Byte.valueOf((byte) 'a'), Byte.valueOf((byte) 'Z')); public byte[] charArrShort = new byte[]{(byte) 'z', (byte) 'A'}; - public List charListShort = Arrays.asList(new Byte((byte) 'z'), new Byte((byte) 'A')); + public List charListShort = Arrays.asList(Byte.valueOf((byte) 'z'), Byte.valueOf((byte) 'A')); public float[] float32Arr = new float[]{0.0f, -1.125f, 1.125f}; public List float32List = Arrays.asList(0.0f, -1.125f, 1.125f); public float[] float32ArrShort = new float[]{1.125f, -1.125f}; @@ -58,7 +58,7 @@ public class ListFixtureData { public byte[] uint8Arr = new byte[]{(byte) 0, (byte) 1, (byte) 255}; public List uint8List = Arrays.asList((byte) 0, (byte) 1, (byte) 255); public byte[] uint8ArrShort = new byte[]{(byte) 255, (byte) 1}; - public List uint8ListShort = Arrays.asList((byte) 255, (byte) 1); + public List uint8ListShort = Arrays.asList((byte) 255, (byte) 1); public short[] int16Arr = new short[]{0, -32768, 32767}; public List int16List = Arrays.asList((short) 0, (short) -32768, (short) 32767); public short[] int16ArrShort = new short[]{32767, -32768}; @@ -84,9 +84,9 @@ public class ListFixtureData { public long[] uint64ArrShort = new long[]{0L, 1L, -1L}; public List uint64ListShort = Arrays.asList(0L, 1L, -1L); public String[] stringArr = new String[]{"", "min value", "max_value"}; - public List stringList = Arrays.asList(stringArr); + public List stringList = Arrays.asList(stringArr); public String[] stringArrShort = new String[]{"max_value", ""}; - public List stringListShort = Arrays.asList(stringArrShort); + public List stringListShort = Arrays.asList(stringArrShort); private rosidl_generator_java.msg.BasicTypes basicTypes = new rosidl_generator_java.msg.BasicTypes(); public rosidl_generator_java.msg.BasicTypes[] basicTypesArr = new rosidl_generator_java.msg.BasicTypes[] {basicTypes, basicTypes, basicTypes}; @@ -125,8 +125,8 @@ public static void setupOnce() { { // Configure log4j. Doing this dynamically so that Android does not complain about missing // the log4j JARs, SLF4J uses Android's native logging mechanism instead. - Class c = Class.forName("org.apache.log4j.BasicConfigurator"); - Method m = c.getDeclaredMethod("configure", (Class[]) null); + Class c = Class.forName("org.apache.log4j.BasicConfigurator"); + Method m = c.getDeclaredMethod("configure", (Class[]) null); Object o = m.invoke(null, (Object[]) null); } catch (Exception e) @@ -139,16 +139,16 @@ public static void setupOnce() { // TODO(jacobperron): Replace with JUnit's assertThrows method when we switch to JUnit 5 // See: https://junit.org/junit5/docs/5.0.1/api/org/junit/jupiter/api/Assertions.html - private static void assertThrows(Class expectedException, Callable func) { + private static void assertThrows(Class expectedException, Runnable func) { try { - func.call(); + func.run(); } catch(Exception exception) { if (expectedException.isInstance(exception)) { return; } } - assertTrue("Callable did not throw the expected exception", false); + assertTrue("Runnable did not throw the expected exception", false); } @Test @@ -293,8 +293,8 @@ public final void testCheckStringConstraints() { @Test public final void testArrays() { - rosidl_generator_java.msg.Arrays arrays = new rosidl_generator_java.msg.Arrays(); - ListFixtureData fixture = new ListFixtureData(); + final rosidl_generator_java.msg.Arrays arrays = new rosidl_generator_java.msg.Arrays(); + final ListFixtureData fixture = new ListFixtureData(); // This value should not change and is asserted at end of test arrays.setAlignmentCheck(42); @@ -304,133 +304,303 @@ public final void testArrays() { assertArrayEquals(fixture.boolArr, arrays.getBoolValues()); assertEquals(fixture.boolList, arrays.getBoolValuesAsList()); assertThrows(IllegalArgumentException.class, - () -> arrays.setBoolValues(new boolean[]{true, false, true, false})); - assertThrows(IllegalArgumentException.class, - () -> arrays.setBoolValues(Arrays.asList(true, false, true, false))); + new Runnable() { + @Override + public void run() { + arrays.setBoolValues(new boolean[]{true, false, true, false}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setBoolValues(Arrays.asList(true, false, true, false)); + } + }); arrays.setByteValues(fixture.byteList); assertArrayEquals(fixture.byteArr, arrays.getByteValues()); assertEquals(fixture.byteList, arrays.getByteValuesAsList()); assertThrows(IllegalArgumentException.class, - () -> arrays.setByteValues(new byte[]{(byte) 1, (byte) 2})); - assertThrows(IllegalArgumentException.class, - () -> arrays.setByteValues(Arrays.asList((byte) 1, (byte) 2))); + new Runnable() { + @Override + public void run() { + arrays.setByteValues(new byte[]{(byte) 1, (byte) 2}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setByteValues(Arrays.asList((byte) 1, (byte) 2)); + } + }); arrays.setCharValues(fixture.charList); assertArrayEquals(fixture.charArr, arrays.getCharValues()); assertEquals(fixture.charList, arrays.getCharValuesAsList()); assertThrows(IllegalArgumentException.class, - () -> arrays.setCharValues(new byte[]{(byte) 'a', (byte) 'b', (byte) 'c', (byte) 'd'})); - assertThrows(IllegalArgumentException.class, - () -> arrays.setCharValues(Arrays.asList((byte) 'a', (byte) 'b', (byte) 'c', (byte) 'd'))); + new Runnable() { + @Override + public void run() { + arrays.setCharValues(new byte[]{(byte) 'a', (byte) 'b', (byte) 'c', (byte) 'd'}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setCharValues(Arrays.asList((byte) 'a', (byte) 'b', (byte) 'c', (byte) 'd')); + } + }); arrays.setFloat32Values(fixture.float32List); assertTrue(Arrays.equals(fixture.float32Arr, arrays.getFloat32Values())); assertEquals(fixture.float32List, arrays.getFloat32ValuesAsList()); assertThrows(IllegalArgumentException.class, - () -> arrays.setFloat32Values(new float[]{1.0f, 2.0f})); - assertThrows(IllegalArgumentException.class, - () -> arrays.setFloat32Values(Arrays.asList(1.0f, 2.0f))); + new Runnable() { + @Override + public void run() { + arrays.setFloat32Values(new float[]{1.0f, 2.0f}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setFloat32Values(Arrays.asList(1.0f, 2.0f)); + } + }); arrays.setFloat64Values(fixture.float64List); assertTrue(Arrays.equals(fixture.float64Arr, arrays.getFloat64Values())); assertEquals(fixture.float64List, arrays.getFloat64ValuesAsList()); assertThrows(IllegalArgumentException.class, - () -> arrays.setFloat64Values(new double[]{1.0, 2.0, 3.0, 4.0})); - assertThrows(IllegalArgumentException.class, - () -> arrays.setFloat64Values(Arrays.asList(1.0, 2.0, 3.0, 4.0))); + new Runnable() { + @Override + public void run() { + arrays.setFloat64Values(new double[]{1.0, 2.0, 3.0, 4.0}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setFloat64Values(Arrays.asList(1.0, 2.0, 3.0, 4.0)); + } + }); arrays.setInt8Values(fixture.int8List); assertArrayEquals(fixture.int8Arr, arrays.getInt8Values()); assertEquals(fixture.int8List, arrays.getInt8ValuesAsList()); assertThrows(IllegalArgumentException.class, - () ->arrays.setInt8Values(new byte[]{(byte) 1, (byte) 2})); - assertThrows(IllegalArgumentException.class, - () ->arrays.setInt8Values(Arrays.asList((byte) 1, (byte) 2))); + new Runnable() { + @Override + public void run() { + arrays.setInt8Values(new byte[]{(byte) 1, (byte) 2}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setInt8Values(Arrays.asList((byte) 1, (byte) 2)); + } + }); arrays.setUint8Values(fixture.uint8List); assertArrayEquals(fixture.uint8Arr, arrays.getUint8Values()); assertEquals(fixture.uint8List, arrays.getUint8ValuesAsList()); assertThrows(IllegalArgumentException.class, - () -> arrays.setUint8Values(new byte[]{(byte) 1, (byte) 2, (byte) 3, (byte) 4})); - assertThrows(IllegalArgumentException.class, - () -> arrays.setUint8Values(Arrays.asList((byte) 1, (byte) 2, (byte) 3, (byte) 4))); + new Runnable() { + @Override + public void run() { + arrays.setUint8Values(new byte[]{(byte) 1, (byte) 2, (byte) 3, (byte) 4}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setUint8Values(Arrays.asList((byte) 1, (byte) 2, (byte) 3, (byte) 4)); + } + }); arrays.setInt16Values(fixture.int16List); assertTrue(Arrays.equals(fixture.int16Arr, arrays.getInt16Values())); assertEquals(fixture.int16List, arrays.getInt16ValuesAsList()); assertThrows(IllegalArgumentException.class, - () -> arrays.setInt16Values(new short[]{(short) 1, (short) 2})); - assertThrows(IllegalArgumentException.class, - () -> arrays.setInt16Values(Arrays.asList((short) 1, (short) 2))); + new Runnable() { + @Override + public void run() { + arrays.setInt16Values(new short[]{(short) 1, (short) 2}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setInt16Values(Arrays.asList((short) 1, (short) 2)); + } + }); arrays.setUint16Values(fixture.uint16List); assertTrue(Arrays.equals(fixture.uint16Arr, arrays.getUint16Values())); assertEquals(fixture.uint16List, arrays.getUint16ValuesAsList()); assertThrows(IllegalArgumentException.class, - () -> arrays.setUint16Values(new short[]{(short) 1, (short) 2, (short) 3, (short) 4})); - assertThrows(IllegalArgumentException.class, - () -> arrays.setUint16Values(Arrays.asList((short) 1, (short) 2, (short) 3, (short) 4))); + new Runnable() { + @Override + public void run() { + arrays.setUint16Values(new short[]{(short) 1, (short) 2, (short) 3, (short) 4}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setUint16Values(Arrays.asList((short) 1, (short) 2, (short) 3, (short) 4)); + } + }); arrays.setInt32Values(fixture.int32List); assertArrayEquals(fixture.int32Arr, arrays.getInt32Values()); assertEquals(fixture.int32List, arrays.getInt32ValuesAsList()); assertThrows(IllegalArgumentException.class, - () -> arrays.setInt32Values(new int[]{1, 2})); - assertThrows(IllegalArgumentException.class, - () -> arrays.setInt32Values(Arrays.asList(1, 2))); + new Runnable() { + @Override + public void run() { + arrays.setInt32Values(new int[]{1, 2}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setInt32Values(Arrays.asList(1, 2)); + } + }); arrays.setUint32Values(fixture.uint32List); assertArrayEquals(fixture.uint32Arr, arrays.getUint32Values()); assertEquals(fixture.uint32List, arrays.getUint32ValuesAsList()); assertThrows(IllegalArgumentException.class, - () -> arrays.setUint32Values(new int[]{1, 2, 3, 4})); - assertThrows(IllegalArgumentException.class, - () -> arrays.setUint32Values(Arrays.asList(1, 2, 3, 4))); + new Runnable() { + @Override + public void run() { + arrays.setUint32Values(new int[]{1, 2, 3, 4}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setUint32Values(Arrays.asList(1, 2, 3, 4)); + } + }); arrays.setInt64Values(fixture.int64List); assertArrayEquals(fixture.int64Arr, arrays.getInt64Values()); assertEquals(fixture.int64List, arrays.getInt64ValuesAsList()); assertThrows(IllegalArgumentException.class, - () -> arrays.setInt64Values(new long[]{1L, 2L})); - assertThrows(IllegalArgumentException.class, - () -> arrays.setInt64Values(Arrays.asList(1L, 2L))); + new Runnable() { + @Override + public void run() { + arrays.setInt64Values(new long[]{1L, 2L}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setInt64Values(Arrays.asList(1L, 2L)); + } + }); arrays.setUint64Values(fixture.uint64List); assertArrayEquals(fixture.uint64Arr, arrays.getUint64Values()); assertEquals(fixture.uint64List, arrays.getUint64ValuesAsList()); assertThrows(IllegalArgumentException.class, - () ->arrays.setUint64Values(new long[]{1L, 2L, 3L, 4L})); - assertThrows(IllegalArgumentException.class, - () ->arrays.setUint64Values(Arrays.asList(1L, 2L, 3L, 4L))); + new Runnable() { + @Override + public void run() { + arrays.setUint64Values(new long[]{1L, 2L, 3L, 4L}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setUint64Values(Arrays.asList(1L, 2L, 3L, 4L)); + } + }); // Test setting/getting fixed length arrays of strings arrays.setStringValues(fixture.stringList); assertArrayEquals(fixture.stringArr, arrays.getStringValues()); assertEquals(fixture.stringList, arrays.getStringValuesAsList()); assertThrows(IllegalArgumentException.class, - () -> arrays.setStringValues(new String[]{"too", "few"})); - assertThrows(IllegalArgumentException.class, - () -> arrays.setStringValues(Arrays.asList("too", "few"))); + new Runnable() { + @Override + public void run() { + arrays.setStringValues(new String[]{"too", "few"}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setStringValues(Arrays.asList("too", "few")); + } + }); // Test setting/getting fixed length arrays of nested types arrays.setBasicTypesValues(fixture.basicTypesList); assertArrayEquals(fixture.basicTypesArr, arrays.getBasicTypesValues()); assertEquals(fixture.basicTypesList, arrays.getBasicTypesValuesAsList()); assertThrows(IllegalArgumentException.class, - () -> arrays.setBasicTypesValues(fixture.basicTypesArrShort)); - assertThrows(IllegalArgumentException.class, - () -> arrays.setBasicTypesValues(fixture.basicTypesListShort)); + new Runnable() { + @Override + public void run() { + arrays.setBasicTypesValues(fixture.basicTypesArrShort); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setBasicTypesValues(fixture.basicTypesListShort); + } + }); arrays.setConstantsValues(fixture.constantsList); assertArrayEquals(fixture.constantsArr, arrays.getConstantsValues()); assertEquals(fixture.constantsList, arrays.getConstantsValuesAsList()); assertThrows(IllegalArgumentException.class, - () -> arrays.setConstantsValues(fixture.constantsArrShort)); - assertThrows(IllegalArgumentException.class, - () -> arrays.setConstantsValues(fixture.constantsListShort)); + new Runnable() { + @Override + public void run() { + arrays.setConstantsValues(fixture.constantsArrShort); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setConstantsValues(fixture.constantsListShort); + } + }); arrays.setDefaultsValues(fixture.defaultsList); assertArrayEquals(fixture.defaultsArr, arrays.getDefaultsValues()); assertEquals(fixture.defaultsList, arrays.getDefaultsValuesAsList()); assertThrows(IllegalArgumentException.class, - () -> arrays.setDefaultsValues(fixture.defaultsArrShort)); - assertThrows(IllegalArgumentException.class, - () -> arrays.setDefaultsValues(fixture.defaultsListShort)); + new Runnable() { + @Override + public void run() { + arrays.setDefaultsValues(fixture.defaultsArrShort); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setDefaultsValues(fixture.defaultsListShort); + } + }); assertEquals(42, arrays.getAlignmentCheck()); } @Test public final void testBoundedSequences() { - rosidl_generator_java.msg.BoundedSequences bounded_seq = new rosidl_generator_java.msg.BoundedSequences(); - ListFixtureData fixture = new ListFixtureData(); + final rosidl_generator_java.msg.BoundedSequences bounded_seq = new rosidl_generator_java.msg.BoundedSequences(); + final ListFixtureData fixture = new ListFixtureData(); // This value should not change and is asserted at end of test bounded_seq.setAlignmentCheck(42); @@ -443,9 +613,19 @@ public final void testBoundedSequences() { assertArrayEquals(fixture.boolArrShort, bounded_seq.getBoolValues()); assertEquals(fixture.boolListShort, bounded_seq.getBoolValuesAsList()); assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setBoolValues(new boolean[]{true, false, true, false})); - assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setBoolValues(Arrays.asList(true, false, true, false))); + new Runnable() { + @Override + public void run() { + bounded_seq.setBoolValues(new boolean[]{true, false, true, false}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setBoolValues(Arrays.asList(true, false, true, false)); + } + }); bounded_seq.setByteValues(fixture.byteList); assertArrayEquals(fixture.byteArr, bounded_seq.getByteValues()); assertEquals(fixture.byteList, bounded_seq.getByteValuesAsList()); @@ -453,9 +633,19 @@ public final void testBoundedSequences() { assertArrayEquals(fixture.byteArrShort, bounded_seq.getByteValues()); assertEquals(fixture.byteListShort, bounded_seq.getByteValuesAsList()); assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setByteValues(new byte[]{(byte) 1, (byte) 2, (byte) 3, (byte) 4})); - assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setByteValues(Arrays.asList((byte) 1, (byte) 2, (byte) 3, (byte) 4))); + new Runnable() { + @Override + public void run() { + bounded_seq.setByteValues(new byte[]{(byte) 1, (byte) 2, (byte) 3, (byte) 4}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setByteValues(Arrays.asList((byte) 1, (byte) 2, (byte) 3, (byte) 4)); + } + }); bounded_seq.setCharValues(fixture.charList); assertArrayEquals(fixture.charArr, bounded_seq.getCharValues()); assertEquals(fixture.charList, bounded_seq.getCharValuesAsList()); @@ -463,9 +653,19 @@ public final void testBoundedSequences() { assertArrayEquals(fixture.charArrShort, bounded_seq.getCharValues()); assertEquals(fixture.charListShort, bounded_seq.getCharValuesAsList()); assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setCharValues(new byte[]{(byte) 'a', (byte) 'b', (byte) 'c', (byte) 'd'})); - assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setCharValues(Arrays.asList((byte) 'a', (byte) 'b', (byte) 'c', (byte) 'd'))); + new Runnable() { + @Override + public void run() { + bounded_seq.setCharValues(new byte[]{(byte) 'a', (byte) 'b', (byte) 'c', (byte) 'd'}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setCharValues(Arrays.asList((byte) 'a', (byte) 'b', (byte) 'c', (byte) 'd')); + } + }); bounded_seq.setFloat32Values(fixture.float32List); assertTrue(Arrays.equals(fixture.float32Arr, bounded_seq.getFloat32Values())); assertEquals(fixture.float32List, bounded_seq.getFloat32ValuesAsList()); @@ -473,9 +673,19 @@ public final void testBoundedSequences() { assertTrue(Arrays.equals(fixture.float32ArrShort, bounded_seq.getFloat32Values())); assertEquals(fixture.float32ListShort, bounded_seq.getFloat32ValuesAsList()); assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setFloat32Values(new float[]{1.0f, 2.0f, 3.0f, 4.0f})); - assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setFloat32Values(Arrays.asList(1.0f, 2.0f, 3.0f, 4.0f))); + new Runnable() { + @Override + public void run() { + bounded_seq.setFloat32Values(new float[]{1.0f, 2.0f, 3.0f, 4.0f}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setFloat32Values(Arrays.asList(1.0f, 2.0f, 3.0f, 4.0f)); + } + }); bounded_seq.setFloat64Values(fixture.float64List); assertTrue(Arrays.equals(fixture.float64Arr, bounded_seq.getFloat64Values())); assertEquals(fixture.float64List, bounded_seq.getFloat64ValuesAsList()); @@ -483,7 +693,12 @@ public final void testBoundedSequences() { assertTrue(Arrays.equals(fixture.float64ArrShort, bounded_seq.getFloat64Values())); assertEquals(fixture.float64ListShort, bounded_seq.getFloat64ValuesAsList()); assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setFloat64Values(Arrays.asList(1.0, 2.0, 3.0, 4.0))); + new Runnable() { + @Override + public void run() { + bounded_seq.setFloat64Values(Arrays.asList(1.0, 2.0, 3.0, 4.0)); + } + }); bounded_seq.setInt8Values(fixture.int8List); assertArrayEquals(fixture.int8Arr, bounded_seq.getInt8Values()); assertEquals(fixture.int8List, bounded_seq.getInt8ValuesAsList()); @@ -491,9 +706,19 @@ public final void testBoundedSequences() { assertArrayEquals(fixture.int8ArrShort, bounded_seq.getInt8Values()); assertEquals(fixture.int8ListShort, bounded_seq.getInt8ValuesAsList()); assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setInt8Values(new byte[]{(byte) 1, (byte) 2, (byte) 3, (byte) 4})); - assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setInt8Values(Arrays.asList((byte) 1, (byte) 2, (byte) 3, (byte) 4))); + new Runnable() { + @Override + public void run() { + bounded_seq.setInt8Values(new byte[]{(byte) 1, (byte) 2, (byte) 3, (byte) 4}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setInt8Values(Arrays.asList((byte) 1, (byte) 2, (byte) 3, (byte) 4)); + } + }); bounded_seq.setUint8Values(fixture.uint8List); assertArrayEquals(fixture.uint8Arr, bounded_seq.getUint8Values()); assertEquals(fixture.uint8List, bounded_seq.getUint8ValuesAsList()); @@ -501,9 +726,19 @@ public final void testBoundedSequences() { assertArrayEquals(fixture.uint8ArrShort, bounded_seq.getUint8Values()); assertEquals(fixture.uint8ListShort, bounded_seq.getUint8ValuesAsList()); assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setUint8Values(new byte[]{(byte) 1, (byte) 2, (byte) 3, (byte) 4})); - assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setUint8Values(Arrays.asList((byte) 1, (byte) 2, (byte) 3, (byte) 4))); + new Runnable() { + @Override + public void run() { + bounded_seq.setUint8Values(new byte[]{(byte) 1, (byte) 2, (byte) 3, (byte) 4}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setUint8Values(Arrays.asList((byte) 1, (byte) 2, (byte) 3, (byte) 4)); + } + }); bounded_seq.setInt16Values(fixture.int16List); assertTrue(Arrays.equals(fixture.int16Arr, bounded_seq.getInt16Values())); assertEquals(fixture.int16List, bounded_seq.getInt16ValuesAsList()); @@ -511,9 +746,19 @@ public final void testBoundedSequences() { assertTrue(Arrays.equals(fixture.int16ArrShort, bounded_seq.getInt16Values())); assertEquals(fixture.int16ListShort, bounded_seq.getInt16ValuesAsList()); assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setInt16Values(new short[]{(short) 1, (short) 2, (short) 3, (short) 4})); - assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setInt16Values(Arrays.asList((short) 1, (short) 2, (short) 3, (short) 4))); + new Runnable() { + @Override + public void run() { + bounded_seq.setInt16Values(new short[]{(short) 1, (short) 2, (short) 3, (short) 4}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setInt16Values(Arrays.asList((short) 1, (short) 2, (short) 3, (short) 4)); + } + }); bounded_seq.setUint16Values(fixture.uint16List); assertTrue(Arrays.equals(fixture.uint16Arr, bounded_seq.getUint16Values())); assertEquals(fixture.uint16List, bounded_seq.getUint16ValuesAsList()); @@ -521,9 +766,19 @@ public final void testBoundedSequences() { assertTrue(Arrays.equals(fixture.uint16ArrShort, bounded_seq.getUint16Values())); assertEquals(fixture.uint16ListShort, bounded_seq.getUint16ValuesAsList()); assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setUint16Values(new short[]{(short) 1, (short) 2, (short) 3, (short) 4})); - assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setUint16Values(Arrays.asList((short) 1, (short) 2, (short) 3, (short) 4))); + new Runnable() { + @Override + public void run() { + bounded_seq.setUint16Values(new short[]{(short) 1, (short) 2, (short) 3, (short) 4}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setUint16Values(Arrays.asList((short) 1, (short) 2, (short) 3, (short) 4)); + } + }); bounded_seq.setInt32Values(fixture.int32List); assertArrayEquals(fixture.int32Arr, bounded_seq.getInt32Values()); assertEquals(fixture.int32List, bounded_seq.getInt32ValuesAsList()); @@ -531,9 +786,19 @@ public final void testBoundedSequences() { assertArrayEquals(fixture.int32ArrShort, bounded_seq.getInt32Values()); assertEquals(fixture.int32ListShort, bounded_seq.getInt32ValuesAsList()); assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setInt32Values(new int[]{1, 2, 3, 4})); - assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setInt32Values(Arrays.asList(1, 2, 3, 4))); + new Runnable() { + @Override + public void run() { + bounded_seq.setInt32Values(new int[]{1, 2, 3, 4}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setInt32Values(Arrays.asList(1, 2, 3, 4)); + } + }); bounded_seq.setUint32Values(fixture.uint32List); assertArrayEquals(fixture.uint32Arr, bounded_seq.getUint32Values()); assertEquals(fixture.uint32List, bounded_seq.getUint32ValuesAsList()); @@ -541,9 +806,19 @@ public final void testBoundedSequences() { assertArrayEquals(fixture.uint32ArrShort, bounded_seq.getUint32Values()); assertEquals(fixture.uint32ListShort, bounded_seq.getUint32ValuesAsList()); assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setUint32Values(new int[]{1, 2, 3, 4})); - assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setUint32Values(Arrays.asList(1, 2, 3, 4))); + new Runnable() { + @Override + public void run() { + bounded_seq.setUint32Values(new int[]{1, 2, 3, 4}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setUint32Values(Arrays.asList(1, 2, 3, 4)); + } + }); bounded_seq.setInt64Values(fixture.int64List); assertArrayEquals(fixture.int64Arr, bounded_seq.getInt64Values()); assertEquals(fixture.int64List, bounded_seq.getInt64ValuesAsList()); @@ -551,9 +826,19 @@ public final void testBoundedSequences() { assertArrayEquals(fixture.int64ArrShort, bounded_seq.getInt64Values()); assertEquals(fixture.int64ListShort, bounded_seq.getInt64ValuesAsList()); assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setInt64Values(new long[]{1L, 2L, 3L, 4L})); - assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setInt64Values(Arrays.asList(1L, 2L, 3L, 4L))); + new Runnable() { + @Override + public void run() { + bounded_seq.setInt64Values(new long[]{1L, 2L, 3L, 4L}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setInt64Values(Arrays.asList(1L, 2L, 3L, 4L)); + } + }); bounded_seq.setUint64Values(fixture.uint64List); assertArrayEquals(fixture.uint64Arr, bounded_seq.getUint64Values()); assertEquals(fixture.uint64List, bounded_seq.getUint64ValuesAsList()); @@ -561,9 +846,19 @@ public final void testBoundedSequences() { assertArrayEquals(fixture.uint64ArrShort, bounded_seq.getUint64Values()); assertEquals(fixture.uint64ListShort, bounded_seq.getUint64ValuesAsList()); assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setUint64Values(new long[]{1L, 2L, 3L, 4L})); - assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setUint64Values(Arrays.asList(1L, 2L, 3L, 4L))); + new Runnable() { + @Override + public void run() { + bounded_seq.setUint64Values(new long[]{1L, 2L, 3L, 4L}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setUint64Values(Arrays.asList(1L, 2L, 3L, 4L)); + } + }); // Test setting/getting fixed length bounded_seq of strings bounded_seq.setStringValues(fixture.stringList); @@ -573,9 +868,19 @@ public final void testBoundedSequences() { assertArrayEquals(fixture.stringArrShort, bounded_seq.getStringValues()); assertEquals(fixture.stringListShort, bounded_seq.getStringValuesAsList()); assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setStringValues(new String[]{"too", "many", "values", "!"})); - assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setStringValues(Arrays.asList("too", "many", "values", "!"))); + new Runnable() { + @Override + public void run() { + bounded_seq.setStringValues(new String[]{"too", "many", "values", "!"}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setStringValues(Arrays.asList("too", "many", "values", "!")); + } + }); // Test setting/getting fixed length bounded_seq of nested types bounded_seq.setBasicTypesValues(fixture.basicTypesList); @@ -585,9 +890,19 @@ public final void testBoundedSequences() { assertArrayEquals(fixture.basicTypesArrShort, bounded_seq.getBasicTypesValues()); assertEquals(fixture.basicTypesListShort, bounded_seq.getBasicTypesValuesAsList()); assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setBasicTypesValues(fixture.basicTypesArrLong)); - assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setBasicTypesValues(fixture.basicTypesListLong)); + new Runnable() { + @Override + public void run() { + bounded_seq.setBasicTypesValues(fixture.basicTypesArrLong); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setBasicTypesValues(fixture.basicTypesListLong); + } + }); bounded_seq.setConstantsValues(fixture.constantsList); assertArrayEquals(fixture.constantsArr, bounded_seq.getConstantsValues()); assertEquals(fixture.constantsList, bounded_seq.getConstantsValuesAsList()); @@ -595,9 +910,19 @@ public final void testBoundedSequences() { assertArrayEquals(fixture.constantsArrShort, bounded_seq.getConstantsValues()); assertEquals(fixture.constantsListShort, bounded_seq.getConstantsValuesAsList()); assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setConstantsValues(fixture.constantsArrLong)); - assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setConstantsValues(fixture.constantsListLong)); + new Runnable() { + @Override + public void run() { + bounded_seq.setConstantsValues(fixture.constantsArrLong); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setConstantsValues(fixture.constantsListLong); + } + }); bounded_seq.setDefaultsValues(fixture.defaultsList); assertArrayEquals(fixture.defaultsArr, bounded_seq.getDefaultsValues()); assertEquals(fixture.defaultsList, bounded_seq.getDefaultsValuesAsList()); @@ -605,9 +930,19 @@ public final void testBoundedSequences() { assertArrayEquals(fixture.defaultsArrShort, bounded_seq.getDefaultsValues()); assertEquals(fixture.defaultsListShort, bounded_seq.getDefaultsValuesAsList()); assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setDefaultsValues(fixture.defaultsArrLong)); - assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setDefaultsValues(fixture.defaultsListLong)); + new Runnable() { + @Override + public void run() { + bounded_seq.setDefaultsValues(fixture.defaultsArrLong); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setDefaultsValues(fixture.defaultsListLong); + } + }); assertEquals(42, bounded_seq.getAlignmentCheck()); } From 775d1bb0d33b58f6f33cce1790e2f1bc89455440 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Mon, 12 Jul 2021 15:08:14 -0700 Subject: [PATCH 47/51] Avoid blocking forever in RCLFuture.get() with timeout (osrf/ros2_java#78) Add overloads for spinOnce methods to include a timeout argument and call this when spinning in the future. This fixes a bug where the future.get(...) call blocks forever, e.g. when waiting on a service response and the service becomes unavailable. Signed-off-by: Jacob Perron --- .../main/java/org/ros2/rcljava/RCLJava.java | 47 +++++++++++++++++-- .../ros2/rcljava/concurrent/RCLFuture.java | 4 +- 2 files changed, 45 insertions(+), 6 deletions(-) diff --git a/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java b/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java index 6c820ed9..92c44f6f 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java +++ b/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java @@ -287,22 +287,61 @@ public static void spin(final ComposableNode composableNode) { getGlobalExecutor().removeNode(composableNode); } - public static void spinOnce(final Node node) { + /** + * Execute one callback for a @{link Node}. + * + * Wait until a callback becomes ready and then execute it. + * + * @param node The node to spin on. + * @param timeout The time to wait for a callback to become ready in nanoseconds. + * If a timeout occurs, then nothing happens and this method returns. + */ + public static void spinOnce(final Node node, long timeout) { ComposableNode composableNode = new ComposableNode() { public Node getNode() { return node; } }; getGlobalExecutor().addNode(composableNode); - getGlobalExecutor().spinOnce(); + getGlobalExecutor().spinOnce(timeout); getGlobalExecutor().removeNode(composableNode); } - public static void spinOnce(final ComposableNode composableNode) { + /** + * Execute one callback for a @{link Node}. + * + * Wait until a callback becomes ready and then execute it. + * + * @param node The node to spin on. + */ + public static void spinOnce(final Node node) { + RCLJava.spinOnce(node, -1); + } + + /** + * Execute one callback for a @{link ComposableNode}. + * + * Wait until a callback becomes ready and then execute it. + * + * @param composableNode The composable node to spin on. + * @param timeout The time to wait for a callback to become ready in nanoseconds. + * If a timeout occurs, then nothing happens and this method returns. + */ + public static void spinOnce(final ComposableNode composableNode, long timeout) { getGlobalExecutor().addNode(composableNode); - getGlobalExecutor().spinOnce(); + getGlobalExecutor().spinOnce(timeout); getGlobalExecutor().removeNode(composableNode); } + /** + * Execute one callback for a @{link ComposableNode}. + * + * Wait until a callback becomes ready and then execute it. + * + * @param composableNode The composable node to spin on. + */ + public static void spinOnce(final ComposableNode composableNode) { + RCLJava.spinOnce(composableNode, -1); + } public static void spinSome(final Node node) { ComposableNode composableNode = new ComposableNode() { diff --git a/rcljava/src/main/java/org/ros2/rcljava/concurrent/RCLFuture.java b/rcljava/src/main/java/org/ros2/rcljava/concurrent/RCLFuture.java index 1ac88b55..5b45ccc4 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/concurrent/RCLFuture.java +++ b/rcljava/src/main/java/org/ros2/rcljava/concurrent/RCLFuture.java @@ -73,13 +73,13 @@ public final V get(final long timeout, final TimeUnit unit) while (RCLJava.ok()) { if (executor != null) { - executor.spinOnce(); + executor.spinOnce(timeoutNS); } else { Node node = nodeReference.get(); if (node == null) { return null; // TODO(esteve) do something } - RCLJava.spinOnce(node); + RCLJava.spinOnce(node, timeoutNS); } if (isDone()) { From c383fdf1bf94b8571c95b8c5d34d5845096d5236 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 29 Jul 2021 13:23:01 -0300 Subject: [PATCH 48/51] Refactor future implementation (osrf/ros2_java#80) Signed-off-by: Ivan Santiago Paunovic --- .../main/java/org/ros2/rcljava/RCLJava.java | 29 ++++++ .../org/ros2/rcljava/client/ClientImpl.java | 2 +- .../ros2/rcljava/concurrent/RCLFuture.java | 45 +++------ .../ros2/rcljava/executors/BaseExecutor.java | 26 +++++ .../org/ros2/rcljava/executors/Executor.java | 6 ++ .../executors/MultiThreadedExecutor.java | 9 ++ .../executors/SingleThreadedExecutor.java | 10 ++ .../client/AsyncParametersClient.java | 3 + .../client/AsyncParametersClientImpl.java | 21 ++-- .../client/SyncParametersClientImpl.java | 96 ++++++------------- .../ros2/rcljava/action/ActionServerTest.java | 24 +---- .../org/ros2/rcljava/client/ClientTest.java | 6 +- .../java/org/ros2/rcljava/node/NodeTest.java | 30 +++--- .../parameters/AsyncParametersClientTest.java | 21 ++-- .../org/ros2/rcljava/timer/TimerTest.java | 6 +- 15 files changed, 170 insertions(+), 164 deletions(-) diff --git a/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java b/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java index 92c44f6f..eadbba7c 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java +++ b/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java @@ -22,6 +22,7 @@ import java.util.Collection; import java.util.Map; import java.util.concurrent.ConcurrentSkipListMap; +import java.util.concurrent.Future; import java.util.concurrent.LinkedBlockingQueue; import org.ros2.rcljava.client.Client; @@ -360,6 +361,34 @@ public static void spinSome(final ComposableNode composableNode) { getGlobalExecutor().removeNode(composableNode); } + public static void spinUntilComplete(final Node node, final Future future, long timeoutNs) { + ComposableNode composableNode = new ComposableNode() { + public Node getNode() { + return node; + } + }; + RCLJava.spinUntilComplete(composableNode, future, timeoutNs); + } + + public static void spinUntilComplete(final Node node, final Future future) + { + RCLJava.spinUntilComplete(node, future, -1); + } + + public static void spinUntilComplete( + final ComposableNode node, final Future future, long timeoutNs) + { + getGlobalExecutor().addNode(node); + getGlobalExecutor().spinUntilComplete(future, timeoutNs); + getGlobalExecutor().removeNode(node); + } + + public static void spinUntilComplete( + final ComposableNode node, final Future future) + { + RCLJava.spinUntilComplete(node, future, -1); + } + public static synchronized void shutdown() { cleanup(); if (RCLJava.defaultContext != null) { diff --git a/rcljava/src/main/java/org/ros2/rcljava/client/ClientImpl.java b/rcljava/src/main/java/org/ros2/rcljava/client/ClientImpl.java index 442aa8ec..f48a3892 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/client/ClientImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/client/ClientImpl.java @@ -87,7 +87,7 @@ public void accept(Future input) {} long sequenceNumber = nativeSendClientRequest( handle, request.getFromJavaConverterInstance(), request.getDestructorInstance(), request); - RCLFuture future = new RCLFuture(this.nodeReference); + RCLFuture future = new RCLFuture(); Map.Entry entry = new AbstractMap.SimpleEntry(callback, future); diff --git a/rcljava/src/main/java/org/ros2/rcljava/concurrent/RCLFuture.java b/rcljava/src/main/java/org/ros2/rcljava/concurrent/RCLFuture.java index 5b45ccc4..2b21891f 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/concurrent/RCLFuture.java +++ b/rcljava/src/main/java/org/ros2/rcljava/concurrent/RCLFuture.java @@ -15,6 +15,7 @@ package org.ros2.rcljava.concurrent; +import java.lang.Deprecated; import java.lang.ref.WeakReference; import java.util.concurrent.ExecutionException; import java.util.concurrent.Future; @@ -22,73 +23,50 @@ import java.util.concurrent.TimeoutException; import org.ros2.rcljava.RCLJava; -import org.ros2.rcljava.executors.Executor; import org.ros2.rcljava.node.Node; public class RCLFuture implements Future { private WeakReference nodeReference; private boolean done = false; private V value = null; - private Executor executor = null; - public RCLFuture(final WeakReference nodeReference) { - this.nodeReference = nodeReference; - } - - public RCLFuture(final Executor executor) { - this.executor = executor; - } + public RCLFuture() {} - public final V get() throws InterruptedException, ExecutionException { + public final synchronized V get() throws InterruptedException, ExecutionException { if(this.value != null) { return this.value; } while (RCLJava.ok() && !isDone()) { - if (executor != null) { - executor.spinOnce(); - } else { - Node node = nodeReference.get(); - if (node == null) { - return null; // TODO(esteve) do something - } - RCLJava.spinOnce(node); - } + this.wait(); } return this.value; } - public final V get(final long timeout, final TimeUnit unit) + public final synchronized V get(final long timeout, final TimeUnit unit) throws InterruptedException, ExecutionException, TimeoutException { if (isDone()) { return value; } - long endTime = TimeUnit.NANOSECONDS.convert(System.currentTimeMillis(), TimeUnit.MILLISECONDS); - - long timeoutNS = TimeUnit.NANOSECONDS.convert(timeout, unit); + long endTime = System.nanoTime(); + long timeoutNS = unit.toNanos(timeout); if (timeoutNS > 0) { endTime += timeoutNS; } while (RCLJava.ok()) { - if (executor != null) { - executor.spinOnce(timeoutNS); - } else { - Node node = nodeReference.get(); - if (node == null) { - return null; // TODO(esteve) do something - } - RCLJava.spinOnce(node, timeoutNS); - } + this.wait(TimeUnit.NANOSECONDS.toMillis(timeoutNS), (int) (timeoutNS % 1000000l)); if (isDone()) { return value; } - long now = TimeUnit.NANOSECONDS.convert(System.currentTimeMillis(), TimeUnit.MILLISECONDS); + long now = System.nanoTime(); if (now >= endTime) { throw new TimeoutException(); + } else { + timeoutNS = endTime - now; } } throw new InterruptedException(); @@ -109,5 +87,6 @@ public final boolean cancel(final boolean mayInterruptIfRunning) { public final synchronized void set(final V value) { this.value = value; done = true; + this.notify(); } } diff --git a/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java b/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java index 5c43b6c4..e500f9ac 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java +++ b/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java @@ -15,6 +15,7 @@ package org.ros2.rcljava.executors; +import java.lang.Math; import java.lang.SuppressWarnings; import java.util.AbstractMap; import java.util.ArrayList; @@ -24,6 +25,7 @@ import java.util.Collection; import java.util.concurrent.BlockingQueue; import java.util.concurrent.ConcurrentHashMap; +import java.util.concurrent.Future; import java.util.concurrent.LinkedBlockingQueue; @@ -439,6 +441,30 @@ private boolean maxDurationNotElapsed(long maxDurationNs, long startNs) { return false; } + public void spinUntilComplete(Future future, long maxDurationNs) { + long startNs = System.nanoTime(); + // only use a blocking call to waitForWork when maxDurationNs < 0 + long waitTimeout = -1; + if (maxDurationNs > 0) { + // We cannot be waiting for work forever, if not we're not going to respect the passed timeout. + // We can neither do a non-blocking call to waitForWork(), because if the future has not yet + // been completed it will result in a busy loop. + // Use an arbitrary timeout to relax cpu usage. + waitTimeout = Math.min(maxDurationNs / 10, 10000000 /* 1ms*/); + } + while (RCLJava.ok() && (maxDurationNs < 0 || maxDurationNotElapsed(maxDurationNs, startNs))) { + waitForWork(waitTimeout); + AnyExecutable anyExecutable = getNextExecutable(); + while (anyExecutable != null) { + executeAnyExecutable(anyExecutable); + if (future.isDone()) { + return; + } + anyExecutable = getNextExecutable(); + } + } + } + private void spinSomeImpl(long maxDurationNs, boolean exhaustive) { long startNs = System.nanoTime(); boolean workAvailable = false; diff --git a/rcljava/src/main/java/org/ros2/rcljava/executors/Executor.java b/rcljava/src/main/java/org/ros2/rcljava/executors/Executor.java index fe9810c9..5268e531 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/executors/Executor.java +++ b/rcljava/src/main/java/org/ros2/rcljava/executors/Executor.java @@ -15,6 +15,8 @@ package org.ros2.rcljava.executors; +import java.util.concurrent.Future; + import org.ros2.rcljava.node.ComposableNode; public interface Executor { @@ -26,6 +28,10 @@ public interface Executor { public void spinOnce(long timeout); + public void spinUntilComplete(Future future, long maxDurationNs); + + public void spinUntilComplete(Future future); + public void spinSome(); public void spinSome(long maxDurationNs); diff --git a/rcljava/src/main/java/org/ros2/rcljava/executors/MultiThreadedExecutor.java b/rcljava/src/main/java/org/ros2/rcljava/executors/MultiThreadedExecutor.java index 1bd41e31..5e813701 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/executors/MultiThreadedExecutor.java +++ b/rcljava/src/main/java/org/ros2/rcljava/executors/MultiThreadedExecutor.java @@ -17,6 +17,7 @@ import java.util.concurrent.Executors; import java.util.concurrent.ExecutorService; +import java.util.concurrent.Future; import org.ros2.rcljava.RCLJava; import org.ros2.rcljava.node.ComposableNode; @@ -55,6 +56,14 @@ public void spinOnce(long timeout) { this.baseExecutor.spinOnce(timeout); } + public void spinUntilComplete(Future future, long timeoutNs) { + this.baseExecutor.spinUntilComplete(future, timeoutNs); + } + + public void spinUntilComplete(Future future) { + this.baseExecutor.spinUntilComplete(future, -1); + } + public void spinSome() { this.spinSome(0); } diff --git a/rcljava/src/main/java/org/ros2/rcljava/executors/SingleThreadedExecutor.java b/rcljava/src/main/java/org/ros2/rcljava/executors/SingleThreadedExecutor.java index 93b41d06..a55259d4 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/executors/SingleThreadedExecutor.java +++ b/rcljava/src/main/java/org/ros2/rcljava/executors/SingleThreadedExecutor.java @@ -15,6 +15,8 @@ package org.ros2.rcljava.executors; +import java.util.concurrent.Future; + import org.ros2.rcljava.RCLJava; import org.ros2.rcljava.node.ComposableNode; import org.ros2.rcljava.executors.BaseExecutor; @@ -38,6 +40,14 @@ public void spinOnce(long timeout) { this.baseExecutor.spinOnce(timeout); } + public void spinUntilComplete(Future future, long timeoutNs) { + this.baseExecutor.spinUntilComplete(future, timeoutNs); + } + + public void spinUntilComplete(Future future) { + this.baseExecutor.spinUntilComplete(future, -1); + } + public void spinSome() { this.spinSome(0); } diff --git a/rcljava/src/main/java/org/ros2/rcljava/parameters/client/AsyncParametersClient.java b/rcljava/src/main/java/org/ros2/rcljava/parameters/client/AsyncParametersClient.java index 5a7a74e7..86a766a8 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/parameters/client/AsyncParametersClient.java +++ b/rcljava/src/main/java/org/ros2/rcljava/parameters/client/AsyncParametersClient.java @@ -19,6 +19,7 @@ import java.util.concurrent.Future; import org.ros2.rcljava.consumers.Consumer; +import org.ros2.rcljava.node.Node; import org.ros2.rcljava.parameters.ParameterType; import org.ros2.rcljava.parameters.ParameterVariant; @@ -59,4 +60,6 @@ public Future> describeParameters( public Future> describeParameters( final List names, final Consumer>> callback); + + public Node getNode(); } \ No newline at end of file diff --git a/rcljava/src/main/java/org/ros2/rcljava/parameters/client/AsyncParametersClientImpl.java b/rcljava/src/main/java/org/ros2/rcljava/parameters/client/AsyncParametersClientImpl.java index 5de84b4f..a9ece715 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/parameters/client/AsyncParametersClientImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/parameters/client/AsyncParametersClientImpl.java @@ -104,8 +104,7 @@ public Future> getParameters(final List names) { public Future> getParameters( final List names, final Consumer>> callback) { - final RCLFuture> futureResult = - new RCLFuture>(new WeakReference(this.node)); + final RCLFuture> futureResult = new RCLFuture>(); final rcl_interfaces.srv.GetParameters_Request request = new rcl_interfaces.srv.GetParameters_Request(); request.setNames(names); @@ -141,8 +140,7 @@ public Future> getParameterTypes(final List names) { public Future> getParameterTypes( final List names, final Consumer>> callback) { - final RCLFuture> futureResult = - new RCLFuture>(new WeakReference(this.node)); + final RCLFuture> futureResult = new RCLFuture>(); final rcl_interfaces.srv.GetParameterTypes_Request request = new rcl_interfaces.srv.GetParameterTypes_Request(); request.setNames(names); @@ -178,8 +176,7 @@ public Future> setParameters( final List parameters, final Consumer>> callback) { final RCLFuture> futureResult = - new RCLFuture>( - new WeakReference(this.node)); + new RCLFuture>(); final rcl_interfaces.srv.SetParameters_Request request = new rcl_interfaces.srv.SetParameters_Request(); List requestParameters = @@ -188,7 +185,6 @@ public Future> setParameters( requestParameters.add(parameterVariant.toParameter()); } request.setParameters(requestParameters); - setParametersClient.asyncSendRequest( request, new Consumer>() { public void accept(final Future future) { @@ -216,7 +212,7 @@ public Future setParametersAtomically( final List parameters, final Consumer> callback) { final RCLFuture futureResult = - new RCLFuture(new WeakReference(this.node)); + new RCLFuture(); final rcl_interfaces.srv.SetParametersAtomically_Request request = new rcl_interfaces.srv.SetParametersAtomically_Request(); List requestParameters = @@ -253,7 +249,7 @@ public Future listParameters( public Future listParameters(final List prefixes, long depth, final Consumer> callback) { final RCLFuture futureResult = - new RCLFuture(new WeakReference(this.node)); + new RCLFuture(); final rcl_interfaces.srv.ListParameters_Request request = new rcl_interfaces.srv.ListParameters_Request(); request.setPrefixes(prefixes); @@ -286,8 +282,7 @@ public Future> describeParameters( final List names, final Consumer>> callback) { final RCLFuture> futureResult = - new RCLFuture>( - new WeakReference(this.node)); + new RCLFuture>(); final rcl_interfaces.srv.DescribeParameters_Request request = new rcl_interfaces.srv.DescribeParameters_Request(); request.setNames(names); @@ -309,4 +304,8 @@ public void accept(final Future }); return futureResult; } + + public Node getNode() { + return this.node; + } } diff --git a/rcljava/src/main/java/org/ros2/rcljava/parameters/client/SyncParametersClientImpl.java b/rcljava/src/main/java/org/ros2/rcljava/parameters/client/SyncParametersClientImpl.java index b8a0a323..5ee58cb8 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/parameters/client/SyncParametersClientImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/parameters/client/SyncParametersClientImpl.java @@ -23,6 +23,7 @@ import java.util.concurrent.ExecutionException; import java.util.concurrent.Future; +import org.ros2.rcljava.RCLJava; import org.ros2.rcljava.client.Client; import org.ros2.rcljava.concurrent.RCLFuture; import org.ros2.rcljava.consumers.Consumer; @@ -33,24 +34,6 @@ import org.ros2.rcljava.parameters.ParameterVariant; public class SyncParametersClientImpl implements SyncParametersClient { - private static class ConsumerHelper implements Consumer> { - private RCLFuture resultFuture; - - public void accept(final Future future) { - T result = null; - try { - result = future.get(); - } catch (Exception e) { - // TODO(esteve): do something - } - this.resultFuture.set(result); - } - - public ConsumerHelper(RCLFuture resultFuture) { - this.resultFuture = resultFuture; - } - } - private Executor executor; public AsyncParametersClient asyncParametersClient; @@ -107,79 +90,54 @@ public SyncParametersClientImpl(final Executor executor, final Node node) this(executor, node, "", QoSProfile.PARAMETERS); } - public List getParameters(final List names) - throws InterruptedException, ExecutionException { + private T spinUntilComplete(Future future) + throws InterruptedException, ExecutionException + { if (executor != null) { - RCLFuture> future = new RCLFuture>(executor); - asyncParametersClient.getParameters( - names, new ConsumerHelper>(future)); - return future.get(); + executor.spinUntilComplete(future); } else { - return asyncParametersClient.getParameters(names, null).get(); + RCLJava.spinUntilComplete(this.asyncParametersClient.getNode(), future); } + return future.get(); + } + + public List getParameters(final List names) + throws InterruptedException, ExecutionException { + Future> future = asyncParametersClient.getParameters(names, null); + return spinUntilComplete(future); } public List getParameterTypes(final List names) throws InterruptedException, ExecutionException { - if (executor != null) { - RCLFuture> future = new RCLFuture>(executor); - asyncParametersClient.getParameterTypes( - names, new ConsumerHelper>(future)); - return future.get(); - } else { - return asyncParametersClient.getParameterTypes(names, null).get(); - } + Future> future = asyncParametersClient.getParameterTypes(names, null); + return spinUntilComplete(future); } public List setParameters( final List parameters) throws InterruptedException, ExecutionException { - if (executor != null) { - RCLFuture> future = - new RCLFuture>(executor); - asyncParametersClient.setParameters( - parameters, new ConsumerHelper>(future)); - return future.get(); - } else { - return asyncParametersClient.setParameters(parameters, null).get(); - } + Future> future = asyncParametersClient.setParameters( + parameters, null); + return spinUntilComplete(future); } public rcl_interfaces.msg.SetParametersResult setParametersAtomically( final List parameters) throws InterruptedException, ExecutionException { - if (executor != null) { - RCLFuture future = - new RCLFuture(executor); - asyncParametersClient.setParametersAtomically( - parameters, new ConsumerHelper(future)); - return future.get(); - } else { - return asyncParametersClient.setParametersAtomically(parameters, null).get(); - } + Future future = asyncParametersClient.setParametersAtomically( + parameters, null); + return spinUntilComplete(future); } public rcl_interfaces.msg.ListParametersResult listParameters( final List prefixes, long depth) throws InterruptedException, ExecutionException { - if (executor != null) { - RCLFuture future = - new RCLFuture(executor); - asyncParametersClient.listParameters( - prefixes, depth, new ConsumerHelper(future)); - return future.get(); - } else { - return asyncParametersClient.listParameters(prefixes, depth, null).get(); - } + Future future = asyncParametersClient.listParameters( + prefixes, depth, null); + return spinUntilComplete(future); } public List describeParameters(final List names) throws InterruptedException, ExecutionException { - if (executor != null) { - RCLFuture> future = - new RCLFuture>(executor); - asyncParametersClient.describeParameters( - names, new ConsumerHelper>(future)); - return future.get(); - } else { - return asyncParametersClient.describeParameters(names, null).get(); - } + Future> future = asyncParametersClient.describeParameters( + names, null); + return spinUntilComplete(future); } } \ No newline at end of file diff --git a/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java b/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java index a9f5d07f..2e49b6ad 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java @@ -139,19 +139,9 @@ public test_msgs.action.Fibonacci_SendGoal_Response sendGoal(int order) throws E Future future = this.mockActionClient.sendGoalClient.asyncSendRequest(request); - test_msgs.action.Fibonacci_SendGoal_Response response = null; long startTime = System.nanoTime(); - while (RCLJava.ok() && !future.isDone()) { - this.executor.spinOnce(1); - response = future.get(100, TimeUnit.MILLISECONDS); - - // Check for timeout - long duration = System.nanoTime() - startTime; - if (TimeUnit.NANOSECONDS.toSeconds(duration) >= 5) { - break; - } - } - return response; + this.executor.spinUntilComplete(future, TimeUnit.SECONDS.toNanos(5)); + return future.get(); } @Test @@ -211,15 +201,7 @@ public final void testCancelGoal() throws Exception { // Wait for cancel response long startTime = System.nanoTime(); - while (RCLJava.ok() && !cancelResponseFuture.isDone()) { - this.executor.spinOnce(100000000); // timeout of 100 milliseconds - - // Check for timeout - long duration = System.nanoTime() - startTime; - if (TimeUnit.NANOSECONDS.toSeconds(duration) >= 5) { - break; - } - } + this.executor.spinUntilComplete(cancelResponseFuture, TimeUnit.SECONDS.toNanos(5)); assertEquals(true, cancelResponseFuture.isDone()); action_msgs.srv.CancelGoal_Response cancelResponse = cancelResponseFuture.get(); diff --git a/rcljava/src/test/java/org/ros2/rcljava/client/ClientTest.java b/rcljava/src/test/java/org/ros2/rcljava/client/ClientTest.java index cf01c02e..c65d2796 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/client/ClientTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/client/ClientTest.java @@ -37,6 +37,7 @@ import org.ros2.rcljava.RCLJava; import org.ros2.rcljava.concurrent.RCLFuture; import org.ros2.rcljava.consumers.TriConsumer; +import org.ros2.rcljava.executors.Executor; import org.ros2.rcljava.node.Node; import org.ros2.rcljava.service.RMWRequestId; import org.ros2.rcljava.service.Service; @@ -97,7 +98,7 @@ public static void tearDownOnce() { @Test public final void testAdd() throws Exception { RCLFuture consumerFuture = - new RCLFuture(new WeakReference(node)); + new RCLFuture(); TestClientConsumer clientConsumer = new TestClientConsumer(consumerFuture); @@ -115,7 +116,8 @@ public final void testAdd() throws Exception { Future responseFuture = client.asyncSendRequest(request); - rcljava.srv.AddTwoInts_Response response = responseFuture.get(10, TimeUnit.SECONDS); + RCLJava.spinUntilComplete(node, responseFuture, TimeUnit.SECONDS.toNanos(10)); + rcljava.srv.AddTwoInts_Response response = responseFuture.get(); // Check that the message was received by the service assertTrue(consumerFuture.isDone()); 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 48a83d1a..9b39e79d 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java @@ -225,7 +225,7 @@ public final void testPubSubStdString() throws Exception { node.createPublisher(std_msgs.msg.String.class, "test_topic_string"); RCLFuture future = - new RCLFuture(new WeakReference(node)); + new RCLFuture(); Subscription subscription = node.createSubscription(std_msgs.msg.String.class, "test_topic_string", @@ -255,7 +255,7 @@ public final void testPubSubBoundedArrayNested() throws Exception { rcljava.msg.BoundedArrayNested.class, "test_topic_bounded_array_nested"); RCLFuture future = - new RCLFuture(new WeakReference(node)); + new RCLFuture(); Subscription subscription = node.createSubscription( @@ -297,7 +297,7 @@ public final void testPubSubBoundedArrayPrimitives() throws Exception { rcljava.msg.BoundedArrayPrimitives.class, "test_topic_bounded_array_primitives"); RCLFuture future = - new RCLFuture(new WeakReference(node)); + new RCLFuture(); Subscription subscription = node.createSubscription( @@ -370,7 +370,7 @@ public final void testPubSubBuiltins() throws Exception { rcljava.msg.Builtins.class, "test_topic_builtins"); RCLFuture future = - new RCLFuture(new WeakReference(node)); + new RCLFuture(); Subscription subscription = node.createSubscription(rcljava.msg.Builtins.class, @@ -417,7 +417,7 @@ public final void testPubSubDynamicArrayNested() throws Exception { rcljava.msg.DynamicArrayNested.class, "test_topic_dynamic_array_nested"); RCLFuture future = - new RCLFuture(new WeakReference(node)); + new RCLFuture(); Subscription subscription = node.createSubscription( @@ -459,7 +459,7 @@ public final void testPubSubDynamicArrayPrimitives() throws Exception { rcljava.msg.DynamicArrayPrimitives.class, "test_topic_dynamic_array_primitives"); RCLFuture future = - new RCLFuture(new WeakReference(node)); + new RCLFuture(); Subscription subscription = node.createSubscription( @@ -532,7 +532,7 @@ public final void testPubSubEmpty() throws Exception { node.createPublisher(rcljava.msg.Empty.class, "test_topic_empty"); RCLFuture future = - new RCLFuture(new WeakReference(node)); + new RCLFuture(); Subscription subscription = node.createSubscription( rcljava.msg.Empty.class, "test_topic_empty", new TestConsumer(future)); @@ -560,7 +560,7 @@ public final void testPubSubFieldsWithSameType() throws Exception { rcljava.msg.FieldsWithSameType.class, "test_topic_fields_with_same_type"); RCLFuture future = - new RCLFuture(new WeakReference(node)); + new RCLFuture(); Subscription subscription = node.createSubscription( @@ -604,7 +604,7 @@ public final void testPubSubNested() throws Exception { node.createPublisher(rcljava.msg.Nested.class, "test_topic_nested"); RCLFuture future = - new RCLFuture(new WeakReference(node)); + new RCLFuture(); Subscription subscription = node.createSubscription(rcljava.msg.Nested.class, "test_topic_nested", @@ -639,7 +639,7 @@ public final void testPubSubPrimitives() throws Exception { rcljava.msg.Primitives.class, "test_topic_primitives"); RCLFuture future = - new RCLFuture(new WeakReference(node)); + new RCLFuture(); Subscription subscription = node.createSubscription(rcljava.msg.Primitives.class, @@ -670,7 +670,7 @@ public final void testPubSubStaticArrayNested() throws Exception { rcljava.msg.StaticArrayNested.class, "test_topic_static_array_nested"); RCLFuture future = - new RCLFuture(new WeakReference(node)); + new RCLFuture(); Subscription subscription = node.createSubscription(rcljava.msg.StaticArrayNested.class, @@ -725,7 +725,7 @@ public final void testPubSubStaticArrayPrimitives() throws Exception { rcljava.msg.StaticArrayPrimitives.class, "test_topic_static_array_primitives"); RCLFuture future = - new RCLFuture(new WeakReference(node)); + new RCLFuture(); Subscription subscription = node.createSubscription( @@ -799,7 +799,7 @@ public final void testPubUInt32() throws Exception { node.createPublisher(rcljava.msg.UInt32.class, "test_topic_uint32"); RCLFuture future = - new RCLFuture(new WeakReference(node)); + new RCLFuture(); Subscription subscription = node.createSubscription(rcljava.msg.UInt32.class, "test_topic_uint32", @@ -833,13 +833,13 @@ public final void testPubUInt32MultipleNodes() throws Exception { Publisher publisher = publisherNode.createPublisher( rcljava.msg.UInt32.class, "test_topic_multiple"); - RCLFuture futureOne = new RCLFuture(executor); + RCLFuture futureOne = new RCLFuture(); Subscription subscriptionOne = subscriptionNodeOne.createSubscription(rcljava.msg.UInt32.class, "test_topic_multiple", new TestConsumer(futureOne)); - RCLFuture futureTwo = new RCLFuture(executor); + RCLFuture futureTwo = new RCLFuture(); Subscription subscriptionTwo = subscriptionNodeTwo.createSubscription(rcljava.msg.UInt32.class, diff --git a/rcljava/src/test/java/org/ros2/rcljava/parameters/AsyncParametersClientTest.java b/rcljava/src/test/java/org/ros2/rcljava/parameters/AsyncParametersClientTest.java index df2f749f..38f3e718 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/parameters/AsyncParametersClientTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/parameters/AsyncParametersClientTest.java @@ -34,6 +34,7 @@ import java.util.List; import java.util.concurrent.Future; +import java.util.concurrent.TimeUnit; import org.ros2.rcljava.RCLJava; import org.ros2.rcljava.concurrent.RCLFuture; @@ -113,19 +114,18 @@ public final void testSetParameters() throws Exception { new ParameterVariant("foo.second", 42), new ParameterVariant("foobar", true)}); RCLFuture> future = - new RCLFuture>( - new WeakReference(this.node)); + new RCLFuture>(); parametersClient.setParameters(parameters, new TestConsumer(future)); - List parameterNames = - Arrays.asList(new String[] {"foo", "bar", "baz", "foo.first", "foo.second", "foobar"}); - + RCLJava.spinUntilComplete(node, future); List setParametersResults = future.get(); assertEquals(6, setParametersResults.size()); for (rcl_interfaces.msg.SetParametersResult result : setParametersResults) { assertEquals(true, result.getSuccessful()); } + List parameterNames = + Arrays.asList(new String[] {"foo", "bar", "baz", "foo.first", "foo.second", "foobar"}); List results = node.getParameters(parameterNames); assertEquals(parameters, results); } @@ -142,10 +142,10 @@ public final void testGetParameters() throws Exception { List parameterNames = Arrays.asList(new String[] {"foo", "bar", "baz", "foo.first", "foo.second", "foobar"}); - RCLFuture> future = - new RCLFuture>(new WeakReference(this.node)); + RCLFuture> future = new RCLFuture>(); parametersClient.getParameters(parameterNames, new TestConsumer(future)); + RCLJava.spinUntilComplete(node, future); assertEquals(parameters, future.get()); } @@ -159,10 +159,11 @@ public final void testListParameters() throws Exception { node.setParameters(parameters); RCLFuture future = - new RCLFuture(new WeakReference(this.node)); + new RCLFuture(); parametersClient.listParameters( Arrays.asList(new String[] {"foo", "bar"}), 10, new TestConsumer(future)); + RCLJava.spinUntilComplete(node, future); assertArrayEquals(new String[] {"foo.first", "foo.second"}, future.get().getNames()); assertArrayEquals(new String[] {"foo"}, future.get().getPrefixes()); } @@ -177,8 +178,7 @@ public final void testDescribeParameters() throws Exception { node.setParameters(parameters); RCLFuture> future = - new RCLFuture>( - new WeakReference(this.node)); + new RCLFuture>(); parametersClient.describeParameters( Arrays.asList(new String[] {"foo", "bar"}), new TestConsumer(future)); @@ -188,6 +188,7 @@ public final void testDescribeParameters() throws Exception { rcl_interfaces.msg.ParameterType.PARAMETER_INTEGER), new rcl_interfaces.msg.ParameterDescriptor().setName("bar").setType( rcl_interfaces.msg.ParameterType.PARAMETER_STRING)}); + RCLJava.spinUntilComplete(node, future); assertEquals(expected, future.get()); } } diff --git a/rcljava/src/test/java/org/ros2/rcljava/timer/TimerTest.java b/rcljava/src/test/java/org/ros2/rcljava/timer/TimerTest.java index a38d2901..461f5734 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/timer/TimerTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/timer/TimerTest.java @@ -87,7 +87,7 @@ public final void testCreateWallTimer() throws Exception { int max_iterations = 4; Node node = RCLJava.createNode("test_timer_node"); - RCLFuture future = new RCLFuture(new WeakReference(node)); + RCLFuture future = new RCLFuture(); TimerCallback timerCallback = new TimerCallback(future, max_iterations); Timer timer = node.createWallTimer(250, TimeUnit.MILLISECONDS, timerCallback); assertNotEquals(0, timer.getHandle()); @@ -95,6 +95,7 @@ public final void testCreateWallTimer() throws Exception { assertEquals( TimeUnit.NANOSECONDS.convert(250, TimeUnit.MILLISECONDS), timer.getTimerPeriodNS()); + RCLJava.spinUntilComplete(node, future); boolean result = future.get(3, TimeUnit.SECONDS); assertTrue(result); assertEquals(4, timerCallback.getCounter()); @@ -109,7 +110,7 @@ public final void testCreateTimer() throws Exception { int max_iterations = 4; Node node = RCLJava.createNode("test_timer_node"); - RCLFuture future = new RCLFuture(new WeakReference(node)); + RCLFuture future = new RCLFuture(); TimerCallback timerCallback = new TimerCallback(future, max_iterations); Timer timer = node.createTimer(250, TimeUnit.MILLISECONDS, timerCallback); assertNotEquals(0, timer.getHandle()); @@ -117,6 +118,7 @@ public final void testCreateTimer() throws Exception { assertEquals( TimeUnit.NANOSECONDS.convert(250, TimeUnit.MILLISECONDS), timer.getTimerPeriodNS()); + RCLJava.spinUntilComplete(node, future); boolean result = future.get(3, TimeUnit.SECONDS); assertTrue(result); assertEquals(4, timerCallback.getCounter()); From 509a7cd7fa7107a5cd165bb6cff4c9fcd621f492 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 12 Oct 2021 10:18:32 -0300 Subject: [PATCH 49/51] Add remove pending request method (osrf/ros2_java#81) Signed-off-by: Ivan Santiago Paunovic --- rcljava/CMakeLists.txt | 1 + .../java/org/ros2/rcljava/client/Client.java | 6 ++-- .../org/ros2/rcljava/client/ClientImpl.java | 28 +++++++++++------- .../ros2/rcljava/client/ResponseFuture.java | 29 +++++++++++++++++++ .../org/ros2/rcljava/client/ClientTest.java | 26 +++++++++++++++++ 5 files changed, 78 insertions(+), 12 deletions(-) create mode 100644 rcljava/src/main/java/org/ros2/rcljava/client/ResponseFuture.java diff --git a/rcljava/CMakeLists.txt b/rcljava/CMakeLists.txt index 85cba623..0e7206bb 100644 --- a/rcljava/CMakeLists.txt +++ b/rcljava/CMakeLists.txt @@ -145,6 +145,7 @@ set(${PROJECT_NAME}_sources "src/main/java/org/ros2/rcljava/action/GoalStatus.java" "src/main/java/org/ros2/rcljava/client/Client.java" "src/main/java/org/ros2/rcljava/client/ClientImpl.java" + "src/main/java/org/ros2/rcljava/client/ResponseFuture.java" "src/main/java/org/ros2/rcljava/concurrent/Callback.java" "src/main/java/org/ros2/rcljava/concurrent/RCLFuture.java" "src/main/java/org/ros2/rcljava/contexts/Context.java" diff --git a/rcljava/src/main/java/org/ros2/rcljava/client/Client.java b/rcljava/src/main/java/org/ros2/rcljava/client/Client.java index 811aa2c5..8043cf6f 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/client/Client.java +++ b/rcljava/src/main/java/org/ros2/rcljava/client/Client.java @@ -30,11 +30,13 @@ public interface Client extends Disposable { void handleResponse(RMWRequestId header, U response); - Future asyncSendRequest( + ResponseFuture asyncSendRequest( final U request); - Future asyncSendRequest( + ResponseFuture asyncSendRequest( final U request, final Consumer> callback); + + boolean removePendingRequest(ResponseFuture future); /** * Check if the service server is available. diff --git a/rcljava/src/main/java/org/ros2/rcljava/client/ClientImpl.java b/rcljava/src/main/java/org/ros2/rcljava/client/ClientImpl.java index f48a3892..c8496fc2 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/client/ClientImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/client/ClientImpl.java @@ -28,7 +28,6 @@ import org.ros2.rcljava.RCLJava; import org.ros2.rcljava.common.JNIUtils; -import org.ros2.rcljava.concurrent.RCLFuture; import org.ros2.rcljava.consumers.Consumer; import org.ros2.rcljava.interfaces.MessageDefinition; import org.ros2.rcljava.interfaces.ServiceDefinition; @@ -53,7 +52,7 @@ public class ClientImpl implements Client { private final WeakReference nodeReference; private long handle; private final String serviceName; - private Map> pendingRequests; + private Map> pendingRequests; private final ServiceDefinition serviceDefinition; @@ -67,43 +66,52 @@ public ClientImpl( this.handle = handle; this.serviceName = serviceName; this.serviceDefinition = serviceDefinition; - this.pendingRequests = new HashMap>(); + this.pendingRequests = new HashMap>(); } public ServiceDefinition getServiceDefinition() { return this.serviceDefinition; } - public final Future + public final ResponseFuture asyncSendRequest(final U request) { return asyncSendRequest(request, new Consumer>() { public void accept(Future input) {} }); } - public final Future + public final ResponseFuture asyncSendRequest(final U request, final Consumer> callback) { synchronized (pendingRequests) { long sequenceNumber = nativeSendClientRequest( handle, request.getFromJavaConverterInstance(), request.getDestructorInstance(), request); - RCLFuture future = new RCLFuture(); + ResponseFuture future = new ResponseFuture(sequenceNumber); - Map.Entry entry = - new AbstractMap.SimpleEntry(callback, future); + Map.Entry entry = + new AbstractMap.SimpleEntry(callback, future); pendingRequests.put(sequenceNumber, entry); return future; } } + public final boolean + removePendingRequest(ResponseFuture future) { + synchronized (pendingRequests) { + Map.Entry entry = pendingRequests.remove( + future.getRequestSequenceNumber()); + return entry != null; + } + } + public final void handleResponse( final RMWRequestId header, final U response) { synchronized (pendingRequests) { long sequenceNumber = header.sequenceNumber; - Map.Entry entry = pendingRequests.remove(sequenceNumber); + Map.Entry entry = pendingRequests.remove(sequenceNumber); if (entry != null) { Consumer callback = entry.getKey(); - RCLFuture future = entry.getValue(); + ResponseFuture future = entry.getValue(); future.set(response); callback.accept(future); return; diff --git a/rcljava/src/main/java/org/ros2/rcljava/client/ResponseFuture.java b/rcljava/src/main/java/org/ros2/rcljava/client/ResponseFuture.java new file mode 100644 index 00000000..9c89fbef --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/client/ResponseFuture.java @@ -0,0 +1,29 @@ +/* Copyright 2021 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.client; + +import org.ros2.rcljava.concurrent.RCLFuture; + + +public class ResponseFuture extends RCLFuture { + public ResponseFuture(long requestSequenceNumber) { + this.requestSequenceNumber = requestSequenceNumber; + } + public long getRequestSequenceNumber() { + return this.requestSequenceNumber; + } + private long requestSequenceNumber; +} diff --git a/rcljava/src/test/java/org/ros2/rcljava/client/ClientTest.java b/rcljava/src/test/java/org/ros2/rcljava/client/ClientTest.java index c65d2796..828345b1 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/client/ClientTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/client/ClientTest.java @@ -16,6 +16,7 @@ package org.ros2.rcljava.client; import static org.junit.Assert.assertEquals; +import static org.junit.Assert.assertFalse; import static org.junit.Assert.assertNotEquals; import static org.junit.Assert.assertTrue; @@ -137,4 +138,29 @@ public final void testAdd() throws Exception { assertEquals(0, service.getHandle()); assertEquals(0, node.getServices().size()); } + + @Test + public final void testRemovePendingRequest() throws Exception { + RCLFuture consumerFuture = + new RCLFuture(); + + TestClientConsumer clientConsumer = new TestClientConsumer(consumerFuture); + + Service service = node.createService( + rcljava.srv.AddTwoInts.class, "add_two_ints", clientConsumer); + + rcljava.srv.AddTwoInts_Request request = new rcljava.srv.AddTwoInts_Request(); + request.setA(2); + request.setB(3); + + Client client = + node.createClient(rcljava.srv.AddTwoInts.class, "add_two_ints"); + + assertTrue(client.waitForService(Duration.ofSeconds(10))); + + ResponseFuture responseFuture = client.asyncSendRequest(request); + + assertTrue(client.removePendingRequest(responseFuture)); + assertFalse(client.removePendingRequest(responseFuture)); + } } From 58aa3516351b5a49ac79f9d3c1c030b95eab9771 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 2 Nov 2021 16:35:31 -0300 Subject: [PATCH 50/51] Add method to prune pending requests in client (osrf/ros2_java#82) Signed-off-by: Ivan Santiago Paunovic --- .../java/org/ros2/rcljava/client/Client.java | 14 +++++ .../org/ros2/rcljava/client/ClientImpl.java | 58 ++++++++++++++++--- .../org/ros2/rcljava/client/ClientTest.java | 33 +++++++++++ 3 files changed, 97 insertions(+), 8 deletions(-) diff --git a/rcljava/src/main/java/org/ros2/rcljava/client/Client.java b/rcljava/src/main/java/org/ros2/rcljava/client/Client.java index 8043cf6f..2e7f8f95 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/client/Client.java +++ b/rcljava/src/main/java/org/ros2/rcljava/client/Client.java @@ -38,6 +38,20 @@ ResponseFuture asy boolean removePendingRequest(ResponseFuture future); + /** + * Remove old pending requests that this client has done. + * Future responses to removed requests will be ignored. + */ + long prunePendingRequests(); + + /** + * Remove old pending requests that where done before the specified time point. + * Future responses to removed requests will be ignored. + * + * @param nanoTime requests done before this timepoint will be removed. + */ + long prunePendingRequestsOlderThan(long nanoTime); + /** * Check if the service server is available. * diff --git a/rcljava/src/main/java/org/ros2/rcljava/client/ClientImpl.java b/rcljava/src/main/java/org/ros2/rcljava/client/ClientImpl.java index c8496fc2..507dd71e 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/client/ClientImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/client/ClientImpl.java @@ -22,6 +22,7 @@ import java.lang.Long; import java.util.AbstractMap; import java.util.HashMap; +import java.util.Iterator; import java.util.Map; import java.util.concurrent.Future; import java.util.concurrent.TimeUnit; @@ -52,7 +53,25 @@ public class ClientImpl implements Client { private final WeakReference nodeReference; private long handle; private final String serviceName; - private Map> pendingRequests; + + private class PendingRequest + { + public Consumer callback; + public ResponseFuture future; + public long requestTimestamp; + + public PendingRequest( + final Consumer callback, + final ResponseFuture future, + final long requestTimestamp) + { + this.callback = callback; + this.future = future; + this.requestTimestamp = requestTimestamp; + } + } + + private Map pendingRequests; private final ServiceDefinition serviceDefinition; @@ -66,7 +85,7 @@ public ClientImpl( this.handle = handle; this.serviceName = serviceName; this.serviceDefinition = serviceDefinition; - this.pendingRequests = new HashMap>(); + this.pendingRequests = new HashMap(); } public ServiceDefinition getServiceDefinition() { @@ -88,8 +107,7 @@ public void accept(Future input) {} request.getDestructorInstance(), request); ResponseFuture future = new ResponseFuture(sequenceNumber); - Map.Entry entry = - new AbstractMap.SimpleEntry(callback, future); + PendingRequest entry = new PendingRequest(callback, future, System.nanoTime()); pendingRequests.put(sequenceNumber, entry); return future; } @@ -98,20 +116,44 @@ public void accept(Future input) {} public final boolean removePendingRequest(ResponseFuture future) { synchronized (pendingRequests) { - Map.Entry entry = pendingRequests.remove( + PendingRequest entry = pendingRequests.remove( future.getRequestSequenceNumber()); return entry != null; } } + public final long + prunePendingRequests() { + synchronized (pendingRequests) { + long size = pendingRequests.size(); + pendingRequests.clear(); + return size; + } + } + + public final long + prunePendingRequestsOlderThan(long nanoTime) { + synchronized (pendingRequests) { + Iterator> iter = pendingRequests.entrySet().iterator(); + long removed = 0; + while(iter.hasNext()) { + if(iter.next().getValue().requestTimestamp < nanoTime) { + iter.remove(); + ++removed; + } + } + return removed; + } + } + public final void handleResponse( final RMWRequestId header, final U response) { synchronized (pendingRequests) { long sequenceNumber = header.sequenceNumber; - Map.Entry entry = pendingRequests.remove(sequenceNumber); + PendingRequest entry = pendingRequests.remove(sequenceNumber); if (entry != null) { - Consumer callback = entry.getKey(); - ResponseFuture future = entry.getValue(); + Consumer callback = entry.callback; + ResponseFuture future = entry.future; future.set(response); callback.accept(future); return; diff --git a/rcljava/src/test/java/org/ros2/rcljava/client/ClientTest.java b/rcljava/src/test/java/org/ros2/rcljava/client/ClientTest.java index 828345b1..b371c860 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/client/ClientTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/client/ClientTest.java @@ -37,6 +37,7 @@ import org.ros2.rcljava.RCLJava; 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.node.Node; @@ -163,4 +164,36 @@ public final void testRemovePendingRequest() throws Exception { assertTrue(client.removePendingRequest(responseFuture)); assertFalse(client.removePendingRequest(responseFuture)); } + + @Test + public final void testPrunePendingRequestsOlderThan() throws Exception { + RCLFuture consumerFuture = + new RCLFuture(); + + TestClientConsumer clientConsumer = new TestClientConsumer(consumerFuture); + + Service service = node.createService( + rcljava.srv.AddTwoInts.class, "add_two_ints", clientConsumer); + + rcljava.srv.AddTwoInts_Request request = new rcljava.srv.AddTwoInts_Request(); + request.setA(2); + request.setB(3); + + Client client = + node.createClient( + rcljava.srv.AddTwoInts.class, "add_two_ints"); + + assertTrue(client.waitForService(Duration.ofSeconds(10))); + + long oldNanoTime = System.nanoTime(); + ResponseFuture responseFuture = client.asyncSendRequest( + request, + new Consumer>() { + public final void accept(final Future futureResponse) {} + }); + + assertEquals(0, client.prunePendingRequestsOlderThan(oldNanoTime)); + assertEquals(1, client.prunePendingRequestsOlderThan(System.nanoTime())); + assertEquals(0, client.prunePendingRequestsOlderThan(System.nanoTime())); + } } From e7bdfa4384b2cbab6d9077145bf125e82f74481d Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Mon, 15 Nov 2021 10:14:22 -0300 Subject: [PATCH 51/51] Fix some java compiler warnings (osrf/ros2_java#79) Signed-off-by: Ivan Santiago Paunovic --- .../ros2/rcljava/action/ActionServerImpl.java | 99 ++++++++++++------- .../java/org/ros2/rcljava/node/NodeImpl.java | 18 ++-- .../ros2/rcljava/publisher/PublisherImpl.java | 6 +- .../subscription/SubscriptionImpl.java | 9 +- .../org/ros2/rcljava/client/ClientTest.java | 2 +- .../java/org/ros2/rcljava/node/NodeTest.java | 44 ++++----- .../parameters/AsyncParametersClientTest.java | 20 ++-- .../parameters/SyncParametersClientTest.java | 2 +- .../org/ros2/rcljava/time/TimeSourceTest.java | 4 + 9 files changed, 123 insertions(+), 81 deletions(-) diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java index ed379c6b..bd6e5f2d 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java @@ -16,6 +16,7 @@ package org.ros2.rcljava.action; import java.lang.ref.WeakReference; +import java.lang.SuppressWarnings; import java.util.ArrayList; import java.util.Collection; import java.util.HashMap; @@ -171,6 +172,8 @@ public synchronized void abort(ResultDefinition result) { /** * {@inheritDoc} */ + // TODO(ivanpauno): Improve generated code API so we don't need this. + @SuppressWarnings("unchecked") public synchronized void publishFeedback(FeedbackDefinition feedback) { Class feedbackMessageType = ActionServerImpl.this.actionTypeInstance.getFeedbackMessageType(); @@ -202,7 +205,7 @@ public final long getHandle() { private synchronized final void toTerminalState(byte status, ResultDefinition result) { nativeUpdateGoalState(this.handle, status); - ResultResponseDefinition resultResponse = ActionServerImpl.this.createResultResponse(); + ResultResponseDefinition resultResponse = ActionServerImpl.this.createResultResponseUnchecked(); resultResponse.setGoalStatus(status); resultResponse.setResult(result); ActionServerImpl.this.sendResult(goalInfo.getGoalId().getUuidAsList(), resultResponse); @@ -337,6 +340,12 @@ public boolean isReady(long waitSetHandle) { return false; } + @SuppressWarnings("unchecked") + private GoalCallback.GoalResponse + handleGoalUnchecked(GoalRequestDefinition requestMessage) { + return this.goalCallback.handleGoal(requestMessage); + } + private ActionServerGoalHandle executeGoalRequest( RMWRequestId rmwRequestId, GoalRequestDefinition requestMessage, @@ -358,7 +367,7 @@ private ActionServerGoalHandle executeGoalRequest( } // Call user callback - GoalCallback.GoalResponse response = this.goalCallback.handleGoal(requestMessage); + GoalCallback.GoalResponse response = this.handleGoalUnchecked(requestMessage); boolean accepted = GoalCallback.GoalResponse.ACCEPT_AND_DEFER == response || GoalCallback.GoalResponse.ACCEPT_AND_EXECUTE == response; @@ -542,17 +551,6 @@ private action_msgs.msg.GoalInfo createGoalInfo(List goalUuid) { return goalInfo; } - private ResultResponseDefinition createResultResponse() { - ResultResponseDefinition resultResponse; - try { - resultResponse = - this.actionTypeInstance.getGetResultResponseType().getDeclaredConstructor().newInstance(); - } catch (ReflectiveOperationException ex) { - throw new IllegalStateException("Failed to instantiate provided action type: ", ex); - } - return resultResponse; - } - // This will store the result, so it can be sent to future result requests, and // will also send a result response to all requests that were already made. private void sendResult(List goalUuid, ResultResponseDefinition resultResponse) { @@ -571,23 +569,61 @@ private void sendResult(List goalUuid, ResultResponseDefinition resultR } } + // TODO(ivanpauno): Improve generated code API so we don't need this. + @SuppressWarnings("unchecked") + private ResultRequestDefinition createResultRequestUnchecked() { + ResultRequestDefinition resultRequest; + try { + resultRequest = + this.actionTypeInstance.getGetResultRequestType().getDeclaredConstructor().newInstance(); + } catch (ReflectiveOperationException ex) { + throw new IllegalStateException("Failed to instantiate provided action type: ", ex); + } + return resultRequest; + } + + // TODO(ivanpauno): Improve generated code API so we don't need this. + @SuppressWarnings("unchecked") + private ResultResponseDefinition createResultResponseUnchecked() { + ResultResponseDefinition resultResponse; + try { + resultResponse = + this.actionTypeInstance.getGetResultResponseType().getDeclaredConstructor().newInstance(); + } catch (ReflectiveOperationException ex) { + throw new IllegalStateException("Failed to instantiate provided action type: ", ex); + } + return resultResponse; + } + + // TODO(ivanpauno): Improve generated code API so we don't need this. + @SuppressWarnings("unchecked") + private GoalRequestDefinition newRequestUnchecked() { + Class requestType = this.actionTypeInstance.getSendGoalRequestType(); + try { + return requestType.getDeclaredConstructor().newInstance(); + } catch (ReflectiveOperationException ex) { + throw new IllegalStateException("Failed to instantiate request: ", ex); + } + } + + // TODO(ivanpauno): Improve generated code API so we don't need this. + @SuppressWarnings("unchecked") + private GoalResponseDefinition newResponseUnchecked() { + Class responseType = this.actionTypeInstance.getSendGoalResponseType(); + try { + return responseType.getDeclaredConstructor().newInstance(); + } catch (ReflectiveOperationException ex) { + throw new IllegalStateException("Failed to instantiate responce: ", ex); + } + } + /** * {@inheritDoc} */ public void execute() { if (this.isGoalRequestReady()) { - Class requestType = this.actionTypeInstance.getSendGoalRequestType(); - Class responseType = this.actionTypeInstance.getSendGoalResponseType(); - - GoalRequestDefinition requestMessage = null; - GoalResponseDefinition responseMessage = null; - - try { - requestMessage = requestType.getDeclaredConstructor().newInstance(); - responseMessage = responseType.getDeclaredConstructor().newInstance(); - } catch (ReflectiveOperationException ex) { - throw new IllegalStateException("Failed to instantiate request or responce: ", ex); - } + GoalRequestDefinition requestMessage = newRequestUnchecked(); + GoalResponseDefinition responseMessage = newResponseUnchecked(); if (requestMessage != null && responseMessage != null) { long requestFromJavaConverterHandle = requestMessage.getFromJavaConverterInstance(); @@ -643,14 +679,7 @@ public void execute() { } if (this.isResultRequestReady()) { - Class requestType = this.actionTypeInstance.getGetResultRequestType(); - - ResultRequestDefinition requestMessage = null; - try { - requestMessage = requestType.getDeclaredConstructor().newInstance(); - } catch (ReflectiveOperationException ex) { - throw new IllegalArgumentException("Failed to instantiate action result request: ", ex); - } + ResultRequestDefinition requestMessage = createResultRequestUnchecked(); if (requestMessage != null) { long requestFromJavaConverterHandle = requestMessage.getFromJavaConverterInstance(); @@ -672,7 +701,7 @@ public void execute() { ResultResponseDefinition resultResponse = null; if (!goalExists) { - resultResponse = this.createResultResponse(); + resultResponse = this.createResultResponseUnchecked(); resultResponse.setGoalStatus(action_msgs.msg.GoalStatus.STATUS_UNKNOWN); } else { resultResponse = this.goalResults.get(goalUuid); @@ -682,7 +711,7 @@ public void execute() { List requestIds = null; requestIds = this.goalRequests.get(goalUuid); if (requestIds == null) { - requestIds = new ArrayList(); + requestIds = new ArrayList(); this.goalRequests.put(goalUuid, requestIds); } requestIds.add(rmwRequestId); 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 3ca6cfb6..0ac0ee9f 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java @@ -879,7 +879,7 @@ public rcl_interfaces.msg.ListParametersResult listParameters( } public final Collection getNodeNames() { - ArrayList nodeNames = new ArrayList(); + ArrayList nodeNames = new ArrayList(); nativeGetNodeNames(this.handle, nodeNames); return nodeNames; } @@ -887,7 +887,7 @@ public final Collection getNodeNames() { private native static final void nativeGetNodeNames(long handle, ArrayList nodeNames); public final Collection getTopicNamesAndTypes() { - Collection namesAndTypes = new ArrayList(); + Collection namesAndTypes = new ArrayList(); nativeGetTopicNamesAndTypes(this.handle, namesAndTypes); return namesAndTypes; } @@ -896,7 +896,7 @@ private static native final void nativeGetTopicNamesAndTypes( long handle, Collection namesAndTypes); public final Collection getServiceNamesAndTypes() { - Collection namesAndTypes = new ArrayList(); + Collection namesAndTypes = new ArrayList(); nativeGetServiceNamesAndTypes(this.handle, namesAndTypes); return namesAndTypes; } @@ -905,7 +905,7 @@ private static native final void nativeGetServiceNamesAndTypes( long handle, Collection namesAndTypes); public final Collection getPublishersInfo(final String topicName) { - ArrayList returnValue = new ArrayList(); + ArrayList returnValue = new ArrayList(); nativeGetPublishersInfo(this.handle, topicName, returnValue); return returnValue; } @@ -914,7 +914,7 @@ 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(); + ArrayList returnValue = new ArrayList(); nativeGetSubscriptionsInfo(this.handle, topicName, returnValue); return returnValue; } @@ -925,7 +925,7 @@ private native static final void nativeGetSubscriptionsInfo( public final Collection getPublisherNamesAndTypesByNode( String nodeName, String nodeNamespace) { - Collection namesAndTypes = new ArrayList(); + Collection namesAndTypes = new ArrayList(); nativeGetPublisherNamesAndTypesByNode(this.handle, nodeName, nodeNamespace, namesAndTypes); return namesAndTypes; } @@ -936,7 +936,7 @@ private static native final Collection nativeGetPublisherNamesAndT public final Collection getSubscriptionNamesAndTypesByNode( String nodeName, String nodeNamespace) { - Collection namesAndTypes = new ArrayList(); + Collection namesAndTypes = new ArrayList(); nativeGetSubscriptionNamesAndTypesByNode(this.handle, nodeName, nodeNamespace, namesAndTypes); return namesAndTypes; } @@ -947,7 +947,7 @@ private static native final Collection nativeGetSubscriptionNamesA public final Collection getServiceNamesAndTypesByNode( String nodeName, String nodeNamespace) { - Collection namesAndTypes = new ArrayList(); + Collection namesAndTypes = new ArrayList(); nativeGetServiceNamesAndTypesByNode(this.handle, nodeName, nodeNamespace, namesAndTypes); return namesAndTypes; } @@ -958,7 +958,7 @@ private static native final Collection nativeGetServiceNamesAndTyp public final Collection getClientNamesAndTypesByNode( String nodeName, String nodeNamespace) { - Collection namesAndTypes = new ArrayList(); + Collection namesAndTypes = new ArrayList(); nativeGetClientNamesAndTypesByNode(this.handle, nodeName, nodeNamespace, namesAndTypes); return namesAndTypes; } diff --git a/rcljava/src/main/java/org/ros2/rcljava/publisher/PublisherImpl.java b/rcljava/src/main/java/org/ros2/rcljava/publisher/PublisherImpl.java index 7911916b..1bd493fa 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/publisher/PublisherImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/publisher/PublisherImpl.java @@ -117,8 +117,8 @@ public final WeakReference getNodeReference() { public final EventHandler createEventHandler(Supplier factory, Consumer callback) { - final WeakReference> weakEventHandlers = new WeakReference( - this.eventHandlers); + final WeakReference> weakEventHandlers = + new WeakReference>(this.eventHandlers); Consumer disposeCallback = new Consumer() { public void accept(EventHandler eventHandler) { Collection eventHandlers = weakEventHandlers.get(); @@ -129,7 +129,7 @@ public void accept(EventHandler eventHandler) { }; T status = factory.get(); long eventHandle = nativeCreateEvent(this.handle, status.getPublisherEventType()); - EventHandler eventHandler = new EventHandlerImpl( + EventHandler eventHandler = new EventHandlerImpl( new WeakReference(this), eventHandle, factory, callback, disposeCallback); this.eventHandlers.add(eventHandler); return eventHandler; diff --git a/rcljava/src/main/java/org/ros2/rcljava/subscription/SubscriptionImpl.java b/rcljava/src/main/java/org/ros2/rcljava/subscription/SubscriptionImpl.java index 7a969af0..34c3e3cf 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/subscription/SubscriptionImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/subscription/SubscriptionImpl.java @@ -129,8 +129,8 @@ public final WeakReference getNodeReference() { public final EventHandler createEventHandler(Supplier factory, Consumer callback) { - final WeakReference> weakEventHandlers = new WeakReference( - this.eventHandlers); + final WeakReference> weakEventHandlers = + new WeakReference>(this.eventHandlers); Consumer disposeCallback = new Consumer() { public void accept(EventHandler eventHandler) { Collection eventHandlers = weakEventHandlers.get(); @@ -141,8 +141,9 @@ public void accept(EventHandler eventHandler) { }; T status = factory.get(); long eventHandle = nativeCreateEvent(this.handle, status.getSubscriptionEventType()); - EventHandler eventHandler = new EventHandlerImpl( - new WeakReference(this), eventHandle, factory, callback, disposeCallback); + EventHandler eventHandler = + new EventHandlerImpl( + new WeakReference(this), eventHandle, factory, callback, disposeCallback); this.eventHandlers.add(eventHandler); return eventHandler; } diff --git a/rcljava/src/test/java/org/ros2/rcljava/client/ClientTest.java b/rcljava/src/test/java/org/ros2/rcljava/client/ClientTest.java index b371c860..49b3ac6e 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/client/ClientTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/client/ClientTest.java @@ -54,7 +54,7 @@ public static void setupOnce() throws Exception { { // Configure log4j. Doing this dynamically so that Android does not complain about missing // the log4j JARs, SLF4J uses Android's native logging mechanism instead. - Class c = Class.forName("org.apache.log4j.BasicConfigurator"); + Class c = Class.forName("org.apache.log4j.BasicConfigurator"); Method m = c.getDeclaredMethod("configure", (Class[]) null); Object o = m.invoke(null, (Object[]) null); } 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 9b39e79d..e1c84690 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java @@ -109,7 +109,7 @@ public static void setupOnce() throws Exception { { // Configure log4j. Doing this dynamically so that Android does not complain about missing // the log4j JARs, SLF4J uses Android's native logging mechanism instead. - Class c = Class.forName("org.apache.log4j.BasicConfigurator"); + Class c = Class.forName("org.apache.log4j.BasicConfigurator"); Method m = c.getDeclaredMethod("configure", (Class[]) null); Object o = m.invoke(null, (Object[]) null); } @@ -974,19 +974,19 @@ public void accept(final Collection namesAndTypes) { namesAndTypes.contains( new NameAndTypes( "/test_get_topic_names_and_types_one", - new ArrayList(Arrays.asList("rcljava/msg/UInt32"))))); + Arrays.asList("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"))))); + 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"))))); + Arrays.asList("rcljava/msg/Empty")))); } }; @@ -1043,13 +1043,13 @@ public void accept(final Collection namesAndTypes) { namesAndTypes.contains( new NameAndTypes( "/test_service_names_and_types_one", - new ArrayList(Arrays.asList("rcljava/srv/AddTwoInts"))))); + 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"))))); + Arrays.asList("rcljava/srv/AddTwoInts")))); } }; @@ -1217,13 +1217,13 @@ public void accept(final Collection local, Collection local, Collection local, Collection local, Collection local, Collection local, Collection local, Collection local, Collection c = Class.forName("org.apache.log4j.BasicConfigurator"); Method m = c.getDeclaredMethod("configure", (Class[]) null); Object o = m.invoke(null, (Object[]) null); } @@ -115,7 +115,9 @@ public final void testSetParameters() throws Exception { RCLFuture> future = new RCLFuture>(); - parametersClient.setParameters(parameters, new TestConsumer(future)); + parametersClient.setParameters( + parameters, + new TestConsumer>(future)); RCLJava.spinUntilComplete(node, future); List setParametersResults = future.get(); @@ -143,7 +145,9 @@ public final void testGetParameters() throws Exception { Arrays.asList(new String[] {"foo", "bar", "baz", "foo.first", "foo.second", "foobar"}); RCLFuture> future = new RCLFuture>(); - parametersClient.getParameters(parameterNames, new TestConsumer(future)); + parametersClient.getParameters( + parameterNames, + new TestConsumer>(future)); RCLJava.spinUntilComplete(node, future); assertEquals(parameters, future.get()); @@ -159,9 +163,12 @@ public final void testListParameters() throws Exception { node.setParameters(parameters); RCLFuture future = - new RCLFuture(); + new RCLFuture(); parametersClient.listParameters( - Arrays.asList(new String[] {"foo", "bar"}), 10, new TestConsumer(future)); + Arrays.asList( + new String[] {"foo", "bar"}), + 10, + new TestConsumer(future)); RCLJava.spinUntilComplete(node, future); assertArrayEquals(new String[] {"foo.first", "foo.second"}, future.get().getNames()); @@ -180,7 +187,8 @@ public final void testDescribeParameters() throws Exception { RCLFuture> future = new RCLFuture>(); parametersClient.describeParameters( - Arrays.asList(new String[] {"foo", "bar"}), new TestConsumer(future)); + Arrays.asList(new String[] {"foo", "bar"}), + new TestConsumer>(future)); List expected = Arrays.asList(new rcl_interfaces.msg.ParameterDescriptor[] { diff --git a/rcljava/src/test/java/org/ros2/rcljava/parameters/SyncParametersClientTest.java b/rcljava/src/test/java/org/ros2/rcljava/parameters/SyncParametersClientTest.java index 21d91688..89b3c346 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/parameters/SyncParametersClientTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/parameters/SyncParametersClientTest.java @@ -59,7 +59,7 @@ public static void setupOnce() throws Exception { { // Configure log4j. Doing this dynamically so that Android does not complain about missing // the log4j JARs, SLF4J uses Android's native logging mechanism instead. - Class c = Class.forName("org.apache.log4j.BasicConfigurator"); + Class c = Class.forName("org.apache.log4j.BasicConfigurator"); Method m = c.getDeclaredMethod("configure", (Class[]) null); Object o = m.invoke(null, (Object[]) null); } diff --git a/rcljava/src/test/java/org/ros2/rcljava/time/TimeSourceTest.java b/rcljava/src/test/java/org/ros2/rcljava/time/TimeSourceTest.java index 84958b86..86bbddff 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/time/TimeSourceTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/time/TimeSourceTest.java @@ -15,6 +15,8 @@ package org.ros2.rcljava.time; +import java.lang.SuppressWarnings; + import static org.junit.Assert.assertTrue; import static org.junit.Assert.assertFalse; @@ -162,6 +164,8 @@ public final void testSetRosTimeIsActiveNoNode() { assertTrue(timeSource.getRosTimeIsActive()); } + // there's no generic parameter type information at runtime, so we cannot write any(Consumer.class) + @SuppressWarnings("unchecked") @Test public final void testSetRosTimeIsActiveWithNode() { when(mockedNode.getParameter("use_sim_time")).thenReturn(new ParameterVariant("use_sim_time", false));