Add base executor objects that can be used by implementors

Signed-off-by: Michael Carroll <mjcarroll@intrinsic.ai>

rclcpp/CMakeLists.txt

@@ -54,6 +54,9 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/executable_list.cpp
   src/rclcpp/executor.cpp
   src/rclcpp/executors.cpp
+  src/rclcpp/executors/executor_notify_waitable.cpp
+  src/rclcpp/executors/executor_entities_collector.cpp
+  src/rclcpp/executors/executor_entities_collection.cpp
   src/rclcpp/executors/multi_threaded_executor.cpp
   src/rclcpp/executors/single_threaded_executor.cpp
   src/rclcpp/executors/static_executor_entities_collector.cpp

rclcpp/include/rclcpp/executors/executor_entities_collection.hpp

@@ -0,0 +1,126 @@
+// Copyright 2023 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__EXECUTOR_ENTITIES_COLLECTION_HPP_
+#define RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTION_HPP_
+
+#include <deque>
+#include <unordered_map>
+#include <vector>
+
+#include "rcpputils/thread_safety_annotations.hpp"
+
+#include <rclcpp/any_executable.hpp>
+#include <rclcpp/node_interfaces/node_base.hpp>
+#include <rclcpp/callback_group.hpp>
+#include <rclcpp/executors/executor_notify_waitable.hpp>
+#include <rclcpp/visibility_control.hpp>
+#include <rclcpp/wait_result.hpp>
+#include <rclcpp/wait_set.hpp>
+
+namespace rclcpp
+{
+namespace executors
+{
+
+struct ExecutorEntitiesCollection
+{
+  struct TimerEntry {
+    rclcpp::TimerBase::WeakPtr timer;
+    rclcpp::CallbackGroup::WeakPtr callback_group;
+  };
+  using TimerCollection = std::unordered_map<const rcl_timer_t *, TimerEntry>;
+
+  struct SubscriptionEntry{
+    rclcpp::SubscriptionBase::WeakPtr subscription;
+    rclcpp::CallbackGroup::WeakPtr callback_group;
+  };
+  using SubscriptionCollection = std::unordered_map<const rcl_subscription_t *, SubscriptionEntry>;
+
+  struct ClientEntry {
+    rclcpp::ClientBase::WeakPtr client;
+    rclcpp::CallbackGroup::WeakPtr callback_group;
+  };
+  using ClientCollection = std::unordered_map<const rcl_client_t *, ClientEntry>;
+
+  struct ServiceEntry {
+    rclcpp::ServiceBase::WeakPtr service;
+    rclcpp::CallbackGroup::WeakPtr callback_group;
+  };
+  using ServiceCollection = std::unordered_map<const rcl_service_t *, ServiceEntry>;
+
+  struct WaitableEntry {
+    rclcpp::Waitable::WeakPtr waitable;
+    rclcpp::CallbackGroup::WeakPtr callback_group;
+  };
+  using WaitableCollection = std::unordered_map<const rclcpp::Waitable *, WaitableEntry>;
+
+  using GuardConditionCollection = std::unordered_map<const rcl_guard_condition_t *,
+                                                      rclcpp::GuardCondition::WeakPtr>;
+
+  TimerCollection timers;
+  SubscriptionCollection subscriptions;
+  ClientCollection clients;
+  ServiceCollection services;
+  GuardConditionCollection guard_conditions;
+  WaitableCollection waitables;
+
+  void clear();
+
+  using TimerUpdatedFunc = std::function<void(rclcpp::TimerBase::SharedPtr)>;
+  void update_timers(const ExecutorEntitiesCollection::TimerCollection & other,
+                     TimerUpdatedFunc timer_added,
+                     TimerUpdatedFunc timer_removed);
+
+  using SubscriptionUpdatedFunc = std::function<void(rclcpp::SubscriptionBase::SharedPtr)>;
+  void update_subscriptions(const ExecutorEntitiesCollection::SubscriptionCollection & other,
+                     SubscriptionUpdatedFunc subscription_added,
+                     SubscriptionUpdatedFunc subscription_removed);
+
+  using ClientUpdatedFunc = std::function<void(rclcpp::ClientBase::SharedPtr)>;
+  void update_clients(const ExecutorEntitiesCollection::ClientCollection & other,
+                     ClientUpdatedFunc client_added,
+                     ClientUpdatedFunc client_removed);
+
+  using ServiceUpdatedFunc = std::function<void(rclcpp::ServiceBase::SharedPtr)>;
+  void update_services(const ExecutorEntitiesCollection::ServiceCollection & other,
+                       ServiceUpdatedFunc service_added,
+                       ServiceUpdatedFunc service_removed);
+
+  using GuardConditionUpdatedFunc = std::function<void(rclcpp::GuardCondition::SharedPtr)>;
+  void update_guard_conditions(const ExecutorEntitiesCollection::GuardConditionCollection & other,
+                               GuardConditionUpdatedFunc guard_condition_added,
+                               GuardConditionUpdatedFunc guard_condition_removed);
+
+  using WaitableUpdatedFunc = std::function<void(rclcpp::Waitable::SharedPtr)>;
+  void update_waitables(const ExecutorEntitiesCollection::WaitableCollection & other,
+                        WaitableUpdatedFunc waitable_added,
+                        WaitableUpdatedFunc waitable_removed);
+  };
+
+void
+build_entities_collection(
+  const std::vector<rclcpp::CallbackGroup::WeakPtr> & callback_groups,
+  ExecutorEntitiesCollection & collection);
+
+std::deque<rclcpp::AnyExecutable>
+ready_executables(
+    const ExecutorEntitiesCollection & collection,
+    rclcpp::WaitResult<rclcpp::WaitSet> & wait_result
+);
+
+}  // namespace executors
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTION_HPP_

rclcpp/include/rclcpp/executors/executor_entities_collector.hpp

@@ -0,0 +1,149 @@
+// Copyright 2023 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__EXECUTOR_ENTITIES_COLLECTOR_HPP_
+#define RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTOR_HPP_
+
+#include <list>
+#include <memory>
+#include <set>
+#include <vector>
+
+#include "rcpputils/thread_safety_annotations.hpp"
+
+#include <rclcpp/any_executable.hpp>
+#include <rclcpp/node_interfaces/node_base.hpp>
+#include <rclcpp/callback_group.hpp>
+#include <rclcpp/executors/executor_notify_waitable.hpp>
+#include <rclcpp/visibility_control.hpp>
+#include <rclcpp/wait_set.hpp>
+#include <rclcpp/wait_result.hpp>
+
+namespace rclcpp
+{
+namespace executors
+{
+
+class ExecutorEntitiesCollector
+{
+public:
+  RCLCPP_PUBLIC
+  ExecutorEntitiesCollector();
+
+  RCLCPP_PUBLIC
+  ~ExecutorEntitiesCollector();
+
+  /// Add a node to the entity collector
+  /**
+   * \param[in] node_ptr a shared pointer that points to a node base interface
+   * \throw std::runtime_error if the node is associated with an executor
+   */
+  RCLCPP_PUBLIC
+  void
+  add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
+
+  /// Remove a node from the entity collector
+  /**
+   * \param[in] node_ptr a shared pointer that points to a node base interface
+   * \throw std::runtime_error if the node is associated with an executor
+   * \throw std::runtime_error if the node is associated with this executor
+   */
+  RCLCPP_PUBLIC
+  void
+  remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
+
+  RCLCPP_PUBLIC
+  bool
+  has_node(const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
+
+  /// Add a callback group to the entity collector
+  /**
+   * \param[in] group_ptr a shared pointer that points to a callback group
+   * \throw std::runtime_error if the callback_group is associated with an executor
+   */
+  RCLCPP_PUBLIC
+  void
+  add_callback_group(rclcpp::CallbackGroup::SharedPtr group_ptr);
+
+  /// Remove a callback group from the entity collector
+  /**
+   * \param[in] group_ptr a shared pointer that points to a callback group
+   * \throw std::runtime_error if the callback_group is not associated with an executor
+   * \throw std::runtime_error if the callback_group is not associated with this executor
+   */
+  RCLCPP_PUBLIC
+  void
+  remove_callback_group(rclcpp::CallbackGroup::SharedPtr group_ptr);
+
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::CallbackGroup::WeakPtr>
+  get_all_callback_groups();
+
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::CallbackGroup::WeakPtr>
+  get_manually_added_callback_groups();
+
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::CallbackGroup::WeakPtr>
+  get_automatically_added_callback_groups();
+
+  RCLCPP_PUBLIC
+  void
+  update_collections();
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<ExecutorNotifyWaitable>
+  get_executor_notify_waitable();
+
+protected:
+  using CallbackGroupCollection = std::set<
+    rclcpp::CallbackGroup::WeakPtr,
+    std::owner_less<rclcpp::CallbackGroup::WeakPtr>>;
+
+  RCLCPP_PUBLIC
+  void
+  add_callback_group_to_collection(rclcpp::CallbackGroup::SharedPtr group_ptr,
+    CallbackGroupCollection & collection) RCPPUTILS_TSA_REQUIRES(mutex_);
+
+  RCLCPP_PUBLIC
+  void
+  remove_callback_group_from_collection(rclcpp::CallbackGroup::SharedPtr group_ptr,
+    CallbackGroupCollection & collection) RCPPUTILS_TSA_REQUIRES(mutex_);
+
+  RCLCPP_PUBLIC
+  void
+  add_automatically_associated_callback_groups() RCPPUTILS_TSA_REQUIRES(mutex_);
+
+  RCLCPP_PUBLIC
+  void
+  prune_invalid_nodes_and_groups() RCPPUTILS_TSA_REQUIRES(mutex_);
+
+  mutable std::mutex mutex_;
+
+  CallbackGroupCollection
+  manually_added_groups_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+
+  CallbackGroupCollection
+  automatically_added_groups_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+
+  /// nodes that are associated with the executor
+  std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>
+  weak_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+
+  std::shared_ptr<ExecutorNotifyWaitable> notify_waitable_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+};
+}  // namespace executors
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTOR_HPP_

rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp

@@ -0,0 +1,73 @@
+// Copyright 2023 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__EXECUTOR_NOTIFY_WAITABLE_HPP_
+#define RCLCPP__EXECUTORS__EXECUTOR_NOTIFY_WAITABLE_HPP_
+
+#include <list>
+#include <memory>
+
+#include "rclcpp/guard_condition.hpp"
+#include "rclcpp/waitable.hpp"
+
+namespace rclcpp
+{
+namespace executors
+{
+
+class ExecutorNotifyWaitable: rclcpp::Waitable
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(ExecutorNotifyWaitable)
+
+  // Constructor
+  RCLCPP_PUBLIC
+  ExecutorNotifyWaitable() = default;
+
+  // Destructor
+  RCLCPP_PUBLIC
+  ~ExecutorNotifyWaitable() override = default;
+
+  RCLCPP_PUBLIC
+  void
+  add_to_wait_set(rcl_wait_set_t * wait_set) override;
+
+  RCLCPP_PUBLIC
+  bool
+  is_ready(rcl_wait_set_t * wait_set) override;
+
+  RCLCPP_PUBLIC
+  void
+  execute(std::shared_ptr<void> & data) override;
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<void>
+  take_data() override;
+
+  RCLCPP_PUBLIC
+  void
+  add_guard_condition(rclcpp::GuardCondition * guard_condition);
+
+  RCLCPP_PUBLIC
+  void
+  remove_guard_condition(rclcpp::GuardCondition * guard_condition);
+
+private:
+  std::list<const rclcpp::GuardCondition *> notify_guard_conditions_;
+};
+
+}  // namespace executors
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXECUTORS__EXECUTOR_NOTIFY_WAITABLE_HPP_