Skip to content

Commit

Permalink
Lint and docs
Browse files Browse the repository at this point in the history
Signed-off-by: Michael Carroll <[email protected]>
  • Loading branch information
mjcarroll committed Apr 3, 2023
1 parent 9dd48ce commit 6267741
Show file tree
Hide file tree
Showing 3 changed files with 57 additions and 5 deletions.
2 changes: 1 addition & 1 deletion rclcpp/include/rclcpp/executor.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -639,7 +639,7 @@ class Executor
/// Guard condition for signaling the rmw layer to wake up for special events.
std::shared_ptr<rclcpp::GuardCondition> 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<rclcpp::GuardCondition> shutdown_guard_condition_;

/// Wait set for managing entities that the rmw layer waits on.
Expand Down
52 changes: 52 additions & 0 deletions rclcpp/include/rclcpp/executors/executor_entities_collector.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -174,37 +174,76 @@ class ExecutorEntitiesCollector
rclcpp::GuardCondition::WeakPtr,
std::owner_less<rclcpp::CallbackGroup::WeakPtr>>;

/// 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();
Expand All @@ -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<ExecutorNotifyWaitable> notify_waitable_;
};
} // namespace executors
Expand Down
8 changes: 4 additions & 4 deletions rclcpp/src/rclcpp/executors/executor_entities_collector.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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);
Expand All @@ -334,15 +334,15 @@ 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_);
}
}
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_);
Expand Down

0 comments on commit 6267741

Please sign in to comment.