Remove fixed guard conditions, add notify guard conditions

* No more fixed guard conditions.

* Add notify guard condition to nodes

rclcpp/CMakeLists.txt

@@ -65,6 +65,7 @@ endmacro()
 
 call_for_each_rmw_implementation(target GENERATE_DEFAULT)
 
+ament_export_dependencies(ament_cmake)
 ament_export_dependencies(rcl_interfaces)
 ament_export_dependencies(rmw)
 ament_export_dependencies(rmw_implementation)

rclcpp/cmake/get_rclcpp_information.cmake

@@ -13,7 +13,7 @@
 # limitations under the License.
 
 #
-# Get all information abut rclcpp for a specific RMW implementation.
+# Get all information about rclcpp for a specific RMW implementation.
 #
 # It sets the common variables _DEFINITIONS, _INCLUDE_DIRS and _LIBRARIES
 # with the given prefix.
@@ -29,7 +29,7 @@ macro(get_rclcpp_information rmw_implementation var_prefix)
   set(${var_prefix}_FOUND TRUE)
 
   # include directories
-  set(${var_prefix}_INCLUDE_DIRS
+  normalize_path(${var_prefix}_INCLUDE_DIRS
     "${rclcpp_DIR}/../../../include")
 
   # libraries
@@ -49,7 +49,7 @@ macro(get_rclcpp_information rmw_implementation var_prefix)
   )
   if(NOT _lib)
     # warn about not existing library and ignore it
-    message(FATAL_ERROR "Package 'rclcpp' doesn't contain the library '${_library_target}'")
+    message(WARNING "Package 'rclcpp' doesn't contain the library '${_library_target}'")
   elseif(NOT IS_ABSOLUTE "${_lib}")
     # the found library must be an absolute path
     message(FATAL_ERROR "Package 'rclcpp' found the library '${_library_target}' at '${_lib}' which is not an absolute path")

rclcpp/include/rclcpp/callback_group.hpp

@@ -66,7 +66,7 @@ public:
   get_service_ptrs() const;
 
   RCLCPP_PUBLIC
-  const std::vector<rclcpp::client::ClientBase::SharedPtr> &
+  const std::vector<rclcpp::client::ClientBase::WeakPtr> &
   get_client_ptrs() const;
 
   RCLCPP_PUBLIC
@@ -100,7 +100,7 @@ private:
   std::vector<rclcpp::subscription::SubscriptionBase::WeakPtr> subscription_ptrs_;
   std::vector<rclcpp::timer::TimerBase::WeakPtr> timer_ptrs_;
   std::vector<rclcpp::service::ServiceBase::SharedPtr> service_ptrs_;
-  std::vector<rclcpp::client::ClientBase::SharedPtr> client_ptrs_;
+  std::vector<rclcpp::client::ClientBase::WeakPtr> client_ptrs_;
   std::atomic_bool can_be_taken_from_;
 };
 

rclcpp/include/rclcpp/client.hpp

@@ -58,9 +58,9 @@ public:
   get_client_handle() const;
 
   virtual std::shared_ptr<void> create_response() = 0;
-  virtual std::shared_ptr<void> create_request_header() = 0;
+  virtual std::shared_ptr<rmw_request_id_t> create_request_header() = 0;
   virtual void handle_response(
-    std::shared_ptr<void> & request_header, std::shared_ptr<void> & response) = 0;
+    std::shared_ptr<rmw_request_id_t> request_header, std::shared_ptr<void> response) = 0;
 
 private:
   RCLCPP_DISABLE_COPY(ClientBase);
@@ -104,20 +104,24 @@ public:
     return std::shared_ptr<void>(new typename ServiceT::Response());
   }
 
-  std::shared_ptr<void> create_request_header()
+  std::shared_ptr<rmw_request_id_t> create_request_header()
   {
     // TODO(wjwwood): This should probably use rmw_request_id's allocator.
     //                (since it is a C type)
-    return std::shared_ptr<void>(new rmw_request_id_t);
+    return std::shared_ptr<rmw_request_id_t>(new rmw_request_id_t);
   }
 
-  void handle_response(std::shared_ptr<void> & request_header, std::shared_ptr<void> & response)
+  void handle_response(std::shared_ptr<rmw_request_id_t> request_header,
+    std::shared_ptr<void> response)
   {
-    auto typed_request_header = std::static_pointer_cast<rmw_request_id_t>(request_header);
+    std::lock_guard<std::mutex> lock(pending_requests_mutex_);
     auto typed_response = std::static_pointer_cast<typename ServiceT::Response>(response);
-    int64_t sequence_number = typed_request_header->sequence_number;
-    // TODO(esteve) this must check if the sequence_number is valid otherwise the
-    // call_promise will be null
+    int64_t sequence_number = request_header->sequence_number;
+    // TODO(esteve) this should throw instead since it is not expected to happen in the first place
+    if (this->pending_requests_.count(sequence_number) == 0) {
+      fprintf(stderr, "Received invalid sequence number. Ignoring...\n");
+      return;
+    }
     auto tuple = this->pending_requests_[sequence_number];
     auto call_promise = std::get<0>(tuple);
     auto callback = std::get<1>(tuple);
@@ -143,6 +147,7 @@ public:
   >
   SharedFuture async_send_request(SharedRequest request, CallbackT && cb)
   {
+    std::lock_guard<std::mutex> lock(pending_requests_mutex_);
     int64_t sequence_number;
     if (RMW_RET_OK != rmw_send_request(get_client_handle(), request.get(), &sequence_number)) {
       // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
@@ -187,6 +192,7 @@ private:
   RCLCPP_DISABLE_COPY(Client);
 
   std::map<int64_t, std::tuple<SharedPromise, CallbackType, SharedFuture>> pending_requests_;
+  std::mutex pending_requests_mutex_;
 };
 
 }  // namespace client

rclcpp/include/rclcpp/executor.hpp

@@ -17,10 +17,12 @@
 
 #include <algorithm>
 #include <cassert>
+#include <chrono>
 #include <cstdlib>
 #include <iostream>
 #include <list>
 #include <memory>
+#include <string>
 #include <vector>
 
 #include "rclcpp/any_executable.hpp"
@@ -42,7 +44,33 @@ namespace executor
  * INTERRUPTED: The future is not complete, spinning was interrupted by Ctrl-C or another error.
  * TIMEOUT: Spinning timed out.
  */
-enum FutureReturnCode {SUCCESS, INTERRUPTED, TIMEOUT};
+enum class FutureReturnCode {SUCCESS, INTERRUPTED, TIMEOUT};
+
+RCLCPP_PUBLIC
+std::ostream &
+operator<<(std::ostream & os, const FutureReturnCode & future_return_code);
+
+RCLCPP_PUBLIC
+std::string
+to_string(const FutureReturnCode & future_return_code);
+
+///
+/**
+ * Options to be passed to the executor constructor.
+ */
+struct ExecutorArgs
+{
+  memory_strategy::MemoryStrategy::SharedPtr memory_strategy;
+  size_t max_conditions = 0;
+};
+
+static inline ExecutorArgs create_default_executor_arguments()
+{
+  ExecutorArgs args;
+  args.memory_strategy = memory_strategies::create_default_strategy();
+  args.max_conditions = 0;
+  return args;
+}
 
 /// Coordinate the order and timing of available communication tasks.
 /**
@@ -62,8 +90,7 @@ public:
   /// Default constructor.
   // \param[in] ms The memory strategy to be used with this executor.
   RCLCPP_PUBLIC
-  explicit Executor(
-    memory_strategy::MemoryStrategy::SharedPtr ms = memory_strategies::create_default_strategy());
+  explicit Executor(const ExecutorArgs & args = create_default_executor_arguments());
 
   /// Default destructor.
   RCLCPP_PUBLIC
@@ -142,7 +169,8 @@ public:
   /**
    * \param[in] executor The executor which will spin the node.
    * \param[in] node_ptr The node to spin.
-   * \param[in] future The future to wait on. If SUCCESS, the future is safe to access after this function
+   * \param[in] future The future to wait on. If SUCCESS, the future is safe to access after this
+     function.
    * \param[in] timeout Optional timeout parameter, which gets passed to Executor::spin_node_once.
      -1 is block forever, 0 is non-blocking.
      If the time spent inside the blocking loop exceeds this timeout, return a TIMEOUT return code.
@@ -160,23 +188,39 @@ public:
     // Check the future before entering the while loop.
     // If the future is already complete, don't try to spin.
     std::future_status status = future.wait_for(std::chrono::seconds(0));
-
-    auto start_time = std::chrono::system_clock::now();
-
-    while (status != std::future_status::ready && rclcpp::utilities::ok()) {
-      spin_once(timeout);
-      if (timeout.count() >= 0) {
-        if (start_time + timeout < std::chrono::system_clock::now()) {
-          return TIMEOUT;
-        }
-      }
-      status = future.wait_for(std::chrono::seconds(0));
-    }
-
-    // If the future completed, and we weren't interrupted by ctrl-C, return the response
     if (status == std::future_status::ready) {
       return FutureReturnCode::SUCCESS;
     }
+
+    auto end_time = std::chrono::steady_clock::now();
+    if (timeout > std::chrono::nanoseconds::zero()) {
+      end_time += timeout;
+    }
+    auto timeout_left = timeout;
+
+    while (rclcpp::utilities::ok()) {
+      // Do one item of work.
+      spin_once(timeout_left);
+      // Check if the future is set, return SUCCESS if it is.
+      status = future.wait_for(std::chrono::seconds(0));
+      if (status == std::future_status::ready) {
+        return FutureReturnCode::SUCCESS;
+      }
+      // If the original timeout is < 0, then this is blocking, never TIMEOUT.
+      if (timeout < std::chrono::nanoseconds::zero()) {
+        continue;
+      }
+      // Otherwise check if we still have time to wait, return TIMEOUT if not.
+      auto now = std::chrono::steady_clock::now();
+      if (now >= end_time) {
+        return FutureReturnCode::TIMEOUT;
+      }
+      // Subtract the elapsed time from the original timeout.
+      using duration_type = std::chrono::duration<int64_t, TimeT>;
+      timeout_left = std::chrono::duration_cast<duration_type>(end_time - now);
+    }
+
+    // The future did not complete before ok() returned false, return INTERRUPTED.
     return FutureReturnCode::INTERRUPTED;
   }
 
@@ -265,6 +309,9 @@ protected:
   /// Guard condition for signaling the rmw layer to wake up for special events.
   rmw_guard_condition_t * interrupt_guard_condition_;
 
+  /// Waitset for managing entities that the rmw layer waits on.
+  rmw_waitset_t * waitset_;
+
   /// The memory strategy: an interface for handling user-defined memory allocation strategies.
   memory_strategy::MemoryStrategy::SharedPtr memory_strategy_;
 
@@ -272,7 +319,6 @@ private:
   RCLCPP_DISABLE_COPY(Executor);
 
   std::vector<std::weak_ptr<rclcpp::node::Node>> weak_nodes_;
-  std::array<void *, 2> guard_cond_handles_;
 };
 
 }  // namespace executor

rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp

@@ -38,7 +38,7 @@ public:
 
   RCLCPP_PUBLIC
   MultiThreadedExecutor(
-    memory_strategy::MemoryStrategy::SharedPtr ms = memory_strategies::create_default_strategy());
+    const executor::ExecutorArgs & args = rclcpp::executor::create_default_executor_arguments());
 
   RCLCPP_PUBLIC
   virtual ~MultiThreadedExecutor();

rclcpp/include/rclcpp/executors/single_threaded_executor.hpp

@@ -47,7 +47,7 @@ public:
   /// Default constructor. See the default constructor for Executor.
   RCLCPP_PUBLIC
   SingleThreadedExecutor(
-    memory_strategy::MemoryStrategy::SharedPtr ms = memory_strategies::create_default_strategy());
+    const executor::ExecutorArgs & args = rclcpp::executor::create_default_executor_arguments());
 
   /// Default destrcutor.
   RCLCPP_PUBLIC

rclcpp/include/rclcpp/memory_strategy.hpp

@@ -49,6 +49,9 @@ public:
   // return the new number of clients
   virtual size_t fill_client_handles(void ** & ptr) = 0;
 
+  // return the new number of guard_conditions
+  virtual size_t fill_guard_condition_handles(void ** & ptr) = 0;
+
   virtual void clear_active_entities() = 0;
 
   virtual void clear_handles() = 0;
@@ -59,6 +62,10 @@ public:
   // \return Shared pointer to the fresh executable.
   virtual rclcpp::executor::AnyExecutable::SharedPtr instantiate_next_executable() = 0;
 
+  virtual void add_guard_condition(const rmw_guard_condition_t * guard_condition) = 0;
+
+  virtual void remove_guard_condition(const rmw_guard_condition_t * guard_condition) = 0;
+
   virtual void
   get_next_subscription(rclcpp::executor::AnyExecutable::SharedPtr any_exec,
     const WeakNodeVector & weak_nodes) = 0;

rclcpp/include/rclcpp/node.hpp

@@ -77,6 +77,9 @@ public:
     const std::string & node_name, rclcpp::context::Context::SharedPtr context,
     bool use_intra_process_comms = false);
 
+  RCLCPP_PUBLIC
+  ~Node();
+
   /// Get the name of the node.
   // \return The name of the node.
   RCLCPP_PUBLIC
@@ -252,6 +255,11 @@ public:
   const CallbackGroupWeakPtrList &
   get_callback_groups() const;
 
+  RCLCPP_PUBLIC
+  rmw_guard_condition_t * get_notify_guard_condition() const;
+
+  std::atomic_bool has_executor;
+
 private:
   RCLCPP_DISABLE_COPY(Node);
 
@@ -277,6 +285,9 @@ private:
 
   mutable std::mutex mutex_;
 
+  /// Guard condition for notifying the Executor of changes to this node.
+  rmw_guard_condition_t * notify_guard_condition_;
+
   std::map<std::string, rclcpp::parameter::ParameterVariant> parameters_;
 
   publisher::Publisher<rcl_interfaces::msg::ParameterEvent>::SharedPtr events_publisher_;

rclcpp/include/rclcpp/node_impl.hpp

@@ -134,6 +134,11 @@ Node::create_publisher(
       shared_publish_callback,
       intra_process_publisher_handle);
   }
+  if (rmw_trigger_guard_condition(notify_guard_condition_) != RMW_RET_OK) {
+    throw std::runtime_error(
+            std::string(
+              "Failed to notify waitset on publisher creation: ") + rmw_get_error_string());
+  }
   return publisher;
 }
 
@@ -243,6 +248,11 @@ Node::create_subscription(
     default_callback_group_->add_subscription(sub_base_ptr);
   }
   number_of_subscriptions_++;
+  if (rmw_trigger_guard_condition(notify_guard_condition_) != RMW_RET_OK) {
+    throw std::runtime_error(
+            std::string(
+              "Failed to notify waitset on subscription creation: ") + rmw_get_error_string());
+  }
   return sub;
 }
 
@@ -289,6 +299,11 @@ Node::create_wall_timer(
     default_callback_group_->add_timer(timer);
   }
   number_of_timers_++;
+  if (rmw_trigger_guard_condition(notify_guard_condition_) != RMW_RET_OK) {
+    throw std::runtime_error(
+            std::string(
+              "Failed to notify waitset on timer creation: ") + rmw_get_error_string());
+  }
   return timer;
 }
 
@@ -333,6 +348,11 @@ Node::create_client(
   }
   number_of_clients_++;
 
+  if (rmw_trigger_guard_condition(notify_guard_condition_) != RMW_RET_OK) {
+    throw std::runtime_error(
+            std::string(
+              "Failed to notify waitset on client creation: ") + rmw_get_error_string());
+  }
   return cli;
 }
 
@@ -374,6 +394,11 @@ Node::create_service(
     default_callback_group_->add_service(serv_base_ptr);
   }
   number_of_services_++;
+  if (rmw_trigger_guard_condition(notify_guard_condition_) != RMW_RET_OK) {
+    throw std::runtime_error(
+            std::string(
+              "Failed to notify waitset on service creation: ") + rmw_get_error_string());
+  }
   return serv;
 }
 

rclcpp/include/rclcpp/service.hpp

@@ -55,9 +55,9 @@ public:
   get_service_handle();
 
   virtual std::shared_ptr<void> create_request() = 0;
-  virtual std::shared_ptr<void> create_request_header() = 0;
+  virtual std::shared_ptr<rmw_request_id_t> create_request_header() = 0;
   virtual void handle_request(
-    std::shared_ptr<void> request_header,
+    std::shared_ptr<rmw_request_id_t> request_header,
     std::shared_ptr<void> request) = 0;
 
 private:
@@ -102,20 +102,20 @@ public:
     return std::shared_ptr<void>(new typename ServiceT::Request());
   }
 
-  std::shared_ptr<void> create_request_header()
+  std::shared_ptr<rmw_request_id_t> create_request_header()
   {
     // TODO(wjwwood): This should probably use rmw_request_id's allocator.
     //                (since it is a C type)
-    return std::shared_ptr<void>(new rmw_request_id_t);
+    return std::shared_ptr<rmw_request_id_t>(new rmw_request_id_t);
   }
 
-  void handle_request(std::shared_ptr<void> request_header, std::shared_ptr<void> request)
+  void handle_request(std::shared_ptr<rmw_request_id_t> request_header,
+    std::shared_ptr<void> request)
   {
     auto typed_request = std::static_pointer_cast<typename ServiceT::Request>(request);
-    auto typed_request_header = std::static_pointer_cast<rmw_request_id_t>(request_header);
     auto response = std::shared_ptr<typename ServiceT::Response>(new typename ServiceT::Response);
-    any_callback_.dispatch(typed_request_header, typed_request, response);
-    send_response(typed_request_header, response);
+    any_callback_.dispatch(request_header, typed_request, response);
+    send_response(request_header, response);
   }
 
   void send_response(

rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp

@@ -23,6 +23,8 @@
 #include "rclcpp/node.hpp"
 #include "rclcpp/visibility_control.hpp"
 
+#include "rmw/types.h"
+
 namespace rclcpp
 {
 namespace memory_strategies
@@ -62,6 +64,26 @@ public:
     allocator_ = std::make_shared<VoidAlloc>();
   }
 
+  void add_guard_condition(const rmw_guard_condition_t * guard_condition)
+  {
+    for (const auto & existing_guard_condition : guard_conditions_) {
+      if (existing_guard_condition == guard_condition) {
+        return;
+      }
+    }
+    guard_conditions_.push_back(guard_condition);
+  }
+
+  void remove_guard_condition(const rmw_guard_condition_t * guard_condition)
+  {
+    for (auto it = guard_conditions_.begin(); it != guard_conditions_.end(); ++it) {
+      if (*it == guard_condition) {
+        guard_conditions_.erase(it);
+        break;
+      }
+    }
+  }
+
   size_t fill_subscriber_handles(void ** & ptr)
   {
     for (auto & subscription : subscriptions_) {
@@ -94,6 +116,17 @@ public:
     return client_handles_.size();
   }
 
+  size_t fill_guard_condition_handles(void ** & ptr)
+  {
+    for (const auto & guard_condition : guard_conditions_) {
+      if (guard_condition) {
+        guard_condition_handles_.push_back(guard_condition->data);
+      }
+    }
+    ptr = guard_condition_handles_.data();
+    return guard_condition_handles_.size();
+  }
+
   void clear_active_entities()
   {
     subscriptions_.clear();
@@ -106,6 +139,7 @@ public:
     subscriber_handles_.clear();
     service_handles_.clear();
     client_handles_.clear();
+    guard_condition_handles_.clear();
   }
 
   void remove_null_handles()
@@ -124,6 +158,11 @@ public:
       std::remove(client_handles_.begin(), client_handles_.end(), nullptr),
       client_handles_.end()
     );
+
+    guard_condition_handles_.erase(
+      std::remove(guard_condition_handles_.begin(), guard_condition_handles_.end(), nullptr),
+      guard_condition_handles_.end()
+    );
   }
 
   bool collect_entities(const WeakNodeVector & weak_nodes)
@@ -151,7 +190,8 @@ public:
             services_.push_back(service);
           }
         }
-        for (auto & client : group->get_client_ptrs()) {
+        for (auto & weak_client : group->get_client_ptrs()) {
+          auto client = weak_client.lock();
           if (client) {
             clients_.push_back(client);
           }
@@ -287,10 +327,12 @@ private:
   VectorRebind<rclcpp::subscription::SubscriptionBase::SharedPtr> subscriptions_;
   VectorRebind<rclcpp::service::ServiceBase::SharedPtr> services_;
   VectorRebind<rclcpp::client::ClientBase::SharedPtr> clients_;
+  VectorRebind<const rmw_guard_condition_t *> guard_conditions_;
 
   VectorRebind<void *> subscriber_handles_;
   VectorRebind<void *> service_handles_;
   VectorRebind<void *> client_handles_;
+  VectorRebind<void *> guard_condition_handles_;
 
   std::shared_ptr<ExecAlloc> executable_allocator_;
   std::shared_ptr<VoidAlloc> allocator_;

rclcpp/include/rclcpp/timer.hpp

@@ -84,10 +84,11 @@ using TimerCallbackType = std::function<void(TimerBase &)>;
 /// Generic timer templated on the clock type. Periodically executes a user-specified callback.
 template<
   typename FunctorT,
-  class Clock = std::chrono::high_resolution_clock,
+  class Clock,
   typename std::enable_if<
-    rclcpp::function_traits::same_arguments<FunctorT, VoidCallbackType>::value ||
-    rclcpp::function_traits::same_arguments<FunctorT, TimerCallbackType>::value
+    (rclcpp::function_traits::same_arguments<FunctorT, VoidCallbackType>::value ||
+    rclcpp::function_traits::same_arguments<FunctorT, TimerCallbackType>::value) &&
+    Clock::is_steady
   >::type * = nullptr
 >
 class GenericTimer : public TimerBase

rclcpp/package.xml

@@ -18,6 +18,8 @@
 
   <depend>rmw_implementation</depend>
 
+  <exec_depend>ament_cmake</exec_depend>
+
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

rclcpp/src/rclcpp/callback_group.cpp

@@ -41,7 +41,7 @@ CallbackGroup::get_service_ptrs() const
   return service_ptrs_;
 }
 
-const std::vector<rclcpp::client::ClientBase::SharedPtr> &
+const std::vector<rclcpp::client::ClientBase::WeakPtr> &
 CallbackGroup::get_client_ptrs() const
 {
   return client_ptrs_;

rclcpp/src/rclcpp/executor.cpp

@@ -12,6 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <algorithm>
+#include <string>
+#include <type_traits>
+
 #include "rclcpp/executor.hpp"
 #include "rclcpp/scope_exit.hpp"
 
@@ -19,15 +23,51 @@
 
 using rclcpp::executor::AnyExecutable;
 using rclcpp::executor::Executor;
+using rclcpp::executor::ExecutorArgs;
+using rclcpp::executor::FutureReturnCode;
 
-Executor::Executor(rclcpp::memory_strategy::MemoryStrategy::SharedPtr ms)
-: spinning(false), interrupt_guard_condition_(rmw_create_guard_condition()),
-  memory_strategy_(ms)
+Executor::Executor(const ExecutorArgs & args)
+: spinning(false),
+  memory_strategy_(args.memory_strategy)
 {
+  interrupt_guard_condition_ = rmw_create_guard_condition();
+  if (!interrupt_guard_condition_) {
+    throw std::runtime_error("Failed to create interrupt guard condition in Executor constructor");
+  }
+
+  // The number of guard conditions is always at least 2: 1 for the ctrl-c guard cond,
+  // and one for the executor's guard cond (interrupt_guard_condition_)
+  // The size of guard conditions is variable because the number of nodes can vary
+
+  // Put the global ctrl-c guard condition in
+  memory_strategy_->add_guard_condition(rclcpp::utilities::get_global_sigint_guard_condition());
+  // Put the executor's guard condition in
+  memory_strategy_->add_guard_condition(interrupt_guard_condition_);
+
+  waitset_ = rmw_create_waitset(args.max_conditions);
+
+  if (!waitset_) {
+    fprintf(stderr,
+      "[rclcpp::error] failed to create waitset: %s\n", rmw_get_error_string_safe());
+    rmw_ret_t status = rmw_destroy_guard_condition(interrupt_guard_condition_);
+    if (status != RMW_RET_OK) {
+      fprintf(stderr,
+        "[rclcpp::error] failed to destroy guard condition: %s\n", rmw_get_error_string_safe());
+    }
+    throw std::runtime_error("Failed to create waitset in Executor constructor");
+  }
 }
 
 Executor::~Executor()
 {
+  // Try to deallocate the waitset.
+  if (waitset_) {
+    rmw_ret_t status = rmw_destroy_waitset(waitset_);
+    if (status != RMW_RET_OK) {
+      fprintf(stderr,
+        "[rclcpp::error] failed to destroy waitset: %s\n", rmw_get_error_string_safe());
+    }
+  }
   // Try to deallocate the interrupt guard condition.
   if (interrupt_guard_condition_ != nullptr) {
     rmw_ret_t status = rmw_destroy_guard_condition(interrupt_guard_condition_);
@@ -41,6 +81,10 @@ Executor::~Executor()
 void
 Executor::add_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify)
 {
+  // If the node already has an executor
+  if (node_ptr->has_executor.exchange(true)) {
+    throw std::runtime_error("Node has already been added to an executor.");
+  }
   // Check to ensure node not already added
   for (auto & weak_node : weak_nodes_) {
     auto node = weak_node.lock();
@@ -57,6 +101,8 @@ Executor::add_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify)
       throw std::runtime_error(rmw_get_error_string_safe());
     }
   }
+  // Add the node's notify condition to the guard condition handles
+  memory_strategy_->add_guard_condition(node_ptr->get_notify_guard_condition());
 }
 
 void
@@ -76,6 +122,7 @@ Executor::remove_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify)
       // *INDENT-ON*
     )
   );
+  node_ptr->has_executor.store(false);
   if (notify) {
     // If the node was matched and removed, interrupt waiting
     if (node_removed) {
@@ -85,6 +132,7 @@ Executor::remove_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify)
       }
     }
   }
+  memory_strategy_->remove_guard_condition(node_ptr->get_notify_guard_condition());
 }
 
 void
@@ -240,7 +288,7 @@ void
 Executor::execute_service(
   rclcpp::service::ServiceBase::SharedPtr service)
 {
-  std::shared_ptr<void> request_header = service->create_request_header();
+  auto request_header = service->create_request_header();
   std::shared_ptr<void> request = service->create_request();
   bool taken = false;
   rmw_ret_t status = rmw_take_request(
@@ -263,7 +311,7 @@ void
 Executor::execute_client(
   rclcpp::client::ClientBase::SharedPtr client)
 {
-  std::shared_ptr<void> request_header = client->create_request_header();
+  auto request_header = client->create_request_header();
   std::shared_ptr<void> response = client->create_response();
   bool taken = false;
   rmw_ret_t status = rmw_take_response(
@@ -305,6 +353,7 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
     );
   }
 
+  memory_strategy_->clear_handles();
   // Use the number of subscriptions to allocate memory in the handles
   rmw_subscriptions_t subscriber_handles;
   subscriber_handles.subscriber_count =
@@ -318,25 +367,10 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
   client_handles.client_count =
     memory_strategy_->fill_client_handles(client_handles.clients);
 
-  // The number of guard conditions is fixed at 2: 1 for the ctrl-c guard cond,
-  // and one for the executor's guard cond (interrupt_guard_condition_)
-  size_t number_of_guard_conds = 2;
-  rmw_guard_conditions_t guard_condition_handles;
-  guard_condition_handles.guard_condition_count = number_of_guard_conds;
-  guard_condition_handles.guard_conditions = static_cast<void **>(guard_cond_handles_.data());
-  if (guard_condition_handles.guard_conditions == NULL &&
-    number_of_guard_conds > 0)
-  {
-    // TODO(wjwwood): Use a different error here? maybe std::bad_alloc?
-    throw std::runtime_error("Could not malloc for guard condition pointers.");
-  }
-  // Put the global ctrl-c guard condition in
-  assert(guard_condition_handles.guard_condition_count > 1);
-  guard_condition_handles.guard_conditions[0] = \
-    rclcpp::utilities::get_global_sigint_guard_condition()->data;
-  // Put the executor's guard condition in
-  guard_condition_handles.guard_conditions[1] = \
-    interrupt_guard_condition_->data;
+  // construct a guard conditions struct
+  rmw_guard_conditions_t guard_conditions;
+  guard_conditions.guard_condition_count =
+    memory_strategy_->fill_guard_condition_handles(guard_conditions.guard_conditions);
 
   rmw_time_t * wait_timeout = NULL;
   rmw_time_t rmw_timeout;
@@ -364,19 +398,14 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
   // Now wait on the waitable subscriptions and timers
   rmw_ret_t status = rmw_wait(
     &subscriber_handles,
-    &guard_condition_handles,
+    &guard_conditions,
     &service_handles,
     &client_handles,
+    waitset_,
     wait_timeout);
   if (status != RMW_RET_OK && status != RMW_RET_TIMEOUT) {
     throw std::runtime_error(rmw_get_error_string_safe());
   }
-  // If ctrl-c guard condition, return directly
-  if (guard_condition_handles.guard_conditions[0] != 0) {
-    // Make sure to free or clean memory
-    memory_strategy_->clear_handles();
-    return;
-  }
 
   memory_strategy_->remove_null_handles();
 }
@@ -545,3 +574,29 @@ Executor::get_next_executable(std::chrono::nanoseconds timeout)
   }
   return any_exec;
 }
+
+std::ostream &
+rclcpp::executor::operator<<(std::ostream & os, const FutureReturnCode & future_return_code)
+{
+  return os << to_string(future_return_code);
+}
+
+std::string
+rclcpp::executor::to_string(const FutureReturnCode & future_return_code)
+{
+  using enum_type = std::underlying_type<FutureReturnCode>::type;
+  std::string prefix = "Unknown enum value (";
+  std::string ret_as_string = std::to_string(static_cast<enum_type>(future_return_code));
+  switch (future_return_code) {
+    case FutureReturnCode::SUCCESS:
+      prefix = "SUCCESS (";
+      break;
+    case FutureReturnCode::INTERRUPTED:
+      prefix = "INTERRUPTED (";
+      break;
+    case FutureReturnCode::TIMEOUT:
+      prefix = "TIMEOUT (";
+      break;
+  }
+  return prefix + ret_as_string + ")";
+}

rclcpp/src/rclcpp/executors.cpp

@@ -27,4 +27,5 @@ rclcpp::spin(node::Node::SharedPtr node_ptr)
   rclcpp::executors::SingleThreadedExecutor exec;
   exec.add_node(node_ptr);
   exec.spin();
+  exec.remove_node(node_ptr);
 }

rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp

@@ -23,8 +23,8 @@
 
 using rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor;
 
-MultiThreadedExecutor::MultiThreadedExecutor(rclcpp::memory_strategy::MemoryStrategy::SharedPtr ms)
-: executor::Executor(ms)
+MultiThreadedExecutor::MultiThreadedExecutor(const rclcpp::executor::ExecutorArgs & args)
+: executor::Executor(args)
 {
   number_of_threads_ = std::thread::hardware_concurrency();
   if (number_of_threads_ == 0) {

rclcpp/src/rclcpp/executors/single_threaded_executor.cpp

@@ -17,10 +17,8 @@
 
 using rclcpp::executors::single_threaded_executor::SingleThreadedExecutor;
 
-SingleThreadedExecutor::SingleThreadedExecutor(
-  rclcpp::memory_strategy::MemoryStrategy::SharedPtr ms)
-: executor::Executor(ms) {}
-
+SingleThreadedExecutor::SingleThreadedExecutor(const rclcpp::executor::ExecutorArgs & args)
+: executor::Executor(args) {}
 
 SingleThreadedExecutor::~SingleThreadedExecutor() {}
 

rclcpp/src/rclcpp/memory_strategy.cpp

@@ -84,8 +84,9 @@ MemoryStrategy::get_client_by_handle(void * client_handle, const WeakNodeVector
       if (!group) {
         continue;
       }
-      for (auto & client : group->get_client_ptrs()) {
-        if (client->get_client_handle()->data == client_handle) {
+      for (auto & weak_client : group->get_client_ptrs()) {
+        auto client = weak_client.lock();
+        if (client && client->get_client_handle()->data == client_handle) {
           return client;
         }
       }
@@ -182,8 +183,9 @@ MemoryStrategy::get_group_by_client(
       if (!group) {
         continue;
       }
-      for (auto & cli : group->get_client_ptrs()) {
-        if (cli == client) {
+      for (auto & weak_client : group->get_client_ptrs()) {
+        auto cli = weak_client.lock();
+        if (cli && cli == client) {
           return group;
         }
       }

rclcpp/src/rclcpp/node.cpp

@@ -36,8 +36,14 @@ Node::Node(
   bool use_intra_process_comms)
 : name_(node_name), context_(context),
   number_of_subscriptions_(0), number_of_timers_(0), number_of_services_(0),
-  use_intra_process_comms_(use_intra_process_comms)
+  use_intra_process_comms_(use_intra_process_comms),
+  notify_guard_condition_(rmw_create_guard_condition())
 {
+  if (!notify_guard_condition_) {
+    throw std::runtime_error("Failed to create guard condition for node: " +
+            std::string(rmw_get_error_string_safe()));
+  }
+  has_executor.store(false);
   size_t domain_id = 0;
   char * ros_domain_id = nullptr;
   const char * env_var = "ROS_DOMAIN_ID";
@@ -50,6 +56,10 @@ Node::Node(
   if (ros_domain_id) {
     uint32_t number = strtoul(ros_domain_id, NULL, 0);
     if (number == (std::numeric_limits<uint32_t>::max)()) {
+      if (rmw_destroy_guard_condition(notify_guard_condition_) != RMW_RET_OK) {
+        fprintf(
+          stderr, "Failed to destroy notify guard condition: %s\n", rmw_get_error_string_safe());
+      }
       throw std::runtime_error("failed to interpret ROS_DOMAIN_ID as integral number");
     }
     domain_id = static_cast<size_t>(number);
@@ -61,6 +71,10 @@ Node::Node(
   auto node = rmw_create_node(name_.c_str(), domain_id);
   if (!node) {
     // *INDENT-OFF*
+    if (rmw_destroy_guard_condition(notify_guard_condition_) != RMW_RET_OK) {
+      fprintf(
+        stderr, "Failed to destroy notify guard condition: %s\n", rmw_get_error_string_safe());
+    }
     throw std::runtime_error(
       std::string("could not create node: ") +
       rmw_get_error_string_safe());
@@ -84,6 +98,14 @@ Node::Node(
     "parameter_events", rmw_qos_profile_parameter_events);
 }
 
+Node::~Node()
+{
+  if (rmw_destroy_guard_condition(notify_guard_condition_) != RMW_RET_OK) {
+    fprintf(stderr, "Warning! Failed to destroy guard condition in Node destructor: %s\n",
+      rmw_get_error_string_safe());
+  }
+}
+
 const std::string &
 Node::get_name() const
 {
@@ -333,3 +355,8 @@ Node::get_callback_groups() const
 {
   return callback_groups_;
 }
+
+rmw_guard_condition_t * Node::get_notify_guard_condition() const
+{
+  return notify_guard_condition_;
+}

rclcpp/test/test_rate.cpp

@@ -39,9 +39,9 @@ TEST(TestRate, rate_basics) {
   rclcpp::utilities::sleep_for(offset);
   ASSERT_TRUE(r.sleep());
   auto two = std::chrono::system_clock::now();
-  delta = two - one;
-  ASSERT_TRUE(period < delta + epsilon);
-  ASSERT_TRUE(period * overrun_ratio > delta);
+  delta = two - start;
+  ASSERT_TRUE(2 * period < delta);
+  ASSERT_TRUE(2 * period * overrun_ratio > delta);
 
   rclcpp::utilities::sleep_for(offset);
   auto two_offset = std::chrono::system_clock::now();
@@ -66,35 +66,35 @@ TEST(TestRate, wall_rate_basics) {
   auto epsilon = std::chrono::milliseconds(1);
   double overrun_ratio = 1.5;
 
-  auto start = std::chrono::system_clock::now();
+  auto start = std::chrono::steady_clock::now();
   rclcpp::rate::WallRate r(period);
   ASSERT_TRUE(r.is_steady());
   ASSERT_TRUE(r.sleep());
-  auto one = std::chrono::system_clock::now();
+  auto one = std::chrono::steady_clock::now();
   auto delta = one - start;
   ASSERT_TRUE(period < delta);
   ASSERT_TRUE(period * overrun_ratio > delta);
 
   rclcpp::utilities::sleep_for(offset);
   ASSERT_TRUE(r.sleep());
-  auto two = std::chrono::system_clock::now();
-  delta = two - one;
-  ASSERT_TRUE(period < delta + epsilon);
-  ASSERT_TRUE(period * overrun_ratio > delta);
+  auto two = std::chrono::steady_clock::now();
+  delta = two - start;
+  ASSERT_TRUE(2 * period < delta + epsilon);
+  ASSERT_TRUE(2 * period * overrun_ratio > delta);
 
   rclcpp::utilities::sleep_for(offset);
-  auto two_offset = std::chrono::system_clock::now();
+  auto two_offset = std::chrono::steady_clock::now();
   r.reset();
   ASSERT_TRUE(r.sleep());
-  auto three = std::chrono::system_clock::now();
+  auto three = std::chrono::steady_clock::now();
   delta = three - two_offset;
   ASSERT_TRUE(period < delta);
   ASSERT_TRUE(period * overrun_ratio > delta);
 
   rclcpp::utilities::sleep_for(offset + period);
-  auto four = std::chrono::system_clock::now();
+  auto four = std::chrono::steady_clock::now();
   ASSERT_FALSE(r.sleep());
-  auto five = std::chrono::system_clock::now();
+  auto five = std::chrono::steady_clock::now();
   delta = five - four;
   ASSERT_TRUE(epsilon > delta);
 }