Merge pull request #33 from mauropasse/mauro/irobot-add-events-executor

Change executor ownership model

Author: iRobot ROS

rclcpp/include/rclcpp/client.hpp

@@ -162,10 +162,6 @@ class ClientBase
     const rclcpp::executors::EventsExecutor * executor,
     rmw_listener_cb_t executor_callback) const;
 
-  RCLCPP_PUBLIC
-  void
-  set_on_destruction_callback(std::function<void(ClientBase *)> on_destruction_callback);
-
 protected:
   RCLCPP_DISABLE_COPY(ClientBase)
 
@@ -181,8 +177,6 @@ class ClientBase
   const rcl_node_t *
   get_rcl_node_handle() const;
 
-  std::function<void(ClientBase *)> on_destruction_callback_;
-
   rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_;
   std::shared_ptr<rcl_node_t> node_handle_;
   std::shared_ptr<rclcpp::Context> context_;

rclcpp/include/rclcpp/executor.hpp

@@ -437,18 +437,6 @@ class Executor
   static void
   execute_client(rclcpp::ClientBase::SharedPtr client);
 
-  RCLCPP_PUBLIC
-  static void
-  execute_subscription(rclcpp::SubscriptionBase * subscription);
-
-  RCLCPP_PUBLIC
-  static void
-  execute_service(rclcpp::ServiceBase * service);
-
-  RCLCPP_PUBLIC
-  static void
-  execute_client(rclcpp::ClientBase * client);
-
   /**
    * \throws std::runtime_error if the wait set can be cleared
    */

rclcpp/include/rclcpp/executors/events_executor.hpp

@@ -16,8 +16,8 @@
 #define RCLCPP__EXECUTORS__EVENTS_EXECUTOR_HPP_
 
 #include <chrono>
-#include <deque>
 #include <memory>
+#include <queue>
 #include <vector>
 
 #include "rclcpp/executor.hpp"
@@ -174,9 +174,7 @@ class EventsExecutor : public rclcpp::Executor
 private:
   RCLCPP_DISABLE_COPY(EventsExecutor)
 
-  // Event queue implementation is a deque only to
-  // facilitate the removal of events from expired entities.
-  using EventQueue = std::deque<rmw_listener_event_t>;
+  using EventQueue = std::queue<rmw_listener_event_t>;
 
   // Executor callback: Push new events into the queue and trigger cv.
   // This function is called by the DDS entities when an event happened,
@@ -192,46 +190,12 @@ class EventsExecutor : public rclcpp::Executor
     {
       std::unique_lock<std::mutex> lock(this_executor->push_mutex_);
 
-      this_executor->event_queue_.push_back(event);
+      this_executor->event_queue_.push(event);
     }
     // Notify that the event queue has some events in it.
     this_executor->event_queue_cv_.notify_one();
   }
 
-  // This function allows to remove an entity from the EventsExecutor.
-  // Entities are any of SubscriptionBase, PublisherBase, ClientBase, ServerBase, Waitable.
-  // After an entity has been removed using this API, it can be safely destroyed without the risk
-  // that the executor would try to access it again.
-  template<typename T>
-  void
-  remove_entity(T * entity)
-  {
-    // We need to unset the callbacks to make sure that after removing events from the
-    // queues, this entity will not push anymore before being completely destroyed.
-    // This assumes that all supported entities implement this function.
-    entity->set_events_executor_callback(nullptr, nullptr);
-
-    // Remove events associated with this entity from the event queue
-    {
-      std::unique_lock<std::mutex> lock(push_mutex_);
-      event_queue_.erase(
-        std::remove_if(
-          event_queue_.begin(), event_queue_.end(),
-          [&entity](rmw_listener_event_t event) {return event.entity == entity;}), event_queue_.end());
-    }
-
-    // Remove events associated with this entity from the local event queue
-    {
-      std::unique_lock<std::mutex> lock(execution_mutex_);
-      execution_event_queue_.erase(
-        std::remove_if(
-          execution_event_queue_.begin(), execution_event_queue_.end(),
-          [&entity](rmw_listener_event_t event) {
-            return event.entity == entity;
-          }), execution_event_queue_.end());
-    }
-  }
-
   /// Extract and execute events from the queue until it is empty
   RCLCPP_PUBLIC
   void
@@ -242,16 +206,14 @@ class EventsExecutor : public rclcpp::Executor
   void
   execute_event(const rmw_listener_event_t & event);
 
-  // We use two instances of EventQueue to allow threads to push events while we execute them
+  // Queue where entities can push events
   EventQueue event_queue_;
-  EventQueue execution_event_queue_;
 
   EventsExecutorEntitiesCollector::SharedPtr entities_collector_;
   EventsExecutorNotifyWaitable::SharedPtr executor_notifier_;
+
   // Mutex to protect the insertion of events in the queue
   std::mutex push_mutex_;
-  // Mutex to protect the execution of events
-  std::mutex execution_mutex_;
   // Variable used to notify when an event is added to the queue
   std::condition_variable event_queue_cv_;
   // Timers manager

rclcpp/include/rclcpp/executors/events_executor_entities_collector.hpp

@@ -18,6 +18,7 @@
 #include <list>
 #include <map>
 #include <memory>
+#include <unordered_map>
 #include <vector>
 
 #include "rclcpp/executors/event_waitable.hpp"
@@ -48,7 +49,9 @@ class EventsExecutor;
  * When this occurs, the execute API takes care of handling changes
  * in the entities currently used by the executor.
  */
-class EventsExecutorEntitiesCollector final : public EventWaitable
+class EventsExecutorEntitiesCollector final
+: public EventWaitable,
+  public std::enable_shared_from_this<EventsExecutorEntitiesCollector>
 {
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(EventsExecutorEntitiesCollector)
@@ -62,6 +65,10 @@ class EventsExecutorEntitiesCollector final : public EventWaitable
   RCLCPP_PUBLIC
   ~EventsExecutorEntitiesCollector();
 
+  // Initialize entities collector
+  RCLCPP_PUBLIC
+  void init();
+
   // The purpose of "execute" is handling the situation of a new entity added to
   // a node, while the executor is already spinning.
   // This consists in setting that entitiy's callback.
@@ -141,6 +148,50 @@ class EventsExecutorEntitiesCollector final : public EventWaitable
   std::vector<rclcpp::CallbackGroup::WeakPtr>
   get_automatically_added_callback_groups_from_nodes();
 
+  ///
+  /**
+   * Get the subscription shared pointer corresponding
+   * to a subscription identifier
+   */
+  RCLCPP_PUBLIC
+  rclcpp::SubscriptionBase::SharedPtr
+  get_subscription(const void * subscription_id);
+
+  ///
+  /**
+   * Get the client shared pointer corresponding
+   * to a client identifier
+   */
+  RCLCPP_PUBLIC
+  rclcpp::ClientBase::SharedPtr
+  get_client(const void * client_id);
+
+  ///
+  /**
+   * Get the service shared pointer corresponding
+   * to a service identifier
+   */
+  RCLCPP_PUBLIC
+  rclcpp::ServiceBase::SharedPtr
+  get_service(const void * service_id);
+
+  ///
+  /**
+   * Get the waitable shared pointer corresponding
+   * to a waitable identifier
+   */
+  RCLCPP_PUBLIC
+  rclcpp::Waitable::SharedPtr
+  get_waitable(const void * waitable_id);
+
+  ///
+  /**
+   * Add a weak pointer to a waitable
+   */
+  RCLCPP_PUBLIC
+  void
+  add_waitable(rclcpp::Waitable::SharedPtr waitable);
+
 private:
   void
   set_callback_group_entities_callbacks(rclcpp::CallbackGroup::SharedPtr group);
@@ -190,6 +241,15 @@ class EventsExecutorEntitiesCollector final : public EventWaitable
 
   /// List of weak nodes registered in the events executor
   std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> weak_nodes_;
+
+  // Maps: entity identifiers to weak pointers from the entities registered in the executor
+  // so in the case of an event providing and ID, we can retrieve and own the corresponding
+  // entity while it performs work
+  std::unordered_map<const void *, rclcpp::SubscriptionBase::WeakPtr> weak_subscriptions_map_;
+  std::unordered_map<const void *, rclcpp::ServiceBase::WeakPtr> weak_services_map_;
+  std::unordered_map<const void *, rclcpp::ClientBase::WeakPtr> weak_clients_map_;
+  std::unordered_map<const void *, rclcpp::Waitable::WeakPtr> weak_waitables_map_;
+
   /// Executor using this entities collector object
   EventsExecutor * associated_executor_ = nullptr;
   /// Instance of the timers manager used by the associated executor

rclcpp/include/rclcpp/executors/events_executor_notify_waitable.hpp

@@ -40,12 +40,7 @@ class EventsExecutorNotifyWaitable final : public EventWaitable
 
   // Destructor
   RCLCPP_PUBLIC
-  virtual ~EventsExecutorNotifyWaitable()
-  {
-    if (on_destruction_callback_) {
-      on_destruction_callback_(this);
-    }
-  }
+  virtual ~EventsExecutorNotifyWaitable() = default;
 
   // The function is a no-op, since we only care of waking up the executor
   RCLCPP_PUBLIC