From b74b19268708f2bff89e9bd530434a7b7c818b31 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 29 Mar 2023 14:33:58 +0000 Subject: [PATCH] Lint Signed-off-by: Michael Carroll --- .../executor_entities_collection.hpp | 10 +-- .../executors/executor_entities_collector.hpp | 13 ++-- .../executors/executor_notify_waitable.hpp | 1 + rclcpp/src/rclcpp/callback_group.cpp | 1 - rclcpp/src/rclcpp/executor.cpp | 64 +++++++++++-------- .../executor_entities_collection.cpp | 8 ++- .../executors/executor_entities_collector.cpp | 22 ++++--- .../executors/executor_notify_waitable.cpp | 4 +- 8 files changed, 71 insertions(+), 52 deletions(-) diff --git a/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp b/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp index f07ab51728..b1aa47a085 100644 --- a/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp +++ b/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp @@ -16,8 +16,8 @@ #define RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTION_HPP_ #include -#include -#include +#include +#include #include "rcpputils/thread_safety_annotations.hpp" @@ -65,7 +65,9 @@ struct ExecutorEntitiesCollection rclcpp::CallbackGroup::WeakPtr callback_group; }; using WaitableCollection = std::unordered_map; - using GuardConditionCollection = std::unordered_map; + + using GuardConditionCollection = std::unordered_map; TimerCollection timers; SubscriptionCollection subscriptions; @@ -121,4 +123,4 @@ ready_executables( } // namespace executors } // namespace rclcpp -#endif // RCLCPP__EXECUTORS__ENTITIES_COLLECTION_HPP_ +#endif // RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTION_HPP_ diff --git a/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp b/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp index e9f52461cd..f26366022c 100644 --- a/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp +++ b/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp @@ -15,9 +15,10 @@ #ifndef RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTOR_HPP_ #define RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTOR_HPP_ -#include #include +#include #include +#include #include "rcpputils/thread_safety_annotations.hpp" @@ -106,17 +107,19 @@ class ExecutorEntitiesCollector get_executor_notify_waitable(); protected: - using CallbackGroupCollection = std::set>; + using CallbackGroupCollection = std::set< + rclcpp::CallbackGroup::WeakPtr, + std::owner_less>; RCLCPP_PUBLIC void add_callback_group_to_collection(rclcpp::CallbackGroup::SharedPtr group_ptr, - CallbackGroupCollection & collection) RCPPUTILS_TSA_REQUIRES(mutex_); + 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_); + CallbackGroupCollection & collection) RCPPUTILS_TSA_REQUIRES(mutex_); RCLCPP_PUBLIC void @@ -143,4 +146,4 @@ class ExecutorEntitiesCollector } // namespace executors } // namespace rclcpp -#endif // RCLCPP__EXECUTORS__ENTITIES_COLLECTOR_HPP_ +#endif // RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTOR_HPP_ diff --git a/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp b/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp index 68bebc3586..474e1521f1 100644 --- a/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp +++ b/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp @@ -16,6 +16,7 @@ #define RCLCPP__EXECUTORS__EXECUTOR_NOTIFY_WAITABLE_HPP_ #include +#include #include "rclcpp/guard_condition.hpp" #include "rclcpp/waitable.hpp" diff --git a/rclcpp/src/rclcpp/callback_group.cpp b/rclcpp/src/rclcpp/callback_group.cpp index bef81a4bae..6f229e353d 100644 --- a/rclcpp/src/rclcpp/callback_group.cpp +++ b/rclcpp/src/rclcpp/callback_group.cpp @@ -122,7 +122,6 @@ CallbackGroup::automatically_add_to_executor_with_node() const rclcpp::GuardCondition::SharedPtr CallbackGroup::get_notify_guard_condition(const rclcpp::Context::SharedPtr context_ptr) { - std::lock_guard lock(notify_guard_condition_mutex_); if (notify_guard_condition_ && context_ptr != notify_guard_condition_->get_context()) { if (associated_with_executor_) { diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 28fc013253..23d5695c89 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -23,6 +23,7 @@ #include "rcl/allocator.h" #include "rcl/error_handling.h" +#include "rclcpp/subscription_wait_set_mask.hpp" #include "rcpputils/scope_exit.hpp" #include "rclcpp/exceptions.hpp" @@ -39,6 +40,8 @@ using namespace std::chrono_literals; using rclcpp::Executor; +static constexpr rclcpp::SubscriptionWaitSetMask kDefaultSubscriptionMask = {true, false, false}; + Executor::Executor(const rclcpp::ExecutorOptions & options) : spinning(false), interrupt_guard_condition_(options.context), @@ -56,35 +59,39 @@ Executor::Executor(const rclcpp::ExecutorOptions & options) } }); - this->collector_.get_executor_notify_waitable()->add_guard_condition(&interrupt_guard_condition_); - this->collector_.get_executor_notify_waitable()->add_guard_condition(shutdown_guard_condition_.get()); + this->collector_.get_executor_notify_waitable()->add_guard_condition( + &interrupt_guard_condition_); + this->collector_.get_executor_notify_waitable()->add_guard_condition( + shutdown_guard_condition_.get()); } Executor::~Executor() { current_collection_.update_timers({}, - [this](auto timer){wait_set_->add_timer(timer);}, - [this](auto timer){wait_set_->remove_timer(timer);}); + [this](auto timer){wait_set_->add_timer(timer);}, + [this](auto timer){wait_set_->remove_timer(timer);}); current_collection_.update_subscriptions({}, - [this](auto subscription){wait_set_->add_subscription(subscription, {true, false, false});}, - [this](auto subscription){wait_set_->remove_subscription(subscription, {true, false, false});}); + [this](auto subscription){ + wait_set_->add_subscription(subscription, kDefaultSubscriptionMask);}, + [this](auto subscription){ + wait_set_->remove_subscription(subscription, kDefaultSubscriptionMask);}); current_collection_.update_clients({}, - [this](auto client){wait_set_->add_client(client);}, - [this](auto client){wait_set_->remove_client(client);}); + [this](auto client){wait_set_->add_client(client);}, + [this](auto client){wait_set_->remove_client(client);}); current_collection_.update_services({}, - [this](auto service){wait_set_->add_service(service);}, - [this](auto service){wait_set_->remove_service(service);}); + [this](auto service){wait_set_->add_service(service);}, + [this](auto service){wait_set_->remove_service(service);}); current_collection_.update_guard_conditions({}, - [this](auto guard_condition){wait_set_->add_guard_condition(guard_condition);}, - [this](auto guard_condition){wait_set_->remove_guard_condition(guard_condition);}); + [this](auto guard_condition){wait_set_->add_guard_condition(guard_condition);}, + [this](auto guard_condition){wait_set_->remove_guard_condition(guard_condition);}); current_collection_.update_waitables({}, - [this](auto waitable){wait_set_->add_waitable(waitable);}, - [this](auto waitable){wait_set_->remove_waitable(waitable);}); + [this](auto waitable){wait_set_->add_waitable(waitable);}, + [this](auto waitable){wait_set_->remove_waitable(waitable);}); // Remove shutdown callback handle registered to Context if (!context_->remove_on_shutdown_callback(shutdown_callback_handle_)) { @@ -510,28 +517,30 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) rclcpp::executors::build_entities_collection(callback_groups, collection); current_collection_.update_timers(collection.timers, - [this](auto timer){wait_set_->add_timer(timer);}, - [this](auto timer){wait_set_->remove_timer(timer);}); + [this](auto timer){wait_set_->add_timer(timer);}, + [this](auto timer){wait_set_->remove_timer(timer);}); current_collection_.update_subscriptions(collection.subscriptions, - [this](auto subscription){wait_set_->add_subscription(subscription, {true, false, false});}, - [this](auto subscription){wait_set_->remove_subscription(subscription, {true, false, false});}); + [this](auto subscription){ + wait_set_->add_subscription(subscription, kDefaultSubscriptionMask);}, + [this](auto subscription){ + wait_set_->remove_subscription(subscription, kDefaultSubscriptionMask);}); current_collection_.update_clients(collection.clients, - [this](auto client){wait_set_->add_client(client);}, - [this](auto client){wait_set_->remove_client(client);}); + [this](auto client){wait_set_->add_client(client);}, + [this](auto client){wait_set_->remove_client(client);}); current_collection_.update_services(collection.services, - [this](auto service){wait_set_->add_service(service);}, - [this](auto service){wait_set_->remove_service(service);}); + [this](auto service){wait_set_->add_service(service);}, + [this](auto service){wait_set_->remove_service(service);}); current_collection_.update_guard_conditions(collection.guard_conditions, - [this](auto guard_condition){wait_set_->add_guard_condition(guard_condition);}, - [this](auto guard_condition){wait_set_->remove_guard_condition(guard_condition);}); + [this](auto guard_condition){wait_set_->add_guard_condition(guard_condition);}, + [this](auto guard_condition){wait_set_->remove_guard_condition(guard_condition);}); current_collection_.update_waitables(collection.waitables, - [this](auto waitable){wait_set_->add_waitable(waitable);}, - [this](auto waitable){wait_set_->remove_waitable(waitable);}); + [this](auto waitable){wait_set_->add_waitable(waitable);}, + [this](auto waitable){wait_set_->remove_waitable(waitable);}); } auto wait_result = wait_set_->wait(timeout); @@ -561,7 +570,8 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable) any_executable = ready_executables_.front(); ready_executables_.pop_front(); - if (any_executable.callback_group && any_executable.callback_group->type() == CallbackGroupType::MutuallyExclusive) + if (any_executable.callback_group && + any_executable.callback_group->type() == CallbackGroupType::MutuallyExclusive) { assert(any_executable.callback_group->can_be_taken_from().load()); any_executable.callback_group->can_be_taken_from().store(false); diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp index cdde3919e0..4963a0439b 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp @@ -197,11 +197,13 @@ void ExecutorEntitiesCollection::update_waitables( void -build_entities_collection(const std::vector & callback_groups, ExecutorEntitiesCollection & collection) +build_entities_collection( + const std::vector & callback_groups, + ExecutorEntitiesCollection & collection) { collection.clear(); - for (auto weak_group_ptr: callback_groups) + for (auto weak_group_ptr : callback_groups) { auto group_ptr = weak_group_ptr.lock(); if (!group_ptr) @@ -334,7 +336,7 @@ ready_executables( } } - for (auto & [handle, entry]: collection.waitables) + for (auto & [handle, entry] : collection.waitables) { auto waitable = entry.waitable.lock(); if (waitable && waitable->is_ready(&rcl_wait_set)) { diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp index 23056d9ec8..5e4ef9e6b1 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp @@ -43,6 +43,7 @@ ExecutorEntitiesCollector::~ExecutorEntitiesCollector() }); weak_nodes_.clear(); + // Disassociate each automatically-added callback group from this executor collector std::for_each( automatically_added_groups_.begin(), automatically_added_groups_.end(), [] (rclcpp::CallbackGroup::WeakPtr weak_group_ptr) { @@ -54,6 +55,7 @@ ExecutorEntitiesCollector::~ExecutorEntitiesCollector() }); automatically_added_groups_.clear(); + // Disassociate each manually-added callback group from this executor collector std::for_each( manually_added_groups_.begin(), manually_added_groups_.end(), [] (rclcpp::CallbackGroup::WeakPtr weak_group_ptr) { @@ -83,7 +85,8 @@ ExecutorEntitiesCollector::add_node(rclcpp::node_interfaces::NodeBaseInterface:: } void -ExecutorEntitiesCollector::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) +ExecutorEntitiesCollector::remove_node( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) { std::lock_guard guard{mutex_}; if (!node_ptr->get_associated_with_executor_atomic().load()) { @@ -144,12 +147,10 @@ ExecutorEntitiesCollector::get_all_callback_groups() this->update_collections(); - std::cout << "get_all_callback_groups: " << manually_added_groups_.size() << " " << automatically_added_groups_.size() << std::endl; - for (const auto & group_ptr : manually_added_groups_) { groups.push_back(group_ptr); } - for (auto const & group_ptr: automatically_added_groups_) { + for (auto const & group_ptr : automatically_added_groups_) { groups.push_back(group_ptr); } return groups; @@ -171,7 +172,7 @@ ExecutorEntitiesCollector::get_automatically_added_callback_groups() { std::vector groups; std::lock_guard guard{mutex_}; - for (auto const & group_ptr: automatically_added_groups_) { + for (auto const & group_ptr : automatically_added_groups_) { groups.push_back(group_ptr); } return groups; @@ -190,8 +191,9 @@ ExecutorEntitiesCollector::get_executor_notify_waitable() } void -ExecutorEntitiesCollector::add_callback_group_to_collection(rclcpp::CallbackGroup::SharedPtr group_ptr, - CallbackGroupCollection & collection) +ExecutorEntitiesCollector::add_callback_group_to_collection( + rclcpp::CallbackGroup::SharedPtr group_ptr, + CallbackGroupCollection & collection) { std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic(); if (has_executor.exchange(true)) { @@ -205,8 +207,9 @@ ExecutorEntitiesCollector::add_callback_group_to_collection(rclcpp::CallbackGrou } void -ExecutorEntitiesCollector::remove_callback_group_from_collection(rclcpp::CallbackGroup::SharedPtr group_ptr, - CallbackGroupCollection & collection) +ExecutorEntitiesCollector::remove_callback_group_from_collection( + rclcpp::CallbackGroup::SharedPtr group_ptr, + CallbackGroupCollection & collection) { rclcpp::CallbackGroup::WeakPtr weak_group_ptr(group_ptr); auto iter = collection.find(weak_group_ptr); @@ -238,7 +241,6 @@ ExecutorEntitiesCollector::add_automatically_associated_callback_groups() if (!group_ptr->get_associated_with_executor_atomic().load() && group_ptr->automatically_add_to_executor_with_node()) { - std::cout << "Automatically adding: " << group_ptr.get() << std::endl; this->add_callback_group_to_collection(group_ptr, this->automatically_added_groups_); } }); diff --git a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp index f88d8d80cd..ae9f7767ea 100644 --- a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp +++ b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp @@ -23,7 +23,7 @@ namespace executors void ExecutorNotifyWaitable::add_to_wait_set(rcl_wait_set_t * wait_set) { - for (auto guard_condition: this->notify_guard_conditions_) { + for (auto guard_condition : this->notify_guard_conditions_) { detail::add_guard_condition_to_rcl_wait_set(*wait_set, *guard_condition); } } @@ -39,7 +39,7 @@ ExecutorNotifyWaitable::is_ready(rcl_wait_set_t * wait_set) continue; } - for (auto guard_condition: this->notify_guard_conditions_) { + for (auto guard_condition : this->notify_guard_conditions_) { if (&guard_condition->get_rcl_guard_condition() == rcl_guard_condition) { return true; }