Add events queue abstract class and implem

rclcpp/include/rclcpp/executors/events_executor.hpp

@@ -24,6 +24,7 @@
 #include "rclcpp/executors/events_executor_entities_collector.hpp"
 #include "rclcpp/executors/events_executor_notify_waitable.hpp"
 #include "rclcpp/executors/timers_manager.hpp"
+#include "rclcpp/experimental/buffers/events_queue.hpp"
 #include "rclcpp/node.hpp"
 
 #include "rmw/listener_event_types.h"
@@ -59,6 +60,7 @@ class EventsExecutor : public rclcpp::Executor
    */
   RCLCPP_PUBLIC
   explicit EventsExecutor(
+    rclcpp::experimental::buffers::EventsQueue::UniquePtr events_queue,
     const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions());
 
   /// Default destrcutor.
@@ -190,10 +192,10 @@ class EventsExecutor : public rclcpp::Executor
     {
       std::unique_lock<std::mutex> lock(this_executor->push_mutex_);
 
-      this_executor->event_queue_.push(event);
+      this_executor->events_queue_->push(event);
     }
     // Notify that the event queue has some events in it.
-    this_executor->event_queue_cv_.notify_one();
+    this_executor->events_queue_cv_.notify_one();
   }
 
   /// Extract and execute events from the queue until it is empty
@@ -207,15 +209,15 @@ class EventsExecutor : public rclcpp::Executor
   execute_event(const rmw_listener_event_t & event);
 
   // Queue where entities can push events
-  EventQueue event_queue_;
+  rclcpp::experimental::buffers::EventsQueue::SharedPtr events_queue_;
 
   EventsExecutorEntitiesCollector::SharedPtr entities_collector_;
   EventsExecutorNotifyWaitable::SharedPtr executor_notifier_;
 
   // Mutex to protect the insertion of events in the queue
   std::mutex push_mutex_;
   // Variable used to notify when an event is added to the queue
-  std::condition_variable event_queue_cv_;
+  std::condition_variable events_queue_cv_;
   // Timers manager
   std::shared_ptr<TimersManager> timers_manager_;
 };

rclcpp/include/rclcpp/experimental/buffers/events_queue.hpp

@@ -0,0 +1,98 @@
+// Copyright 2021 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__EXPERIMENTAL__BUFFERS__EVENTS_QUEUE_HPP_
+#define RCLCPP__EXPERIMENTAL__BUFFERS__EVENTS_QUEUE_HPP_
+
+#include <queue>
+
+#include "rclcpp/macros.hpp"
+
+#include "rmw/listener_event_types.h"
+
+namespace rclcpp
+{
+namespace experimental
+{
+namespace buffers
+{
+
+/**
+ * @brief This abstract class is intended to be used as
+ * a wrapper around a queue. The derived classes should chose
+ * which container to use and the strategies for push and prune
+ * events from the queue.
+ */
+class EventsQueue
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(EventsQueue)
+
+  /**
+   * @brief Destruct the object.
+   */
+  RCLCPP_PUBLIC
+  virtual ~EventsQueue() = default;
+
+  /**
+   * @brief push event into the queue
+   * @param event The event to push into the queue
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  push(rmw_listener_event_t event) = 0;
+
+  /**
+   * @brief removes front element from the queue
+   * @return iterator
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  pop() = 0;
+
+  /**
+   * @brief gets the front event from the queue
+   * @return the front event
+   */
+  RCLCPP_PUBLIC
+  virtual
+  rmw_listener_event_t
+  front() = 0;
+
+  /**
+   * @brief Test whether queue is empty
+   * @return true if the queue's size is 0, false otherwise.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  empty() = 0;
+
+  /**
+   * @brief gets a queue with all events accumulated on it
+   * @return queue with events
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::queue<rmw_listener_event_t>
+  get_all_events() = 0;
+};
+
+}  // namespace buffers
+}  // namespace experimental
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPERIMENTAL__BUFFERS__EVENTS_QUEUE_HPP_

rclcpp/include/rclcpp/experimental/buffers/performance_events_queue.hpp

@@ -0,0 +1,116 @@
+// Copyright 2021 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__EXPERIMENTAL__BUFFERS__PERFORMANCE_EVENTS_QUEUE_HPP_
+#define RCLCPP__EXPERIMENTAL__BUFFERS__PERFORMANCE_EVENTS_QUEUE_HPP_
+
+#include <queue>
+
+#include "rclcpp/executors/events_queue.hpp"
+
+namespace rclcpp
+{
+namespace experimental
+{
+namespace buffers
+{
+
+/**
+ * @brief This class provides a simple queue implementation
+ * based on a std::queue. As the objective is having a CPU peformant
+ * queue, it does not performs any checks about the size of
+ * the queue, so the queue size could grow unbounded.
+ * It does not implement any pruning mechanisms.
+ */
+class PerformanceEventsQueue : public EventsQueue
+{
+public:
+
+  using EventQueue = std::queue<rmw_listener_event_t>;
+
+  RCLCPP_PUBLIC
+  ~PerformanceEventsQueue() = default;
+
+  /**
+   * @brief push event into the queue
+   * @param event The event to push into the queue
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  push(rmw_listener_event_t event)
+  {
+    event_queue_.push(event);
+  }
+
+  /**
+   * @brief removes front element from the queue
+   * @return iterator
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  pop()
+  {
+    event_queue_.pop();
+  }
+
+  /**
+   * @brief gets the front event from the queue
+   * @return the front event
+   */
+  RCLCPP_PUBLIC
+  virtual
+  rmw_listener_event_t
+  front()
+  {
+     return event_queue_.front();
+  }
+
+  /**
+   * @brief Test whether queue is empty
+   * @return true if the queue's size is 0, false otherwise.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  empty()
+  {
+    return event_queue_.empty();
+  }
+
+  /**
+   * @brief gets a queue with all events on it
+   * @return queue with events
+   */
+  RCLCPP_PUBLIC
+  virtual
+  EventQueue
+  get_all_events()
+  {
+    EventQueue local_queue;
+    std::swap(event_queue_, local_queue);
+    return local_queue;
+  }
+
+private:
+  EventQueue event_queue_;
+};
+
+}  // namespace buffers
+}  // namespace experimental
+}  // namespace rclcpp
+
+
+#endif  // RCLCPP__EXPERIMENTAL__BUFFERS__PERFORMANCE_EVENTS_QUEUE_HPP_

rclcpp/src/rclcpp/executors/events_executor.cpp

@@ -24,15 +24,19 @@
 using namespace std::chrono_literals;
 
 using rclcpp::executors::EventsExecutor;
+using rclcpp::experimental::buffers::EventsQueue;
 
 EventsExecutor::EventsExecutor(
+  EventsQueue::UniquePtr events_queue,
   const rclcpp::ExecutorOptions & options)
 : rclcpp::Executor(options)
 {
   timers_manager_ = std::make_shared<TimersManager>(context_);
   entities_collector_ = std::make_shared<EventsExecutorEntitiesCollector>(this);
   entities_collector_->init();
 
+  events_queue_ = std::move(events_queue);
+
   // Setup the executor notifier to wake up the executor when some guard conditions are tiggered.
   // The added guard conditions are guaranteed to not go out of scope before the executor itself.
   executor_notifier_ = std::make_shared<EventsExecutorNotifyWaitable>();
@@ -51,7 +55,7 @@ EventsExecutor::spin()
   RCLCPP_SCOPE_EXIT(this->spinning.store(false););
 
   // When condition variable is notified, check this predicate to proceed
-  auto has_event_predicate = [this]() {return !event_queue_.empty();};
+  auto has_event_predicate = [this]() {return !events_queue_->empty();};
 
   // Local event queue to allow entities to push events while we execute them
   EventQueue execution_event_queue;
@@ -61,10 +65,10 @@ EventsExecutor::spin()
   while (rclcpp::ok(context_) && spinning.load()) {
     std::unique_lock<std::mutex> push_lock(push_mutex_);
     // We wait here until something has been pushed to the event queue
-    event_queue_cv_.wait(push_lock, has_event_predicate);
-    // We got an event! Swap queues
-    std::swap(execution_event_queue, event_queue_);
-    // After swapping the queues, we don't need the lock anymore
+    events_queue_cv_.wait(push_lock, has_event_predicate);
+    // Local event queue to allow entities to push events while we execute them
+    execution_event_queue = events_queue_->get_all_events();
+    // Unlock the mutex
     push_lock.unlock();
     // Consume all available events, this queue will be empty at the end of the function
     this->consume_all_events(execution_event_queue);
@@ -91,10 +95,8 @@ EventsExecutor::spin_some(std::chrono::nanoseconds max_duration)
   // - An executor event is received and processed
 
   // When condition variable is notified, check this predicate to proceed
-  auto has_event_predicate = [this]() {return !event_queue_.empty();};
+  auto has_event_predicate = [this]() {return !events_queue_->empty();};
 
-  // Local event queue to allow entities to push events while we execute them
-  EventQueue execution_event_queue;
 
   // Select the smallest between input max_duration and timer timeout
   auto next_timer_timeout = timers_manager_->get_head_timeout();
@@ -104,10 +106,10 @@ EventsExecutor::spin_some(std::chrono::nanoseconds max_duration)
 
   std::unique_lock<std::mutex> push_lock(push_mutex_);
   // Wait until timeout or event
-  event_queue_cv_.wait_for(push_lock, max_duration, has_event_predicate);
-  // Time to swap queues as the wait is over
-  std::swap(execution_event_queue, event_queue_);
-  // After swapping the queues, we don't need the lock anymore
+  events_queue_cv_.wait_for(push_lock, max_duration, has_event_predicate);
+  // Local event queue to allow entities to push events while we execute them
+  EventQueue execution_event_queue = events_queue_->get_all_events();
+  // We don't need the lock anymore
   push_lock.unlock();
 
   // Execute all ready timers
@@ -129,7 +131,7 @@ EventsExecutor::spin_all(std::chrono::nanoseconds max_duration)
   RCLCPP_SCOPE_EXIT(this->spinning.store(false););
 
   // When condition variable is notified, check this predicate to proceed
-  auto has_event_predicate = [this]() {return !event_queue_.empty();};
+  auto has_event_predicate = [this]() {return !events_queue_->empty();};
 
   // Local event queue to allow entities to push events while we execute them
   EventQueue execution_event_queue;
@@ -149,15 +151,15 @@ EventsExecutor::spin_all(std::chrono::nanoseconds max_duration)
   {
     // Wait once until timeout or event
     std::unique_lock<std::mutex> push_lock(push_mutex_);
-    event_queue_cv_.wait_for(push_lock, max_duration, has_event_predicate);
+    events_queue_cv_.wait_for(push_lock, max_duration, has_event_predicate);
   }
 
   auto timeout = timers_manager_->get_head_timeout();
 
   // Keep executing until no more work to do or timeout expired
   while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) {
     std::unique_lock<std::mutex> push_lock(push_mutex_);
-    std::swap(execution_event_queue, event_queue_);
+    execution_event_queue = events_queue_->get_all_events();
     push_lock.unlock();
 
     // Exit if there is no more work to do
@@ -188,21 +190,21 @@ EventsExecutor::spin_once_impl(std::chrono::nanoseconds timeout)
   }
 
   // When condition variable is notified, check this predicate to proceed
-  auto has_event_predicate = [this]() {return !event_queue_.empty();};
+  auto has_event_predicate = [this]() {return !events_queue_->empty();};
 
   rmw_listener_event_t event;
   bool has_event = false;
 
   {
     // Wait until timeout or event arrives
     std::unique_lock<std::mutex> lock(push_mutex_);
-    event_queue_cv_.wait_for(lock, timeout, has_event_predicate);
+    events_queue_cv_.wait_for(lock, timeout, has_event_predicate);
 
     // Grab first event from queue if it exists
-    has_event = !event_queue_.empty();
+    has_event = !events_queue_->empty();
     if (has_event) {
-      event = event_queue_.front();
-      event_queue_.pop();
+      event = events_queue_->front();
+      events_queue_->pop();
     }
   }