[rclcpp] Add class Waitable (#589)

* [rclcpp] Add class Waitable

Provides a virtual API for interacting with wait sets.

* [rclcpp] Add node interface for Waitables

* [rclcpp] Implement node interface for Waitables

* [rclcpp] Integrate Waitable entities with executor

* Implement remaining logic for integrating Waitables

* Add visibility macros and other refactoring to Waitable class

* Return zero size for entities in a Waitable by default

* Bugfix: Clear list of waitable handles

* Bugfix: update Waitable handle list based on readiness

* Bugfix: update for loop condition

* Give node a node_waitables_

* Give lifecycle node a node_waitables

rclcpp/CMakeLists.txt

@@ -54,6 +54,7 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/node_interfaces/node_services.cpp
   src/rclcpp/node_interfaces/node_timers.cpp
   src/rclcpp/node_interfaces/node_topics.cpp
+  src/rclcpp/node_interfaces/node_waitables.cpp
   src/rclcpp/parameter.cpp
   src/rclcpp/parameter_value.cpp
   src/rclcpp/parameter_client.cpp
@@ -68,6 +69,7 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/timer.cpp
   src/rclcpp/type_support.cpp
   src/rclcpp/utilities.cpp
+  src/rclcpp/waitable.cpp
 )
 
 # "watch" template for changes

rclcpp/include/rclcpp/any_executable.hpp

@@ -25,6 +25,7 @@
 #include "rclcpp/subscription.hpp"
 #include "rclcpp/timer.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rclcpp/waitable.hpp"
 
 namespace rclcpp
 {
@@ -45,6 +46,7 @@ struct AnyExecutable
   rclcpp::TimerBase::SharedPtr timer;
   rclcpp::ServiceBase::SharedPtr service;
   rclcpp::ClientBase::SharedPtr client;
+  rclcpp::Waitable::SharedPtr waitable;
   // These are used to keep the scope on the containing items
   rclcpp::callback_group::CallbackGroup::SharedPtr callback_group;
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base;

rclcpp/include/rclcpp/callback_group.hpp

@@ -25,6 +25,7 @@
 #include "rclcpp/subscription.hpp"
 #include "rclcpp/timer.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rclcpp/waitable.hpp"
 
 namespace rclcpp
 {
@@ -35,6 +36,7 @@ namespace node_interfaces
 class NodeServices;
 class NodeTimers;
 class NodeTopics;
+class NodeWaitables;
 }  // namespace node_interfaces
 
 namespace callback_group
@@ -51,6 +53,7 @@ class CallbackGroup
   friend class rclcpp::node_interfaces::NodeServices;
   friend class rclcpp::node_interfaces::NodeTimers;
   friend class rclcpp::node_interfaces::NodeTopics;
+  friend class rclcpp::node_interfaces::NodeWaitables;
 
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(CallbackGroup)
@@ -74,6 +77,10 @@ public:
   const std::vector<rclcpp::ClientBase::WeakPtr> &
   get_client_ptrs() const;
 
+  RCLCPP_PUBLIC
+  const std::vector<rclcpp::Waitable::WeakPtr> &
+  get_waitable_ptrs() const;
+
   RCLCPP_PUBLIC
   std::atomic_bool &
   can_be_taken_from();
@@ -101,6 +108,10 @@ protected:
   void
   add_client(const rclcpp::ClientBase::SharedPtr client_ptr);
 
+  RCLCPP_PUBLIC
+  void
+  add_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr);
+
   CallbackGroupType type_;
   // Mutex to protect the subsequent vectors of pointers.
   mutable std::mutex mutex_;
@@ -108,6 +119,7 @@ protected:
   std::vector<rclcpp::TimerBase::WeakPtr> timer_ptrs_;
   std::vector<rclcpp::ServiceBase::WeakPtr> service_ptrs_;
   std::vector<rclcpp::ClientBase::WeakPtr> client_ptrs_;
+  std::vector<rclcpp::Waitable::WeakPtr> waitable_ptrs_;
   std::atomic_bool can_be_taken_from_;
 };
 

rclcpp/include/rclcpp/memory_strategy.hpp

@@ -25,6 +25,7 @@
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rclcpp/waitable.hpp"
 
 namespace rclcpp
 {
@@ -52,6 +53,7 @@ public:
   virtual size_t number_of_ready_clients() const = 0;
   virtual size_t number_of_ready_timers() const = 0;
   virtual size_t number_of_guard_conditions() const = 0;
+  virtual size_t number_of_waitables() const = 0;
 
   virtual bool add_handles_to_wait_set(rcl_wait_set_t * wait_set) = 0;
   virtual void clear_handles() = 0;
@@ -76,6 +78,11 @@ public:
     rclcpp::executor::AnyExecutable & any_exec,
     const WeakNodeVector & weak_nodes) = 0;
 
+  virtual void
+  get_next_waitable(
+    rclcpp::executor::AnyExecutable & any_exec,
+    const WeakNodeVector & weak_nodes) = 0;
+
   virtual rcl_allocator_t
   get_allocator() = 0;
 
@@ -113,6 +120,11 @@ public:
   get_group_by_client(
     rclcpp::ClientBase::SharedPtr client,
     const WeakNodeVector & weak_nodes);
+
+  static rclcpp::callback_group::CallbackGroup::SharedPtr
+  get_group_by_waitable(
+    rclcpp::Waitable::SharedPtr waitable,
+    const WeakNodeVector & weak_nodes);
 };
 
 }  // namespace memory_strategy

rclcpp/include/rclcpp/node.hpp

@@ -49,6 +49,7 @@
 #include "rclcpp/node_interfaces/node_services_interface.hpp"
 #include "rclcpp/node_interfaces/node_timers_interface.hpp"
 #include "rclcpp/node_interfaces/node_topics_interface.hpp"
+#include "rclcpp/node_interfaces/node_waitables_interface.hpp"
 #include "rclcpp/parameter.hpp"
 #include "rclcpp/publisher.hpp"
 #include "rclcpp/service.hpp"
@@ -442,6 +443,11 @@ public:
   rclcpp::node_interfaces::NodeServicesInterface::SharedPtr
   get_node_services_interface();
 
+  /// Return the Node's internal NodeWaitablesInterface implementation.
+  RCLCPP_PUBLIC
+  rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr
+  get_node_waitables_interface();
+
   /// Return the Node's internal NodeParametersInterface implementation.
   RCLCPP_PUBLIC
   rclcpp::node_interfaces::NodeParametersInterface::SharedPtr
@@ -462,6 +468,7 @@ private:
   rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
   rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_;
   rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_;
+  rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_;
 
   bool use_intra_process_comms_;
 };

rclcpp/include/rclcpp/node_interfaces/node_waitables.hpp

@@ -0,0 +1,60 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// 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 RCLCPP__NODE_INTERFACES__NODE_WAITABLES_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_WAITABLES_HPP_
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/client.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_waitables_interface.hpp"
+#include "rclcpp/waitable.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Implementation of the NodeWaitables part of the Node API.
+class NodeWaitables : public NodeWaitablesInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeWaitables)
+
+  RCLCPP_PUBLIC
+  explicit NodeWaitables(rclcpp::node_interfaces::NodeBaseInterface * node_base);
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeWaitables();
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  add_waitable(
+    rclcpp::Waitable::SharedPtr waitable_base_ptr,
+    rclcpp::callback_group::CallbackGroup::SharedPtr group);
+
+private:
+  RCLCPP_DISABLE_COPY(NodeWaitables)
+
+  rclcpp::node_interfaces::NodeBaseInterface * node_base_;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_WAITABLES_HPP_

rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp

@@ -0,0 +1,45 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// 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 RCLCPP__NODE_INTERFACES__NODE_WAITABLES_INTERFACE_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_WAITABLES_INTERFACE_HPP_
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rclcpp/waitable.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Pure virtual interface class for the NodeWaitables part of the Node API.
+class NodeWaitablesInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeWaitablesInterface)
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  add_waitable(
+    rclcpp::Waitable::SharedPtr waitable_ptr,
+    rclcpp::callback_group::CallbackGroup::SharedPtr group) = 0;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_WAITABLES_INTERFACE_HPP_

rclcpp/include/rclcpp/rclcpp.hpp

@@ -151,5 +151,6 @@
 #include "rclcpp/time.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rclcpp/waitable.hpp"
 
 #endif  // RCLCPP__RCLCPP_HPP_

rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp

@@ -87,30 +87,42 @@ public:
     service_handles_.clear();
     client_handles_.clear();
     timer_handles_.clear();
+    waitable_handles_.clear();
   }
 
   virtual void remove_null_handles(rcl_wait_set_t * wait_set)
   {
-    for (size_t i = 0; i < wait_set->size_of_subscriptions; ++i) {
+    // TODO(jacobperron): Check if wait set sizes are what we expect them to be?
+    //                    e.g. wait_set->size_of_clients == client_handles_.size()
+
+    // Important to use subscription_handles_.size() instead of wait set's size since
+    // there may be more subscriptions in the wait set due to Waitables added to the end.
+    // The same logic applies for other entities.
+    for (size_t i = 0; i < subscription_handles_.size(); ++i) {
       if (!wait_set->subscriptions[i]) {
         subscription_handles_[i].reset();
       }
     }
-    for (size_t i = 0; i < wait_set->size_of_services; ++i) {
+    for (size_t i = 0; i < service_handles_.size(); ++i) {
       if (!wait_set->services[i]) {
         service_handles_[i].reset();
       }
     }
-    for (size_t i = 0; i < wait_set->size_of_clients; ++i) {
+    for (size_t i = 0; i < client_handles_.size(); ++i) {
       if (!wait_set->clients[i]) {
         client_handles_[i].reset();
       }
     }
-    for (size_t i = 0; i < wait_set->size_of_timers; ++i) {
+    for (size_t i = 0; i < timer_handles_.size(); ++i) {
       if (!wait_set->timers[i]) {
         timer_handles_[i].reset();
       }
     }
+    for (size_t i = 0; i < waitable_handles_.size(); ++i) {
+      if (!waitable_handles_[i]->is_ready(wait_set)) {
+        waitable_handles_[i].reset();
+      }
+    }
 
     subscription_handles_.erase(
       std::remove(subscription_handles_.begin(), subscription_handles_.end(), nullptr),
@@ -131,6 +143,11 @@ public:
       std::remove(timer_handles_.begin(), timer_handles_.end(), nullptr),
       timer_handles_.end()
     );
+
+    waitable_handles_.erase(
+      std::remove(waitable_handles_.begin(), waitable_handles_.end(), nullptr),
+      waitable_handles_.end()
+    );
   }
 
   bool collect_entities(const WeakNodeVector & weak_nodes)
@@ -175,6 +192,12 @@ public:
             timer_handles_.push_back(timer->get_timer_handle());
           }
         }
+        for (auto & weak_waitable : group->get_waitable_ptrs()) {
+          auto waitable = weak_waitable.lock();
+          if (waitable) {
+            waitable_handles_.push_back(waitable);
+          }
+        }
       }
     }
     return has_invalid_weak_nodes;
@@ -227,6 +250,15 @@ public:
         return false;
       }
     }
+
+    for (auto waitable : waitable_handles_) {
+      if (!waitable->add_to_wait_set(wait_set)) {
+        RCUTILS_LOG_ERROR_NAMED(
+          "rclcpp",
+          "Couldn't add waitable to wait set: %s", rcl_get_error_string().str);
+        return false;
+      }
+    }
     return true;
   }
 
@@ -342,6 +374,39 @@ public:
     }
   }
 
+  virtual void
+  get_next_waitable(executor::AnyExecutable & any_exec, const WeakNodeVector & weak_nodes)
+  {
+    auto it = waitable_handles_.begin();
+    while (it != waitable_handles_.end()) {
+      auto waitable = *it;
+      if (waitable) {
+        // Find the group for this handle and see if it can be serviced
+        auto group = get_group_by_waitable(waitable, weak_nodes);
+        if (!group) {
+          // Group was not found, meaning the waitable is not valid...
+          // Remove it from the ready list and continue looking
+          it = waitable_handles_.erase(it);
+          continue;
+        }
+        if (!group->can_be_taken_from().load()) {
+          // Group is mutually exclusive and is being used, so skip it for now
+          // Leave it to be checked next time, but continue searching
+          ++it;
+          continue;
+        }
+        // Otherwise it is safe to set and return the any_exec
+        any_exec.waitable = waitable;
+        any_exec.callback_group = group;
+        any_exec.node_base = get_node_by_group(group, weak_nodes);
+        waitable_handles_.erase(it);
+        return;
+      }
+      // Else, the waitable is no longer valid, remove it and continue
+      it = waitable_handles_.erase(it);
+    }
+  }
+
   virtual rcl_allocator_t get_allocator()
   {
     return rclcpp::allocator::get_rcl_allocator<void *, VoidAlloc>(*allocator_.get());
@@ -349,27 +414,52 @@ public:
 
   size_t number_of_ready_subscriptions() const
   {
-    return subscription_handles_.size();
+    size_t number_of_subscriptions = subscription_handles_.size();
+    for (auto waitable : waitable_handles_) {
+      number_of_subscriptions += waitable->get_number_of_ready_subscriptions();
+    }
+    return number_of_subscriptions;
   }
 
   size_t number_of_ready_services() const
   {
-    return service_handles_.size();
+    size_t number_of_services = service_handles_.size();
+    for (auto waitable : waitable_handles_) {
+      number_of_services += waitable->get_number_of_ready_services();
+    }
+    return number_of_services;
   }
 
   size_t number_of_ready_clients() const
   {
-    return client_handles_.size();
+    size_t number_of_clients = client_handles_.size();
+    for (auto waitable : waitable_handles_) {
+      number_of_clients += waitable->get_number_of_ready_clients();
+    }
+    return number_of_clients;
   }
 
   size_t number_of_guard_conditions() const
   {
-    return guard_conditions_.size();
+    size_t number_of_guard_conditions = guard_conditions_.size();
+    for (auto waitable : waitable_handles_) {
+      number_of_guard_conditions += waitable->get_number_of_ready_guard_conditions();
+    }
+    return number_of_guard_conditions;
   }
 
   size_t number_of_ready_timers() const
   {
-    return timer_handles_.size();
+    size_t number_of_timers = timer_handles_.size();
+    for (auto waitable : waitable_handles_) {
+      number_of_timers += waitable->get_number_of_ready_timers();
+    }
+    return number_of_timers;
+  }
+
+  size_t number_of_waitables() const
+  {
+    return waitable_handles_.size();
   }
 
 private:
@@ -383,6 +473,7 @@ private:
   VectorRebind<std::shared_ptr<const rcl_service_t>> service_handles_;
   VectorRebind<std::shared_ptr<const rcl_client_t>> client_handles_;
   VectorRebind<std::shared_ptr<const rcl_timer_t>> timer_handles_;
+  VectorRebind<std::shared_ptr<Waitable>> waitable_handles_;
 
   std::shared_ptr<VoidAlloc> allocator_;
 };

rclcpp/include/rclcpp/waitable.hpp

@@ -0,0 +1,141 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// 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 RCLCPP__WAITABLE_HPP_
+#define RCLCPP__WAITABLE_HPP_
+
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+#include "rcl/wait.h"
+
+namespace rclcpp
+{
+
+class Waitable
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Waitable)
+
+  /// Get the number of ready subscriptions
+  /**
+   * Returns a value of 0 by default.
+   * This should be overridden if the Waitable contains one or more subscriptions.
+   * \return The number of subscriptions associated with the Waitable.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  size_t
+  get_number_of_ready_subscriptions();
+
+  /// Get the number of ready timers
+  /**
+   * Returns a value of 0 by default.
+   * This should be overridden if the Waitable contains one or more timers.
+   * \return The number of timers associated with the Waitable.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  size_t
+  get_number_of_ready_timers();
+
+  /// Get the number of ready clients
+  /**
+   * Returns a value of 0 by default.
+   * This should be overridden if the Waitable contains one or more clients.
+   * \return The number of clients associated with the Waitable.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  size_t
+  get_number_of_ready_clients();
+
+  /// Get the number of ready services
+  /**
+   * Returns a value of 0 by default.
+   * This should be overridden if the Waitable contains one or more services.
+   * \return The number of services associated with the Waitable.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  size_t
+  get_number_of_ready_services();
+
+  /// Get the number of ready guard_conditions
+  /**
+   * Returns a value of 0 by default.
+   * This should be overridden if the Waitable contains one or more guard_conditions.
+   * \return The number of guard_conditions associated with the Waitable.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  size_t
+  get_number_of_ready_guard_conditions();
+
+  // TODO(jacobperron): smart pointer?
+  /// Add the Waitable to a wait set.
+  /**
+   * \param[in] wait_set A handle to the wait set to add the Waitable to.
+   * \return `true` if the Waitable is added successfully, `false` otherwise.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  add_to_wait_set(rcl_wait_set_t * wait_set) = 0;
+
+  /// Check if the Waitable is ready.
+  /**
+   * The input wait set should be the same that was used in a previously call to
+   * `add_to_wait_set()`.
+   * The wait set should also have been previously waited on with `rcl_wait()`.
+   *
+   * \param[in] wait_set A handle to the wait set the Waitable was previously added to
+   *   and that has been waited on.
+   * \return `true` if the Waitable is ready, `false` otherwise.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  is_ready(rcl_wait_set_t *) = 0;
+
+  /// Execute any entities of the Waitable that are ready.
+  /**
+   * Before calling this method, the Waitable should be added to a wait set,
+   * waited on, and then updated.
+   *
+   * Example usage:
+   *
+   * ```
+   * // ... create a wait set and a Waitable
+   * // Add the Waitable to the wait set
+   * bool add_ret = waitable.add_to_wait_set(wait_set);
+   * // ... error handling
+   * // Wait
+   * rcl_ret_t wait_ret = rcl_wait(wait_set);
+   * // ... error handling
+   * // Update the Waitable
+   * waitable.update(wait_set);
+   * // Execute any entities of the Waitable that may be ready
+   * waitable.execute();
+   * ```
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  execute() = 0;
+};  // class Waitable
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__WAITABLE_HPP_

rclcpp/src/rclcpp/callback_group.cpp

@@ -51,6 +51,13 @@ CallbackGroup::get_client_ptrs() const
   return client_ptrs_;
 }
 
+const std::vector<rclcpp::Waitable::WeakPtr> &
+CallbackGroup::get_waitable_ptrs() const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  return waitable_ptrs_;
+}
+
 std::atomic_bool &
 CallbackGroup::can_be_taken_from()
 {
@@ -91,3 +98,10 @@ CallbackGroup::add_client(const rclcpp::ClientBase::SharedPtr client_ptr)
   std::lock_guard<std::mutex> lock(mutex_);
   client_ptrs_.push_back(client_ptr);
 }
+
+void
+CallbackGroup::add_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  waitable_ptrs_.push_back(waitable_ptr);
+}

rclcpp/src/rclcpp/executor.cpp

@@ -281,6 +281,9 @@ Executor::execute_any_executable(AnyExecutable & any_exec)
   if (any_exec.client) {
     execute_client(any_exec.client);
   }
+  if (any_exec.waitable) {
+    any_exec.waitable->execute();
+  }
   // Reset the callback_group, regardless of type
   any_exec.callback_group->can_be_taken_from().store(true);
   // Wake the wait, because it may need to be recalculated or work that
@@ -427,6 +430,7 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
     throw std::runtime_error("Couldn't clear wait set");
   }
 
+  // The size of waitables are accounted for in size of the other entities
   rcl_ret_t ret = rcl_wait_set_resize(
     &wait_set_, memory_strategy_->number_of_ready_subscriptions(),
     memory_strategy_->number_of_guard_conditions(), memory_strategy_->number_of_ready_timers(),
@@ -549,6 +553,11 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable)
   if (any_executable.client) {
     return true;
   }
+  // Check the waitables to see if there are any that are ready
+  memory_strategy_->get_next_waitable(any_executable, weak_nodes_);
+  if (any_executable.waitable) {
+    return true;
+  }
   // If there is no ready executable, return a null ptr
   return false;
 }

rclcpp/src/rclcpp/memory_strategy.cpp

@@ -200,3 +200,29 @@ MemoryStrategy::get_group_by_client(
   }
   return nullptr;
 }
+
+rclcpp::callback_group::CallbackGroup::SharedPtr
+MemoryStrategy::get_group_by_waitable(
+  rclcpp::Waitable::SharedPtr waitable,
+  const WeakNodeVector & weak_nodes)
+{
+  for (auto & weak_node : weak_nodes) {
+    auto node = weak_node.lock();
+    if (!node) {
+      continue;
+    }
+    for (auto & weak_group : node->get_callback_groups()) {
+      auto group = weak_group.lock();
+      if (!group) {
+        continue;
+      }
+      for (auto & weak_waitable : group->get_waitable_ptrs()) {
+        auto group_waitable = weak_waitable.lock();
+        if (group_waitable && group_waitable == waitable) {
+          return group;
+        }
+      }
+    }
+  }
+  return nullptr;
+}

rclcpp/src/rclcpp/node.cpp

@@ -31,6 +31,7 @@
 #include "rclcpp/node_interfaces/node_services.hpp"
 #include "rclcpp/node_interfaces/node_timers.hpp"
 #include "rclcpp/node_interfaces/node_topics.hpp"
+#include "rclcpp/node_interfaces/node_waitables.hpp"
 
 using rclcpp::Node;
 using rclcpp::exceptions::throw_from_rcl_error;
@@ -80,6 +81,7 @@ Node::Node(
       node_graph_,
       node_services_
     )),
+  node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())),
   use_intra_process_comms_(use_intra_process_comms)
 {
 }
@@ -276,3 +278,9 @@ Node::get_node_parameters_interface()
 {
   return node_parameters_;
 }
+
+rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr
+Node::get_node_waitables_interface()
+{
+  return node_waitables_;
+}

rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp

@@ -0,0 +1,53 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// 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 "rclcpp/node_interfaces/node_waitables.hpp"
+
+#include <string>
+
+using rclcpp::node_interfaces::NodeWaitables;
+
+NodeWaitables::NodeWaitables(rclcpp::node_interfaces::NodeBaseInterface * node_base)
+: node_base_(node_base)
+{}
+
+NodeWaitables::~NodeWaitables()
+{}
+
+void
+NodeWaitables::add_waitable(
+  rclcpp::Waitable::SharedPtr waitable_ptr,
+  rclcpp::callback_group::CallbackGroup::SharedPtr group)
+{
+  if (group) {
+    if (!node_base_->callback_group_in_node(group)) {
+      // TODO(jacobperron): use custom exception
+      throw std::runtime_error("Cannot create waitable, group not in node.");
+    }
+    group->add_waitable(waitable_ptr);
+  } else {
+    node_base_->get_default_callback_group()->add_waitable(waitable_ptr);
+  }
+
+  // Notify the executor that a new waitable was created using the parent Node.
+  {
+    auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock();
+    if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
+      throw std::runtime_error(
+              std::string("Failed to notify wait set on waitable creation: ") +
+              rmw_get_error_string().str
+      );
+    }
+  }
+}

rclcpp/src/rclcpp/waitable.cpp

@@ -0,0 +1,47 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// 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 "rclcpp/waitable.hpp"
+
+using rclcpp::Waitable;
+
+size_t
+Waitable::get_number_of_ready_subscriptions()
+{
+  return 0u;
+}
+
+size_t
+Waitable::get_number_of_ready_timers()
+{
+  return 0u;
+}
+
+size_t
+Waitable::get_number_of_ready_clients()
+{
+  return 0u;
+}
+
+size_t
+Waitable::get_number_of_ready_services()
+{
+  return 0u;
+}
+
+size_t
+Waitable::get_number_of_ready_guard_conditions()
+{
+  return 0u;
+}

rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp

@@ -44,6 +44,7 @@
 #include "rclcpp/node_interfaces/node_services_interface.hpp"
 #include "rclcpp/node_interfaces/node_timers_interface.hpp"
 #include "rclcpp/node_interfaces/node_topics_interface.hpp"
+#include "rclcpp/node_interfaces/node_waitables_interface.hpp"
 #include "rclcpp/parameter.hpp"
 #include "rclcpp/publisher.hpp"
 #include "rclcpp/service.hpp"
@@ -390,6 +391,11 @@ public:
   rclcpp::node_interfaces::NodeParametersInterface::SharedPtr
   get_node_parameters_interface();
 
+  /// Return the Node's internal NodeWaitablesInterface implementation.
+  RCLCPP_LIFECYCLE_PUBLIC
+  rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr
+  get_node_waitables_interface();
+
   //
   // LIFECYCLE COMPONENTS
   //
@@ -515,6 +521,7 @@ private:
   rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
   rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_;
   rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_;
+  rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_;
 
   bool use_intra_process_comms_;
 

rclcpp_lifecycle/src/lifecycle_node.cpp

@@ -35,6 +35,7 @@
 #include "rclcpp/node_interfaces/node_services.hpp"
 #include "rclcpp/node_interfaces/node_timers.hpp"
 #include "rclcpp/node_interfaces/node_topics.hpp"
+#include "rclcpp/node_interfaces/node_waitables.hpp"
 #include "rclcpp/parameter_service.hpp"
 
 #include "lifecycle_node_interface_impl.hpp"  // implementation
@@ -87,6 +88,7 @@ LifecycleNode::LifecycleNode(
       node_graph_,
       node_services_
     )),
+  node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())),
   use_intra_process_comms_(use_intra_process_comms),
   impl_(new LifecycleNodeInterfaceImpl(node_base_, node_services_))
 {
@@ -297,6 +299,12 @@ LifecycleNode::get_node_parameters_interface()
   return node_parameters_;
 }
 
+rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr
+LifecycleNode::get_node_waitables_interface()
+{
+  return node_waitables_;
+}
+
 
 ////
 bool