Implement rclnodejs.spinOnce (#563)

mattrichard · web-flow · commit 9db8939aacc8 · 2020-02-17T10:58:03.000+08:00
Adds spinOnce support to rclnodejs. Unlike spin which runs in a separate thread, spinOnce will run synchronously and block the caller until the event loop has finished or times-out. Ideally, this should result in more predictable execution of unit tests that rely on the event loop. spinOnce cannot be used if already spinning and will throw error if attempted. Usage: let node = rclnodejs.createNode('my_node'); rclnodejs.spinOnce(node); Fix #562
diff --git a/index.js b/index.js
@@ -190,6 +190,22 @@ let rcl = {
     node.startSpinning(this._context.handle(), timeout);
   },
 
+  /**
+   * Execute one item of work or wait until a timeout expires.
+   * @param {Node} node - The node to be spun.
+   * @param {number} [timeout=10] - ms to wait, block forever if negative, don't wait if 0, default is 10.
+   * @return {undefined}
+   */
+  spinOnce(node, timeout = 10) {
+    if (!(node instanceof rclnodejs.ShadowNode)) {
+      throw new TypeError('Invalid argument.');
+    }
+    if (node.spinning) {
+      throw new Error('The node is already spinning.');
+    }
+    node.spinOnce(this._context.handle(), timeout);
+  },
+
   /**
    * @param {Context} context - The context to be shutdown.
    * @return {undefined}
diff --git a/src/executor.cpp b/src/executor.cpp
@@ -15,7 +15,6 @@
 #include "executor.hpp"
 
 #include <rcl/error_handling.h>
-#include <rcl/wait.h>
 #include <stdexcept>
 #include <string>
 
@@ -55,6 +54,34 @@ void Executor::Start(rcl_context_t* context, int32_t time_out) {
   }
 }
 
+void Executor::SpinOnce(rcl_context_t* context, int32_t time_out) {
+  if (!running_.load()) {
+    try {
+      rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+      rcl_ret_t ret =
+          rcl_wait_set_init(&wait_set, 0, 2, 0, 0, 0, 0, context,
+                            rcl_get_default_allocator());
+      if (ret != RCL_RET_OK) {
+        throw std::runtime_error(std::string("Init waitset failed: ") +
+                                 rcl_get_error_string().str);
+      }
+
+      if (WaitForReadyCallbacks(&wait_set, time_out))
+        ExecuteReadyHandles();
+
+      if (rcl_wait_set_fini(&wait_set) != RCL_RET_OK) {
+        std::string error_message =
+            std::string("Failed to destroy guard waitset:") +
+            std::string(rcl_get_error_string().str);
+        throw std::runtime_error(error_message);
+      }
+    } catch (...) {
+      g_exception_ptr = std::current_exception();
+      ExecuteReadyHandles();
+    }
+  }
+}
+
 void Executor::Stop() {
   if (running_.load()) {
     // Stop thread first, and then uv_close
@@ -82,15 +109,7 @@ void Executor::Stop() {
 
 void Executor::DoWork(uv_async_t* handle) {
   Executor* executor = reinterpret_cast<Executor*>(handle->data);
-  if (executor->delegate_) {
-    if (g_exception_ptr) {
-      executor->delegate_->CatchException(g_exception_ptr);
-      rcl_reset_error();
-      g_exception_ptr = nullptr;
-    }
-    executor->delegate_->Execute(
-        executor->handle_manager_->get_ready_handles());
-  }
+  executor->ExecuteReadyHandles();
 }
 
 void Executor::Run(void* arg) {
@@ -114,55 +133,12 @@ void Executor::Run(void* arg) {
 
       {
         ScopedMutex mutex(handle_manager->mutex());
-        if (handle_manager->is_empty())
-          continue;
-
-        if (rcl_wait_set_resize(&wait_set, handle_manager->subscription_count(),
-                                handle_manager->guard_condition_count() + 1u,
-                                handle_manager->timer_count(),
-                                handle_manager->client_count(),
-                                handle_manager->service_count(),
-                                // TODO(minggang): support events.
-                                0u) != RCL_RET_OK) {
-          std::string error_message = std::string("Failed to resize: ") +
-                                      std::string(rcl_get_error_string().str);
-          throw std::runtime_error(error_message);
-        }
-
-        if (!handle_manager->AddHandlesToWaitSet(&wait_set)) {
-          throw std::runtime_error("Couldn't fill waitset");
-        }
-
-        rcl_wait_set_add_guard_condition(&wait_set, g_sigint_gc, nullptr);
-
-        int64_t time_out =
-            executor->time_out() < 0 ? -1 : RCL_MS_TO_NS(executor->time_out());
-
-        rcl_ret_t status = rcl_wait(&wait_set, time_out);
-        if (status == RCL_RET_WAIT_SET_EMPTY) {
-        } else if (status != RCL_RET_OK && status != RCL_RET_TIMEOUT) {
-          throw std::runtime_error(std::string("rcl_wait() failed: ") +
-                                   rcl_get_error_string().str);
-        } else {
-          if (wait_set.size_of_guard_conditions == 1 &&
-              wait_set.guard_conditions[0]) {
-            executor->running_.store(false);
-          }
-
-          handle_manager->CollectReadyHandles(&wait_set);
-
+        if (executor->WaitForReadyCallbacks(&wait_set, executor->time_out())) {
           if (!uv_is_closing(
                   reinterpret_cast<uv_handle_t*>(executor->async_))) {
             uv_async_send(executor->async_);
           }
         }
-
-        if (rcl_wait_set_clear(&wait_set) != RCL_RET_OK) {
-          std::string error_message =
-              std::string("Failed to clear wait set: ") +
-              std::string(rcl_get_error_string().str);
-          throw std::runtime_error(error_message);
-        }
       }
     }
 
@@ -176,4 +152,64 @@ void Executor::Run(void* arg) {
   }
 }
 
+bool Executor::WaitForReadyCallbacks(
+    rcl_wait_set_t* wait_set, int32_t time_out) {
+  if (handle_manager_->is_empty())
+    return false;
+
+  if (rcl_wait_set_resize(wait_set, handle_manager_->subscription_count(),
+                          handle_manager_->guard_condition_count() + 1u,
+                          handle_manager_->timer_count(),
+                          handle_manager_->client_count(),
+                          handle_manager_->service_count(),
+                          // TODO(minggang): support events.
+                          0u) != RCL_RET_OK) {
+    std::string error_message = std::string("Failed to resize: ") +
+                                std::string(rcl_get_error_string().str);
+    throw std::runtime_error(error_message);
+  }
+
+  if (!handle_manager_->AddHandlesToWaitSet(wait_set)) {
+    throw std::runtime_error("Couldn't fill waitset");
+  }
+
+  rcl_wait_set_add_guard_condition(wait_set, g_sigint_gc, nullptr);
+
+  time_out = time_out < 0 ? -1 : RCL_MS_TO_NS(time_out);
+
+  rcl_ret_t status = rcl_wait(wait_set, time_out);
+  if (status == RCL_RET_WAIT_SET_EMPTY) {
+  } else if (status != RCL_RET_OK && status != RCL_RET_TIMEOUT) {
+    throw std::runtime_error(std::string("rcl_wait() failed: ") +
+                              rcl_get_error_string().str);
+  } else {
+    if (wait_set->size_of_guard_conditions == 1 &&
+        wait_set->guard_conditions[0]) {
+      running_.store(false);
+    }
+
+    handle_manager_->CollectReadyHandles(wait_set);
+  }
+
+  if (rcl_wait_set_clear(wait_set) != RCL_RET_OK) {
+    std::string error_message =
+        std::string("Failed to clear wait set: ") +
+        std::string(rcl_get_error_string().str);
+    throw std::runtime_error(error_message);
+  }
+
+  return status != RCL_RET_WAIT_SET_EMPTY;
+}
+
+void Executor::ExecuteReadyHandles() {
+  if (delegate_) {
+    if (g_exception_ptr) {
+      delegate_->CatchException(g_exception_ptr);
+      rcl_reset_error();
+      g_exception_ptr = nullptr;
+    }
+    delegate_->Execute(handle_manager_->get_ready_handles());
+  }
+}
+
 }  // namespace rclnodejs
diff --git a/src/executor.hpp b/src/executor.hpp
@@ -16,6 +16,7 @@
 #define RCLNODEJS_EXECUTOR_HPP_
 
 #include <uv.h>
+#include <rcl/wait.h>
 
 #include <atomic>
 #include <exception>
@@ -43,12 +44,16 @@ class Executor {
 
   void Start(rcl_context_t* context, int32_t time_out);
   void Stop();
+  void SpinOnce(rcl_context_t* context, int32_t time_out);
   int32_t time_out() { return time_out_; }
 
   static void DoWork(uv_async_t* handle);
   static void Run(void* arg);
 
  private:
+  bool WaitForReadyCallbacks(rcl_wait_set_t* wait_set, int32_t time_out);
+  void ExecuteReadyHandles();
+
   uv_async_t* async_;
   uv_thread_t thread_;
 
diff --git a/src/shadow_node.cpp b/src/shadow_node.cpp
@@ -52,6 +52,7 @@ void ShadowNode::Init(v8::Local<v8::Object> exports) {
   Nan::SetPrototypeMethod(tpl, "start", Start);
   Nan::SetPrototypeMethod(tpl, "stop", Stop);
   Nan::SetPrototypeMethod(tpl, "syncHandles", SyncHandles);
+  Nan::SetPrototypeMethod(tpl, "spinOnce", SpinOnce);
 
   constructor.Reset(tpl->GetFunction());
   exports->Set(Nan::New("ShadowNode").ToLocalChecked(), tpl->GetFunction());
@@ -82,6 +83,11 @@ void ShadowNode::StartRunning(rcl_context_t* context, int32_t timeout) {
   executor_->Start(context, timeout);
 }
 
+void ShadowNode::RunOnce(rcl_context_t* context, int32_t timeout) {
+  handle_manager_->CollectHandles(this->handle());
+  executor_->SpinOnce(context, timeout);
+}
+
 NAN_METHOD(ShadowNode::Start) {
   auto* me = Nan::ObjectWrap::Unwrap<ShadowNode>(info.Holder());
   RclHandle* context_handle = RclHandle::Unwrap<RclHandle>(info[0]->ToObject());
@@ -102,6 +108,18 @@ NAN_METHOD(ShadowNode::Stop) {
   info.GetReturnValue().Set(Nan::Undefined());
 }
 
+NAN_METHOD(ShadowNode::SpinOnce) {
+  auto* me = Nan::ObjectWrap::Unwrap<ShadowNode>(info.Holder());
+  RclHandle* context_handle = RclHandle::Unwrap<RclHandle>(info[0]->ToObject());
+  auto timeout = Nan::To<int32_t>(info[1]).FromJust();
+  rcl_context_t* context =
+      reinterpret_cast<rcl_context_t*>(context_handle->ptr());
+  if (me)
+    me->RunOnce(context, timeout);
+
+  info.GetReturnValue().Set(Nan::Undefined());
+}
+
 NAN_METHOD(ShadowNode::SyncHandles) {
   auto* me = Nan::ObjectWrap::Unwrap<ShadowNode>(info.Holder());
   if (me) {
diff --git a/src/shadow_node.hpp b/src/shadow_node.hpp
@@ -34,6 +34,7 @@ class ShadowNode : public Nan::ObjectWrap,
   static void Init(v8::Local<v8::Object> exports);
   void StartRunning(rcl_context_t* context, int32_t timeout);
   void StopRunning();
+  void RunOnce(rcl_context_t* context, int32_t timeout);
 
   Nan::Persistent<v8::Object>* rcl_handle() { return rcl_handle_.get(); }
   HandleManager* handle_manager() { return handle_manager_.get(); }
@@ -51,6 +52,7 @@ class ShadowNode : public Nan::ObjectWrap,
   static NAN_METHOD(Stop);
   static NAN_METHOD(Start);
   static NAN_METHOD(SyncHandles);
+  static NAN_METHOD(SpinOnce);
   static NAN_GETTER(HandleGetter);
   static NAN_SETTER(HandleSetter);
 
diff --git a/test/test-guard-condition.js b/test/test-guard-condition.js
@@ -15,11 +15,9 @@
 const assert = require('assert');
 const sinon = require('sinon');
 const rclnodejs = require('../index.js');
-const utils = require('./utils.js');
 
 describe('rclnodejs guard condition test suite', function() {
   var node;
-  var timeout = 10;
   this.timeout(60 * 1000);
 
   before(function() {
@@ -32,46 +30,45 @@ describe('rclnodejs guard condition test suite', function() {
 
   beforeEach(function() {
     node = rclnodejs.createNode('guard_node');
-    rclnodejs.spin(node, timeout);
   });
 
   afterEach(function() {
     node.destroy();
   });
 
-  it('Test trigger', async function() {
+  it('Test trigger', function() {
     let callback = sinon.spy();
 
     const gc = node.createGuardCondition(callback);
 
-    await utils.delay(timeout + 1);
+    rclnodejs.spinOnce(node);
     assert(callback.notCalled);
 
     gc.trigger();
-    await utils.delay(timeout + 1);
+    rclnodejs.spinOnce(node);
     assert(callback.calledOnce);
 
     node.destroyGuardCondition(gc);
   });
 
-  it('Test double trigger', async function() {
+  it('Test double trigger', function() {
     let callback1 = sinon.spy();
     let callback2 = sinon.spy();
 
     const gc1 = node.createGuardCondition(callback1);
     const gc2 = node.createGuardCondition(callback2);
 
-    await utils.delay(timeout + 1);
+    rclnodejs.spinOnce(node);
     assert(callback1.notCalled);
     assert(callback2.notCalled);
 
     gc1.trigger();
     gc2.trigger();
-    await utils.delay(timeout + 1);
+    rclnodejs.spinOnce(node);
     assert(callback1.calledOnce);
     assert(callback2.calledOnce);
 
-    await utils.delay(timeout + 1);
+    rclnodejs.spinOnce(node);
     assert(callback1.calledOnce);
     assert(callback2.calledOnce);
 
diff --git a/test/test-spinning.js b/test/test-spinning.js
diff --git a/test/utils.js b/test/utils.js
diff --git a/types/index.d.ts b/types/index.d.ts
