diff --git a/rcljava/CMakeLists.txt b/rcljava/CMakeLists.txt index 6c278879..b2d0943f 100644 --- a/rcljava/CMakeLists.txt +++ b/rcljava/CMakeLists.txt @@ -61,6 +61,7 @@ set(${PROJECT_NAME}_jni_sources "src/main/cpp/org_ros2_rcljava_executors_BaseExecutor.cpp" "src/main/cpp/org_ros2_rcljava_events_EventHandlerImpl.cpp" "src/main/cpp/org_ros2_rcljava_events_publisher_statuses_LivelinessLost.cpp" + "src/main/cpp/org_ros2_rcljava_events_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_service_ServiceImpl.cpp" @@ -133,6 +134,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/events/publisher_statuses/LivelinessLost.java" + "src/main/java/org/ros2/rcljava/events/publisher_statuses/OfferedQosIncompatible.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" diff --git a/rcljava/include/org_ros2_rcljava_events_publisher_statuses_OfferedQosIncompatible.h b/rcljava/include/org_ros2_rcljava_events_publisher_statuses_OfferedQosIncompatible.h new file mode 100644 index 00000000..0c17d341 --- /dev/null +++ b/rcljava/include/org_ros2_rcljava_events_publisher_statuses_OfferedQosIncompatible.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_events_publisher_statuses_OfferedQosIncompatible */ + +#ifndef ORG_ROS2_RCLJAVA_EVENTS_PUBLISHER_STATUSES_OFFEREDQOSINCOMPATIBLE_H_ +#define ORG_ROS2_RCLJAVA_EVENTS_PUBLISHER_STATUSES_OFFEREDQOSINCOMPATIBLE_H_ +#ifdef __cplusplus +extern "C" { +#endif + +/* + * Class: org_ros2_rcljava_events_publisher_statuses_OfferedQosIncompatible + * Method: nAllocateRCLStatusEvent + * Signature: ()J + */ +JNIEXPORT jlong JNICALL +Java_org_ros2_rcljava_events_publisher_1statuses_OfferedQosIncompatible_nAllocateRCLStatusEvent( + JNIEnv *, jclass); + +/* + * Class: org_ros2_rcljava_events_publisher_statuses_OfferedQosIncompatible + * Method: nDeallocateRCLStatusEvent + * Signature: (J)V + */ +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_events_publisher_1statuses_OfferedQosIncompatible_nDeallocateRCLStatusEvent( + JNIEnv *, jclass, jlong); + +/* + * Class: org_ros2_rcljava_events_publisher_statuses_OfferedQosIncompatible + * Method: nFromRCLEvent + * Signature: (J)V + */ +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_events_publisher_1statuses_OfferedQosIncompatible_nFromRCLEvent( + JNIEnv *, jobject, jlong); + +/* + * Class: org_ros2_rcljava_events_publisher_statuses_OfferedQosIncompatible + * Method: nGetPublisherEventType + * Signature: ()I + */ +JNIEXPORT jint JNICALL +Java_org_ros2_rcljava_events_publisher_1statuses_OfferedQosIncompatible_nGetPublisherEventType( + JNIEnv *, jclass); + +#ifdef __cplusplus +} +#endif +#endif // ORG_ROS2_RCLJAVA_EVENTS_PUBLISHER_STATUSES_OFFEREDQOSINCOMPATIBLE_H_ diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_events_publisher_statuses_LivelinessLost.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_events_publisher_statuses_LivelinessLost.cpp index 95bad689..1b658a28 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_events_publisher_statuses_LivelinessLost.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_events_publisher_statuses_LivelinessLost.cpp @@ -17,7 +17,7 @@ #include #include -#include "rmw/events_statuses/liveliness_lost.h" +#include "rmw/types.h" #include "rcl/event.h" #include "rcljava_common/exceptions.hpp" @@ -53,11 +53,11 @@ Java_org_ros2_rcljava_events_publisher_1statuses_LivelinessLost_nativeFromRCLEve } // TODO(ivanpauno): class and field lookup could be done at startup time jclass clazz = env->GetObjectClass(self); - jfieldID total_count_fid = env->GetFieldID(clazz, "total_count", "I"); + jfieldID total_count_fid = env->GetFieldID(clazz, "totalCount", "I"); if (env->ExceptionCheck()) { return; } - jfieldID total_count_change_fid = env->GetFieldID(clazz, "total_count_change", "I"); + jfieldID total_count_change_fid = env->GetFieldID(clazz, "totalCountChange", "I"); if (env->ExceptionCheck()) { return; } diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_events_publisher_statuses_OfferedQosIncompatible.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_events_publisher_statuses_OfferedQosIncompatible.cpp new file mode 100644 index 00000000..ed3f4fcd --- /dev/null +++ b/rcljava/src/main/cpp/org_ros2_rcljava_events_publisher_statuses_OfferedQosIncompatible.cpp @@ -0,0 +1,120 @@ +// 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_events_publisher_statuses_OfferedQosIncompatible.h" + +#include +#include + +#include "rmw/incompatible_qos_events_statuses.h" +#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_events_publisher_1statuses_OfferedQosIncompatible_nAllocateRCLStatusEvent( + JNIEnv * env, jclass) +{ + void * p = malloc(sizeof(rmw_offered_qos_incompatible_event_status_t)); + if (!p) { + rcljava_throw_exception( + env, "java/lang/OutOfMemoryError", "failed to allocate offered qos incompatible status"); + } + return reinterpret_cast(p); +} + +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_events_publisher_1statuses_OfferedQosIncompatible_nDeallocateRCLStatusEvent( + JNIEnv *, jclass, jlong handle) +{ + free(reinterpret_cast(handle)); +} + +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_events_publisher_1statuses_OfferedQosIncompatible_nFromRCLEvent( + 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"); + } + // TODO(ivanpauno): class and field lookup could be done at startup time + jclass clazz = env->GetObjectClass(self); + jclass qos_kind_clazz = env->FindClass( + "org/ros2/rcljava/events/publisher_statuses/OfferedQosIncompatible$PolicyKind"); + if (env->ExceptionCheck()) { + return; + } + 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; + } + const char * enum_class_path = + "Lorg/ros2/rcljava/events/publisher_statuses/OfferedQosIncompatible$PolicyKind;"; + jfieldID policy_kind_fid = env->GetFieldID(clazz, "lastPolicyKind", enum_class_path); + if (env->ExceptionCheck()) { + return; + } + + jfieldID enum_value_fid; + switch (p->last_policy_kind) { + case RMW_QOS_POLICY_INVALID: + enum_value_fid = env->GetStaticFieldID(qos_kind_clazz, "INVALID", enum_class_path); + break; + case RMW_QOS_POLICY_DURABILITY: + enum_value_fid = env->GetStaticFieldID(qos_kind_clazz, "DURABILITY", enum_class_path); + break; + case RMW_QOS_POLICY_DEADLINE: + enum_value_fid = env->GetStaticFieldID(qos_kind_clazz, "DEADLINE", enum_class_path); + break; + case RMW_QOS_POLICY_LIVELINESS: + enum_value_fid = env->GetStaticFieldID(qos_kind_clazz, "LIVELINESS", enum_class_path); + break; + case RMW_QOS_POLICY_RELIABILITY: + enum_value_fid = env->GetStaticFieldID(qos_kind_clazz, "RELIABILITY", enum_class_path); + break; + case RMW_QOS_POLICY_HISTORY: + enum_value_fid = env->GetStaticFieldID(qos_kind_clazz, "HISTORY", enum_class_path); + break; + case RMW_QOS_POLICY_LIFESPAN: + enum_value_fid = env->GetStaticFieldID(qos_kind_clazz, "LIFESPAN", enum_class_path); + break; + default: + rcljava_throw_exception( + env, "java/lang/IllegalStateException", "unknown rmw qos policy kind"); + break; + } + if (env->ExceptionCheck()) { + return; + } + jobject enum_value = env->GetStaticObjectField(qos_kind_clazz, enum_value_fid); + + env->SetIntField(self, total_count_fid, p->total_count); + env->SetIntField(self, total_count_change_fid, p->total_count_change); + env->SetObjectField(self, policy_kind_fid, enum_value); +} + +JNIEXPORT jint JNICALL +Java_org_ros2_rcljava_events_publisher_1statuses_OfferedQosIncompatible_nGetPublisherEventType( + JNIEnv *, jclass) +{ + return RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS; +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/events/publisher_statuses/LivelinessLost.java b/rcljava/src/main/java/org/ros2/rcljava/events/publisher_statuses/LivelinessLost.java index 615126be..003e3ff5 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/events/publisher_statuses/LivelinessLost.java +++ b/rcljava/src/main/java/org/ros2/rcljava/events/publisher_statuses/LivelinessLost.java @@ -26,8 +26,8 @@ * This class serves as a bridge between a rmw_liveliness_lost_status_t and RCLJava. */ public class LivelinessLost implements PublisherEventStatus { - int total_count; - int total_count_change; + public int totalCount; + public int totalCountChange; public final long allocateRCLStatusEvent() { return nativeAllocateRCLStatusEvent(); diff --git a/rcljava/src/main/java/org/ros2/rcljava/events/publisher_statuses/OfferedQosIncompatible.java b/rcljava/src/main/java/org/ros2/rcljava/events/publisher_statuses/OfferedQosIncompatible.java new file mode 100644 index 00000000..9b506703 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/events/publisher_statuses/OfferedQosIncompatible.java @@ -0,0 +1,76 @@ +// 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.events.publisher_statuses; + +import java.util.function.Supplier; + +import org.ros2.rcljava.common.JNIUtils; +import org.ros2.rcljava.events.PublisherEventStatus; + +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +/** + * This class serves as a bridge between a rmw_qos_incompatible_event_status_t and RCLJava. + */ +public class OfferedQosIncompatible implements PublisherEventStatus { + public int totalCount; + public int totalCountChange; + public PolicyKind lastPolicyKind; + + public enum PolicyKind { + INVALID, + DURABILITY, + DEADLINE, + LIVELINESS, + RELIABILITY, + HISTORY, + LIFESPAN; + } + + public final long allocateRCLStatusEvent() { + return nAllocateRCLStatusEvent(); + } + public final void deallocateRCLStatusEvent(long handle) { + nDeallocateRCLStatusEvent(handle); + } + public final void fromRCLEvent(long handle) { + nFromRCLEvent(handle); + } + public final int getPublisherEventType() { + return nGetPublisherEventType(); + } + // TODO(ivanpauno): Remove this when -source 8 can be used (method references for the win) + public static final Supplier factory = new Supplier() { + public OfferedQosIncompatible get() { + return new OfferedQosIncompatible(); + } + }; + + private static final Logger logger = LoggerFactory.getLogger(OfferedQosIncompatible.class); + static { + try { + JNIUtils.loadImplementation(OfferedQosIncompatible.class); + } catch (UnsatisfiedLinkError ule) { + logger.error("Native code library failed to load.\n" + ule); + System.exit(1); + } + } + + private static native long nAllocateRCLStatusEvent(); + private static native void nDeallocateRCLStatusEvent(long handle); + private native void nFromRCLEvent(long handle); + private static native int nGetPublisherEventType(); +} diff --git a/rcljava/src/test/java/org/ros2/rcljava/SpinTest.java b/rcljava/src/test/java/org/ros2/rcljava/SpinTest.java index f015d260..c7273fbe 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/SpinTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/SpinTest.java @@ -18,6 +18,7 @@ import static org.junit.Assert.assertEquals; import static org.junit.Assert.assertNotEquals; +import java.lang.System; import java.util.concurrent.TimeUnit; import org.junit.AfterClass; @@ -26,11 +27,20 @@ import org.ros2.rcljava.RCLJava; import org.ros2.rcljava.concurrent.Callback; +import org.ros2.rcljava.consumers.Consumer; +import org.ros2.rcljava.events.EventHandler; +import org.ros2.rcljava.events.publisher_statuses.OfferedQosIncompatible; import org.ros2.rcljava.executors.Executor; 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.publisher.Publisher; +import org.ros2.rcljava.qos.policies.Durability; +import org.ros2.rcljava.qos.policies.History; +import org.ros2.rcljava.qos.policies.Reliability; +import org.ros2.rcljava.qos.QoSProfile; +import org.ros2.rcljava.subscription.Subscription; public class SpinTest { public static class TimerCallback implements Callback { @@ -333,4 +343,61 @@ public Node getNode() { executor.spinAll(100*1000*1000); assertEquals(1, timerCallback.getCounter()); } + + // custom event consumer + public static class EventConsumer implements Consumer { + public boolean done = false; + + public void accept(final OfferedQosIncompatible status) { + assertEquals(status.totalCount, 1); + assertEquals(status.totalCountChange, 1); + assertEquals(status.lastPolicyKind, OfferedQosIncompatible.PolicyKind.RELIABILITY); + this.done = true; + } + } + + @Test + public final void testSpinEvent() { + String identifier = RCLJava.getRMWIdentifier(); + if (identifier.equals("rmw_fastrtps_cpp") || identifier.equals("rmw_fastrtps_dynamic_cpp")) { + // OfferedQosIncompatible event not supported in these implementations + return; + } + final Node node = RCLJava.createNode("test_node_spin_event"); + Publisher publisher = node.createPublisher( + std_msgs.msg.String.class, "test_topic_spin_event", new QoSProfile( + History.KEEP_LAST, 1, + Reliability.BEST_EFFORT, + Durability.VOLATILE, + false)); + // create a OfferedQoSIncompatible event handler with custom event consumer + EventConsumer eventConsumer = new EventConsumer(); + EventHandler eventHandler = publisher.createEventHandler( + OfferedQosIncompatible.factory, eventConsumer + ); + // create an incompatible subscription (reliable vs best effort publisher) + Subscription subscription = node.createSubscription( + std_msgs.msg.String.class, "test_topic_spin_event", + new Consumer() { + public void accept(final std_msgs.msg.String msg) {} + }, + new QoSProfile( + History.KEEP_LAST, 1, + Reliability.RELIABLE, + Durability.VOLATILE, + false)); + // set up executor + ComposableNode composableNode = new ComposableNode() { + public Node getNode() { + return node; + } + }; + Executor executor = new SingleThreadedExecutor(); + executor.addNode(composableNode); + long start = System.currentTimeMillis(); + do { + executor.spinAll((1000 + System.currentTimeMillis() - start) * 1000 * 1000); + } while (!eventConsumer.done && System.currentTimeMillis() < start + 1000); + assert(eventConsumer.done); + } } diff --git a/rcljava/src/test/java/org/ros2/rcljava/publisher/PublisherTest.java b/rcljava/src/test/java/org/ros2/rcljava/publisher/PublisherTest.java index 472d8ea0..3ef726e8 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/publisher/PublisherTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/publisher/PublisherTest.java @@ -25,6 +25,8 @@ import org.ros2.rcljava.consumers.Consumer; import org.ros2.rcljava.events.EventHandler; import org.ros2.rcljava.events.publisher_statuses.LivelinessLost; +import org.ros2.rcljava.events.publisher_statuses.OfferedQosIncompatible; +import org.ros2.rcljava.exceptions.RCLException; import org.ros2.rcljava.node.Node; public class PublisherTest { @@ -54,10 +56,42 @@ public final void testCreateLivelinessLostEvent() { node.createPublisher(std_msgs.msg.String.class, "test_topic"); EventHandler eventHandler = publisher.createEventHandler( LivelinessLost.factory, new Consumer() { - public void accept(final LivelinessLost status) {} + public void accept(final LivelinessLost 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()); + } + + @Test + public final void testCreateOfferedQosIncompatibleEvent() { + 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"); + Publisher publisher = + node.createPublisher(std_msgs.msg.String.class, "test_topic"); + EventHandler eventHandler = publisher.createEventHandler( + OfferedQosIncompatible.factory, new Consumer() { + public void accept(final OfferedQosIncompatible status) { + assertEquals(status.totalCount, 0); + assertEquals(status.totalCountChange, 0); + assertEquals(status.lastPolicyKind, OfferedQosIncompatible.PolicyKind.INVALID); + } + } + ); + assertNotEquals(0, eventHandler.getHandle()); + // force executing the callback, so we check that taking an event works + eventHandler.executeCallback(); RCLJava.shutdown(); assertEquals(0, eventHandler.getHandle()); }