diff --git a/rcljava/CMakeLists.txt b/rcljava/CMakeLists.txt index 6b65e4c8..ba8e6bcd 100644 --- a/rcljava/CMakeLists.txt +++ b/rcljava/CMakeLists.txt @@ -65,7 +65,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_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}) @@ -167,6 +167,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 f413ab04..f5570857 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/Node.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/Node.java @@ -172,9 +172,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 2525fef6..9980faab 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,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; @@ -97,6 +98,11 @@ public class NodeImpl implements Node { */ private Clock clock; + /** + * The clock used for creating wall timers. + */ + private Clock wall_clock; + private TimeSource timeSource; /** @@ -157,6 +163,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); } @@ -396,16 +403,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 f015d260..798c22ba 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/SpinTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/SpinTest.java @@ -30,7 +30,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; public class SpinTest { public static class TimerCallback implements Callback { @@ -74,7 +74,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() { @@ -94,7 +94,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() { @@ -114,7 +114,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() { @@ -134,7 +134,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() { @@ -162,7 +162,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() { @@ -190,8 +190,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()); @@ -220,8 +220,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()); @@ -250,7 +250,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() { @@ -279,8 +279,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()); @@ -309,8 +309,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 2500f5cf..fbad664e 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/timer/TimerTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/timer/TimerTest.java @@ -24,6 +24,7 @@ import java.util.concurrent.Future; import java.util.concurrent.TimeUnit; +import org.junit.AfterClass; import org.junit.BeforeClass; import org.junit.Test; @@ -60,32 +61,56 @@ public int getCounter() { 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 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()); } }