Initial version of callback-group-based Executor.

Signed-off-by: Ralph Lange <[email protected]>

rclcpp/include/rclcpp/callback_group.hpp

@@ -48,6 +48,13 @@ enum class CallbackGroupType
   Reentrant
 };
 
+enum class RealTimeClass
+{
+  RealTimeCritical,
+  SoftRealTime,
+  BestEffort
+};
+
 class CallbackGroup
 {
   friend class rclcpp::node_interfaces::NodeServices;
@@ -59,7 +66,9 @@ class CallbackGroup
   RCLCPP_SMART_PTR_DEFINITIONS(CallbackGroup)
 
   RCLCPP_PUBLIC
-  explicit CallbackGroup(CallbackGroupType group_type);
+  explicit CallbackGroup(
+    CallbackGroupType group_type,
+    RealTimeClass real_time_class = RealTimeClass::BestEffort);
 
   RCLCPP_PUBLIC
   const std::vector<rclcpp::SubscriptionBase::WeakPtr> &
@@ -89,6 +98,14 @@ class CallbackGroup
   const CallbackGroupType &
   type() const;
 
+  RCLCPP_PUBLIC
+  const RealTimeClass &
+  real_time_class() const;
+
+  RCLCPP_PUBLIC
+  std::atomic_bool &
+  get_associated_with_executor_atomic();
+
 protected:
   RCLCPP_DISABLE_COPY(CallbackGroup)
 
@@ -117,8 +134,10 @@ class CallbackGroup
   remove_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr) noexcept;
 
   CallbackGroupType type_;
+  RealTimeClass real_time_class_;
   // Mutex to protect the subsequent vectors of pointers.
   mutable std::mutex mutex_;
+  std::atomic_bool associated_with_executor_;
   std::vector<rclcpp::SubscriptionBase::WeakPtr> subscription_ptrs_;
   std::vector<rclcpp::TimerBase::WeakPtr> timer_ptrs_;
   std::vector<rclcpp::ServiceBase::WeakPtr> service_ptrs_;

rclcpp/include/rclcpp/executor.hpp

@@ -24,6 +24,7 @@
 #include <memory>
 #include <string>
 #include <vector>
+#include <map>
 
 #include "rcl/guard_condition.h"
 #include "rcl/wait.h"
@@ -111,6 +112,21 @@ class Executor
   virtual void
   spin() = 0;
 
+  /// Add a callback group to an executor.
+  RCLCPP_PUBLIC
+  virtual void
+  add_callback_group(
+    rclcpp::callback_group::CallbackGroup::SharedPtr group_ptr,
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify = true);
+
+  /// Remove a callback group from the executor.
+  RCLCPP_PUBLIC
+  virtual void
+  remove_callback_group(
+    rclcpp::callback_group::CallbackGroup::SharedPtr group_ptr,
+    bool notify = true);
+
+
   /// Add a node to the executor.
   /**
    * An executor can have zero or more nodes which provide work during `spin` functions.
@@ -328,6 +344,11 @@ class Executor
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
   get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group);
 
+  RCLCPP_PUBLIC
+  bool
+  has_node(const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) const;
+
+
   RCLCPP_PUBLIC
   rclcpp::callback_group::CallbackGroup::SharedPtr
   get_group_by_timer(rclcpp::TimerBase::SharedPtr timer);
@@ -364,7 +385,10 @@ class Executor
 private:
   RCLCPP_DISABLE_COPY(Executor)
 
-  std::vector<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> weak_nodes_;
+  typedef std::map<rclcpp::callback_group::CallbackGroup::WeakPtr,
+      rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
+      std::owner_less<rclcpp::callback_group::CallbackGroup::WeakPtr>> WeakCallbackGroupsToNodesMap;
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes_;
 };
 
 }  // namespace executor

rclcpp/include/rclcpp/memory_strategy.hpp

@@ -15,6 +15,7 @@
 #ifndef RCLCPP__MEMORY_STRATEGY_HPP_
 #define RCLCPP__MEMORY_STRATEGY_HPP_
 
+#include <map>
 #include <memory>
 #include <vector>
 
@@ -42,11 +43,13 @@ class RCLCPP_PUBLIC MemoryStrategy
 {
 public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(MemoryStrategy)
-  using WeakNodeVector = std::vector<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>;
+  using WeakCallbackGroupsToNodesMap = std::map<rclcpp::callback_group::CallbackGroup::WeakPtr,
+      rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
+      std::owner_less<rclcpp::callback_group::CallbackGroup::WeakPtr>>;
 
   virtual ~MemoryStrategy() = default;
 
-  virtual bool collect_entities(const WeakNodeVector & weak_nodes) = 0;
+  virtual bool collect_entities(const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) = 0;
 
   virtual size_t number_of_ready_subscriptions() const = 0;
   virtual size_t number_of_ready_services() const = 0;
@@ -66,65 +69,65 @@ class RCLCPP_PUBLIC MemoryStrategy
   virtual void
   get_next_subscription(
     rclcpp::executor::AnyExecutable & any_exec,
-    const WeakNodeVector & weak_nodes) = 0;
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) = 0;
 
   virtual void
   get_next_service(
     rclcpp::executor::AnyExecutable & any_exec,
-    const WeakNodeVector & weak_nodes) = 0;
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) = 0;
 
   virtual void
   get_next_client(
     rclcpp::executor::AnyExecutable & any_exec,
-    const WeakNodeVector & weak_nodes) = 0;
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) = 0;
 
   virtual void
   get_next_waitable(
     rclcpp::executor::AnyExecutable & any_exec,
-    const WeakNodeVector & weak_nodes) = 0;
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) = 0;
 
   virtual rcl_allocator_t
   get_allocator() = 0;
 
   static rclcpp::SubscriptionBase::SharedPtr
   get_subscription_by_handle(
     std::shared_ptr<const rcl_subscription_t> subscriber_handle,
-    const WeakNodeVector & weak_nodes);
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::ServiceBase::SharedPtr
   get_service_by_handle(
     std::shared_ptr<const rcl_service_t> service_handle,
-    const WeakNodeVector & weak_nodes);
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::ClientBase::SharedPtr
   get_client_by_handle(
     std::shared_ptr<const rcl_client_t> client_handle,
-    const WeakNodeVector & weak_nodes);
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
   get_node_by_group(
     rclcpp::callback_group::CallbackGroup::SharedPtr group,
-    const WeakNodeVector & weak_nodes);
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::callback_group::CallbackGroup::SharedPtr
   get_group_by_subscription(
     rclcpp::SubscriptionBase::SharedPtr subscription,
-    const WeakNodeVector & weak_nodes);
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::callback_group::CallbackGroup::SharedPtr
   get_group_by_service(
     rclcpp::ServiceBase::SharedPtr service,
-    const WeakNodeVector & weak_nodes);
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::callback_group::CallbackGroup::SharedPtr
   get_group_by_client(
     rclcpp::ClientBase::SharedPtr client,
-    const WeakNodeVector & weak_nodes);
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::callback_group::CallbackGroup::SharedPtr
   get_group_by_waitable(
     rclcpp::Waitable::SharedPtr waitable,
-    const WeakNodeVector & weak_nodes);
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 };
 
 }  // namespace memory_strategy

rclcpp/include/rclcpp/node.hpp

@@ -130,7 +130,10 @@ class Node : public std::enable_shared_from_this<Node>
   /// Create and return a callback group.
   RCLCPP_PUBLIC
   rclcpp::callback_group::CallbackGroup::SharedPtr
-  create_callback_group(rclcpp::callback_group::CallbackGroupType group_type);
+  create_callback_group(
+    rclcpp::callback_group::CallbackGroupType group_type,
+    rclcpp::callback_group::RealTimeClass real_time_class =
+    rclcpp::callback_group::RealTimeClass::BestEffort);
 
   /// Return the list of callback groups in the node.
   RCLCPP_PUBLIC

rclcpp/include/rclcpp/node_interfaces/node_base.hpp

@@ -85,7 +85,10 @@ class NodeBase : public NodeBaseInterface
   RCLCPP_PUBLIC
   virtual
   rclcpp::callback_group::CallbackGroup::SharedPtr
-  create_callback_group(rclcpp::callback_group::CallbackGroupType group_type);
+  create_callback_group(
+    rclcpp::callback_group::CallbackGroupType group_type,
+    rclcpp::callback_group::RealTimeClass real_time_class =
+    rclcpp::callback_group::RealTimeClass::BestEffort);
 
   RCLCPP_PUBLIC
   virtual
@@ -102,11 +105,6 @@ class NodeBase : public NodeBaseInterface
   const std::vector<rclcpp::callback_group::CallbackGroup::WeakPtr> &
   get_callback_groups() const;
 
-  RCLCPP_PUBLIC
-  virtual
-  std::atomic_bool &
-  get_associated_with_executor_atomic();
-
   RCLCPP_PUBLIC
   virtual
   rcl_guard_condition_t *
@@ -127,8 +125,6 @@ class NodeBase : public NodeBaseInterface
   rclcpp::callback_group::CallbackGroup::SharedPtr default_callback_group_;
   std::vector<rclcpp::callback_group::CallbackGroup::WeakPtr> callback_groups_;
 
-  std::atomic_bool associated_with_executor_;
-
   /// Guard condition for notifying the Executor of changes to this node.
   mutable std::recursive_mutex notify_guard_condition_mutex_;
   rcl_guard_condition_t notify_guard_condition_ = rcl_get_zero_initialized_guard_condition();

rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp

@@ -95,7 +95,10 @@ class NodeBaseInterface
   RCLCPP_PUBLIC
   virtual
   rclcpp::callback_group::CallbackGroup::SharedPtr
-  create_callback_group(rclcpp::callback_group::CallbackGroupType group_type) = 0;
+  create_callback_group(
+    rclcpp::callback_group::CallbackGroupType group_type,
+    rclcpp::callback_group::RealTimeClass real_time_class =
+    rclcpp::callback_group::RealTimeClass::BestEffort) = 0;
 
   /// Return the default callback group.
   RCLCPP_PUBLIC
@@ -115,12 +118,6 @@ class NodeBaseInterface
   const std::vector<rclcpp::callback_group::CallbackGroup::WeakPtr> &
   get_callback_groups() const = 0;
 
-  /// Return the atomic bool which is used to ensure only one executor is used.
-  RCLCPP_PUBLIC
-  virtual
-  std::atomic_bool &
-  get_associated_with_executor_atomic() = 0;
-
   /// Return guard condition that should be notified when the internal node state changes.
   /**
    * For example, this should be notified when a publisher is added or removed.