diff --git a/index.js b/index.js index 7fd53874..43c02bde 100644 --- a/index.js +++ b/index.js @@ -31,6 +31,7 @@ const validator = require('./lib/validator.js'); const Time = require('./lib/time.js'); const TimeSource = require('./lib/time_source.js'); const { Clock, ROSClock } = require('./lib/clock.js'); +const ClockType = require('./lib/clock_type.js'); const Duration = require('./lib/duration.js'); const Context = require('./lib/context.js'); @@ -99,6 +100,9 @@ let rcl = { /** {@link ROSClock} class */ ROSClock: ROSClock, + /** {@link ClockType} enum */ + ClockType: ClockType, + /** {@link Duration} class */ Duration: Duration, diff --git a/lib/clock.js b/lib/clock.js index 171dae5a..eaddd7c6 100644 --- a/lib/clock.js +++ b/lib/clock.js @@ -32,6 +32,13 @@ class Clock { this._handle = rclnodejs.createClock(this._clockType); } + /** + * @ignore + */ + get handle() { + return this._handle; + } + /** * Get ClockType of this Clock object. * @return {ClockType} Return the type of the clock. diff --git a/lib/node.js b/lib/node.js index 50bb7bf3..343d29a7 100644 --- a/lib/node.js +++ b/lib/node.js @@ -32,6 +32,7 @@ const Publisher = require('./publisher.js'); const QoS = require('./qos.js'); const Service = require('./service.js'); const Subscription = require('./subscription.js'); +const TimeSource = require('./time_source.js'); const Timer = require('./timer.js'); const debug = require('debug')('rclnodejs:node'); @@ -91,6 +92,13 @@ class Node { } } + // Clock that has support for ROS time. + // Note: parameter overrides and parameter event publisher need to be ready at this point + // to be able to declare 'use_sim_time' if it was not declared yet. + this._clock = new Clock.ROSClock(); + this._timeSource = new TimeSource(this); + this._timeSource.attachClock(this._clock); + if (options.startParameterServices) { this._parameterService = new ParameterService(this); this._parameterService.start(); @@ -217,9 +225,15 @@ class Node { * @param {number} period - The number representing period in millisecond. * @param {function} callback - The callback to be called when timeout. * @param {Context} context - The context, default is Context.defaultContext(). + * @param {Clock} clock - The clock which the timer gets time from. * @return {Timer} - An instance of Timer. */ - createTimer(period, callback, context = Context.defaultContext()) { + createTimer( + period, + callback, + context = Context.defaultContext(), + clock = null + ) { if (typeof period !== 'number' || typeof callback !== 'function') { throw new TypeError('Invalid argument'); } @@ -237,7 +251,13 @@ class Node { ); } - let timerHandle = rclnodejs.createTimer(period, context.handle()); + const timerClock = clock || this._clock; + + let timerHandle = rclnodejs.createTimer( + timerClock.handle, + context.handle(), + period + ); let timer = new Timer(timerHandle, period, callback); debug('Finish creating timer, period = %d.', period); this._timers.push(timer); @@ -458,6 +478,7 @@ class Node { } this.handle.release(); + this._clock = null; this._timers = []; this._publishers = []; this._subscriptions = []; @@ -560,6 +581,14 @@ class Node { return this._logger; } + /** + * Get the clock used by the node. + * @returns {Clock} - The nodes clock. + */ + getClock() { + return this._clock; + } + /** * Get the list of published topics discovered by the provided node for the remote node name. * @param {string} nodeName - The name of the node. @@ -1036,7 +1065,7 @@ class Node { PARAMETER_EVENT_MSG_TYPE ))(); - const secondsAndNanos = new Clock.ROSClock().now().secondsAndNanoseconds; + const secondsAndNanos = this._clock.now().secondsAndNanoseconds; parameterEvent.stamp = { sec: secondsAndNanos.seconds, nanosec: secondsAndNanos.nanoseconds, diff --git a/src/rcl_bindings.cpp b/src/rcl_bindings.cpp index be5b78ab..76b279a4 100644 --- a/src/rcl_bindings.cpp +++ b/src/rcl_bindings.cpp @@ -329,31 +329,27 @@ NAN_METHOD(TriggerGuardCondition) { } NAN_METHOD(CreateTimer) { - int64_t period_ms = Nan::To(info[0]).FromJust(); + RclHandle* clock_handle = RclHandle::Unwrap( + Nan::To(info[0]).ToLocalChecked()); + rcl_clock_t* clock = reinterpret_cast(clock_handle->ptr()); RclHandle* context_handle = RclHandle::Unwrap( Nan::To(info[1]).ToLocalChecked()); rcl_context_t* context = reinterpret_cast(context_handle->ptr()); + int64_t period_ms = Nan::To(info[2]).FromJust(); + rcl_timer_t* timer = reinterpret_cast(malloc(sizeof(rcl_timer_t))); *timer = rcl_get_zero_initialized_timer(); - rcl_clock_t* clock = - reinterpret_cast(malloc(sizeof(rcl_clock_t))); - rcl_allocator_t allocator = rcl_get_default_allocator(); - THROW_ERROR_IF_NOT_EQUAL(RCL_RET_OK, - rcl_clock_init(RCL_STEADY_TIME, clock, &allocator), - rcl_get_error_string().str); + THROW_ERROR_IF_NOT_EQUAL( RCL_RET_OK, rcl_timer_init(timer, clock, context, RCL_MS_TO_NS(period_ms), nullptr, rcl_get_default_allocator()), rcl_get_error_string().str); - auto js_obj = RclHandle::NewInstance(timer, nullptr, [timer, clock] { - rcl_ret_t ret_clock = rcl_clock_fini(clock); - free(clock); - rcl_ret_t ret_timer = rcl_timer_fini(timer); - return ret_clock || ret_timer; + auto js_obj = RclHandle::NewInstance(timer, clock_handle, [timer] { + return rcl_timer_fini(timer); }); info.GetReturnValue().Set(js_obj); } diff --git a/test/test-node.js b/test/test-node.js index e4bf3408..d471a3e5 100644 --- a/test/test-node.js +++ b/test/test-node.js @@ -359,6 +359,12 @@ describe('rcl node methods testing', function() { assert.equal(logger.fatal('message fatal'), true); }); + it('node.getClock', function() { + var clock = node.getClock(); + assert.ok(clock); + assert.strictEqual(clock.clockType, rclnodejs.ClockType.ROS_TIME); + }); + it('node.getNodeNames', function() { var nodeNames = node.getNodeNames(); diff --git a/test/types/main.ts b/test/types/main.ts index ad6af5f5..6f24c34f 100644 --- a/test/types/main.ts +++ b/test/types/main.ts @@ -48,6 +48,9 @@ node.namespace(); // $ExpectType Logging node.getLogger(); +// $ExpectType Clock +node.getClock(); + // $ExpectType void rclnodejs.spin(node); diff --git a/types/node.d.ts b/types/node.d.ts index 7879bb7d..decbcbb0 100644 --- a/types/node.d.ts +++ b/types/node.d.ts @@ -1,5 +1,6 @@ // eslint camelcase: ["error", {ignoreImports: true}] +import { Clock } from 'rclnodejs'; import { Logging } from 'rclnodejs'; import { Parameters } from 'rclnodejs'; import { rcl_interfaces as rclinterfaces } from 'rclnodejs'; @@ -165,6 +166,13 @@ declare module 'rclnodejs' { */ getLogger(): Logging; + /** + * Get the clock used by the node. + * + * @returns The nodes clock. + */ + getClock(): Clock; + /** * Get the nodeOptions provided through the constructor. */