Skip to content

Commit 54ebfb3

Browse files
clalancettejacobperron
clalancette
authored and
jacobperron
committed
Revamp Time (osrf#6)
* Start revamping Time and Clock classes. Signed-off-by: Chris Lalancette <clalancette@openrobotics.org> * Fixes from review. Signed-off-by: Chris Lalancette <clalancette@openrobotics.org> * Add in logging to the TimeSource class. Signed-off-by: Chris Lalancette <clalancette@openrobotics.org>
1 parent 60ac293 commit 54ebfb3

File tree

10 files changed

+410
-89
lines changed

10 files changed

+410
-89
lines changed

‎rcljava/CMakeLists.txt‎

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@ find_package(rcl_interfaces REQUIRED)
1111
find_package(rcljava_common REQUIRED)
1212
find_package(rmw REQUIRED)
1313
find_package(rmw_implementation_cmake REQUIRED)
14+
find_package(rosgraph_msgs REQUIRED)
1415

1516
include(CrossCompilingExtra)
1617

@@ -108,6 +109,7 @@ foreach(_jni_source ${${PROJECT_NAME}_jni_sources})
108109
"rcljava_common"
109110
"builtin_interfaces"
110111
"rcl_interfaces"
112+
"rosgraph_msgs"
111113
)
112114

113115
target_include_directories(${_target_name}
@@ -191,6 +193,7 @@ set(${PROJECT_NAME}_sources
191193
"src/main/java/org/ros2/rcljava/subscription/statuses/RequestedQosIncompatible.java"
192194
"src/main/java/org/ros2/rcljava/time/Clock.java"
193195
"src/main/java/org/ros2/rcljava/time/ClockType.java"
196+
"src/main/java/org/ros2/rcljava/time/TimeSource.java"
194197
"src/main/java/org/ros2/rcljava/timer/Timer.java"
195198
"src/main/java/org/ros2/rcljava/timer/WallTimer.java"
196199
"src/main/java/org/ros2/rcljava/timer/WallTimerImpl.java"
@@ -204,6 +207,7 @@ add_jar("${PROJECT_NAME}_jar"
204207
${rcljava_common_JARS}
205208
${builtin_interfaces_JARS}
206209
${rcl_interfaces_JARS}
210+
${rosgraph_msgs_JARS}
207211
)
208212

209213
install_jar("${PROJECT_NAME}_jar" "share/${PROJECT_NAME}/java")
@@ -241,6 +245,7 @@ if(BUILD_TESTING)
241245
DEPENDENCIES
242246
builtin_interfaces
243247
rcl_interfaces
248+
rosgraph_msgs
244249
${_java_type_supports}
245250
SKIP_INSTALL
246251
)
@@ -323,6 +328,15 @@ if(BUILD_TESTING)
323328
list_append_unique(_deps_library_dirs ${_dep_dir})
324329
endforeach()
325330

331+
foreach(_dep_lib ${rosgraph_msgs_interfaces_LIBRARIES})
332+
get_filename_component(_dep_dir "${_dep_lib}" DIRECTORY)
333+
list_append_unique(_deps_library_dirs ${_dep_dir})
334+
endforeach()
335+
foreach(_dep_lib ${rosgraph_msgs_JNI_LIBRARIES})
336+
get_filename_component(_dep_dir "${_dep_lib}" DIRECTORY)
337+
list_append_unique(_deps_library_dirs ${_dep_dir})
338+
endforeach()
339+
326340
list_append_unique(_deps_library_dirs ${CMAKE_CURRENT_BINARY_DIR})
327341
list_append_unique(_deps_library_dirs ${CMAKE_CURRENT_BINARY_DIR}/rcljava)
328342
list_append_unique(_deps_library_dirs ${CMAKE_CURRENT_BINARY_DIR}/rosidl_generator_java/rcljava/msg/)
@@ -343,6 +357,7 @@ if(BUILD_TESTING)
343357
"${std_msgs_JARS}"
344358
"${builtin_interfaces_JARS}"
345359
"${rcl_interfaces_JARS}"
360+
"${rosgraph_msgs_JARS}"
346361
"${_${PROJECT_NAME}_jar_file}"
347362
"${_${PROJECT_NAME}_messages_jar_file}"
348363
APPEND_LIBRARY_DIRS

‎rcljava/include/org_ros2_rcljava_time_Clock.h‎

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,40 @@ extern "C" {
2828
JNIEXPORT jlong
2929
JNICALL Java_org_ros2_rcljava_time_Clock_nativeCreateClockHandle(JNIEnv *, jclass, jobject);
3030

31+
/*
32+
* Class: org_ros2_rcljava_time_Clock
33+
* Method: nativeGetNow
34+
* Signature: (J)J
35+
*/
36+
JNIEXPORT jlong
37+
JNICALL Java_org_ros2_rcljava_time_Clock_nativeGetNow(JNIEnv * env, jclass, jlong);
38+
39+
/*
40+
* Class: org_ros2_rcljava_time_Clock
41+
* Method: nativeRosTimeOverrideEnabled
42+
* Signature: (J)Z
43+
*/
44+
JNIEXPORT jboolean
45+
JNICALL Java_org_ros2_rcljava_time_Clock_nativeRosTimeOverrideEnabled(JNIEnv * env, jclass, jlong);
46+
47+
/*
48+
* Class: org_ros2_rcljava_time_Clock
49+
* Method: nativeSetRosTimeOverrideEnabled
50+
* Signature: (JZ)V
51+
*/
52+
JNIEXPORT void
53+
JNICALL Java_org_ros2_rcljava_time_Clock_nativeSetRosTimeOverrideEnabled(
54+
JNIEnv * env, jclass, jlong, jboolean);
55+
56+
/*
57+
* Class: org_ros2_rcljava_time_Clock
58+
* Method: nativeSetRosTimeOverride
59+
* Signature: (JJ)V
60+
*/
61+
JNIEXPORT void
62+
JNICALL Java_org_ros2_rcljava_time_Clock_nativeSetRosTimeOverride(
63+
JNIEnv * env, jclass, jlong, jlong);
64+
3165
/*
3266
* Class: org_ros2_rcljava_time_Clock
3367
* Method: nativeDispose

‎rcljava/package.xml‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -18,11 +18,13 @@
1818
<build_depend>rcpputils</build_depend>
1919
<build_depend>rmw_implementation_cmake</build_depend>
2020
<build_depend>rmw</build_depend>
21+
<build_depend>rosgraph_msgs</build_depend>
2122
<build_depend>rosidl_generator_c</build_depend>
2223
<build_depend>rosidl_typesupport_c</build_depend>
2324
<build_export_depend>builtin_interfaces</build_export_depend>
2425
<build_export_depend>rcl_interfaces</build_export_depend>
2526
<build_export_depend>rmw</build_export_depend>
27+
<build_export_depend>rosgraph_msgs</build_export_depend>
2628
<build_export_depend>rosidl_runtime_c</build_export_depend>
2729
<build_export_depend>rosidl_generator_java</build_export_depend>
2830
<build_export_depend>rosidl_typesupport_c</build_export_depend>
@@ -33,6 +35,7 @@
3335
<exec_depend>rcpputils</exec_depend>
3436
<exec_depend>rmw_implementation_cmake</exec_depend>
3537
<exec_depend>rmw_implementation</exec_depend>
38+
<exec_depend>rosgraph_msgs</exec_depend>
3639
<exec_depend>rosidl_runtime_c</exec_depend>
3740
<exec_depend>rosidl_parser</exec_depend>
3841

‎rcljava/src/main/cpp/org_ros2_rcljava_time_Clock.cpp‎

Lines changed: 83 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -62,8 +62,89 @@ Java_org_ros2_rcljava_time_Clock_nativeCreateClockHandle(JNIEnv * env, jclass, j
6262
return 0;
6363
}
6464

65-
jlong clock_handle = reinterpret_cast<jlong>(clock);
66-
return clock_handle;
65+
return reinterpret_cast<jlong>(clock);
66+
}
67+
68+
JNIEXPORT jlong JNICALL
69+
Java_org_ros2_rcljava_time_Clock_nativeGetNow(JNIEnv * env, jclass, jlong clock_handle)
70+
{
71+
rcl_clock_t * clock = reinterpret_cast<rcl_clock_t *>(clock_handle);
72+
rcl_time_point_value_t nanoseconds;
73+
rcl_ret_t ret = rcl_clock_get_now(clock, &nanoseconds);
74+
if (ret != RCL_RET_OK) {
75+
std::string msg = "Failed to get time: " + std::string(rcl_get_error_string().str);
76+
rcl_reset_error();
77+
rcljava_throw_rclexception(env, ret, msg);
78+
return 0;
79+
}
80+
81+
return static_cast<jlong>(nanoseconds);
82+
}
83+
84+
JNIEXPORT jboolean JNICALL
85+
Java_org_ros2_rcljava_time_Clock_nativeRosTimeOverrideEnabled(
86+
JNIEnv * env, jclass, jlong clock_handle)
87+
{
88+
rcl_clock_t * clock = reinterpret_cast<rcl_clock_t *>(clock_handle);
89+
90+
if (!rcl_clock_valid(clock)) {
91+
return false;
92+
}
93+
94+
bool is_enabled = false;
95+
rcl_ret_t ret = rcl_is_enabled_ros_time_override(clock, &is_enabled);
96+
if (ret != RCL_RET_OK) {
97+
std::string msg = "Failed to check ros_time_override_status: " +
98+
std::string(rcl_get_error_string().str);
99+
rcl_reset_error();
100+
rcljava_throw_rclexception(env, ret, msg);
101+
return false;
102+
}
103+
104+
return is_enabled;
105+
}
106+
107+
JNIEXPORT void JNICALL
108+
Java_org_ros2_rcljava_time_Clock_nativeSetRosTimeOverrideEnabled(
109+
JNIEnv * env, jclass, jlong clock_handle, jboolean enabled)
110+
{
111+
rcl_clock_t * clock = reinterpret_cast<rcl_clock_t *>(clock_handle);
112+
113+
if (!rcl_clock_valid(clock)) {
114+
return;
115+
}
116+
117+
rcl_ret_t ret;
118+
if (enabled) {
119+
ret = rcl_enable_ros_time_override(clock);
120+
} else {
121+
ret = rcl_disable_ros_time_override(clock);
122+
}
123+
if (ret != RCL_RET_OK) {
124+
std::string msg = "Failed to set ROS time override enable for clock: " +
125+
std::string(rcl_get_error_string().str);
126+
rcl_reset_error();
127+
rcljava_throw_rclexception(env, ret, msg);
128+
}
129+
}
130+
131+
JNIEXPORT void JNICALL
132+
Java_org_ros2_rcljava_time_Clock_nativeSetRosTimeOverride(
133+
JNIEnv * env, jclass, jlong clock_handle, jlong nanos)
134+
{
135+
rcl_clock_t * clock = reinterpret_cast<rcl_clock_t *>(clock_handle);
136+
137+
if (!rcl_clock_valid(clock)) {
138+
return;
139+
}
140+
141+
rcl_ret_t ret = rcl_set_ros_time_override(clock, static_cast<rcl_time_point_value_t>(nanos));
142+
if (ret != RCL_RET_OK) {
143+
std::string msg = "Failed to set ROS time override for clock: " +
144+
std::string(rcl_get_error_string().str);
145+
rcl_reset_error();
146+
rcljava_throw_rclexception(env, ret, msg);
147+
}
67148
}
68149

69150
JNIEXPORT void JNICALL

‎rcljava/src/main/java/org/ros2/rcljava/Time.java‎

Lines changed: 21 additions & 40 deletions
Original file line numberDiff line numberDiff line change
@@ -35,56 +35,37 @@ public final class Time {
3535
}
3636
}
3737

38-
/**
39-
* Private constructor so this cannot be instantiated.
40-
*/
41-
private Time() {}
38+
private final ClockType clockType;
4239

43-
public static builtin_interfaces.msg.Time now() {
44-
return Time.now(ClockType.SYSTEM_TIME);
45-
}
46-
47-
public static builtin_interfaces.msg.Time now(ClockType clockType) {
48-
long rclTime = 0;
49-
50-
switch (clockType) {
51-
case ROS_TIME:
52-
throw new UnsupportedOperationException("ROS Time is currently not implemented");
53-
case SYSTEM_TIME:
54-
rclTime = rclSystemTimeNow();
55-
break;
56-
case STEADY_TIME:
57-
rclTime = rclSteadyTimeNow();
58-
break;
59-
}
40+
private final long nanoseconds;
6041

61-
builtin_interfaces.msg.Time msgTime = new builtin_interfaces.msg.Time();
62-
msgTime.setSec((int) TimeUnit.SECONDS.convert(rclTime, TimeUnit.NANOSECONDS));
63-
msgTime.setNanosec((int) rclTime % (1000 * 1000 * 1000));
64-
return msgTime;
42+
public Time() {
43+
this(0, 0, ClockType.SYSTEM_TIME);
6544
}
6645

67-
public static long toNanoseconds(builtin_interfaces.msg.Time msgTime) {
68-
return TimeUnit.NANOSECONDS.convert(msgTime.getSec(), TimeUnit.SECONDS)
69-
+ (msgTime.getNanosec() & 0x00000000ffffffffL);
46+
public Time(final long nanos, final ClockType ct) {
47+
this(0, nanos, ct);
7048
}
7149

72-
public static long difference(
73-
builtin_interfaces.msg.Time msgTime1, builtin_interfaces.msg.Time msgTime2) {
74-
long difference =
75-
(Time.toNanoseconds(msgTime1) - Time.toNanoseconds(msgTime2)) & 0x00000000ffffffffL;
76-
return difference;
50+
public Time(final builtin_interfaces.msg.Time time_msg, final ClockType ct) {
51+
this(time_msg.getSec(), time_msg.getNanosec(), ct);
7752
}
7853

79-
private static long rclSystemTimeNow() {
80-
return nativeRCLSystemTimeNow(); // new RuntimeException("Could not get current time");
54+
public Time(final long secs, final long nanos, final ClockType ct) {
55+
if (secs < 0 || nanos < 0) {
56+
// TODO(clalancette): Make this a custom exception
57+
throw new IllegalArgumentException("seconds and nanoseconds must not be negative");
58+
}
59+
this.clockType = ct;
60+
this.nanoseconds = TimeUnit.SECONDS.toNanos(secs) + nanos;
8161
}
8262

83-
private static long rclSteadyTimeNow() {
84-
return nativeRCLSteadyTimeNow(); // new RuntimeException("Could not get current time");
85-
}
8663

87-
private static native long nativeRCLSystemTimeNow();
64+
public long nanoseconds() {
65+
return nanoseconds;
66+
}
8867

89-
private static native long nativeRCLSteadyTimeNow();
68+
public ClockType clockType() {
69+
return clockType;
70+
}
9071
}

‎rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java‎

Lines changed: 6 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -46,6 +46,7 @@
4646
import org.ros2.rcljava.subscription.SubscriptionImpl;
4747
import org.ros2.rcljava.time.Clock;
4848
import org.ros2.rcljava.time.ClockType;
49+
import org.ros2.rcljava.time.TimeSource;
4950
import org.ros2.rcljava.timer.Timer;
5051
import org.ros2.rcljava.timer.WallTimer;
5152
import org.ros2.rcljava.timer.WallTimerImpl;
@@ -98,6 +99,8 @@ public class NodeImpl implements Node {
9899
*/
99100
private Clock clock;
100101

102+
private TimeSource timeSource;
103+
101104
/**
102105
* All the @{link Subscription}s that have been created through this instance.
103106
*/
@@ -143,7 +146,6 @@ class ParameterAndDescriptor {
143146
* be zero.
144147
*/
145148
public NodeImpl(final long handle, final Context context, final boolean allowUndeclaredParameters) {
146-
this.clock = new Clock(ClockType.SYSTEM_TIME);
147149
this.handle = handle;
148150
this.context = context;
149151
this.publishers = new LinkedBlockingQueue<Publisher>();
@@ -156,6 +158,9 @@ public NodeImpl(final long handle, final Context context, final boolean allowUnd
156158
this.allowUndeclaredParameters = allowUndeclaredParameters;
157159
this.parameterCallbacksMutex = new Object();
158160
this.parameterCallbacks = new ArrayList<ParameterCallback>();
161+
this.clock = new Clock(ClockType.ROS_TIME);
162+
this.timeSource = new TimeSource(this);
163+
this.timeSource.attachClock(this.clock);
159164
}
160165

161166
/**

0 commit comments

Comments
 (0)