Add events queue abstract class and implem

rclcpp/include/rclcpp/executors/events_executor.hpp

@@ -24,6 +24,8 @@
 #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/experimental/buffers/simple_events_queue.hpp"
 #include "rclcpp/node.hpp"
 
 #include "rmw/listener_event_types.h"
@@ -55,10 +57,13 @@ class EventsExecutor : public rclcpp::Executor
 
   /// Default constructor. See the default constructor for Executor.
   /**
+   * \param[in] events_queue The queue used to store events.
    * \param[in] options Options used to configure the executor.
    */
   RCLCPP_PUBLIC
   explicit EventsExecutor(
+    rclcpp::experimental::buffers::EventsQueue::UniquePtr events_queue = std::make_unique<
+      rclcpp::experimental::buffers::SimpleEventsQueue>(),
     const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions());
 
   /// Default destrcutor.
@@ -190,10 +195,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 +212,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/bounded_events_queue.hpp

@@ -0,0 +1,146 @@
+// 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__BOUNDED_EVENTS_QUEUE_HPP_
+#define RCLCPP__EXPERIMENTAL__BUFFERS__BOUNDED_EVENTS_QUEUE_HPP_
+
+#include <queue>
+#include <utility>
+
+#include "rclcpp/executors/events_queue.hpp"
+
+namespace rclcpp
+{
+namespace experimental
+{
+namespace buffers
+{
+
+/**
+ * @brief This class provides a bounded queue implementation
+ * based on a std::queue. Before pushing events into the queue
+ * checks the queue size. In case of exceeding the size it performs
+ * a prune of the queue.
+ */
+class BoundedEventsQueue : public EventsQueue
+{
+public:
+  RCLCPP_PUBLIC
+  ~BoundedEventsQueue() = default;
+
+  /**
+   * @brief push event into the queue
+   * @param event The event to push into the queue
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  push(const rmw_listener_event_t & event)
+  {
+    if (event_queue_.size() >= queue_size_limit_) {
+      // Simple prune strategy: Remove all elements in the queue.
+      this->init();
+    }
+    event_queue_.push(event);
+  }
+
+  /**
+   * @brief removes front element from the queue
+   * The element removed is the "oldest" element in the queue whose
+   * value can be retrieved by calling member front().
+   */
+  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() const
+  {
+    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() const
+  {
+    return event_queue_.empty();
+  }
+
+  /**
+   * @brief Initializes the queue
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  init()
+  {
+    // Make sure the queue is empty when we start
+    std::queue<rmw_listener_event_t> local_queue;
+    std::swap(event_queue_, local_queue);
+  }
+
+
+  /**
+   * @brief gets a queue with all events accumulated on it since
+   * the last call. The member queue is empty when the call returns.
+   * @return std::queue with events
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::queue<rmw_listener_event_t>
+  get_all_events()
+  {
+    std::queue<rmw_listener_event_t> local_queue;
+    std::swap(event_queue_, local_queue);
+    return local_queue;
+  }
+
+  /**
+   * @brief sets the queue size limit
+   * @param limit The queue size limit
+   */
+  RCLCPP_PUBLIC
+  void
+  set_queue_size_limit(size_t queue_size_limit)
+  {
+    queue_size_limit_ = queue_size_limit;
+  }
+
+private:
+  std::queue<rmw_listener_event_t> event_queue_;
+
+  size_t queue_size_limit_ = 1000;
+};
+
+}  // namespace buffers
+}  // namespace experimental
+}  // namespace rclcpp
+
+
+#endif  // RCLCPP__EXPERIMENTAL__BUFFERS__BOUNDED_EVENTS_QUEUE_HPP_

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

@@ -0,0 +1,109 @@
+// 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/executors/events_executor_entities_collector.hpp"
+#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(const rmw_listener_event_t & event) = 0;
+
+  /**
+   * @brief removes front element from the queue
+   * The element removed is the "oldest" element in the queue whose
+   * value can be retrieved by calling member front().
+   */
+  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() const = 0;
+
+  /**
+   * @brief Test whether queue is empty
+   * @return true if the queue's size is 0, false otherwise.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  empty() const = 0;
+
+  /**
+   * @brief Initializes the queue
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  init() = 0;
+
+  /**
+   * @brief gets a queue with all events accumulated on it since
+   * the last call. The member queue is empty when the call returns.
+   * @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_