From ffa4734acbe6ab5b8e201571273cc700e9839652 Mon Sep 17 00:00:00 2001 From: Minggang Wang Date: Tue, 16 Dec 2025 15:50:12 +0800 Subject: [PATCH 01/10] Add ClockEvent support --- binding.gyp | 1 + index.js | 4 + lib/clock_event.js | 88 +++++++++++++ src/addon.cpp | 2 + src/clock_event.cpp | 253 +++++++++++++++++++++++++++++++++++++ src/clock_event.hpp | 62 +++++++++ test/test-clock-event.js | 55 ++++++++ test/types/index.test-d.ts | 14 ++ types/clock_event.d.ts | 51 ++++++++ types/index.d.ts | 1 + 10 files changed, 531 insertions(+) create mode 100644 lib/clock_event.js create mode 100644 src/clock_event.cpp create mode 100644 src/clock_event.hpp create mode 100644 test/test-clock-event.js create mode 100644 types/clock_event.d.ts diff --git a/binding.gyp b/binding.gyp index d741fc9a..f3bbf2ec 100644 --- a/binding.gyp +++ b/binding.gyp @@ -26,6 +26,7 @@ './src/rcl_action_server_bindings.cpp', './src/rcl_bindings.cpp', './src/rcl_client_bindings.cpp', + './src/clock_event.cpp', './src/rcl_context_bindings.cpp', './src/rcl_graph_bindings.cpp', './src/rcl_guard_condition_bindings.cpp', diff --git a/index.js b/index.js index 5a4ba581..ec10650e 100644 --- a/index.js +++ b/index.js @@ -17,6 +17,7 @@ const DistroUtils = require('./lib/distro.js'); const RMWUtils = require('./lib/rmw.js'); const { Clock, ROSClock } = require('./lib/clock.js'); +const ClockEvent = require('./lib/clock_event.js'); const ClockType = require('./lib/clock_type.js'); const { compareVersions } = require('./lib/utils.js'); const Context = require('./lib/context.js'); @@ -188,6 +189,9 @@ let rcl = { /** {@link Clock} class */ Clock: Clock, + /** {@link ClockEvent} class */ + ClockEvent: ClockEvent, + /** {@link ClockType} enum */ ClockType: ClockType, diff --git a/lib/clock_event.js b/lib/clock_event.js new file mode 100644 index 00000000..2545e922 --- /dev/null +++ b/lib/clock_event.js @@ -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} - 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} - 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} - 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; diff --git a/src/addon.cpp b/src/addon.cpp index d5f2380a..5975f6e8 100644 --- a/src/addon.cpp +++ b/src/addon.cpp @@ -15,6 +15,7 @@ #include #include +#include "clock_event.hpp" #include "macros.h" #include "rcl_action_client_bindings.h" #include "rcl_action_goal_bindings.h" @@ -74,6 +75,7 @@ Napi::Object InitModule(Napi::Env env, Napi::Object exports) { rclnodejs::InitActionGoalBindings(env, exports); rclnodejs::InitActionServerBindings(env, exports); rclnodejs::InitClientBindings(env, exports); + rclnodejs::InitClockEventBindings(env, exports); rclnodejs::InitContextBindings(env, exports); rclnodejs::InitGraphBindings(env, exports); rclnodejs::InitGuardConditionBindings(env, exports); diff --git a/src/clock_event.cpp b/src/clock_event.cpp new file mode 100644 index 00000000..f94b0498 --- /dev/null +++ b/src/clock_event.cpp @@ -0,0 +1,253 @@ +// 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. + +#include "clock_event.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "macros.h" +#include "rcl_handle.h" + +namespace rclnodejs { + +template +void ClockEvent::wait_until(rcl_clock_t* clock, rcl_time_point_t until) { + // Synchronize because clock epochs might differ + rcl_time_point_value_t now_value; + rcl_ret_t ret = rcl_clock_get_now(clock, &now_value); + if (RCL_RET_OK != ret) { + throw std::runtime_error("failed to get current time"); + } + rcl_time_point_t rcl_entry; + rcl_entry.nanoseconds = now_value; + rcl_entry.clock_type = clock->type; + + const typename ClockType::time_point chrono_entry = ClockType::now(); + + rcl_duration_t delta_t; + ret = rcl_difference_times(&rcl_entry, &until, &delta_t); + + if (RCL_RET_OK != ret) { + throw std::runtime_error("failed to subtract times"); + } + + // Cast because system clock resolution is too big for nanoseconds on Windows + // & OSX + const typename ClockType::time_point chrono_until = + chrono_entry + std::chrono::duration_cast( + std::chrono::nanoseconds(delta_t.nanoseconds)); + + std::unique_lock lock(mutex_); + cv_.wait_until(lock, chrono_until, [this]() { return state_; }); +} + +void ClockEvent::wait_until_ros(rcl_clock_t* clock, rcl_time_point_t until) { + bool is_enabled; + rcl_ret_t ret = rcl_is_enabled_ros_time_override(clock, &is_enabled); + if (RCL_RET_OK != ret) { + throw std::runtime_error("failed to check if ros time override is enabled"); + } + + // Check if ROS time is enabled in C++ to avoid TOCTTOU with TimeSource + if (is_enabled) { + std::unique_lock lock(mutex_); + // Caller must have setup a time jump callback to wake this event + cv_.wait(lock, [this]() { return state_; }); + } else { + // ROS time not enabled is system time + wait_until(clock, until); + } +} + +bool ClockEvent::is_set() { + std::unique_lock lock(mutex_); + return state_; +} + +void ClockEvent::set() { + { + std::unique_lock lock(mutex_); + state_ = true; + } + cv_.notify_all(); +} + +void ClockEvent::clear() { + { + std::unique_lock lock(mutex_); + state_ = false; + } + cv_.notify_all(); +} + +// Explicit instantiation +template void ClockEvent::wait_until( + rcl_clock_t* clock, rcl_time_point_t until); +template void ClockEvent::wait_until( + rcl_clock_t* clock, rcl_time_point_t until); + +enum class WaitType { Steady, System, Ros }; + +class ClockEventWaitWorker : public Napi::AsyncWorker { + public: + ClockEventWaitWorker(Napi::Env env, ClockEvent* event, rcl_clock_t* clock, + int64_t until, WaitType type) + : Napi::AsyncWorker(env), + event_(event), + clock_(clock), + until_(until), + type_(type), + deferred_(Napi::Promise::Deferred::New(env)) {} + + ~ClockEventWaitWorker() {} + + void Execute() override { + try { + rcl_time_point_t until_time_point; + until_time_point.nanoseconds = until_; + until_time_point.clock_type = clock_->type; + + switch (type_) { + case WaitType::Ros: + event_->wait_until_ros(clock_, until_time_point); + break; + case WaitType::Steady: + event_->wait_until(clock_, + until_time_point); + break; + case WaitType::System: + event_->wait_until(clock_, + until_time_point); + break; + } + } catch (const std::exception& e) { + SetError(e.what()); + } + } + + void OnOK() override { deferred_.Resolve(Env().Undefined()); } + + void OnError(const Napi::Error& e) override { deferred_.Reject(e.Value()); } + + Napi::Promise Promise() { return deferred_.Promise(); } + + private: + ClockEvent* event_; + rcl_clock_t* clock_; + int64_t until_; + WaitType type_; + Napi::Promise::Deferred deferred_; +}; + +Napi::Value CreateClockEvent(const Napi::CallbackInfo& info) { + Napi::Env env = info.Env(); + ClockEvent* event = new ClockEvent(); + return RclHandle::NewInstance(env, event, nullptr, [](void* ptr) { + delete static_cast(ptr); + }); +} + +Napi::Value ClockEventWaitUntilSteady(const Napi::CallbackInfo& info) { + Napi::Env env = info.Env(); + RclHandle* event_handle = RclHandle::Unwrap(info[0].As()); + ClockEvent* event = static_cast(event_handle->ptr()); + + RclHandle* clock_handle = RclHandle::Unwrap(info[1].As()); + rcl_clock_t* clock = static_cast(clock_handle->ptr()); + + bool lossless; + int64_t until = info[2].As().Int64Value(&lossless); + + auto worker = + new ClockEventWaitWorker(env, event, clock, until, WaitType::Steady); + worker->Queue(); + return worker->Promise(); +} + +Napi::Value ClockEventWaitUntilSystem(const Napi::CallbackInfo& info) { + Napi::Env env = info.Env(); + RclHandle* event_handle = RclHandle::Unwrap(info[0].As()); + ClockEvent* event = static_cast(event_handle->ptr()); + + RclHandle* clock_handle = RclHandle::Unwrap(info[1].As()); + rcl_clock_t* clock = static_cast(clock_handle->ptr()); + + bool lossless; + int64_t until = info[2].As().Int64Value(&lossless); + + auto worker = + new ClockEventWaitWorker(env, event, clock, until, WaitType::System); + worker->Queue(); + return worker->Promise(); +} + +Napi::Value ClockEventWaitUntilRos(const Napi::CallbackInfo& info) { + Napi::Env env = info.Env(); + RclHandle* event_handle = RclHandle::Unwrap(info[0].As()); + ClockEvent* event = static_cast(event_handle->ptr()); + + RclHandle* clock_handle = RclHandle::Unwrap(info[1].As()); + rcl_clock_t* clock = static_cast(clock_handle->ptr()); + + bool lossless; + int64_t until = info[2].As().Int64Value(&lossless); + + auto worker = + new ClockEventWaitWorker(env, event, clock, until, WaitType::Ros); + worker->Queue(); + return worker->Promise(); +} + +Napi::Value ClockEventIsSet(const Napi::CallbackInfo& info) { + RclHandle* event_handle = RclHandle::Unwrap(info[0].As()); + ClockEvent* event = static_cast(event_handle->ptr()); + return Napi::Boolean::New(info.Env(), event->is_set()); +} + +Napi::Value ClockEventSet(const Napi::CallbackInfo& info) { + RclHandle* event_handle = RclHandle::Unwrap(info[0].As()); + ClockEvent* event = static_cast(event_handle->ptr()); + event->set(); + return info.Env().Undefined(); +} + +Napi::Value ClockEventClear(const Napi::CallbackInfo& info) { + RclHandle* event_handle = RclHandle::Unwrap(info[0].As()); + ClockEvent* event = static_cast(event_handle->ptr()); + event->clear(); + return info.Env().Undefined(); +} + +void InitClockEventBindings(Napi::Env env, Napi::Object exports) { + exports.Set("createClockEvent", Napi::Function::New(env, CreateClockEvent)); + exports.Set("clockEventWaitUntilSteady", + Napi::Function::New(env, ClockEventWaitUntilSteady)); + exports.Set("clockEventWaitUntilSystem", + Napi::Function::New(env, ClockEventWaitUntilSystem)); + exports.Set("clockEventWaitUntilRos", + Napi::Function::New(env, ClockEventWaitUntilRos)); + exports.Set("clockEventIsSet", Napi::Function::New(env, ClockEventIsSet)); + exports.Set("clockEventSet", Napi::Function::New(env, ClockEventSet)); + exports.Set("clockEventClear", Napi::Function::New(env, ClockEventClear)); +} + +} // namespace rclnodejs diff --git a/src/clock_event.hpp b/src/clock_event.hpp new file mode 100644 index 00000000..c6a0bf50 --- /dev/null +++ b/src/clock_event.hpp @@ -0,0 +1,62 @@ +// 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. + +#ifndef SRC_CLOCK_EVENT_HPP_ +#define SRC_CLOCK_EVENT_HPP_ + +#include +#include + +#include +#include +#include + +namespace rclnodejs { + +class ClockEvent { + public: + /// Wait until a time specified by a system or steady clock. + /// \param clock the clock to use for time synchronization with until + /// \param until this method will block until this time is reached. + template + void wait_until(rcl_clock_t* clock, rcl_time_point_t until); + + /// Wait until a time specified by a ROS clock. + /// \warning the caller is responsible for creating a time jump callback to + /// set this event when the target ROS time is reached. when a given ROS time + /// is reached. \param clock the clock to use for time synchronization. \param + /// until this method will block until this time is reached. + void wait_until_ros(rcl_clock_t* clock, rcl_time_point_t until); + + /// Indicate if the ClockEvent is set. + /// \return True if the ClockEvent is set. + bool is_set(); + + /// Set the event. + void set(); + + /// Clear the event. + void clear(); + + private: + bool state_ = false; + std::mutex mutex_; + std::condition_variable cv_; +}; + +void InitClockEventBindings(Napi::Env env, Napi::Object exports); + +} // namespace rclnodejs + +#endif // SRC_CLOCK_EVENT_HPP_ diff --git a/test/test-clock-event.js b/test/test-clock-event.js new file mode 100644 index 00000000..4dff65c4 --- /dev/null +++ b/test/test-clock-event.js @@ -0,0 +1,55 @@ +'use strict'; + +const assert = require('assert'); +const rclnodejs = require('../index.js'); +const { Clock, ClockType, Time, Duration } = rclnodejs; + +describe('ClockEvent', function () { + let node; + this.timeout(10000); + + before(async function () { + await rclnodejs.init(); + node = rclnodejs.createNode('test_clock_event_node'); + rclnodejs.spin(node); + }); + + after(function () { + rclnodejs.shutdown(); + }); + + it('should wait until steady time', async function () { + const clock = new Clock(ClockType.STEADY_TIME); + const event = new rclnodejs.ClockEvent(); + const now = clock.now(); + const until = now.add(new Duration(1n, 0n)); // 1 second later + + const start = Date.now(); + await event.waitUntilSteady(clock, until.nanoseconds); + const end = Date.now(); + + assert(end - start >= 1000); + }); + + it('should wait until system time', async function () { + const clock = new Clock(ClockType.SYSTEM_TIME); + const event = new rclnodejs.ClockEvent(); + const now = clock.now(); + const until = now.add(new Duration(1n, 0n)); // 1 second later + + const start = Date.now(); + await event.waitUntilSystem(clock, until.nanoseconds); + const end = Date.now(); + + assert(end - start >= 1000); + }); + + it('should set and clear event', function () { + const event = new rclnodejs.ClockEvent(); + assert.strictEqual(event.isSet(), false); + event.set(); + assert.strictEqual(event.isSet(), true); + event.clear(); + assert.strictEqual(event.isSet(), false); + }); +}); diff --git a/test/types/index.test-d.ts b/test/types/index.test-d.ts index e4eb7d32..c8047007 100644 --- a/test/types/index.test-d.ts +++ b/test/types/index.test-d.ts @@ -28,6 +28,20 @@ expectType>( rclnodejs.ros2Launch('package_name', 'launch_file', ['arg1', 'arg2']) ); +// ---- ClockEvent ---- +const clockEvent = new rclnodejs.ClockEvent(); +expectType(clockEvent); +expectType>( + clockEvent.waitUntilSteady(new rclnodejs.Clock(), 100n) +); +expectType>( + clockEvent.waitUntilSystem(new rclnodejs.Clock(), 100n) +); +expectType>(clockEvent.waitUntilRos(new rclnodejs.Clock(), 100n)); +expectType(clockEvent.isSet()); +expectType(clockEvent.set()); +expectType(clockEvent.clear()); + // ---- DistroUtil ---- expectType(rclnodejs.DistroUtils.getDistroId()); expectType( diff --git a/types/clock_event.d.ts b/types/clock_event.d.ts new file mode 100644 index 00000000..299627f6 --- /dev/null +++ b/types/clock_event.d.ts @@ -0,0 +1,51 @@ +declare module 'rclnodejs' { + /** + * Class representing a ClockEvent in ROS + */ + class ClockEvent { + /** + * Create a ClockEvent. + */ + constructor(); + + /** + * Wait until a time specified by a steady clock. + * @param clock - The clock to use for time synchronization. + * @param until - The time to wait until. + * @returns A promise that resolves when the time is reached. + */ + waitUntilSteady(clock: Clock, until: bigint): Promise; + + /** + * Wait until a time specified by a system clock. + * @param clock - The clock to use for time synchronization. + * @param until - The time to wait until. + * @returns A promise that resolves when the time is reached. + */ + waitUntilSystem(clock: Clock, until: bigint): Promise; + + /** + * Wait until a time specified by a ROS clock. + * @param clock - The clock to use for time synchronization. + * @param until - The time to wait until. + * @returns A promise that resolves when the time is reached. + */ + waitUntilRos(clock: Clock, until: bigint): Promise; + + /** + * Indicate if the ClockEvent is set. + * @returns True if the ClockEvent is set. + */ + isSet(): boolean; + + /** + * Set the event. + */ + set(): void; + + /** + * Clear the event. + */ + clear(): void; + } +} diff --git a/types/index.d.ts b/types/index.d.ts index 356803f5..18a7e01e 100644 --- a/types/index.d.ts +++ b/types/index.d.ts @@ -1,4 +1,5 @@ /// +/// /// import { ChildProcess } from 'child_process'; From adf69d6f9f08ec9c265cd7b765890da632155df4 Mon Sep 17 00:00:00 2001 From: Minggang Wang Date: Wed, 17 Dec 2025 13:38:05 +0800 Subject: [PATCH 02/10] Update src/clock_event.hpp Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- src/clock_event.hpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/clock_event.hpp b/src/clock_event.hpp index c6a0bf50..4b524af1 100644 --- a/src/clock_event.hpp +++ b/src/clock_event.hpp @@ -33,10 +33,9 @@ class ClockEvent { void wait_until(rcl_clock_t* clock, rcl_time_point_t until); /// Wait until a time specified by a ROS clock. - /// \warning the caller is responsible for creating a time jump callback to - /// set this event when the target ROS time is reached. when a given ROS time - /// is reached. \param clock the clock to use for time synchronization. \param - /// until this method will block until this time is reached. + /// \warning The caller is responsible for creating a time jump callback to set this event when the target ROS time is reached. + /// \param clock The clock to use for time synchronization. + /// \param until This method will block until this time is reached. void wait_until_ros(rcl_clock_t* clock, rcl_time_point_t until); /// Indicate if the ClockEvent is set. From f845382f70b8923b6d2b1f677d1068b8e5d02c19 Mon Sep 17 00:00:00 2001 From: Minggang Wang Date: Wed, 17 Dec 2025 13:51:45 +0800 Subject: [PATCH 03/10] Address comments --- test/test-clock-event.js | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/test/test-clock-event.js b/test/test-clock-event.js index 4dff65c4..bc0c6495 100644 --- a/test/test-clock-event.js +++ b/test/test-clock-event.js @@ -2,7 +2,7 @@ const assert = require('assert'); const rclnodejs = require('../index.js'); -const { Clock, ClockType, Time, Duration } = rclnodejs; +const { Clock, ClockType, Duration } = rclnodejs; describe('ClockEvent', function () { let node; @@ -44,6 +44,19 @@ describe('ClockEvent', function () { assert(end - start >= 1000); }); + it('should wait until ros time', async function () { + const clock = new Clock(ClockType.ROS_TIME); + const event = new rclnodejs.ClockEvent(); + const now = clock.now(); + const until = now.add(new Duration(1n, 0n)); // 1 second later + + const start = Date.now(); + await event.waitUntilRos(clock, until.nanoseconds); + const end = Date.now(); + + assert(end - start >= 1000); + }); + it('should set and clear event', function () { const event = new rclnodejs.ClockEvent(); assert.strictEqual(event.isSet(), false); From ccc1d0f2c19bb5fd95be29049aaa4126e57f0d4b Mon Sep 17 00:00:00 2001 From: Minggang Wang Date: Wed, 17 Dec 2025 14:04:40 +0800 Subject: [PATCH 04/10] Fix lint error --- src/clock_event.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/clock_event.hpp b/src/clock_event.hpp index 4b524af1..ddde76ef 100644 --- a/src/clock_event.hpp +++ b/src/clock_event.hpp @@ -33,7 +33,8 @@ class ClockEvent { void wait_until(rcl_clock_t* clock, rcl_time_point_t until); /// Wait until a time specified by a ROS clock. - /// \warning The caller is responsible for creating a time jump callback to set this event when the target ROS time is reached. + /// \warning The caller is responsible for creating a time jump callback + /// to set this event when the target ROS time is reached. /// \param clock The clock to use for time synchronization. /// \param until This method will block until this time is reached. void wait_until_ros(rcl_clock_t* clock, rcl_time_point_t until); From 70b1f7b987c666a700ffa51f2f109288a26cfbe5 Mon Sep 17 00:00:00 2001 From: Minggang Wang Date: Tue, 23 Dec 2025 14:10:47 +0800 Subject: [PATCH 05/10] Add sleepUntil/sleepFor for Clock --- index.js | 4 + lib/clock.js | 106 ++++++++++++++ lib/clock_change.js | 49 +++++++ src/rcl_timer_bindings.cpp | 1 + test/test-clock-change-integration.js | 170 +++++++++++++++++++++ test/test-clock-change.js | 67 +++++++++ test/test-clock-sleep.js | 203 ++++++++++++++++++++++++++ test/types/index.test-d.ts | 48 ++++++ types/clock.d.ts | 37 ++++- types/clock_change.d.ts | 27 ++++ types/index.d.ts | 1 + 11 files changed, 712 insertions(+), 1 deletion(-) create mode 100644 lib/clock_change.js create mode 100644 test/test-clock-change-integration.js create mode 100644 test/test-clock-change.js create mode 100644 test/test-clock-sleep.js create mode 100644 types/clock_change.d.ts diff --git a/index.js b/index.js index ec10650e..9b8b7bae 100644 --- a/index.js +++ b/index.js @@ -19,6 +19,7 @@ const RMWUtils = require('./lib/rmw.js'); const { Clock, ROSClock } = require('./lib/clock.js'); const ClockEvent = require('./lib/clock_event.js'); const ClockType = require('./lib/clock_type.js'); +const ClockChange = require('./lib/clock_change.js'); const { compareVersions } = require('./lib/utils.js'); const Context = require('./lib/context.js'); const debug = require('debug')('rclnodejs'); @@ -195,6 +196,9 @@ let rcl = { /** {@link ClockType} enum */ ClockType: ClockType, + /** {@link ClockChange} enum */ + ClockChange: ClockChange, + /** {@link Context} class */ Context: Context, diff --git a/lib/clock.js b/lib/clock.js index 576585e4..ecf7efe0 100644 --- a/lib/clock.js +++ b/lib/clock.js @@ -17,6 +17,7 @@ const rclnodejs = require('./native_loader.js'); const Time = require('./time.js'); const ClockType = require('./clock_type.js'); +const ClockChange = require('./clock_change.js'); const { TypeValidationError } = require('./errors.js'); /** @@ -121,6 +122,111 @@ 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 which when shut down will cause this sleep to wake early. + * If context is null, then the default context is used. + * @return {Promise} Promise that resolves to true if 'until' was reached, + * or false if it woke for another reason (time jump, context shutdown). + * @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) { + const ClockEvent = require('./clock_event.js'); + const Context = require('./context.js'); + + 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} 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); + } } /** diff --git a/lib/clock_change.js b/lib/clock_change.js new file mode 100644 index 00000000..1a57ac71 --- /dev/null +++ b/lib/clock_change.js @@ -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; diff --git a/src/rcl_timer_bindings.cpp b/src/rcl_timer_bindings.cpp index b19dc9f1..b95eb734 100644 --- a/src/rcl_timer_bindings.cpp +++ b/src/rcl_timer_bindings.cpp @@ -272,6 +272,7 @@ Napi::Value GetTimerNextCallTime(const Napi::CallbackInfo& info) { return env.Null(); } else { THROW_ERROR_IF_NOT_EQUAL(RCL_RET_OK, ret, rcl_get_error_string().str); + return env.Undefined(); // Safeguard return, should not reach here } } #endif diff --git a/test/test-clock-change-integration.js b/test/test-clock-change-integration.js new file mode 100644 index 00000000..5b2c07d8 --- /dev/null +++ b/test/test-clock-change-integration.js @@ -0,0 +1,170 @@ +'use strict'; + +const assert = require('assert'); +const rclnodejs = require('../index.js'); +const { Clock, ROSClock, ClockType, ClockChange, Duration, Time } = rclnodejs; + +describe('ClockChange integration with sleep methods', function () { + this.timeout(5000); + + before(async function () { + await rclnodejs.init(); + }); + + after(function () { + rclnodejs.shutdown(); + }); + + it('should use ClockChange enum in clock jump callback', function (done) { + const clock = new ROSClock(); + let callbackInvoked = false; + let receivedClockChange = null; + + const callbackObj = { + _pre_callback: () => {}, + _post_callback: (jumpInfo) => { + callbackInvoked = true; + receivedClockChange = jumpInfo.clock_change; + + // Verify the clock_change value matches ClockChange enum + assert.ok( + receivedClockChange === ClockChange.ROS_TIME_NO_CHANGE || + receivedClockChange === ClockChange.ROS_TIME_ACTIVATED || + receivedClockChange === ClockChange.ROS_TIME_DEACTIVATED || + receivedClockChange === ClockChange.SYSTEM_TIME_NO_CHANGE, + 'clock_change should be a valid ClockChange enum value' + ); + }, + }; + + // Add callback with threshold + clock.addClockCallback(callbackObj, true, 0n, 0n); + + // Enable ROS time override to trigger a clock change + clock.isRosTimeActive = true; + + // Set time to trigger callback + const timePoint = new Time(100n, 0n, ClockType.ROS_TIME); + clock.rosTimeOverride = timePoint; + + // Give it some time to process + setTimeout(() => { + clock.removeClockCallback(callbackObj); + + assert.ok( + callbackInvoked, + 'Clock jump callback should have been invoked' + ); + assert.ok( + receivedClockChange !== null, + 'Should have received clock_change value' + ); + + // The change should be ROS_TIME_ACTIVATED since we enabled ROS time + assert.strictEqual( + receivedClockChange, + ClockChange.ROS_TIME_ACTIVATED, + 'Clock change should be ROS_TIME_ACTIVATED' + ); + + done(); + }, 100); + }); + + it('should detect ROS_TIME_ACTIVATED in sleep callback', async function () { + const clock = new ROSClock(); + clock.isRosTimeActive = false; // Start with ROS time disabled + + // Create a promise that will race with the sleep + let detectedActivation = false; + + const callbackObj = { + _pre_callback: () => {}, + _post_callback: (jumpInfo) => { + if (jumpInfo.clock_change === ClockChange.ROS_TIME_ACTIVATED) { + detectedActivation = true; + } + }, + }; + + clock.addClockCallback(callbackObj, true, 0n, 0n); + + // Trigger ROS time activation after a short delay + setTimeout(() => { + clock.isRosTimeActive = true; + const timePoint = new Time(200n, 0n, ClockType.ROS_TIME); + clock.rosTimeOverride = timePoint; + }, 50); + + // Wait a bit for callback to trigger + await new Promise((resolve) => setTimeout(resolve, 150)); + + clock.removeClockCallback(callbackObj); + + assert.ok( + detectedActivation, + 'Should have detected ROS_TIME_ACTIVATED event' + ); + }); + + it('should use ClockChange constants for meaningful comparisons', function () { + // Verify that the enum values are what we expect + assert.strictEqual( + ClockChange.ROS_TIME_ACTIVATED, + 2, + 'ROS_TIME_ACTIVATED should be 2' + ); + assert.strictEqual( + ClockChange.ROS_TIME_DEACTIVATED, + 3, + 'ROS_TIME_DEACTIVATED should be 3' + ); + + // Test that we can use the enum for comparisons + const testValue = 2; + assert.ok( + testValue === ClockChange.ROS_TIME_ACTIVATED, + 'Should be able to compare numeric values with enum' + ); + }); + + it('should provide better code readability with ClockChange enum', function () { + // This test demonstrates the improved readability + const clockChangeValue = 2; + + // Old way (magic number): + const isActivatedOldWay = clockChangeValue === 2; + + // New way (enum): + const isActivatedNewWay = + clockChangeValue === ClockChange.ROS_TIME_ACTIVATED; + + assert.strictEqual( + isActivatedOldWay, + isActivatedNewWay, + 'Both approaches should give same result' + ); + + // The enum approach is more self-documenting + assert.ok( + isActivatedNewWay, + 'Using ClockChange.ROS_TIME_ACTIVATED is more meaningful' + ); + }); + + it('ClockChange enum should be accessible via rclnodejs module', function () { + const rclnodejs = require('../index.js'); + + assert.ok(rclnodejs.ClockChange, 'ClockChange should be exported'); + assert.strictEqual( + typeof rclnodejs.ClockChange, + 'object', + 'ClockChange should be an object' + ); + assert.strictEqual( + rclnodejs.ClockChange.ROS_TIME_ACTIVATED, + 2, + 'ROS_TIME_ACTIVATED should be accessible' + ); + }); +}); diff --git a/test/test-clock-change.js b/test/test-clock-change.js new file mode 100644 index 00000000..be64e614 --- /dev/null +++ b/test/test-clock-change.js @@ -0,0 +1,67 @@ +'use strict'; + +const assert = require('assert'); +const rclnodejs = require('../index.js'); +const { ClockChange } = rclnodejs; + +describe('ClockChange enum', function () { + it('should define all clock change constants', function () { + assert.strictEqual(typeof ClockChange, 'object'); + assert.strictEqual(typeof ClockChange.ROS_TIME_NO_CHANGE, 'number'); + assert.strictEqual(typeof ClockChange.ROS_TIME_ACTIVATED, 'number'); + assert.strictEqual(typeof ClockChange.ROS_TIME_DEACTIVATED, 'number'); + assert.strictEqual(typeof ClockChange.SYSTEM_TIME_NO_CHANGE, 'number'); + }); + + it('should have correct numeric values', function () { + assert.strictEqual(ClockChange.ROS_TIME_NO_CHANGE, 1); + assert.strictEqual(ClockChange.ROS_TIME_ACTIVATED, 2); + assert.strictEqual(ClockChange.ROS_TIME_DEACTIVATED, 3); + assert.strictEqual(ClockChange.SYSTEM_TIME_NO_CHANGE, 4); + }); + + it('should match RCL constants', function () { + // These values should match the C/C++ rcl_clock_change_t enum + // RCL_ROS_TIME_NO_CHANGE = 1 + // RCL_ROS_TIME_ACTIVATED = 2 + // RCL_ROS_TIME_DEACTIVATED = 3 + // RCL_SYSTEM_TIME_NO_CHANGE = 4 + assert.strictEqual(ClockChange.ROS_TIME_NO_CHANGE, 1); + assert.strictEqual(ClockChange.ROS_TIME_ACTIVATED, 2); + assert.strictEqual(ClockChange.ROS_TIME_DEACTIVATED, 3); + assert.strictEqual(ClockChange.SYSTEM_TIME_NO_CHANGE, 4); + }); + + it('should be exportable from rclnodejs module', function () { + const { ClockChange: ImportedClockChange } = require('../index.js'); + assert.strictEqual(ImportedClockChange, ClockChange); + assert.strictEqual(ImportedClockChange.ROS_TIME_ACTIVATED, 2); + }); + + it('should have all expected properties', function () { + const expectedKeys = [ + 'ROS_TIME_NO_CHANGE', + 'ROS_TIME_ACTIVATED', + 'ROS_TIME_DEACTIVATED', + 'SYSTEM_TIME_NO_CHANGE', + ]; + + const actualKeys = Object.keys(ClockChange); + expectedKeys.forEach((key) => { + assert.ok( + actualKeys.includes(key), + `ClockChange should have property ${key}` + ); + }); + }); + + it('should have distinct values', function () { + const values = Object.values(ClockChange); + const uniqueValues = new Set(values); + assert.strictEqual( + values.length, + uniqueValues.size, + 'All ClockChange values should be unique' + ); + }); +}); diff --git a/test/test-clock-sleep.js b/test/test-clock-sleep.js new file mode 100644 index 00000000..3cdfcaf6 --- /dev/null +++ b/test/test-clock-sleep.js @@ -0,0 +1,203 @@ +'use strict'; + +const assert = require('assert'); +const rclnodejs = require('../index.js'); +const { Clock, ROSClock, ClockType, Duration, Time } = rclnodejs; + +describe('Clock sleep methods', function () { + this.timeout(15000); + + before(async function () { + await rclnodejs.init(); + }); + + after(function () { + rclnodejs.shutdown(); + }); + + describe('sleepFor', function () { + it('should sleep for the specified duration with STEADY_TIME', async function () { + const clock = new Clock(ClockType.STEADY_TIME); + const duration = new Duration(0n, 200000000n); // 200 milliseconds + + const start = Date.now(); + const result = await clock.sleepFor(duration); + const elapsed = Date.now() - start; + + assert.strictEqual(result, true, 'Sleep should return true'); + assert.ok( + elapsed >= 190 && elapsed <= 250, + `Elapsed time ${elapsed}ms should be approximately 200ms` + ); + }); + + it('should sleep for the specified duration with SYSTEM_TIME', async function () { + const clock = new Clock(ClockType.SYSTEM_TIME); + const duration = new Duration(0n, 150000000n); // 150 milliseconds + + const start = Date.now(); + const result = await clock.sleepFor(duration); + const elapsed = Date.now() - start; + + assert.strictEqual(result, true, 'Sleep should return true'); + assert.ok( + elapsed >= 140 && elapsed <= 200, + `Elapsed time ${elapsed}ms should be approximately 150ms` + ); + }); + + it('should sleep for the specified duration with ROSClock', async function () { + const clock = new ROSClock(); + const duration = new Duration(0n, 100000000n); // 100 milliseconds + + const start = Date.now(); + const result = await clock.sleepFor(duration); + const elapsed = Date.now() - start; + + assert.strictEqual(result, true, 'Sleep should return true'); + assert.ok( + elapsed >= 90 && elapsed <= 150, + `Elapsed time ${elapsed}ms should be approximately 100ms` + ); + }); + + it('should sleep for 1 second', async function () { + const clock = new Clock(ClockType.STEADY_TIME); + const duration = new Duration(1n, 0n); // 1 second + + const startTime = clock.now(); + const result = await clock.sleepFor(duration); + const endTime = clock.now(); + + const elapsed = endTime.nanoseconds - startTime.nanoseconds; + assert.strictEqual(result, true, 'Sleep should return true'); + assert.ok( + elapsed >= 950000000n && elapsed <= 1100000000n, + `Elapsed time should be approximately 1 second, got ${Number(elapsed) / 1e9}s` + ); + }); + }); + + describe('sleepUntil', function () { + it('should sleep until the specified time with STEADY_TIME', async function () { + const clock = new Clock(ClockType.STEADY_TIME); + const currentTime = clock.now(); + const targetTime = new Time( + 0n, + currentTime.nanoseconds + 200000000n, // +200ms + ClockType.STEADY_TIME + ); + + const start = Date.now(); + const result = await clock.sleepUntil(targetTime); + const elapsed = Date.now() - start; + + assert.strictEqual(result, true, 'Sleep should return true'); + assert.ok( + elapsed >= 190 && elapsed <= 250, + `Elapsed time ${elapsed}ms should be approximately 200ms` + ); + }); + + it('should sleep until the specified time with SYSTEM_TIME', async function () { + const clock = new Clock(ClockType.SYSTEM_TIME); + const currentTime = clock.now(); + const targetTime = new Time( + 0n, + currentTime.nanoseconds + 150000000n, // +150ms + ClockType.SYSTEM_TIME + ); + + const start = Date.now(); + const result = await clock.sleepUntil(targetTime); + const elapsed = Date.now() - start; + + assert.strictEqual(result, true, 'Sleep should return true'); + assert.ok( + elapsed >= 140 && elapsed <= 200, + `Elapsed time ${elapsed}ms should be approximately 150ms` + ); + }); + + it('should throw TypeError for mismatched clock types', async function () { + const clock = new Clock(ClockType.STEADY_TIME); + const targetTime = new Time(0n, 1000000000n, ClockType.SYSTEM_TIME); + + try { + await clock.sleepUntil(targetTime); + assert.fail('Should have thrown TypeError'); + } catch (err) { + assert.ok(err instanceof TypeError); + assert.ok( + err.message.includes('clock type'), + 'Error message should mention clock type mismatch' + ); + } + }); + + it('should sleep until a time in the past (return immediately)', async function () { + const clock = new Clock(ClockType.STEADY_TIME); + const currentTime = clock.now(); + // Set target time in the past + const targetTime = new Time( + 0n, + currentTime.nanoseconds - 1000000000n, // -1 second + ClockType.STEADY_TIME + ); + + const start = Date.now(); + const result = await clock.sleepUntil(targetTime); + const elapsed = Date.now() - start; + + // Should return almost immediately since target is in the past + assert.ok( + elapsed < 50, + `Should return quickly for past time, took ${elapsed}ms` + ); + // Result depends on implementation - it reached the target time immediately + assert.ok(result === true || result === false); + }); + }); + + describe('error handling', function () { + it('should throw error for invalid context', async function () { + const clock = new Clock(ClockType.STEADY_TIME); + const Context = require('../lib/context.js'); + const invalidContext = new Context(); + // Don't initialize the context + + const duration = new Duration(0n, 100000000n); + + try { + await clock.sleepFor(duration, invalidContext); + assert.fail('Should have thrown error for invalid context'); + } catch (err) { + assert.ok( + err.message.includes('Context') || + err.message.includes('initialized') || + err.message.includes('shutdown') + ); + } + }); + }); + + describe('integration with ROSClock', function () { + it('should work with ROSClock when ROS time is inactive', async function () { + const clock = new ROSClock(); + // Ensure ROS time is not active (should behave like system time) + clock.isRosTimeActive = false; + + const duration = new Duration(0n, 100000000n); // 100ms + + const start = Date.now(); + const result = await clock.sleepFor(duration); + const elapsed = Date.now() - start; + + assert.strictEqual(result, true); + assert.ok( + elapsed >= 90 && elapsed <= 150, + `Elapsed time ${elapsed}ms should be approximately 100ms` + ); + }); + }); +}); diff --git a/test/types/index.test-d.ts b/test/types/index.test-d.ts index c8047007..2b25c6e3 100644 --- a/test/types/index.test-d.ts +++ b/test/types/index.test-d.ts @@ -365,6 +365,54 @@ expectType(clock.now()); expectType(clock.addClockCallback({}, true, 0n, 0n)); expectType(clock.removeClockCallback({})); +// Clock sleep methods +const sleepDuration = new rclnodejs.Duration(1n, 0n); +const sleepTargetTime = new rclnodejs.Time( + 0n, + 1000000000n, + rclnodejs.ClockType.SYSTEM_TIME +); +const sleepContext = rclnodejs.Context.defaultContext(); + +// sleepFor signatures +expectType>(clock.sleepFor(sleepDuration)); +expectType>(clock.sleepFor(sleepDuration, sleepContext)); +expectType>(clock.sleepFor(sleepDuration, null)); +expectType>(clock.sleepFor(sleepDuration, undefined)); + +// sleepUntil signatures +expectType>(clock.sleepUntil(sleepTargetTime)); +expectType>(clock.sleepUntil(sleepTargetTime, sleepContext)); +expectType>(clock.sleepUntil(sleepTargetTime, null)); +expectType>(clock.sleepUntil(sleepTargetTime, undefined)); + +// ROSClock sleep methods +const rosClock = new rclnodejs.ROSClock(); +expectType>(rosClock.sleepFor(sleepDuration)); +expectType>(rosClock.sleepUntil(sleepTargetTime)); + +// ---- ClockChange ----- +expectAssignable( + rclnodejs.ClockChange.ROS_TIME_NO_CHANGE +); +expectAssignable( + rclnodejs.ClockChange.ROS_TIME_ACTIVATED +); +expectAssignable( + rclnodejs.ClockChange.ROS_TIME_DEACTIVATED +); +expectAssignable( + rclnodejs.ClockChange.SYSTEM_TIME_NO_CHANGE +); + +// ClockChange in callback +const clockCallback: rclnodejs.ClockCallbackObject = { + _post_callback: (jumpInfo) => { + expectType(jumpInfo.clock_change); + expectType(jumpInfo.delta); + }, +}; + // ---- Logging ----- const logger = rclnodejs.Logging.getLogger('test_logger'); expectType(logger); diff --git a/types/clock.d.ts b/types/clock.d.ts index 275fcfa7..ce58bf74 100644 --- a/types/clock.d.ts +++ b/types/clock.d.ts @@ -6,7 +6,7 @@ declare module 'rclnodejs' { /** * Type of clock change that occurred. */ - clock_change: number; + clock_change: ClockChange; /** * Time delta in nanoseconds. @@ -74,6 +74,41 @@ declare module 'rclnodejs' { * @returns The current time. */ now(): Time; + + /** + * 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 until - Time at which this function should stop sleeping. + * @param context - Context which when shut down will cause this sleep to wake early. + * If context is null or undefined, then the default context is used. + * @returns Promise that resolves to true if 'until' was reached, + * or false if it woke for another reason (time jump, context shutdown). + * @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. + */ + sleepUntil(until: Time, context?: Context | null): Promise; + + /** + * 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 of time to sleep for. + * @param context - Context which when shut down will cause this sleep to wake early. + * If context is null or undefined, then the default context is used. + * @returns 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. + */ + sleepFor(duration: Duration, context?: Context | null): Promise; } /** diff --git a/types/clock_change.d.ts b/types/clock_change.d.ts new file mode 100644 index 00000000..1cf48c9e --- /dev/null +++ b/types/clock_change.d.ts @@ -0,0 +1,27 @@ +declare module 'rclnodejs' { + /** + * Clock change type identifiers + * Represents the type of clock change that occurred during a time jump. + */ + enum ClockChange { + /** + * The source before and after the jump is ROS_TIME. + */ + ROS_TIME_NO_CHANGE = 1, + + /** + * The source switched to ROS_TIME from SYSTEM_TIME. + */ + ROS_TIME_ACTIVATED = 2, + + /** + * The source switched to SYSTEM_TIME from ROS_TIME. + */ + ROS_TIME_DEACTIVATED = 3, + + /** + * The source before and after the jump is SYSTEM_TIME. + */ + SYSTEM_TIME_NO_CHANGE = 4, + } +} diff --git a/types/index.d.ts b/types/index.d.ts index 18a7e01e..653e3ad0 100644 --- a/types/index.d.ts +++ b/types/index.d.ts @@ -1,5 +1,6 @@ /// /// +/// /// import { ChildProcess } from 'child_process'; From 659c6363df676013490ba1bff41f4cc8365e3706 Mon Sep 17 00:00:00 2001 From: Minggang Wang Date: Tue, 23 Dec 2025 16:21:42 +0800 Subject: [PATCH 06/10] Address comments --- lib/clock.js | 19 +++++++++++++------ src/clock_event.cpp | 15 +++++++++++++++ test/test-clock-change-integration.js | 2 +- test/types/index.test-d.ts | 2 +- 4 files changed, 30 insertions(+), 8 deletions(-) diff --git a/lib/clock.js b/lib/clock.js index ecf7efe0..97d1ae09 100644 --- a/lib/clock.js +++ b/lib/clock.js @@ -18,6 +18,8 @@ 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'); /** @@ -131,16 +133,21 @@ class Clock { * this function to cease sleeping and return false. * * @param {Time} until - Time at which this function should stop sleeping. - * @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} Promise that resolves to true if 'until' was reached, - * or false if it woke for another reason (time jump, context shutdown). + * @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} 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) { - const ClockEvent = require('./clock_event.js'); - const Context = require('./context.js'); + if (!(until instanceof Time)) { + throw new TypeValidationError('until', until, 'Time', { + entityType: 'clock', + }); + } if (!context) { context = Context.defaultContext(); diff --git a/src/clock_event.cpp b/src/clock_event.cpp index f94b0498..9070daa4 100644 --- a/src/clock_event.cpp +++ b/src/clock_event.cpp @@ -176,6 +176,11 @@ Napi::Value ClockEventWaitUntilSteady(const Napi::CallbackInfo& info) { bool lossless; int64_t until = info[2].As().Int64Value(&lossless); + if (!lossless) { + Napi::TypeError::New(env, "until value lost precision during conversion") + .ThrowAsJavaScriptException(); + return env.Undefined(); + } auto worker = new ClockEventWaitWorker(env, event, clock, until, WaitType::Steady); @@ -193,6 +198,11 @@ Napi::Value ClockEventWaitUntilSystem(const Napi::CallbackInfo& info) { bool lossless; int64_t until = info[2].As().Int64Value(&lossless); + if (!lossless) { + Napi::TypeError::New(env, "until value lost precision during conversion") + .ThrowAsJavaScriptException(); + return env.Undefined(); + } auto worker = new ClockEventWaitWorker(env, event, clock, until, WaitType::System); @@ -210,6 +220,11 @@ Napi::Value ClockEventWaitUntilRos(const Napi::CallbackInfo& info) { bool lossless; int64_t until = info[2].As().Int64Value(&lossless); + if (!lossless) { + Napi::TypeError::New(env, "until value lost precision during conversion") + .ThrowAsJavaScriptException(); + return env.Undefined(); + } auto worker = new ClockEventWaitWorker(env, event, clock, until, WaitType::Ros); diff --git a/test/test-clock-change-integration.js b/test/test-clock-change-integration.js index 5b2c07d8..126e2552 100644 --- a/test/test-clock-change-integration.js +++ b/test/test-clock-change-integration.js @@ -2,7 +2,7 @@ const assert = require('assert'); const rclnodejs = require('../index.js'); -const { Clock, ROSClock, ClockType, ClockChange, Duration, Time } = rclnodejs; +const { Clock, ROSClock, ClockType, ClockChange, Time } = rclnodejs; describe('ClockChange integration with sleep methods', function () { this.timeout(5000); diff --git a/test/types/index.test-d.ts b/test/types/index.test-d.ts index 2b25c6e3..0c41c4f9 100644 --- a/test/types/index.test-d.ts +++ b/test/types/index.test-d.ts @@ -406,7 +406,7 @@ expectAssignable( ); // ClockChange in callback -const clockCallback: rclnodejs.ClockCallbackObject = { +const _clockCallback: rclnodejs.ClockCallbackObject = { _post_callback: (jumpInfo) => { expectType(jumpInfo.clock_change); expectType(jumpInfo.delta); From 8e0762019ac0894a5d9b8ff569b3a299bd3a73a4 Mon Sep 17 00:00:00 2001 From: Minggang Wang Date: Tue, 23 Dec 2025 16:43:27 +0800 Subject: [PATCH 07/10] Merge clock change tests into one js file --- test/test-clock-change-integration.js | 170 -------------------------- test/test-clock-change.js | 167 ++++++++++++++++++++++++- 2 files changed, 166 insertions(+), 171 deletions(-) delete mode 100644 test/test-clock-change-integration.js diff --git a/test/test-clock-change-integration.js b/test/test-clock-change-integration.js deleted file mode 100644 index 126e2552..00000000 --- a/test/test-clock-change-integration.js +++ /dev/null @@ -1,170 +0,0 @@ -'use strict'; - -const assert = require('assert'); -const rclnodejs = require('../index.js'); -const { Clock, ROSClock, ClockType, ClockChange, Time } = rclnodejs; - -describe('ClockChange integration with sleep methods', function () { - this.timeout(5000); - - before(async function () { - await rclnodejs.init(); - }); - - after(function () { - rclnodejs.shutdown(); - }); - - it('should use ClockChange enum in clock jump callback', function (done) { - const clock = new ROSClock(); - let callbackInvoked = false; - let receivedClockChange = null; - - const callbackObj = { - _pre_callback: () => {}, - _post_callback: (jumpInfo) => { - callbackInvoked = true; - receivedClockChange = jumpInfo.clock_change; - - // Verify the clock_change value matches ClockChange enum - assert.ok( - receivedClockChange === ClockChange.ROS_TIME_NO_CHANGE || - receivedClockChange === ClockChange.ROS_TIME_ACTIVATED || - receivedClockChange === ClockChange.ROS_TIME_DEACTIVATED || - receivedClockChange === ClockChange.SYSTEM_TIME_NO_CHANGE, - 'clock_change should be a valid ClockChange enum value' - ); - }, - }; - - // Add callback with threshold - clock.addClockCallback(callbackObj, true, 0n, 0n); - - // Enable ROS time override to trigger a clock change - clock.isRosTimeActive = true; - - // Set time to trigger callback - const timePoint = new Time(100n, 0n, ClockType.ROS_TIME); - clock.rosTimeOverride = timePoint; - - // Give it some time to process - setTimeout(() => { - clock.removeClockCallback(callbackObj); - - assert.ok( - callbackInvoked, - 'Clock jump callback should have been invoked' - ); - assert.ok( - receivedClockChange !== null, - 'Should have received clock_change value' - ); - - // The change should be ROS_TIME_ACTIVATED since we enabled ROS time - assert.strictEqual( - receivedClockChange, - ClockChange.ROS_TIME_ACTIVATED, - 'Clock change should be ROS_TIME_ACTIVATED' - ); - - done(); - }, 100); - }); - - it('should detect ROS_TIME_ACTIVATED in sleep callback', async function () { - const clock = new ROSClock(); - clock.isRosTimeActive = false; // Start with ROS time disabled - - // Create a promise that will race with the sleep - let detectedActivation = false; - - const callbackObj = { - _pre_callback: () => {}, - _post_callback: (jumpInfo) => { - if (jumpInfo.clock_change === ClockChange.ROS_TIME_ACTIVATED) { - detectedActivation = true; - } - }, - }; - - clock.addClockCallback(callbackObj, true, 0n, 0n); - - // Trigger ROS time activation after a short delay - setTimeout(() => { - clock.isRosTimeActive = true; - const timePoint = new Time(200n, 0n, ClockType.ROS_TIME); - clock.rosTimeOverride = timePoint; - }, 50); - - // Wait a bit for callback to trigger - await new Promise((resolve) => setTimeout(resolve, 150)); - - clock.removeClockCallback(callbackObj); - - assert.ok( - detectedActivation, - 'Should have detected ROS_TIME_ACTIVATED event' - ); - }); - - it('should use ClockChange constants for meaningful comparisons', function () { - // Verify that the enum values are what we expect - assert.strictEqual( - ClockChange.ROS_TIME_ACTIVATED, - 2, - 'ROS_TIME_ACTIVATED should be 2' - ); - assert.strictEqual( - ClockChange.ROS_TIME_DEACTIVATED, - 3, - 'ROS_TIME_DEACTIVATED should be 3' - ); - - // Test that we can use the enum for comparisons - const testValue = 2; - assert.ok( - testValue === ClockChange.ROS_TIME_ACTIVATED, - 'Should be able to compare numeric values with enum' - ); - }); - - it('should provide better code readability with ClockChange enum', function () { - // This test demonstrates the improved readability - const clockChangeValue = 2; - - // Old way (magic number): - const isActivatedOldWay = clockChangeValue === 2; - - // New way (enum): - const isActivatedNewWay = - clockChangeValue === ClockChange.ROS_TIME_ACTIVATED; - - assert.strictEqual( - isActivatedOldWay, - isActivatedNewWay, - 'Both approaches should give same result' - ); - - // The enum approach is more self-documenting - assert.ok( - isActivatedNewWay, - 'Using ClockChange.ROS_TIME_ACTIVATED is more meaningful' - ); - }); - - it('ClockChange enum should be accessible via rclnodejs module', function () { - const rclnodejs = require('../index.js'); - - assert.ok(rclnodejs.ClockChange, 'ClockChange should be exported'); - assert.strictEqual( - typeof rclnodejs.ClockChange, - 'object', - 'ClockChange should be an object' - ); - assert.strictEqual( - rclnodejs.ClockChange.ROS_TIME_ACTIVATED, - 2, - 'ROS_TIME_ACTIVATED should be accessible' - ); - }); -}); diff --git a/test/test-clock-change.js b/test/test-clock-change.js index be64e614..44e81c46 100644 --- a/test/test-clock-change.js +++ b/test/test-clock-change.js @@ -2,7 +2,7 @@ const assert = require('assert'); const rclnodejs = require('../index.js'); -const { ClockChange } = rclnodejs; +const { Clock, ROSClock, ClockType, ClockChange, Time } = rclnodejs; describe('ClockChange enum', function () { it('should define all clock change constants', function () { @@ -65,3 +65,168 @@ describe('ClockChange enum', function () { ); }); }); + +describe('ClockChange integration with sleep methods', function () { + this.timeout(5000); + + before(async function () { + await rclnodejs.init(); + }); + + after(function () { + rclnodejs.shutdown(); + }); + + it('should use ClockChange enum in clock jump callback', function (done) { + const clock = new ROSClock(); + let callbackInvoked = false; + let receivedClockChange = null; + + const callbackObj = { + _pre_callback: () => {}, + _post_callback: (jumpInfo) => { + callbackInvoked = true; + receivedClockChange = jumpInfo.clock_change; + + // Verify the clock_change value matches ClockChange enum + assert.ok( + receivedClockChange === ClockChange.ROS_TIME_NO_CHANGE || + receivedClockChange === ClockChange.ROS_TIME_ACTIVATED || + receivedClockChange === ClockChange.ROS_TIME_DEACTIVATED || + receivedClockChange === ClockChange.SYSTEM_TIME_NO_CHANGE, + 'clock_change should be a valid ClockChange enum value' + ); + }, + }; + + // Add callback with threshold + clock.addClockCallback(callbackObj, true, 0n, 0n); + + // Enable ROS time override to trigger a clock change + clock.isRosTimeActive = true; + + // Set time to trigger callback + const timePoint = new Time(100n, 0n, ClockType.ROS_TIME); + clock.rosTimeOverride = timePoint; + + // Give it some time to process + setTimeout(() => { + clock.removeClockCallback(callbackObj); + + assert.ok( + callbackInvoked, + 'Clock jump callback should have been invoked' + ); + assert.ok( + receivedClockChange !== null, + 'Should have received clock_change value' + ); + + // The change should be ROS_TIME_ACTIVATED since we enabled ROS time + assert.strictEqual( + receivedClockChange, + ClockChange.ROS_TIME_ACTIVATED, + 'Clock change should be ROS_TIME_ACTIVATED' + ); + + done(); + }, 100); + }); + + it('should detect ROS_TIME_ACTIVATED in sleep callback', async function () { + const clock = new ROSClock(); + clock.isRosTimeActive = false; // Start with ROS time disabled + + // Create a promise that will race with the sleep + let detectedActivation = false; + + const callbackObj = { + _pre_callback: () => {}, + _post_callback: (jumpInfo) => { + if (jumpInfo.clock_change === ClockChange.ROS_TIME_ACTIVATED) { + detectedActivation = true; + } + }, + }; + + clock.addClockCallback(callbackObj, true, 0n, 0n); + + // Trigger ROS time activation after a short delay + setTimeout(() => { + clock.isRosTimeActive = true; + const timePoint = new Time(200n, 0n, ClockType.ROS_TIME); + clock.rosTimeOverride = timePoint; + }, 50); + + // Wait a bit for callback to trigger + await new Promise((resolve) => setTimeout(resolve, 150)); + + clock.removeClockCallback(callbackObj); + + assert.ok( + detectedActivation, + 'Should have detected ROS_TIME_ACTIVATED event' + ); + }); + + it('should use ClockChange constants for meaningful comparisons', function () { + // Verify that the enum values are what we expect + assert.strictEqual( + ClockChange.ROS_TIME_ACTIVATED, + 2, + 'ROS_TIME_ACTIVATED should be 2' + ); + assert.strictEqual( + ClockChange.ROS_TIME_DEACTIVATED, + 3, + 'ROS_TIME_DEACTIVATED should be 3' + ); + + // Test that we can use the enum for comparisons + const testValue = 2; + assert.ok( + testValue === ClockChange.ROS_TIME_ACTIVATED, + 'Should be able to compare numeric values with enum' + ); + }); + + it('should provide better code readability with ClockChange enum', function () { + // This test demonstrates the improved readability + const clockChangeValue = 2; + + // Old way (magic number): + const isActivatedOldWay = clockChangeValue === 2; + + // New way (enum): + const isActivatedNewWay = + clockChangeValue === ClockChange.ROS_TIME_ACTIVATED; + + assert.strictEqual( + isActivatedOldWay, + isActivatedNewWay, + 'Both approaches should give same result' + ); + + // The enum approach is more self-documenting + assert.ok( + isActivatedNewWay, + 'Using ClockChange.ROS_TIME_ACTIVATED is more meaningful' + ); + }); + + it('ClockChange enum should be accessible via rclnodejs module', function () { + const rclnodejs = require('../index.js'); + + assert.ok(rclnodejs.ClockChange, 'ClockChange should be exported'); + assert.strictEqual( + typeof rclnodejs.ClockChange, + 'object', + 'ClockChange should be an object' + ); + assert.strictEqual( + rclnodejs.ClockChange.ROS_TIME_ACTIVATED, + 2, + 'ROS_TIME_ACTIVATED should be accessible' + ); + }); +}); From 4ae6e042d14b005704104b6e65b5e7eb88f6440f Mon Sep 17 00:00:00 2001 From: Minggang Wang Date: Tue, 23 Dec 2025 17:14:03 +0800 Subject: [PATCH 08/10] Add license --- test/test-clock-change.js | 14 ++++++++++++++ test/test-clock-event.js | 14 ++++++++++++++ test/test-clock-sleep.js | 14 ++++++++++++++ 3 files changed, 42 insertions(+) diff --git a/test/test-clock-change.js b/test/test-clock-change.js index 44e81c46..2270d43d 100644 --- a/test/test-clock-change.js +++ b/test/test-clock-change.js @@ -1,3 +1,17 @@ +// 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 assert = require('assert'); diff --git a/test/test-clock-event.js b/test/test-clock-event.js index bc0c6495..f093d921 100644 --- a/test/test-clock-event.js +++ b/test/test-clock-event.js @@ -1,3 +1,17 @@ +// 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 assert = require('assert'); diff --git a/test/test-clock-sleep.js b/test/test-clock-sleep.js index 3cdfcaf6..dad48f2b 100644 --- a/test/test-clock-sleep.js +++ b/test/test-clock-sleep.js @@ -1,3 +1,17 @@ +// 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 assert = require('assert'); From e88f1bc841ec2730eaef66c3cf70939bf3c0c90b Mon Sep 17 00:00:00 2001 From: Minggang Wang Date: Tue, 23 Dec 2025 17:27:23 +0800 Subject: [PATCH 09/10] Add test for active ros time --- test/test-clock-sleep.js | 82 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 82 insertions(+) diff --git a/test/test-clock-sleep.js b/test/test-clock-sleep.js index dad48f2b..c9a676b3 100644 --- a/test/test-clock-sleep.js +++ b/test/test-clock-sleep.js @@ -213,5 +213,87 @@ describe('Clock sleep methods', function () { `Elapsed time ${elapsed}ms should be approximately 100ms` ); }); + + it('should work with ROSClock when ROS time is active', async function () { + // Create a node with use_sim_time parameter set to true + const options = new rclnodejs.NodeOptions(); + options.parameterOverrides.push( + new rclnodejs.Parameter( + 'use_sim_time', + rclnodejs.ParameterType.PARAMETER_BOOL, + true + ) + ); + + const node = rclnodejs.createNode( + 'TestClockSleepRosTime', + '', + rclnodejs.Context.defaultContext(), + options + ); + + try { + // Set up TimeSource to activate ROS time + const TimeSource = require('../lib/time_source.js'); + const timeSource = new TimeSource(node); + const clock = new ROSClock(); + timeSource.attachClock(clock); + + // Start spinning the node to receive clock messages + rclnodejs.spin(node); + + // Verify ROS time is active + assert.strictEqual(clock.isRosTimeActive, true); + + // Create a clock publisher to simulate time + const clockPub = node.createPublisher( + 'rosgraph_msgs/msg/Clock', + '/clock' + ); + + // Publish initial time: 10 seconds + const startSec = 10; + clockPub.publish({ clock: { sec: startSec, nanosec: 0 } }); + + // Wait a bit for the message to be processed + await new Promise((resolve) => setTimeout(resolve, 100)); + + // Get current ROS time (should be 10 seconds) + const currentRosTime = clock.now(); + assert.ok( + currentRosTime.nanoseconds >= BigInt(startSec) * 1000000000n, + 'ROS time should be at least 10 seconds' + ); + + // Test sleepFor with simulated time + // We'll publish clock messages while sleeping to advance time + const sleepDuration = new Duration(2n, 0n); // 2 seconds of ROS time + + const sleepPromise = clock.sleepFor(sleepDuration); + + // Publish clock messages to advance time + let currentSimTime = startSec; + const publishInterval = setInterval(() => { + currentSimTime += 1; // Advance by 1 second each time + clockPub.publish({ clock: { sec: currentSimTime, nanosec: 0 } }); + }, 100); // Publish every 100ms wall time + + // Wait for sleep to complete + const result = await sleepPromise; + clearInterval(publishInterval); + + assert.strictEqual(result, true, 'Sleep should complete successfully'); + + // Verify that ROS time advanced + const finalRosTime = clock.now(); + assert.ok( + finalRosTime.nanoseconds >= + currentRosTime.nanoseconds + sleepDuration.nanoseconds, + 'ROS time should have advanced by at least the sleep duration' + ); + } finally { + node.destroy(); + } + }); }); }); From d88f55ff4246880ecb8fc72ccb0d6dfc01d60729 Mon Sep 17 00:00:00 2001 From: Minggang Wang Date: Thu, 25 Dec 2025 13:32:55 +0800 Subject: [PATCH 10/10] Add comments for test --- test/test-clock-event.js | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/test/test-clock-event.js b/test/test-clock-event.js index f093d921..6e72a091 100644 --- a/test/test-clock-event.js +++ b/test/test-clock-event.js @@ -59,6 +59,10 @@ describe('ClockEvent', function () { }); it('should wait until ros time', async function () { + // Note: This tests ROS_TIME clock when ROS time override is not enabled. + // In this case, waitUntilRos() falls back to system time behavior. + // For testing with active ROS time (TimeSource + published clock messages), + // see test-clock-sleep.js "should work with ROSClock when ROS time is active" const clock = new Clock(ClockType.ROS_TIME); const event = new rclnodejs.ClockEvent(); const now = clock.now();