Remove fixed guard conditions, add notify guard conditions

* No more fixed guard conditions.

* Add notify guard condition to nodes

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,19 +104,19 @@ 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)
   {
     std::lock_guard<std::mutex> lock(pending_requests_mutex_);
-    auto typed_request_header = std::static_pointer_cast<rmw_request_id_t>(request_header);
     auto typed_response = std::static_pointer_cast<typename ServiceT::Response>(response);
-    int64_t sequence_number = typed_request_header->sequence_number;
+    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");

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,15 @@ 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);
 
 ///
 /**
@@ -159,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.
@@ -177,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;
   }
 
@@ -279,8 +306,6 @@ protected:
   /// Spinning state, used to prevent multi threaded calls to spin and to cancel blocking spins.
   std::atomic_bool spinning;
 
-  rmw_guard_conditions_t fixed_guard_conditions_;
-
   /// Guard condition for signaling the rmw layer to wake up for special events.
   rmw_guard_condition_t * interrupt_guard_condition_;
 
@@ -294,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/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,9 @@ public:
   const CallbackGroupWeakPtrList &
   get_callback_groups() const;
 
+  RCLCPP_PUBLIC
+  rmw_guard_condition_t * get_notify_guard_condition() const;
+
   std::atomic_bool has_executor;
 
 private:
@@ -279,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)
@@ -288,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/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"
 
@@ -20,6 +24,7 @@
 using rclcpp::executor::AnyExecutable;
 using rclcpp::executor::Executor;
 using rclcpp::executor::ExecutorArgs;
+using rclcpp::executor::FutureReturnCode;
 
 Executor::Executor(const ExecutorArgs & args)
 : spinning(false),
@@ -30,23 +35,16 @@ Executor::Executor(const ExecutorArgs & args)
     throw std::runtime_error("Failed to create interrupt guard condition in Executor constructor");
   }
 
-  // The number of guard conditions is fixed at 2: 1 for the ctrl-c guard cond,
+  // 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_)
-  // These guard conditions are permanently attached to the waitset.
-  const size_t number_of_guard_conds = 2;
-  fixed_guard_conditions_.guard_condition_count = number_of_guard_conds;
-  fixed_guard_conditions_.guard_conditions = static_cast<void **>(guard_cond_handles_.data());
+  // The size of guard conditions is variable because the number of nodes can vary
 
   // Put the global ctrl-c guard condition in
-  assert(fixed_guard_conditions_.guard_condition_count > 1);
-  fixed_guard_conditions_.guard_conditions[0] = \
-    rclcpp::utilities::get_global_sigint_guard_condition()->data;
+  memory_strategy_->add_guard_condition(rclcpp::utilities::get_global_sigint_guard_condition());
   // Put the executor's guard condition in
-  fixed_guard_conditions_.guard_conditions[1] = interrupt_guard_condition_->data;
+  memory_strategy_->add_guard_condition(interrupt_guard_condition_);
 
-  // The waitset adds the fixed guard conditions to the middleware waitset on initialization,
-  // and removes the guard conditions in rmw_destroy_waitset.
-  waitset_ = rmw_create_waitset(&fixed_guard_conditions_, args.max_conditions);
+  waitset_ = rmw_create_waitset(args.max_conditions);
 
   if (!waitset_) {
     fprintf(stderr,
@@ -103,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
@@ -132,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
@@ -287,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(
@@ -310,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(
@@ -352,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 =
@@ -365,7 +367,10 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
   client_handles.client_count =
     memory_strategy_->fill_client_handles(client_handles.clients);
 
-  // Don't pass guard conditions to rmw_wait; they are permanent fixtures of the waitset
+  // 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;
@@ -393,7 +398,7 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
   // Now wait on the waitable subscriptions and timers
   rmw_ret_t status = rmw_wait(
     &subscriber_handles,
-    nullptr,
+    &guard_conditions,
     &service_handles,
     &client_handles,
     waitset_,
@@ -569,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/node.cpp

@@ -36,8 +36,13 @@ 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;
@@ -51,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);
@@ -62,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());
@@ -85,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
 {
@@ -334,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_;
+}