|
17 | 17 | const rclnodejs = require('./native_loader.js'); |
18 | 18 | const Time = require('./time.js'); |
19 | 19 | const ClockType = require('./clock_type.js'); |
| 20 | +const ClockChange = require('./clock_change.js'); |
| 21 | +const Context = require('./context.js'); |
| 22 | +const ClockEvent = require('./clock_event.js'); |
20 | 23 | const { TypeValidationError } = require('./errors.js'); |
21 | 24 |
|
22 | 25 | /** |
@@ -121,6 +124,116 @@ class Clock { |
121 | 124 | const nowInNanosec = rclnodejs.clockGetNow(this._handle); |
122 | 125 | return new Time(0n, nowInNanosec, this._clockType); |
123 | 126 | } |
| 127 | + |
| 128 | + /** |
| 129 | + * Sleep until a specific time is reached on this Clock. |
| 130 | + * |
| 131 | + * When using a ROSClock, this may sleep forever if the TimeSource is misconfigured |
| 132 | + * and the context is never shut down. ROS time being activated or deactivated causes |
| 133 | + * this function to cease sleeping and return false. |
| 134 | + * |
| 135 | + * @param {Time} until - Time at which this function should stop sleeping. |
| 136 | + * @param {Context} [context=null] - Context whose validity will be checked while sleeping. |
| 137 | + * If context is null, then the default context is used. If the context is found to be |
| 138 | + * shut down before or by the time the wait completes, the returned promise resolves to false. |
| 139 | + * @return {Promise<boolean>} Promise that resolves to true if 'until' was reached without |
| 140 | + * detecting a time jump or a shut down context, or false otherwise (for example, if a time |
| 141 | + * jump occurred or the context is no longer valid when the wait completes). |
| 142 | + * @throws {TypeError} if until is specified for a different type of clock than this one. |
| 143 | + * @throws {Error} if context has not been initialized or is shutdown. |
| 144 | + */ |
| 145 | + async sleepUntil(until, context = null) { |
| 146 | + if (!(until instanceof Time)) { |
| 147 | + throw new TypeValidationError('until', until, 'Time', { |
| 148 | + entityType: 'clock', |
| 149 | + }); |
| 150 | + } |
| 151 | + |
| 152 | + if (!context) { |
| 153 | + context = Context.defaultContext(); |
| 154 | + } |
| 155 | + |
| 156 | + if (!context.isValid()) { |
| 157 | + throw new Error('Context is not initialized or has been shut down'); |
| 158 | + } |
| 159 | + |
| 160 | + if (until.clockType !== this._clockType) { |
| 161 | + throw new TypeError( |
| 162 | + "until's clock type does not match this clock's type" |
| 163 | + ); |
| 164 | + } |
| 165 | + |
| 166 | + const event = new ClockEvent(); |
| 167 | + let timeSourceChanged = false; |
| 168 | + |
| 169 | + // Callback to wake when time jumps and is past target time |
| 170 | + const callbackObject = { |
| 171 | + _pre_callback: () => { |
| 172 | + // Optional pre-callback - no action needed |
| 173 | + }, |
| 174 | + _post_callback: (jumpInfo) => { |
| 175 | + // ROS time being activated or deactivated changes the epoch, |
| 176 | + // so sleep time loses its meaning |
| 177 | + timeSourceChanged = |
| 178 | + timeSourceChanged || |
| 179 | + jumpInfo.clock_change === ClockChange.ROS_TIME_ACTIVATED || |
| 180 | + jumpInfo.clock_change === ClockChange.ROS_TIME_DEACTIVATED; |
| 181 | + |
| 182 | + if (timeSourceChanged || this.now().nanoseconds >= until.nanoseconds) { |
| 183 | + event.set(); |
| 184 | + } |
| 185 | + }, |
| 186 | + }; |
| 187 | + |
| 188 | + // Setup jump callback with minimal forward threshold |
| 189 | + this.addClockCallback( |
| 190 | + callbackObject, |
| 191 | + true, // onClockChange |
| 192 | + 1n, // minForward (1 nanosecond - any forward jump triggers) |
| 193 | + 0n // minBackward (0 disables backward threshold) |
| 194 | + ); |
| 195 | + |
| 196 | + try { |
| 197 | + // Wait based on clock type |
| 198 | + if (this._clockType === ClockType.SYSTEM_TIME) { |
| 199 | + await event.waitUntilSystem(this, until.nanoseconds); |
| 200 | + } else if (this._clockType === ClockType.STEADY_TIME) { |
| 201 | + await event.waitUntilSteady(this, until.nanoseconds); |
| 202 | + } else if (this._clockType === ClockType.ROS_TIME) { |
| 203 | + await event.waitUntilRos(this, until.nanoseconds); |
| 204 | + } |
| 205 | + } finally { |
| 206 | + // Always clean up callback |
| 207 | + this.removeClockCallback(callbackObject); |
| 208 | + } |
| 209 | + |
| 210 | + if (!context.isValid() || timeSourceChanged) { |
| 211 | + return false; |
| 212 | + } |
| 213 | + |
| 214 | + return this.now().nanoseconds >= until.nanoseconds; |
| 215 | + } |
| 216 | + |
| 217 | + /** |
| 218 | + * Sleep for a specified duration. |
| 219 | + * |
| 220 | + * Equivalent to: clock.sleepUntil(clock.now() + duration, context) |
| 221 | + * |
| 222 | + * When using a ROSClock, this may sleep forever if the TimeSource is misconfigured |
| 223 | + * and the context is never shut down. ROS time being activated or deactivated causes |
| 224 | + * this function to cease sleeping and return false. |
| 225 | + * |
| 226 | + * @param {Duration} duration - Duration of time to sleep for. |
| 227 | + * @param {Context} [context=null] - Context which when shut down will cause this sleep to wake early. |
| 228 | + * If context is null, then the default context is used. |
| 229 | + * @return {Promise<boolean>} Promise that resolves to true if the full duration was slept, |
| 230 | + * or false if it woke for another reason. |
| 231 | + * @throws {Error} if context has not been initialized or is shutdown. |
| 232 | + */ |
| 233 | + async sleepFor(duration, context = null) { |
| 234 | + const until = this.now().add(duration); |
| 235 | + return this.sleepUntil(until, context); |
| 236 | + } |
124 | 237 | } |
125 | 238 |
|
126 | 239 | /** |
|
0 commit comments