Skip to content

Commit b322530

Browse files
clalancettejacobperron
clalancette
authored and
jacobperron
committed
Revamp Time (#6)
* Start revamping Time and Clock classes. Signed-off-by: Chris Lalancette <[email protected]> * Fixes from review. Signed-off-by: Chris Lalancette <[email protected]> * Add in logging to the TimeSource class. Signed-off-by: Chris Lalancette <[email protected]>
1 parent af574cb commit b322530

File tree

10 files changed

+410
-90
lines changed

10 files changed

+410
-90
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

@@ -105,6 +106,7 @@ foreach(_jni_source ${${PROJECT_NAME}_jni_sources})
105106
"rcljava_common"
106107
"builtin_interfaces"
107108
"rcl_interfaces"
109+
"rosgraph_msgs"
108110
)
109111

110112
target_include_directories(${_target_name}
@@ -184,6 +186,7 @@ set(${PROJECT_NAME}_sources
184186
"src/main/java/org/ros2/rcljava/subscription/statuses/RequestedQosIncompatible.java"
185187
"src/main/java/org/ros2/rcljava/time/Clock.java"
186188
"src/main/java/org/ros2/rcljava/time/ClockType.java"
189+
"src/main/java/org/ros2/rcljava/time/TimeSource.java"
187190
"src/main/java/org/ros2/rcljava/timer/Timer.java"
188191
"src/main/java/org/ros2/rcljava/timer/WallTimer.java"
189192
"src/main/java/org/ros2/rcljava/timer/WallTimerImpl.java"
@@ -197,6 +200,7 @@ add_jar("${PROJECT_NAME}_jar"
197200
${rcljava_common_JARS}
198201
${builtin_interfaces_JARS}
199202
${rcl_interfaces_JARS}
203+
${rosgraph_msgs_JARS}
200204
)
201205

202206
install_jar("${PROJECT_NAME}_jar" "share/${PROJECT_NAME}/java")
@@ -234,6 +238,7 @@ if(BUILD_TESTING)
234238
DEPENDENCIES
235239
builtin_interfaces
236240
rcl_interfaces
241+
rosgraph_msgs
237242
${_java_type_supports}
238243
SKIP_INSTALL
239244
)
@@ -316,6 +321,15 @@ if(BUILD_TESTING)
316321
list_append_unique(_deps_library_dirs ${_dep_dir})
317322
endforeach()
318323

324+
foreach(_dep_lib ${rosgraph_msgs_interfaces_LIBRARIES})
325+
get_filename_component(_dep_dir "${_dep_lib}" DIRECTORY)
326+
list_append_unique(_deps_library_dirs ${_dep_dir})
327+
endforeach()
328+
foreach(_dep_lib ${rosgraph_msgs_JNI_LIBRARIES})
329+
get_filename_component(_dep_dir "${_dep_lib}" DIRECTORY)
330+
list_append_unique(_deps_library_dirs ${_dep_dir})
331+
endforeach()
332+
319333
list_append_unique(_deps_library_dirs ${CMAKE_CURRENT_BINARY_DIR})
320334
list_append_unique(_deps_library_dirs ${CMAKE_CURRENT_BINARY_DIR}/rcljava)
321335
list_append_unique(_deps_library_dirs ${CMAKE_CURRENT_BINARY_DIR}/rosidl_generator_java/rcljava/msg/)
@@ -336,6 +350,7 @@ if(BUILD_TESTING)
336350
"${std_msgs_JARS}"
337351
"${builtin_interfaces_JARS}"
338352
"${rcl_interfaces_JARS}"
353+
"${rosgraph_msgs_JARS}"
339354
"${_${PROJECT_NAME}_jar_file}"
340355
"${_${PROJECT_NAME}_messages_jar_file}"
341356
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
@@ -17,11 +17,13 @@
1717
<build_depend>rcl</build_depend>
1818
<build_depend>rmw_implementation_cmake</build_depend>
1919
<build_depend>rmw</build_depend>
20+
<build_depend>rosgraph_msgs</build_depend>
2021
<build_depend>rosidl_generator_c</build_depend>
2122
<build_depend>rosidl_typesupport_c</build_depend>
2223
<build_export_depend>builtin_interfaces</build_export_depend>
2324
<build_export_depend>rcl_interfaces</build_export_depend>
2425
<build_export_depend>rmw</build_export_depend>
26+
<build_export_depend>rosgraph_msgs</build_export_depend>
2527
<build_export_depend>rosidl_runtime_c</build_export_depend>
2628
<build_export_depend>rosidl_generator_java</build_export_depend>
2729
<build_export_depend>rosidl_typesupport_c</build_export_depend>
@@ -31,6 +33,7 @@
3133
<exec_depend>rcl</exec_depend>
3234
<exec_depend>rmw_implementation_cmake</exec_depend>
3335
<exec_depend>rmw_implementation</exec_depend>
36+
<exec_depend>rosgraph_msgs</exec_depend>
3437
<exec_depend>rosidl_runtime_c</exec_depend>
3538
<exec_depend>rosidl_parser</exec_depend>
3639

‎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
@@ -42,6 +42,7 @@
4242
import org.ros2.rcljava.subscription.SubscriptionImpl;
4343
import org.ros2.rcljava.time.Clock;
4444
import org.ros2.rcljava.time.ClockType;
45+
import org.ros2.rcljava.time.TimeSource;
4546
import org.ros2.rcljava.timer.Timer;
4647
import org.ros2.rcljava.timer.WallTimer;
4748
import org.ros2.rcljava.timer.WallTimerImpl;
@@ -96,6 +97,8 @@ public class NodeImpl implements Node {
9697
*/
9798
private Clock clock;
9899

100+
private TimeSource timeSource;
101+
99102
/**
100103
* All the @{link Subscription}s that have been created through this instance.
101104
*/
@@ -141,7 +144,6 @@ class ParameterAndDescriptor {
141144
* be zero.
142145
*/
143146
public NodeImpl(final long handle, final Context context, final boolean allowUndeclaredParameters) {
144-
this.clock = new Clock(ClockType.SYSTEM_TIME);
145147
this.handle = handle;
146148
this.context = context;
147149
this.publishers = new LinkedBlockingQueue<Publisher>();
@@ -154,6 +156,9 @@ public NodeImpl(final long handle, final Context context, final boolean allowUnd
154156
this.allowUndeclaredParameters = allowUndeclaredParameters;
155157
this.parameterCallbacksMutex = new Object();
156158
this.parameterCallbacks = new ArrayList<ParameterCallback>();
159+
this.clock = new Clock(ClockType.ROS_TIME);
160+
this.timeSource = new TimeSource(this);
161+
this.timeSource.attachClock(this.clock);
157162
}
158163

159164
/**

0 commit comments

Comments
 (0)