-
Notifications
You must be signed in to change notification settings - Fork 83
Add ClockEvent support #1354
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Add ClockEvent support #1354
Changes from all commits
ffa4734
adf69d6
f845382
ccc1d0f
70b1f7b
659c636
8e07620
4ae6e04
e88f1bc
d88f55f
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
| Original file line number | Diff line number | Diff line change | ||||||||||||||||||||||||||||
|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|
|
|
@@ -17,6 +17,9 @@ | |||||||||||||||||||||||||||||
| const rclnodejs = require('./native_loader.js'); | ||||||||||||||||||||||||||||||
| const Time = require('./time.js'); | ||||||||||||||||||||||||||||||
| const ClockType = require('./clock_type.js'); | ||||||||||||||||||||||||||||||
| const ClockChange = require('./clock_change.js'); | ||||||||||||||||||||||||||||||
| const Context = require('./context.js'); | ||||||||||||||||||||||||||||||
| const ClockEvent = require('./clock_event.js'); | ||||||||||||||||||||||||||||||
| const { TypeValidationError } = require('./errors.js'); | ||||||||||||||||||||||||||||||
|
|
||||||||||||||||||||||||||||||
| /** | ||||||||||||||||||||||||||||||
|
|
@@ -121,6 +124,116 @@ class Clock { | |||||||||||||||||||||||||||||
| const nowInNanosec = rclnodejs.clockGetNow(this._handle); | ||||||||||||||||||||||||||||||
| return new Time(0n, nowInNanosec, this._clockType); | ||||||||||||||||||||||||||||||
| } | ||||||||||||||||||||||||||||||
|
|
||||||||||||||||||||||||||||||
| /** | ||||||||||||||||||||||||||||||
| * Sleep until a specific time is reached on this Clock. | ||||||||||||||||||||||||||||||
| * | ||||||||||||||||||||||||||||||
| * When using a ROSClock, this may sleep forever if the TimeSource is misconfigured | ||||||||||||||||||||||||||||||
| * and the context is never shut down. ROS time being activated or deactivated causes | ||||||||||||||||||||||||||||||
| * this function to cease sleeping and return false. | ||||||||||||||||||||||||||||||
| * | ||||||||||||||||||||||||||||||
| * @param {Time} until - Time at which this function should stop sleeping. | ||||||||||||||||||||||||||||||
| * @param {Context} [context=null] - Context whose validity will be checked while sleeping. | ||||||||||||||||||||||||||||||
| * If context is null, then the default context is used. If the context is found to be | ||||||||||||||||||||||||||||||
| * shut down before or by the time the wait completes, the returned promise resolves to false. | ||||||||||||||||||||||||||||||
| * @return {Promise<boolean>} Promise that resolves to true if 'until' was reached without | ||||||||||||||||||||||||||||||
| * detecting a time jump or a shut down context, or false otherwise (for example, if a time | ||||||||||||||||||||||||||||||
| * jump occurred or the context is no longer valid when the wait completes). | ||||||||||||||||||||||||||||||
| * @throws {TypeError} if until is specified for a different type of clock than this one. | ||||||||||||||||||||||||||||||
| * @throws {Error} if context has not been initialized or is shutdown. | ||||||||||||||||||||||||||||||
| */ | ||||||||||||||||||||||||||||||
| async sleepUntil(until, context = null) { | ||||||||||||||||||||||||||||||
| if (!(until instanceof Time)) { | ||||||||||||||||||||||||||||||
| throw new TypeValidationError('until', until, 'Time', { | ||||||||||||||||||||||||||||||
| entityType: 'clock', | ||||||||||||||||||||||||||||||
| }); | ||||||||||||||||||||||||||||||
| } | ||||||||||||||||||||||||||||||
|
|
||||||||||||||||||||||||||||||
| if (!context) { | ||||||||||||||||||||||||||||||
| context = Context.defaultContext(); | ||||||||||||||||||||||||||||||
| } | ||||||||||||||||||||||||||||||
|
|
||||||||||||||||||||||||||||||
| if (!context.isValid()) { | ||||||||||||||||||||||||||||||
| throw new Error('Context is not initialized or has been shut down'); | ||||||||||||||||||||||||||||||
| } | ||||||||||||||||||||||||||||||
|
|
||||||||||||||||||||||||||||||
| if (until.clockType !== this._clockType) { | ||||||||||||||||||||||||||||||
| throw new TypeError( | ||||||||||||||||||||||||||||||
| "until's clock type does not match this clock's type" | ||||||||||||||||||||||||||||||
| ); | ||||||||||||||||||||||||||||||
| } | ||||||||||||||||||||||||||||||
|
|
||||||||||||||||||||||||||||||
| const event = new ClockEvent(); | ||||||||||||||||||||||||||||||
| let timeSourceChanged = false; | ||||||||||||||||||||||||||||||
|
|
||||||||||||||||||||||||||||||
| // Callback to wake when time jumps and is past target time | ||||||||||||||||||||||||||||||
| const callbackObject = { | ||||||||||||||||||||||||||||||
| _pre_callback: () => { | ||||||||||||||||||||||||||||||
| // Optional pre-callback - no action needed | ||||||||||||||||||||||||||||||
| }, | ||||||||||||||||||||||||||||||
| _post_callback: (jumpInfo) => { | ||||||||||||||||||||||||||||||
| // ROS time being activated or deactivated changes the epoch, | ||||||||||||||||||||||||||||||
| // so sleep time loses its meaning | ||||||||||||||||||||||||||||||
| timeSourceChanged = | ||||||||||||||||||||||||||||||
| timeSourceChanged || | ||||||||||||||||||||||||||||||
| jumpInfo.clock_change === ClockChange.ROS_TIME_ACTIVATED || | ||||||||||||||||||||||||||||||
| jumpInfo.clock_change === ClockChange.ROS_TIME_DEACTIVATED; | ||||||||||||||||||||||||||||||
|
|
||||||||||||||||||||||||||||||
| if (timeSourceChanged || this.now().nanoseconds >= until.nanoseconds) { | ||||||||||||||||||||||||||||||
| event.set(); | ||||||||||||||||||||||||||||||
| } | ||||||||||||||||||||||||||||||
| }, | ||||||||||||||||||||||||||||||
| }; | ||||||||||||||||||||||||||||||
|
|
||||||||||||||||||||||||||||||
| // Setup jump callback with minimal forward threshold | ||||||||||||||||||||||||||||||
| this.addClockCallback( | ||||||||||||||||||||||||||||||
| callbackObject, | ||||||||||||||||||||||||||||||
| true, // onClockChange | ||||||||||||||||||||||||||||||
| 1n, // minForward (1 nanosecond - any forward jump triggers) | ||||||||||||||||||||||||||||||
| 0n // minBackward (0 disables backward threshold) | ||||||||||||||||||||||||||||||
| ); | ||||||||||||||||||||||||||||||
|
|
||||||||||||||||||||||||||||||
| try { | ||||||||||||||||||||||||||||||
| // Wait based on clock type | ||||||||||||||||||||||||||||||
| if (this._clockType === ClockType.SYSTEM_TIME) { | ||||||||||||||||||||||||||||||
| await event.waitUntilSystem(this, until.nanoseconds); | ||||||||||||||||||||||||||||||
| } else if (this._clockType === ClockType.STEADY_TIME) { | ||||||||||||||||||||||||||||||
| await event.waitUntilSteady(this, until.nanoseconds); | ||||||||||||||||||||||||||||||
| } else if (this._clockType === ClockType.ROS_TIME) { | ||||||||||||||||||||||||||||||
| await event.waitUntilRos(this, until.nanoseconds); | ||||||||||||||||||||||||||||||
| } | ||||||||||||||||||||||||||||||
| } finally { | ||||||||||||||||||||||||||||||
| // Always clean up callback | ||||||||||||||||||||||||||||||
| this.removeClockCallback(callbackObject); | ||||||||||||||||||||||||||||||
| } | ||||||||||||||||||||||||||||||
|
|
||||||||||||||||||||||||||||||
| if (!context.isValid() || timeSourceChanged) { | ||||||||||||||||||||||||||||||
| return false; | ||||||||||||||||||||||||||||||
| } | ||||||||||||||||||||||||||||||
|
|
||||||||||||||||||||||||||||||
| return this.now().nanoseconds >= until.nanoseconds; | ||||||||||||||||||||||||||||||
| } | ||||||||||||||||||||||||||||||
|
|
||||||||||||||||||||||||||||||
| /** | ||||||||||||||||||||||||||||||
| * Sleep for a specified duration. | ||||||||||||||||||||||||||||||
| * | ||||||||||||||||||||||||||||||
| * Equivalent to: clock.sleepUntil(clock.now() + duration, context) | ||||||||||||||||||||||||||||||
| * | ||||||||||||||||||||||||||||||
| * When using a ROSClock, this may sleep forever if the TimeSource is misconfigured | ||||||||||||||||||||||||||||||
| * and the context is never shut down. ROS time being activated or deactivated causes | ||||||||||||||||||||||||||||||
| * this function to cease sleeping and return false. | ||||||||||||||||||||||||||||||
| * | ||||||||||||||||||||||||||||||
| * @param {Duration} duration - Duration of time to sleep for. | ||||||||||||||||||||||||||||||
| * @param {Context} [context=null] - Context which when shut down will cause this sleep to wake early. | ||||||||||||||||||||||||||||||
| * If context is null, then the default context is used. | ||||||||||||||||||||||||||||||
| * @return {Promise<boolean>} Promise that resolves to true if the full duration was slept, | ||||||||||||||||||||||||||||||
| * or false if it woke for another reason. | ||||||||||||||||||||||||||||||
| * @throws {Error} if context has not been initialized or is shutdown. | ||||||||||||||||||||||||||||||
| */ | ||||||||||||||||||||||||||||||
| async sleepFor(duration, context = null) { | ||||||||||||||||||||||||||||||
| const until = this.now().add(duration); | ||||||||||||||||||||||||||||||
| return this.sleepUntil(until, context); | ||||||||||||||||||||||||||||||
|
Comment on lines
+234
to
+235
|
||||||||||||||||||||||||||||||
| const until = this.now().add(duration); | |
| return this.sleepUntil(until, context); | |
| if ( | |
| !duration || | |
| typeof duration !== 'object' || | |
| !duration.constructor || | |
| duration.constructor.name !== 'Duration' | |
| ) { | |
| throw new TypeValidationError('duration', duration, 'Duration', { | |
| entityType: 'clock', | |
| }); | |
| } | |
| const until = this.now().add(duration); | |
| return this.sleepUntil(until, context); |
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,49 @@ | ||
| // Copyright (c) 2025 The Robot Web Tools Contributors. All rights reserved. | ||
| // | ||
| // 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. | ||
|
|
||
| 'use strict'; | ||
|
|
||
| /** | ||
| * Enum for ClockChange | ||
| * Represents the type of clock change that occurred during a time jump. | ||
| * @readonly | ||
| * @enum {number} | ||
| */ | ||
| const ClockChange = { | ||
| /** | ||
| * The source before and after the jump is ROS_TIME. | ||
| * @member {number} | ||
| */ | ||
| ROS_TIME_NO_CHANGE: 1, | ||
|
|
||
| /** | ||
| * The source switched to ROS_TIME from SYSTEM_TIME. | ||
| * @member {number} | ||
| */ | ||
| ROS_TIME_ACTIVATED: 2, | ||
|
|
||
| /** | ||
| * The source switched to SYSTEM_TIME from ROS_TIME. | ||
| * @member {number} | ||
| */ | ||
| ROS_TIME_DEACTIVATED: 3, | ||
|
|
||
| /** | ||
| * The source before and after the jump is SYSTEM_TIME. | ||
| * @member {number} | ||
| */ | ||
| SYSTEM_TIME_NO_CHANGE: 4, | ||
| }; | ||
|
|
||
| module.exports = ClockChange; |
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,88 @@ | ||
| // Copyright (c) 2025, The Robot Web Tools Contributors | ||
| // | ||
| // 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. | ||
|
|
||
| 'use strict'; | ||
|
|
||
| const rclnodejs = require('./native_loader.js'); | ||
|
|
||
| /** | ||
| * @class - Class representing a ClockEvent in ROS | ||
| */ | ||
| class ClockEvent { | ||
| constructor() { | ||
| this._handle = rclnodejs.createClockEvent(); | ||
| } | ||
|
|
||
| /** | ||
| * Wait until a time specified by a steady clock. | ||
| * @param {Clock} clock - The clock to use for time synchronization. | ||
| * @param {bigint} until - The time to wait until. | ||
| * @return {Promise<void>} - A promise that resolves when the time is reached. | ||
| */ | ||
| async waitUntilSteady(clock, until) { | ||
| return rclnodejs.clockEventWaitUntilSteady( | ||
| this._handle, | ||
| clock.handle, | ||
| until | ||
| ); | ||
| } | ||
|
|
||
| /** | ||
| * Wait until a time specified by a system clock. | ||
| * @param {Clock} clock - The clock to use for time synchronization. | ||
| * @param {bigint} until - The time to wait until. | ||
| * @return {Promise<void>} - A promise that resolves when the time is reached. | ||
| */ | ||
| async waitUntilSystem(clock, until) { | ||
| return rclnodejs.clockEventWaitUntilSystem( | ||
| this._handle, | ||
| clock.handle, | ||
| until | ||
| ); | ||
| } | ||
|
|
||
| /** | ||
| * Wait until a time specified by a ROS clock. | ||
| * @param {Clock} clock - The clock to use for time synchronization. | ||
| * @param {bigint} until - The time to wait until. | ||
| * @return {Promise<void>} - A promise that resolves when the time is reached. | ||
| */ | ||
| async waitUntilRos(clock, until) { | ||
| return rclnodejs.clockEventWaitUntilRos(this._handle, clock.handle, until); | ||
| } | ||
|
|
||
| /** | ||
| * Indicate if the ClockEvent is set. | ||
| * @return {boolean} - True if the ClockEvent is set. | ||
| */ | ||
| isSet() { | ||
| return rclnodejs.clockEventIsSet(this._handle); | ||
| } | ||
|
|
||
| /** | ||
| * Set the event. | ||
| */ | ||
| set() { | ||
| rclnodejs.clockEventSet(this._handle); | ||
| } | ||
|
|
||
| /** | ||
| * Clear the event. | ||
| */ | ||
| clear() { | ||
| rclnodejs.clockEventClear(this._handle); | ||
| } | ||
| } | ||
|
|
||
| module.exports = ClockEvent; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Missing input validation for the 'until' parameter. The function accesses 'until.clockType' and 'until.nanoseconds' without first verifying that 'until' is an instance of Time. This could lead to unclear errors if an invalid argument is passed. Consider adding a validation check similar to the one in ROSClock.set rosTimeOverride (line 276) before accessing the properties.