Merge pull request ros2#2 from alsora/asoragna/add-events-executor

Asoragna/add events executor

rclcpp/CMakeLists.txt

@@ -50,10 +50,13 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/executor.cpp
   src/rclcpp/executors.cpp
   src/rclcpp/expand_topic_or_service_name.cpp
+  src/rclcpp/executors/events_executor.cpp
+  src/rclcpp/executors/events_executor_entities_collector.cpp
   src/rclcpp/executors/multi_threaded_executor.cpp
   src/rclcpp/executors/single_threaded_executor.cpp
   src/rclcpp/executors/static_executor_entities_collector.cpp
   src/rclcpp/executors/static_single_threaded_executor.cpp
+  src/rclcpp/executors/timers_manager.cpp
   src/rclcpp/future_return_code.cpp
   src/rclcpp/graph_listener.cpp
   src/rclcpp/guard_condition.cpp

rclcpp/include/rclcpp/client.hpp

@@ -150,6 +150,10 @@ class ClientBase
   bool
   exchange_in_use_by_wait_set_state(bool in_use_state);
 
+  RCLCPP_PUBLIC
+  void
+  set_callback(const void * executor_context, Event_callback executor_callback) const;
+
 protected:
   RCLCPP_DISABLE_COPY(ClientBase)
 

rclcpp/include/rclcpp/executor.hpp

@@ -437,6 +437,18 @@ 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
    */
@@ -540,7 +552,7 @@ class Executor
   RCLCPP_DISABLE_COPY(Executor)
 
   RCLCPP_PUBLIC
-  void
+  virtual void
   spin_once_impl(std::chrono::nanoseconds timeout);
 
   typedef std::map<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,

rclcpp/include/rclcpp/executors.hpp

@@ -18,6 +18,7 @@
 #include <future>
 #include <memory>
 
+#include "rclcpp/executors/events_executor.hpp"
 #include "rclcpp/executors/multi_threaded_executor.hpp"
 #include "rclcpp/executors/single_threaded_executor.hpp"
 #include "rclcpp/executors/static_single_threaded_executor.hpp"

rclcpp/include/rclcpp/executors/events_executor.hpp

@@ -0,0 +1,179 @@
+// Copyright 2020 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__EXECUTORS__EVENTS_EXECUTOR_HPP_
+#define RCLCPP__EXECUTORS__EVENTS_EXECUTOR_HPP_
+
+#include <chrono>
+#include <memory>
+#include <queue>
+
+#include "rcutils/event_types.h"
+
+#include "rclcpp/executor.hpp"
+#include "rclcpp/executors/events_executor_entities_collector.hpp"
+#include "rclcpp/executors/timers_manager.hpp"
+#include "rclcpp/node.hpp"
+
+namespace rclcpp
+{
+namespace executors
+{
+
+/// Events executor implementation
+/**
+ * Add description
+ *
+ * To run this executor:
+ * rclcpp::executors::EventsExecutor executor;
+ * executor.add_node(node);
+ * executor.spin();
+ * executor.remove_node(node);
+ */
+class EventsExecutor : public rclcpp::Executor
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(EventsExecutor)
+
+  /// Default constructor. See the default constructor for Executor.
+  /**
+   * \param[in] options Options used to configure the executor.
+   */
+  RCLCPP_PUBLIC
+  explicit EventsExecutor(
+    const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions());
+
+  /// Default destrcutor.
+  RCLCPP_PUBLIC
+  virtual ~EventsExecutor();
+
+  /// Events executor implementation of spin.
+  /**
+   * This function will block until work comes in, execute it, and keep blocking.
+   * It will only be interrupted by a CTRL-C (managed by the global signal handler).
+   * \throws std::runtime_error when spin() called while already spinning
+   */
+  RCLCPP_PUBLIC
+  void
+  spin() override;
+
+  /// Events executor implementation of spin some
+  /**
+   * executor.provide_callbacks();
+   * while(condition) {
+   *   executor.spin_some();
+   * }
+   */
+  RCLCPP_PUBLIC
+  void
+  spin_some(std::chrono::nanoseconds max_duration) override;
+
+  RCLCPP_PUBLIC
+  void
+  spin_all(std::chrono::nanoseconds max_duration) override;
+
+  /// Add a node to the executor.
+  /**
+   * \sa rclcpp::Executor::add_node
+   */
+  RCLCPP_PUBLIC
+  void
+  add_node(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+    bool notify = true) override;
+
+  /// Convenience function which takes Node and forwards NodeBaseInterface.
+  /**
+   * \sa rclcpp::EventsExecutor::add_node
+   */
+  RCLCPP_PUBLIC
+  void
+  add_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true) override;
+
+  /// Remove a node from the executor.
+  /**
+   * \sa rclcpp::Executor::remove_node
+   */
+  RCLCPP_PUBLIC
+  void
+  remove_node(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+    bool notify = true) override;
+
+  /// Convenience function which takes Node and forwards NodeBaseInterface.
+  /**
+   * \sa rclcpp::Executor::remove_node
+   */
+  RCLCPP_PUBLIC
+  void
+  remove_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true) override;
+
+protected:
+  /// Waits for events and then executes them
+  RCLCPP_PUBLIC
+  void
+  handle_events();
+
+  RCLCPP_PUBLIC
+  void
+  spin_once_impl(std::chrono::nanoseconds timeout) override;
+
+private:
+  RCLCPP_DISABLE_COPY(EventsExecutor)
+
+  EventsExecutorEntitiesCollector::SharedPtr entities_collector_;
+
+  /// Extract and execute events from the queue until it is empty
+  RCLCPP_PUBLIC
+  void
+  consume_all_events(std::queue<EventQ> &queue);
+
+  // Execute a single event
+  RCLCPP_PUBLIC
+  void
+  execute_event(const EventQ &event);
+
+  // Executor callback: Push new events into the queue and trigger cv.
+  // This function is called by the DDS entities when an event happened,
+  // like a subscription receiving a message.
+  static void
+  push_event(const void * executor_ptr, EventQ event)
+  {
+    // Cast executor_ptr to this (need to remove constness)
+    auto this_executor = const_cast<executors::EventsExecutor*>(
+                  static_cast<const executors::EventsExecutor*>(executor_ptr));
+
+    // Event queue mutex scope
+    {
+      std::unique_lock<std::mutex> lock(this_executor->event_queue_mutex_);
+
+      this_executor->event_queue_.push(event);
+    }
+    // Notify that the event queue has some events in it.
+    this_executor->event_queue_cv_.notify_one();
+  }
+
+  // Event queue members
+  std::queue<EventQ> event_queue_;
+  std::mutex event_queue_mutex_;
+  std::condition_variable event_queue_cv_;
+
+  // Timers heap manager
+  std::shared_ptr<TimersManager> timers_manager_;
+};
+
+}  // namespace executors
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXECUTORS__EVENTS_EXECUTOR_HPP_

rclcpp/include/rclcpp/executors/events_executor_entities_collector.hpp

@@ -0,0 +1,105 @@
+// Copyright 2020 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__EXECUTORS__EVENTS_EXECUTOR_ENTITIES_COLLECTOR_HPP_
+#define RCLCPP__EXECUTORS__EVENTS_EXECUTOR_ENTITIES_COLLECTOR_HPP_
+
+#include <list>
+
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/waitable.hpp"
+
+namespace rclcpp
+{
+namespace executors
+{
+
+class EventsExecutorEntitiesCollector final : public rclcpp::Waitable
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(EventsExecutorEntitiesCollector)
+
+  using TimerFn = std::function<void (const rclcpp::TimerBase::SharedPtr & timer)>;
+  using ClearTimersFn = std::function<void (void)>;
+
+  // Constructor
+  RCLCPP_PUBLIC
+  EventsExecutorEntitiesCollector() = default;
+
+  // Destructor
+  RCLCPP_PUBLIC
+  ~EventsExecutorEntitiesCollector();
+
+  RCLCPP_PUBLIC
+  void
+  init(
+    void * executor_context,
+    Event_callback executor_callback,
+    TimerFn push_timer,
+    TimerFn clear_timer,
+    ClearTimersFn clear_all_timers);
+
+  RCLCPP_PUBLIC
+  void
+  execute() override;
+
+  RCLCPP_PUBLIC
+  void
+  add_node(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
+
+  RCLCPP_PUBLIC
+  void
+  remove_node(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
+
+  RCLCPP_PUBLIC
+  bool
+  is_ready(rcl_wait_set_t * wait_set) override
+  {
+    (void)wait_set;
+    return false;
+  }
+
+  RCLCPP_PUBLIC
+  bool
+  add_to_wait_set(rcl_wait_set_t * wait_set) override
+  {
+    (void)wait_set;
+    return true;
+  }
+
+private:
+  void
+  set_entities_callbacks();
+
+  /// List of weak nodes registered in the events executor
+  std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> weak_nodes_;
+
+  /// Context (associated executor)
+  void * executor_context_ = nullptr;
+
+  /// Events callback
+  Event_callback executor_callback_ = nullptr;
+
+  /// Function pointers to push and clear timers from the timers heap
+  TimerFn push_timer_ = nullptr;
+  TimerFn clear_timer_ = nullptr;
+  ClearTimersFn clear_all_timers_ = nullptr;
+};
+
+}  // namespace executors
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXECUTORS__EVENTS_EXECUTOR_ENTITIES_COLLECTOR_HPP_