From 8826dc9ae08708683e5bf2cf2052ebd9fa060983 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 5 Aug 2020 21:58:04 +0000 Subject: [PATCH 1/3] Start revamping Time and Clock classes. Signed-off-by: Chris Lalancette --- rcljava/CMakeLists.txt | 15 ++ rcljava/include/org_ros2_rcljava_time_Clock.h | 34 ++++ rcljava/package.xml | 2 + .../main/cpp/org_ros2_rcljava_time_Clock.cpp | 85 ++++++++- .../src/main/java/org/ros2/rcljava/Time.java | 63 +++---- .../java/org/ros2/rcljava/node/NodeImpl.java | 7 +- .../java/org/ros2/rcljava/time/Clock.java | 40 ++++- .../org/ros2/rcljava/time/TimeSource.java | 161 ++++++++++++++++++ .../test/java/org/ros2/rcljava/TimeTest.java | 65 +++---- .../ros2/rcljava/node/NodeParametersTest.java | 3 +- 10 files changed, 385 insertions(+), 90 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 dda754a0..7b31d515 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) @@ -97,6 +98,7 @@ foreach(_jni_source ${${PROJECT_NAME}_jni_sources}) "rcljava_common" "builtin_interfaces" "rcl_interfaces" + "rosgraph_msgs" ) target_include_directories(${_target_name} @@ -163,6 +165,7 @@ set(${PROJECT_NAME}_sources "src/main/java/org/ros2/rcljava/subscription/SubscriptionImpl.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" @@ -176,6 +179,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") @@ -213,6 +217,7 @@ if(BUILD_TESTING) DEPENDENCIES builtin_interfaces rcl_interfaces + rosgraph_msgs ${_java_type_supports} SKIP_INSTALL ) @@ -293,6 +298,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/) @@ -313,6 +327,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 9f7e58d1..201eeb83 100644 --- a/rcljava/package.xml +++ b/rcljava/package.xml @@ -17,6 +17,7 @@ rcl rmw_implementation_cmake rmw + rosgraph_msgs rosidl_generator_c rosidl_typesupport_c builtin_interfaces @@ -31,6 +32,7 @@ rcl 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..ebde4744 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/Time.java +++ b/rcljava/src/main/java/org/ros2/rcljava/Time.java @@ -23,7 +23,7 @@ import org.ros2.rcljava.common.JNIUtils; import org.ros2.rcljava.time.ClockType; -public final class Time { +public class Time { private static final Logger logger = LoggerFactory.getLogger(Time.class); static { @@ -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 0189e302..2525fef6 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java @@ -42,6 +42,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; @@ -96,6 +97,8 @@ public class NodeImpl implements Node { */ private Clock clock; + private TimeSource timeSource; + /** * All the @{link Subscription}s that have been created through this instance. */ @@ -141,7 +144,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(); @@ -154,6 +156,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..eca88145 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); @@ -52,6 +53,10 @@ public class Clock implements Disposable { */ private static native long nativeCreateClockHandle(ClockType clockType); + public Clock() { + this(ClockType.SYSTEM_TIME); + } + /** * Constructor. * @@ -62,6 +67,23 @@ public Clock(ClockType clockType) { this.handle = Clock.nativeCreateClockHandle(clockType); } + public Time now() { + long nanos = nativeGetNow(this.handle); + return new Time(0, nanos, this.clockType); + } + + public boolean getRosTimeIsActive() { + return nativeRosTimeOverrideEnabled(this.handle); + } + + public void setRosTimeIsActive(boolean enabled) { + nativeSetRosTimeOverrideEnabled(this.handle, enabled); + } + + public void setRosTimeOverride(Time time) { + nativeSetRosTimeOverride(this.handle, time.nanoseconds()); + } + /** * @return The clock type. */ @@ -69,6 +91,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). * 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..f45990a9 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/time/TimeSource.java @@ -0,0 +1,161 @@ +/* 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 java.util.ArrayList; +import java.util.List; + +public class TimeSource { + private Node node; + private boolean rosTimeIsActive; + private ArrayList associatedClocks; + private Subscription clockSub; + private Time lastTimeSet; + private ParameterCallback simTimeCB; + + public TimeSource() { + this(null); + } + + public TimeSource(Node node) { + this.rosTimeIsActive = false; + this.associatedClocks = new ArrayList(); + 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(); + } + // TODO(clalancette): what to do if it is the wrong type? + } + + 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); + } +} diff --git a/rcljava/src/test/java/org/ros2/rcljava/TimeTest.java b/rcljava/src/test/java/org/ros2/rcljava/TimeTest.java index 5e8abf32..f84d7ae0 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/TimeTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/TimeTest.java @@ -16,9 +16,6 @@ package org.ros2.rcljava; import static org.junit.Assert.assertEquals; -import static org.junit.Assert.fail; - -import java.util.concurrent.TimeUnit; import org.junit.BeforeClass; import org.junit.Test; @@ -33,50 +30,34 @@ public static void setupOnce() throws Exception { org.apache.log4j.BasicConfigurator.configure(); } - // @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 353eabda..0e3679bc 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/node/NodeParametersTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/node/NodeParametersTest.java @@ -337,7 +337,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 63bf5829dce37ff2c7fc8404c16e3bfc0c2d4aa6 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 11 Aug 2020 18:39:35 +0000 Subject: [PATCH 2/3] Fixes from review. Signed-off-by: Chris Lalancette --- rcljava/package.xml | 1 + .../src/main/java/org/ros2/rcljava/Time.java | 2 +- .../java/org/ros2/rcljava/time/Clock.java | 24 ++++++++++++++----- .../org/ros2/rcljava/time/TimeSource.java | 12 +++++++--- 4 files changed, 29 insertions(+), 10 deletions(-) diff --git a/rcljava/package.xml b/rcljava/package.xml index 201eeb83..9a7bcbb4 100644 --- a/rcljava/package.xml +++ b/rcljava/package.xml @@ -23,6 +23,7 @@ builtin_interfaces rcl_interfaces rmw + rosgraph_msgs rosidl_runtime_c rosidl_generator_java rosidl_typesupport_c diff --git a/rcljava/src/main/java/org/ros2/rcljava/Time.java b/rcljava/src/main/java/org/ros2/rcljava/Time.java index ebde4744..f5b864e0 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/Time.java +++ b/rcljava/src/main/java/org/ros2/rcljava/Time.java @@ -23,7 +23,7 @@ import org.ros2.rcljava.common.JNIUtils; import org.ros2.rcljava.time.ClockType; -public class Time { +public final class Time { private static final Logger logger = LoggerFactory.getLogger(Time.class); static { 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 eca88145..f4428121 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/time/Clock.java +++ b/rcljava/src/main/java/org/ros2/rcljava/time/Clock.java @@ -45,6 +45,8 @@ public class Clock implements Disposable { */ private final ClockType clockType; + private Object clockHandleMutex; + /** * Create an RCL clock (rcl_clock_t). * @@ -65,23 +67,31 @@ public Clock() { public Clock(ClockType clockType) { this.clockType = clockType; this.handle = Clock.nativeCreateClockHandle(clockType); + this.clockHandleMutex = new Object(); } public Time now() { - long nanos = nativeGetNow(this.handle); - return new Time(0, nanos, this.clockType); + synchronized(this.clockHandleMutex) { + return new Time(0, nativeGetNow(this.handle), this.clockType); + } } public boolean getRosTimeIsActive() { - return nativeRosTimeOverrideEnabled(this.handle); + synchronized(this.clockHandleMutex) { + return nativeRosTimeOverrideEnabled(this.handle); + } } public void setRosTimeIsActive(boolean enabled) { - nativeSetRosTimeOverrideEnabled(this.handle, enabled); + synchronized(this.clockHandleMutex) { + nativeSetRosTimeOverrideEnabled(this.handle, enabled); + } } public void setRosTimeOverride(Time time) { - nativeSetRosTimeOverride(this.handle, time.nanoseconds()); + synchronized(this.clockHandleMutex) { + nativeSetRosTimeOverride(this.handle, time.nanoseconds()); + } } /** @@ -114,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 index f45990a9..232e8366 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/time/TimeSource.java +++ b/rcljava/src/main/java/org/ros2/rcljava/time/TimeSource.java @@ -25,13 +25,15 @@ import org.ros2.rcljava.time.ClockType; import org.ros2.rcljava.Time; -import java.util.ArrayList; +import java.util.Collection; import java.util.List; +import java.util.concurrent.LinkedBlockingQueue; + public class TimeSource { private Node node; private boolean rosTimeIsActive; - private ArrayList associatedClocks; + private final Collection associatedClocks; private Subscription clockSub; private Time lastTimeSet; private ParameterCallback simTimeCB; @@ -42,7 +44,7 @@ public TimeSource() { public TimeSource(Node node) { this.rosTimeIsActive = false; - this.associatedClocks = new ArrayList(); + this.associatedClocks = new LinkedBlockingQueue(); this.clockSub = null; this.lastTimeSet = new Time(0, 0, ClockType.ROS_TIME); if (node != null) { @@ -158,4 +160,8 @@ public void attachClock(Clock clock) { clock.setRosTimeIsActive(this.rosTimeIsActive); this.associatedClocks.add(clock); } + + public void detachClock(Clock clock) { + this.associatedClocks.remove(clock); + } } From fbda601c80e5f155367d4b333555c46f938e95b6 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 11 Aug 2020 18:59:08 +0000 Subject: [PATCH 3/3] Add in logging to the TimeSource class. Signed-off-by: Chris Lalancette --- .../main/java/org/ros2/rcljava/time/TimeSource.java | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) 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 232e8366..44318c2d 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/time/TimeSource.java +++ b/rcljava/src/main/java/org/ros2/rcljava/time/TimeSource.java @@ -25,12 +25,15 @@ 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 class TimeSource { +public final class TimeSource { private Node node; private boolean rosTimeIsActive; private final Collection associatedClocks; @@ -38,6 +41,8 @@ public class TimeSource { private Time lastTimeSet; private ParameterCallback simTimeCB; + private static final Logger logger = LoggerFactory.getLogger(TimeSource.class); + public TimeSource() { this(null); } @@ -108,8 +113,9 @@ public void attachNode(Node node) { 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"); } - // TODO(clalancette): what to do if it is the wrong type? } class SimTimeCB implements ParameterCallback {