From 6267741212c2370cf2f109ea692ab7458f2b2097 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Mon, 3 Apr 2023 18:20:10 +0000 Subject: [PATCH] Lint and docs Signed-off-by: Michael Carroll --- rclcpp/include/rclcpp/executor.hpp | 2 +- .../executors/executor_entities_collector.hpp | 52 +++++++++++++++++++ .../executors/executor_entities_collector.cpp | 8 +-- 3 files changed, 57 insertions(+), 5 deletions(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 43e15eda59..3e654faa54 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -639,7 +639,7 @@ class Executor /// Guard condition for signaling the rmw layer to wake up for special events. std::shared_ptr interrupt_guard_condition_; - /// Guard condition for signaling the rmw layer to wake up for system shutdown. + /// Guard condition for signaling the rmw layer to wake up for system shutdown. std::shared_ptr shutdown_guard_condition_; /// Wait set for managing entities that the rmw layer waits on. diff --git a/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp b/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp index d60e01bfdb..36fe9d5906 100644 --- a/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp +++ b/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp @@ -174,37 +174,76 @@ class ExecutorEntitiesCollector rclcpp::GuardCondition::WeakPtr, std::owner_less>; + /// Implementation of removing a node from the collector. + /** + * This will disassociate the node from the collector and remove any + * automatically-added callback groups + * + * This takes and returns an iterator so it may be used as: + * + * it = remove_weak_node(it); + * + * \param[in] weak_node iterator to the weak node to be removed + * \return Valid updated iterator in the same collection + */ RCLCPP_PUBLIC NodeCollection::iterator remove_weak_node(NodeCollection::iterator weak_node); + /// Implementation of removing a callback gruop from the collector. + /** + * This will disassociate the callback group from the collector + * + * This takes and returns an iterator so it may be used as: + * + * it = remove_weak_callback_group(it); + * + * \param[in] weak_group_it iterator to the weak group to be removed + * \param[in] collection the collection to remove the group from + * (manually or automatically added) + * \return Valid updated iterator in the same collection + */ RCLCPP_PUBLIC CallbackGroupCollection::iterator remove_weak_callback_group( CallbackGroupCollection::iterator weak_group_it, CallbackGroupCollection & collection); + /// Implementation of adding a callback group + /** + * \param[in] group_ptr the group to add + * \param[in] collection the collection to add the group to + */ RCLCPP_PUBLIC void add_callback_group_to_collection( rclcpp::CallbackGroup::SharedPtr group_ptr, CallbackGroupCollection & collection); + /// Implementation of removing a callback group + /** + * \param[in] group_ptr the group to remove + * \param[in] collection the collection to remove the group from + */ RCLCPP_PUBLIC void remove_callback_group_from_collection( rclcpp::CallbackGroup::SharedPtr group_ptr, CallbackGroupCollection & collection); + /// Iterate over queued added/remove nodes and callback_groups RCLCPP_PUBLIC void process_queues(); + /// Check a collection of nodes and add any new callback_groups that + /// are set to be automatically associated via the node. RCLCPP_PUBLIC void add_automatically_associated_callback_groups( const NodeCollection & nodes_to_check); + /// Check all nodes and group for expired weak pointers and remove them. RCLCPP_PUBLIC void prune_invalid_nodes_and_groups(); @@ -218,15 +257,28 @@ class ExecutorEntitiesCollector /// nodes that are associated with the executor NodeCollection weak_nodes_; + /// mutex to protect pending queues std::mutex pending_mutex_; + + /// nodes that have been added since the last update. NodeCollection pending_added_nodes_; + + /// nodes that have been removed since the last update. NodeCollection pending_removed_nodes_; + + /// callback groups that have been added since the last update. CallbackGroupCollection pending_manually_added_groups_; + + /// callback groups that have been removed since the last update. CallbackGroupCollection pending_manually_removed_groups_; + /// Track guard conditions associated with added nodes WeakNodesToGuardConditionsMap weak_nodes_to_guard_conditions_; + + /// Track guard conditions associated with added callback groups WeakGroupsToGuardConditionsMap weak_groups_to_guard_conditions_; + /// Waitable to add guard conditions to std::shared_ptr notify_waitable_; }; } // namespace executors diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp index 4afdeb3a2b..fc9af38dc2 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp @@ -295,7 +295,7 @@ ExecutorEntitiesCollector::process_queues() { std::lock_guard pending_lock(pending_mutex_); - for (auto weak_node_ptr: pending_added_nodes_) { + for (auto weak_node_ptr : pending_added_nodes_) { auto node_ptr = weak_node_ptr.lock(); if (!node_ptr) { continue; @@ -310,7 +310,7 @@ ExecutorEntitiesCollector::process_queues() } pending_added_nodes_.clear(); - for (auto weak_node_ptr: pending_removed_nodes_) { + for (auto weak_node_ptr : pending_removed_nodes_) { auto node_it = weak_nodes_.find(weak_node_ptr); if (node_it != weak_nodes_.end()) { remove_weak_node(node_it); @@ -334,7 +334,7 @@ ExecutorEntitiesCollector::process_queues() } pending_removed_nodes_.clear(); - for (auto weak_group_ptr: pending_manually_added_groups_) { + for (auto weak_group_ptr : pending_manually_added_groups_) { auto group_ptr = weak_group_ptr.lock(); if (group_ptr) { this->add_callback_group_to_collection(group_ptr, manually_added_groups_); @@ -342,7 +342,7 @@ ExecutorEntitiesCollector::process_queues() } pending_manually_added_groups_.clear(); - for (auto weak_group_ptr: pending_manually_removed_groups_) { + for (auto weak_group_ptr : pending_manually_removed_groups_) { auto group_ptr = weak_group_ptr.lock(); if (group_ptr) { this->remove_callback_group_from_collection(group_ptr, manually_added_groups_);