Add support for guard conditions (#561)

Implements guard conditions in rclnodejs. In addition, now only entities that are ready to be handled will be executed. This was initially needed since guard conditions should only run when triggered, but was extended to all entities. The approach taken was passing all RclHandles that are ready to be executed back to the main process.

Fix #315

Author: mattrichard

index.js

@@ -118,6 +118,7 @@ let rcl = {
     node.init(nodeName, namespace);
     debug('Finish initializing node, name = %s and namespace = %s.', nodeName, namespace);
     node.handle = handle;
+    node.context = context;
     this._nodes.push(node);
     return node;
   },

lib/guard_condition.js

@@ -0,0 +1,49 @@
+// 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('bindings')('rclnodejs');
+const Entity = require('./entity.js');
+const Context = require('./context.js');
+
+/**
+ * @class - Class representing a guard condition in ROS
+ * @hideconstructor
+ */
+
+class GuardCondition extends Entity {
+  constructor(handle, callback) {
+    super(handle, null, null);
+
+    this._callback = callback;
+  }
+
+  get callback() {
+    return this._callback;
+  }
+
+  static createGuardCondition(callback, context = Context.defaultContext()) {
+    let handle = rclnodejs.createGuardCondition(context.handle());
+    return new GuardCondition(handle, callback);
+  }
+
+  /**
+   * Triggers the guard condition.
+   * @returns {undefined}
+   */
+  trigger() {
+    rclnodejs.triggerGuardCondition(this.handle);
+  }
+}
+
+module.exports = GuardCondition;

lib/node.js

@@ -24,6 +24,7 @@ const QoS = require('./qos.js');
 const debug = require('debug')('rclnodejs:node');
 const loader = require('./interface_loader.js');
 const Context = require('./context.js');
+const GuardCondition = require('./guard_condition.js');
 /**
  * @class - Class representing a Node in ROS
  * @hideconstructor
@@ -36,6 +37,7 @@ class Node {
     this._clients = [];
     this._services = [];
     this._timers = [];
+    this._guards = [];
     this._name = name;
 
     if (namespace.length === 0) {
@@ -47,15 +49,22 @@ class Node {
     this.spinning = false;
   }
 
-  execute() {
-    this._timers.forEach((timer) => {
+  execute(handles) {
+    let timersReady = this._timers.filter((timer) => handles.indexOf(timer.handle) !== -1);
+    let guardsReady = this._guards.filter((guard) => handles.indexOf(guard.handle) !== -1);
+    let subscriptionsReady = this._subscriptions.filter((subscription) =>
+      handles.indexOf(subscription.handle) !== -1);
+    let clientsReady = this._clients.filter((client) => handles.indexOf(client.handle) !== -1);
+    let servicesReady = this._services.filter((service) => handles.indexOf(service.handle) !== -1);
+
+    timersReady.forEach((timer) => {
       if (timer.isReady()) {
         rclnodejs.callTimer(timer.handle);
         timer.callback();
       }
     });
 
-    this._subscriptions.forEach((subscription) => {
+    subscriptionsReady.forEach((subscription) => {
       let Message = subscription.typeClass;
       let msg = new Message();
       let success = rclnodejs.rclTake(subscription.handle, msg.toRawROS());
@@ -65,7 +74,11 @@ class Node {
       Message.destoryRawROS(msg);
     });
 
-    this._clients.forEach((client) => {
+    guardsReady.forEach((guard) => {
+      guard.callback();
+    });
+
+    clientsReady.forEach((client) => {
       let Response = client.typeClass.Response;
       let response = new Response();
       let success = rclnodejs.rclTakeResponse(client.handle, client.sequenceNumber, response.toRawROS());
@@ -75,7 +88,7 @@ class Node {
       Response.destoryRawROS(response);
     });
 
-    this._services.forEach((service) => {
+    servicesReady.forEach((service) => {
       let Request = service.typeClass.Request;
       let request = new Request();
       let header = rclnodejs.rclTakeRequest(service.handle, this.handle, request.toRawROS());
@@ -313,6 +326,24 @@ class Node {
     return service;
   }
 
+  /**
+   * Create a guard condition.
+   * @param {Function} callback - The callback to be called when the guard condition is triggered.
+   * @return {GuardCondition} - An instance of GuardCondition.
+   */
+  createGuardCondition(callback) {
+    if (typeof (callback) !== 'function') {
+      throw new TypeError('Invalid argument');
+    }
+
+    let guard = GuardCondition.createGuardCondition(callback, this.context);
+    debug('Finish creating guard condition');
+    this._guards.push(guard);
+    this.syncHandles();
+
+    return guard;
+  }
+
   /**
    * Destroy all resource allocated by this node, including
    * <code>Timer</code>s/<code>Publisher</code>s/<code>Subscription</code>s
@@ -330,6 +361,7 @@ class Node {
     this._subscriptions = [];
     this._clients = [];
     this._services = [];
+    this._guards = [];
   }
 
   /**
@@ -392,6 +424,18 @@ class Node {
     this._destroyEntity(timer, this._timers);
   }
 
+  /**
+   * Destroy a guard condition.
+   * @param {GuardCondition} guard - The guard condition to be destroyed.
+   * @return {undefined}
+   */
+  destroyGuardCondition(guard) {
+    if (!(guard instanceof GuardCondition)) {
+      throw new TypeError('Invalid argument');
+    }
+    this._destroyEntity(guard, this._guards);
+  }
+
   /* Get the name of the node.
    * @return {string}
    */

package.json

@@ -36,6 +36,7 @@
     "dtslint": "^2.0.2",
     "eslint": "^5.14.1",
     "mocha": "^6.0.1",
+    "sinon": "^8.1.1",
     "tree-kill": "^1.2.1",
     "typescript": "^3.7.2"
   },

src/executor.cpp

@@ -88,7 +88,8 @@ void Executor::DoWork(uv_async_t* handle) {
       rcl_reset_error();
       g_exception_ptr = nullptr;
     }
-    executor->delegate_->Execute();
+    executor->delegate_->Execute(
+        executor->handle_manager_->get_ready_handles());
   }
 }
 
@@ -117,8 +118,8 @@ void Executor::Run(void* arg) {
           continue;
 
         if (rcl_wait_set_resize(&wait_set, handle_manager->subscription_count(),
-                                // TODO(minggang): support guard conditions
-                                1u, handle_manager->timer_count(),
+                                handle_manager->guard_condition_count() + 1u,
+                                handle_manager->timer_count(),
                                 handle_manager->client_count(),
                                 handle_manager->service_count(),
                                 // TODO(minggang): support events.
@@ -147,6 +148,9 @@ void Executor::Run(void* arg) {
               wait_set.guard_conditions[0]) {
             executor->running_.store(false);
           }
+
+          handle_manager->CollectReadyHandles(&wait_set);
+
           if (!uv_is_closing(
                   reinterpret_cast<uv_handle_t*>(executor->async_))) {
             uv_async_send(executor->async_);

src/executor.hpp

@@ -19,6 +19,9 @@
 
 #include <atomic>
 #include <exception>
+#include <vector>
+
+#include "rcl_handle.hpp"
 
 struct rcl_context_t;
 
@@ -30,7 +33,8 @@ class Executor {
  public:
   class Delegate {
    public:
-    virtual void Execute() = 0;
+    virtual void Execute(
+        const std::vector<rclnodejs::RclHandle *>& handles) = 0;
     virtual void CatchException(std::exception_ptr e_ptr) = 0;
   };
 

src/handle_manager.cpp

@@ -16,7 +16,6 @@
 
 #include <vector>
 
-#include "rcl_handle.hpp"
 #include "spdlog/spdlog.h"
 
 namespace rclnodejs {
@@ -47,45 +46,92 @@ void HandleManager::CollectHandles(const v8::Local<v8::Object> node) {
         Nan::Get(node, Nan::New("_clients").ToLocalChecked());
     Nan::MaybeLocal<v8::Value> services =
         Nan::Get(node, Nan::New("_services").ToLocalChecked());
+    Nan::MaybeLocal<v8::Value> guard_conditions =
+        Nan::Get(node, Nan::New("_guards").ToLocalChecked());
 
     CollectHandlesByType(timers.ToLocalChecked()->ToObject(), &timers_);
     CollectHandlesByType(subscriptions.ToLocalChecked()->ToObject(),
                          &subscriptions_);
     CollectHandlesByType(clients.ToLocalChecked()->ToObject(), &clients_);
     CollectHandlesByType(services.ToLocalChecked()->ToObject(), &services_);
+    CollectHandlesByType(guard_conditions.ToLocalChecked()->ToObject(),
+                         &guard_conditions_);
   }
 
   is_synchronizing_.store(false);
   uv_sem_post(&sem_);
 
   SPDLOG_DEBUG(
       spdlog::get("rclnodejs"),
-      "Add {0:d} timers, {1:d} subscriptions, {2:d} clients, {3:d} services.",
-      timers_.size(), subscriptions_.size(), clients_.size(), services_.size());
+      "Add {0:d} timers, {1:d} subscriptions, {2:d} clients, " +
+          "{3:d} services, {4:d} guards.",
+      timers_.size(),
+      subscriptions_.size(),
+      clients_.size(),
+      services_.size(),
+      guard_conditions_.size());
 }
 
 bool HandleManager::AddHandlesToWaitSet(rcl_wait_set_t* wait_set) {
   for (auto& timer : timers_) {
-    if (rcl_wait_set_add_timer(wait_set, timer, nullptr) != RCL_RET_OK)
+    rcl_timer_t* rcl_timer = reinterpret_cast<rcl_timer_t*>(timer->ptr());
+    if (rcl_wait_set_add_timer(wait_set, rcl_timer, nullptr) != RCL_RET_OK)
       return false;
   }
   for (auto& subscription : subscriptions_) {
-    if (rcl_wait_set_add_subscription(wait_set, subscription, nullptr) !=
+    rcl_subscription_t* rcl_subscription  =
+        reinterpret_cast<rcl_subscription_t*>(subscription->ptr());
+    if (rcl_wait_set_add_subscription(wait_set, rcl_subscription, nullptr) !=
         RCL_RET_OK)
       return false;
   }
   for (auto& client : clients_) {
-    if (rcl_wait_set_add_client(wait_set, client, nullptr) != RCL_RET_OK)
+    rcl_client_t* rcl_client = reinterpret_cast<rcl_client_t*>(client->ptr());
+    if (rcl_wait_set_add_client(wait_set, rcl_client, nullptr) != RCL_RET_OK)
       return false;
   }
   for (auto& service : services_) {
-    if (rcl_wait_set_add_service(wait_set, service, nullptr) != RCL_RET_OK)
+    rcl_service_t* rcl_service =
+        reinterpret_cast<rcl_service_t*>(service->ptr());
+    if (rcl_wait_set_add_service(wait_set, rcl_service, nullptr) != RCL_RET_OK)
+      return false;
+  }
+  for (auto& guard_condition : guard_conditions_) {
+    rcl_guard_condition_t* rcl_guard_condition =
+        reinterpret_cast<rcl_guard_condition_t*>(guard_condition->ptr());
+    if (rcl_wait_set_add_guard_condition(wait_set, rcl_guard_condition, nullptr)
+        !=  RCL_RET_OK)
       return false;
   }
 
   return true;
 }
 
+void HandleManager::CollectReadyHandles(rcl_wait_set_t* wait_set) {
+  ready_handles_.clear();
+
+  CollectReadyHandlesByType(
+    wait_set->subscriptions,
+    wait_set->size_of_subscriptions,
+    subscriptions_);
+  CollectReadyHandlesByType(
+    wait_set->clients,
+    wait_set->size_of_clients,
+    clients_);
+  CollectReadyHandlesByType(
+    wait_set->services,
+    wait_set->size_of_services,
+    services_);
+  CollectReadyHandlesByType(
+    wait_set->timers,
+    wait_set->size_of_timers,
+    timers_);
+  CollectReadyHandlesByType(
+    wait_set->guard_conditions,
+    wait_set->size_of_guard_conditions,
+    guard_conditions_);
+}
+
 void HandleManager::ClearHandles() {
   timers_.clear();
   clients_.clear();
@@ -94,10 +140,9 @@ void HandleManager::ClearHandles() {
   guard_conditions_.clear();
 }
 
-template <typename T>
 void HandleManager::CollectHandlesByType(
     const v8::Local<v8::Object>& typeObject,
-    std::vector<const T*>* vec) {
+    std::vector<rclnodejs::RclHandle*>* vec) {
   Nan::HandleScope scope;
 
   if (typeObject->IsArray()) {
@@ -112,7 +157,23 @@ void HandleManager::CollectHandlesByType(
       rclnodejs::RclHandle* rcl_handle =
           rclnodejs::RclHandle::Unwrap<rclnodejs::RclHandle>(
               handle.ToLocalChecked()->ToObject());
-      vec->push_back(reinterpret_cast<T*>(rcl_handle->ptr()));
+      vec->push_back(rcl_handle);
+    }
+  }
+}
+
+template<typename T>
+void HandleManager::CollectReadyHandlesByType(
+    const T** struct_ptr,
+    size_t size,
+    const std::vector<rclnodejs::RclHandle*>& handles) {
+  for (size_t idx = 0; idx < size; ++idx) {
+    if (struct_ptr[idx]) {
+      for (auto& handle : handles) {
+        if (struct_ptr[idx] == handle->ptr()) {
+          ready_handles_.push_back(handle);
+        }
+      }
     }
   }
 }