Revamp Time

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

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

rcljava/package.xml

@@ -17,11 +17,13 @@
   <build_depend>rcl</build_depend>
   <build_depend>rmw_implementation_cmake</build_depend>
   <build_depend>rmw</build_depend>
+  <build_depend>rosgraph_msgs</build_depend>
   <build_depend>rosidl_generator_c</build_depend>
   <build_depend>rosidl_typesupport_c</build_depend>
   <build_export_depend>builtin_interfaces</build_export_depend>
   <build_export_depend>rcl_interfaces</build_export_depend>
   <build_export_depend>rmw</build_export_depend>
+  <build_export_depend>rosgraph_msgs</build_export_depend>
   <build_export_depend>rosidl_runtime_c</build_export_depend>
   <build_export_depend>rosidl_generator_java</build_export_depend>
   <build_export_depend>rosidl_typesupport_c</build_export_depend>
@@ -31,6 +33,7 @@
   <exec_depend>rcl</exec_depend>
   <exec_depend>rmw_implementation_cmake</exec_depend>
   <exec_depend>rmw_implementation</exec_depend>
+  <exec_depend>rosgraph_msgs</exec_depend>
   <exec_depend>rosidl_runtime_c</exec_depend>
   <exec_depend>rosidl_parser</exec_depend>
 

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<jlong>(clock);
-  return clock_handle;
+  return reinterpret_cast<jlong>(clock);
+}
+
+JNIEXPORT jlong JNICALL
+Java_org_ros2_rcljava_time_Clock_nativeGetNow(JNIEnv * env, jclass, jlong clock_handle)
+{
+  rcl_clock_t * clock = reinterpret_cast<rcl_clock_t *>(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<jlong>(nanoseconds);
+}
+
+JNIEXPORT jboolean JNICALL
+Java_org_ros2_rcljava_time_Clock_nativeRosTimeOverrideEnabled(
+  JNIEnv * env, jclass, jlong clock_handle)
+{
+  rcl_clock_t * clock = reinterpret_cast<rcl_clock_t *>(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<rcl_clock_t *>(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<rcl_clock_t *>(clock_handle);
+
+  if (!rcl_clock_valid(clock)) {
+    return;
+  }
+
+  rcl_ret_t ret = rcl_set_ros_time_override(clock, static_cast<rcl_time_point_value_t>(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

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;
+  }
 }

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<Publisher>();
@@ -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<ParameterCallback>();
+    this.clock = new Clock(ClockType.ROS_TIME);
+    this.timeSource = new TimeSource(this);
+    this.timeSource.attachClock(this.clock);
   }
 
   /**