Address reviewer feedback and fix templates

Signed-off-by: Michael Carroll <[email protected]>

Author: mjcarroll

rclcpp/CMakeLists.txt

@@ -54,9 +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/executor_entities_collector.cpp
+  src/rclcpp/executors/executor_notify_waitable.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

@@ -35,16 +35,19 @@ namespace executors
 template<typename EntityValueType>
 struct CollectionEntry
 {
-  typename EntityValueType::WeakPtr entity;
+  using EntityWeakPtr = typename EntityValueType::WeakPtr;
+  using EntitySharedPtr = typename EntityValueType::SharedPtr;
+
+  EntityWeakPtr entity;
   rclcpp::CallbackGroup::WeakPtr callback_group;
 };
 
 template<typename CollectionType>
 void update_entities(
   const CollectionType & update_from,
-  CollectionType update_to,
-  std::function<void(typename CollectionType::mapped_type::EntitySharedPtr)> on_added,
-  std::function<void(typename CollectionType::mapped_type::EntitySharedPtr)> on_removed
+  CollectionType & update_to,
+  std::function<void(typename CollectionType::EntitySharedPtr)> on_added,
+  std::function<void(typename CollectionType::EntitySharedPtr)> on_removed
 )
 {
   for (auto it = update_to.begin(); it != update_to.end(); ) {
@@ -60,7 +63,7 @@ void update_entities(
   }
   for (auto it = update_from.begin(); it != update_from.end(); ++it) {
     if (update_to.count(it->first) == 0) {
-      auto entity = it->entity.lock();
+      auto entity = it->second.entity.lock();
       if (entity) {
         on_added(entity);
       }
@@ -82,7 +85,7 @@ class EntityCollection
     std::function<void(EntitySharedPtr)> on_added,
     std::function<void(EntitySharedPtr)> on_removed)
   {
-    update_entities(*this, other, on_added, on_removed);
+    update_entities(other, *this, on_added, on_removed);
   }
 };
 
@@ -118,6 +121,7 @@ struct ExecutorEntitiesCollection
   GuardConditionCollection guard_conditions;
   WaitableCollection waitables;
 
+  bool empty() const;
   void clear();
 };
 

rclcpp/include/rclcpp/executors/executor_entities_collector.hpp

@@ -143,6 +143,9 @@ class ExecutorEntitiesCollector
   std::vector<rclcpp::CallbackGroup::WeakPtr>
   get_automatically_added_callback_groups();
 
+  RCLCPP_PUBLIC
+  std::shared_ptr<ExecutorNotifyWaitable> get_notify_waitable();
+
   /// Update the underlying collections
   /**
    * This will prune nodes and callback groups that are no longer valid as well
@@ -153,10 +156,34 @@ class ExecutorEntitiesCollector
   update_collections();
 
 protected:
+  using NodeCollection = std::set<
+    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
+    std::owner_less<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>>;
+
   using CallbackGroupCollection = std::set<
     rclcpp::CallbackGroup::WeakPtr,
     std::owner_less<rclcpp::CallbackGroup::WeakPtr>>;
 
+  using WeakNodesToGuardConditionsMap = std::map<
+    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
+    const rclcpp::GuardCondition *,
+    std::owner_less<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>>;
+
+  using WeakGroupsToGuardConditionsMap = std::map<
+    rclcpp::CallbackGroup::WeakPtr,
+    const rclcpp::GuardCondition *,
+    std::owner_less<rclcpp::CallbackGroup::WeakPtr>>;
+
+  RCLCPP_PUBLIC
+  NodeCollection::iterator
+  remove_weak_node(NodeCollection::iterator weak_node) RCPPUTILS_TSA_REQUIRES(mutex_);
+
+  RCLCPP_PUBLIC
+  CallbackGroupCollection::iterator
+  remove_weak_callback_group(
+    CallbackGroupCollection::iterator weak_group_it,
+    CallbackGroupCollection & collection) RCPPUTILS_TSA_REQUIRES(mutex_);
+
   RCLCPP_PUBLIC
   void
   add_callback_group_to_collection(
@@ -172,7 +199,7 @@ class ExecutorEntitiesCollector
   RCLCPP_PUBLIC
   void
   add_automatically_associated_callback_groups(
-    std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> nodes_to_check)
+    const NodeCollection & nodes_to_check)
   RCPPUTILS_TSA_REQUIRES(mutex_);
 
   RCLCPP_PUBLIC
@@ -181,19 +208,25 @@ class ExecutorEntitiesCollector
 
   mutable std::mutex mutex_;
 
+  /// Callback groups that were added via `add_callback_group`
   CallbackGroupCollection
   manually_added_groups_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
 
+  /// Callback groups that were added by their association with added nodes
   CallbackGroupCollection
   automatically_added_groups_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
 
   /// nodes that are associated with the executor
-  std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>
+  NodeCollection
   weak_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
 
+  WeakNodesToGuardConditionsMap weak_nodes_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+
+  WeakGroupsToGuardConditionsMap weak_groups_to_guard_conditions_ 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

@@ -85,15 +85,15 @@ class ExecutorNotifyWaitable : public rclcpp::Waitable
    */
   RCLCPP_PUBLIC
   void
-  add_guard_condition(rclcpp::GuardCondition * guard_condition);
+  add_guard_condition(const rclcpp::GuardCondition * guard_condition);
 
   /// Remove a guard condition from being waited on.
   /**
    * \param[in] guard_condition The guard condition to remove.
    */
   RCLCPP_PUBLIC
   void
-  remove_guard_condition(rclcpp::GuardCondition * guard_condition);
+  remove_guard_condition(const rclcpp::GuardCondition * guard_condition);
 
   /// Get the number of ready guard_conditions
   /**
@@ -107,8 +107,17 @@ class ExecutorNotifyWaitable : public rclcpp::Waitable
   /// Callback to run when waitable executes
   std::function<void(void)> execute_callback_;
 
+  /// The collection of guard conditions to be waited on.
+  std::mutex guard_condition_mutex_;
+
   /// The collection of guard conditions to be waited on.
   std::list<const rclcpp::GuardCondition *> notify_guard_conditions_;
+
+  /// The collection of guard conditions to be waited on.
+  std::list<const rclcpp::GuardCondition *> to_add_;
+
+  /// The collection of guard conditions to be waited on.
+  std::list<const rclcpp::GuardCondition *> to_remove_;
 };
 
 }  // namespace executors

rclcpp/src/rclcpp/callback_group.cpp

@@ -141,6 +141,7 @@ CallbackGroup::get_notify_guard_condition(const rclcpp::Context::SharedPtr conte
 rclcpp::GuardCondition::SharedPtr
 CallbackGroup::get_notify_guard_condition()
 {
+  std::lock_guard<std::recursive_mutex> lock(notify_guard_condition_mutex_);
   auto context_ptr = this->get_context_();
   if (context_ptr && context_ptr->is_valid()) {
     std::lock_guard<std::recursive_mutex> lock(notify_guard_condition_mutex_);
@@ -156,6 +157,8 @@ CallbackGroup::get_notify_guard_condition()
     }
 
     return notify_guard_condition_;
+  } else {
+    throw std::runtime_error("Couldn't get guard condition from invalid context");
   }
   return nullptr;
 }

rclcpp/src/rclcpp/executors/executor_entities_collection.cpp

@@ -18,6 +18,15 @@ namespace rclcpp
 {
 namespace executors
 {
+bool ExecutorEntitiesCollection::empty() const
+{
+  return (subscriptions.empty() &&
+          timers.empty() &&
+          guard_conditions.empty() &&
+          clients.empty() &&
+          services.empty() &&
+           waitables.empty());
+}
 
 void ExecutorEntitiesCollection::clear()
 {
@@ -29,6 +38,7 @@ void ExecutorEntitiesCollection::clear()
   waitables.clear();
 }
 
+
 void
 build_entities_collection(
   const std::vector<rclcpp::CallbackGroup::WeakPtr> & callback_groups,
@@ -42,43 +52,46 @@ build_entities_collection(
       continue;
     }
 
-    group_ptr->collect_all_ptrs(
-      [&collection, weak_group_ptr](const rclcpp::SubscriptionBase::SharedPtr & subscription) {
-        collection.subscriptions.insert(
-          {
-            subscription->get_subscription_handle().get(),
-            {subscription, weak_group_ptr}
-          });
-      },
-      [&collection, weak_group_ptr](const rclcpp::ServiceBase::SharedPtr & service) {
-        collection.services.insert(
-          {
-            service->get_service_handle().get(),
-            {service, weak_group_ptr}
-          });
-      },
-      [&collection, weak_group_ptr](const rclcpp::ClientBase::SharedPtr & client) {
-        collection.clients.insert(
-          {
-            client->get_client_handle().get(),
-            {client, weak_group_ptr}
-          });
-      },
-      [&collection, weak_group_ptr](const rclcpp::TimerBase::SharedPtr & timer) {
-        collection.timers.insert(
-          {
-            timer->get_timer_handle().get(),
-            {timer, weak_group_ptr}
-          });
-      },
-      [&collection, weak_group_ptr](const rclcpp::Waitable::SharedPtr & waitable) {
-        collection.waitables.insert(
-          {
-            waitable.get(),
-            {waitable, weak_group_ptr}
-          });
-      }
-    );
+    if (group_ptr->can_be_taken_from().load())
+    {
+      group_ptr->collect_all_ptrs(
+        [&collection, weak_group_ptr](const rclcpp::SubscriptionBase::SharedPtr & subscription) {
+          collection.subscriptions.insert(
+            {
+              subscription->get_subscription_handle().get(),
+              {subscription, weak_group_ptr}
+            });
+        },
+        [&collection, weak_group_ptr](const rclcpp::ServiceBase::SharedPtr & service) {
+          collection.services.insert(
+            {
+              service->get_service_handle().get(),
+              {service, weak_group_ptr}
+            });
+        },
+        [&collection, weak_group_ptr](const rclcpp::ClientBase::SharedPtr & client) {
+          collection.clients.insert(
+            {
+              client->get_client_handle().get(),
+              {client, weak_group_ptr}
+            });
+        },
+        [&collection, weak_group_ptr](const rclcpp::TimerBase::SharedPtr & timer) {
+          collection.timers.insert(
+            {
+              timer->get_timer_handle().get(),
+              {timer, weak_group_ptr}
+            });
+        },
+        [&collection, weak_group_ptr](const rclcpp::Waitable::SharedPtr & waitable) {
+          collection.waitables.insert(
+            {
+              waitable.get(),
+              {waitable, weak_group_ptr}
+            });
+        }
+      );
+    }
   }
 }
 
@@ -88,24 +101,28 @@ void check_ready(
   std::deque<rclcpp::AnyExecutable> & executables,
   size_t size_of_waited_entities,
   typename EntityCollectionType::Key * waited_entities,
-  std::function<bool(rclcpp::AnyExecutable,
-  typename EntityCollectionType::EntitySharedPtr)> fill_executable)
+  std::function<bool(rclcpp::AnyExecutable &,
+                     typename EntityCollectionType::EntitySharedPtr &)> fill_executable)
 {
   for (size_t ii = 0; ii < size_of_waited_entities; ++ii) {
     if (waited_entities[ii]) {
       auto entity_iter = collection.find(waited_entities[ii]);
       if (entity_iter != collection.end()) {
         auto entity = entity_iter->second.entity.lock();
-        auto callback_group = entity_iter->second.callback_group.lock();
-
-        if (!entity || !callback_group || !callback_group->can_be_taken_from().load()) {
+        if (!entity) {
           continue;
         }
 
+        auto callback_group = entity_iter->second.callback_group.lock();
+        if (callback_group && !callback_group->can_be_taken_from().load()) {
+          continue;
+        }
         rclcpp::AnyExecutable exec;
+
         exec.callback_group = callback_group;
         if (fill_executable(exec, entity)) {
           executables.push_back(exec);
+        } else {
         }
       }
     }
@@ -123,15 +140,13 @@ ready_executables(
   if (wait_result.kind() != rclcpp::WaitResultKind::Ready) {
     return ret;
   }
-
   auto rcl_wait_set = wait_result.get_wait_set().get_rcl_wait_set();
-
   check_ready(
     collection.timers,
     ret,
     rcl_wait_set.size_of_timers,
     rcl_wait_set.timers,
-    [](auto exec, auto timer) {
+    [](rclcpp::AnyExecutable & exec, auto timer) {
       if (!timer->call()) {
         return false;
       }
@@ -144,17 +159,18 @@ ready_executables(
     ret,
     rcl_wait_set.size_of_subscriptions,
     rcl_wait_set.subscriptions,
-    [](auto exec, auto subscription) {
+    [](rclcpp::AnyExecutable & exec, auto subscription) {
       exec.subscription = subscription;
       return true;
     });
 
+
   check_ready(
     collection.services,
     ret,
     rcl_wait_set.size_of_services,
     rcl_wait_set.services,
-    [](auto exec, auto service) {
+    [](rclcpp::AnyExecutable & exec, auto service) {
       exec.service = service;
       return true;
     });
@@ -164,7 +180,7 @@ ready_executables(
     ret,
     rcl_wait_set.size_of_clients,
     rcl_wait_set.clients,
-    [](auto exec, auto client) {
+    [](rclcpp::AnyExecutable & exec, auto client) {
       exec.client = client;
       return true;
     });
@@ -173,7 +189,7 @@ ready_executables(
     auto waitable = entry.entity.lock();
     if (waitable && waitable->is_ready(&rcl_wait_set)) {
       auto group = entry.callback_group.lock();
-      if (!group || !group->can_be_taken_from().load()) {
+      if (group && !group->can_be_taken_from().load()) {
         continue;
       }