From 2bf88de912e0bb3011cbccbc5c9f466f704f0b94 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 29 Mar 2023 15:06:17 +0000 Subject: [PATCH 01/22] Deprecate callback_group call taking context Signed-off-by: Michael Carroll --- rclcpp/include/rclcpp/callback_group.hpp | 32 +++++++++++++++++- rclcpp/src/rclcpp/callback_group.cpp | 33 ++++++++++++++++++- rclcpp/src/rclcpp/executor.cpp | 3 +- .../src/rclcpp/node_interfaces/node_base.cpp | 9 +++++ 4 files changed, 73 insertions(+), 4 deletions(-) diff --git a/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp index 7d03edf343..279c642c0e 100644 --- a/rclcpp/include/rclcpp/callback_group.hpp +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -89,6 +89,8 @@ class CallbackGroup * added to the executor in either case. * * \param[in] group_type The type of the callback group. + * \param[in] get_node_context Lambda to retrieve the node context when + * checking that the creating node is valid and using the guard condition. * \param[in] automatically_add_to_executor_with_node A boolean that * determines whether a callback group is automatically added to an executor * with the node with which it is associated. @@ -96,6 +98,7 @@ class CallbackGroup RCLCPP_PUBLIC explicit CallbackGroup( CallbackGroupType group_type, + std::function get_node_context, bool automatically_add_to_executor_with_node = true); /// Default destructor. @@ -137,6 +140,18 @@ class CallbackGroup return _find_ptrs_if_impl(func, waitable_ptrs_); } + /// Return if the node that created this callback group still exists + /** + * As nodes can share ownership of callback groups with an executor, this + * may be used to ensures that the executor doesn't operate on a callback + * group that has outlived it's creating node. + * + * \return true if the creating node still exists, otherwise false + */ + RCLCPP_PUBLIC + bool + has_valid_node(); + RCLCPP_PUBLIC std::atomic_bool & can_be_taken_from(); @@ -178,11 +193,24 @@ class CallbackGroup bool automatically_add_to_executor_with_node() const; - /// Defer creating the notify guard condition and return it. + /// Retrieve the guard condition used to signal changes to this callback group. + /** + * \param[in] context_ptr context to use when creating the guard condition + * \return guard condition if it is valid, otherwise nullptr. + */ + [[deprecated("Use get_notify_guard_condition() without arguments")]] RCLCPP_PUBLIC rclcpp::GuardCondition::SharedPtr get_notify_guard_condition(const rclcpp::Context::SharedPtr context_ptr); + /// Retrieve the guard condition used to signal changes to this callback group. + /** + * \return guard condition if it is valid, otherwise nullptr. + */ + RCLCPP_PUBLIC + rclcpp::GuardCondition::SharedPtr + get_notify_guard_condition(); + /// Trigger the notify guard condition. RCLCPP_PUBLIC void @@ -234,6 +262,8 @@ class CallbackGroup std::shared_ptr notify_guard_condition_ = nullptr; std::recursive_mutex notify_guard_condition_mutex_; + std::function get_context_; + private: template typename TypeT::SharedPtr _find_ptrs_if_impl( diff --git a/rclcpp/src/rclcpp/callback_group.cpp b/rclcpp/src/rclcpp/callback_group.cpp index 734c781a69..2a41c4c446 100644 --- a/rclcpp/src/rclcpp/callback_group.cpp +++ b/rclcpp/src/rclcpp/callback_group.cpp @@ -31,10 +31,12 @@ using rclcpp::CallbackGroupType; CallbackGroup::CallbackGroup( CallbackGroupType group_type, + std::function get_context, bool automatically_add_to_executor_with_node) : type_(group_type), associated_with_executor_(false), can_be_taken_from_(true), - automatically_add_to_executor_with_node_(automatically_add_to_executor_with_node) + automatically_add_to_executor_with_node_(automatically_add_to_executor_with_node), + get_context_(get_context) {} CallbackGroup::~CallbackGroup() @@ -42,6 +44,12 @@ CallbackGroup::~CallbackGroup() trigger_notify_guard_condition(); } +bool +CallbackGroup::has_valid_node() +{ + return nullptr != this->get_context_(); +} + std::atomic_bool & CallbackGroup::can_be_taken_from() { @@ -111,6 +119,7 @@ CallbackGroup::automatically_add_to_executor_with_node() const return automatically_add_to_executor_with_node_; } +// \TODO(mjcarroll) Deprecated, remove on tock rclcpp::GuardCondition::SharedPtr CallbackGroup::get_notify_guard_condition(const rclcpp::Context::SharedPtr context_ptr) { @@ -129,6 +138,28 @@ CallbackGroup::get_notify_guard_condition(const rclcpp::Context::SharedPtr conte return notify_guard_condition_; } +rclcpp::GuardCondition::SharedPtr +CallbackGroup::get_notify_guard_condition() +{ + auto context_ptr = this->get_context_(); + if (context_ptr && context_ptr->is_valid()) { + std::lock_guard lock(notify_guard_condition_mutex_); + if (notify_guard_condition_ && context_ptr != notify_guard_condition_->get_context()) { + if (associated_with_executor_) { + trigger_notify_guard_condition(); + } + notify_guard_condition_ = nullptr; + } + + if (!notify_guard_condition_) { + notify_guard_condition_ = std::make_shared(context_ptr); + } + + return notify_guard_condition_; + } + return nullptr; +} + void CallbackGroup::trigger_notify_guard_condition() { diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 32b895c1e3..197822e087 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -222,8 +222,7 @@ Executor::add_callback_group_to_map( weak_groups_to_nodes_.insert(std::make_pair(weak_group_ptr, node_ptr)); if (node_ptr->get_context()->is_valid()) { - auto callback_group_guard_condition = - group_ptr->get_notify_guard_condition(node_ptr->get_context()); + auto callback_group_guard_condition = group_ptr->get_notify_guard_condition(); weak_groups_to_guard_conditions_[weak_group_ptr] = callback_group_guard_condition.get(); // Add the callback_group's notify condition to the guard condition handles memory_strategy_->add_guard_condition(*callback_group_guard_condition); diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index 89d8acf6fa..e2100851d6 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -202,8 +202,17 @@ NodeBase::create_callback_group( rclcpp::CallbackGroupType group_type, bool automatically_add_to_executor_with_node) { + auto weak_ptr = this->weak_from_this(); auto group = std::make_shared( group_type, + [this]() -> rclcpp::Context::SharedPtr { + auto weak_ptr = this->weak_from_this(); + auto node_ptr = weak_ptr.lock(); + if (node_ptr) { + return node_ptr->get_context(); + } + return nullptr; + }, automatically_add_to_executor_with_node); std::lock_guard lock(callback_groups_mutex_); callback_groups_.push_back(group); From 90996351037928533e998afe82e81a63fbea0938 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 29 Mar 2023 14:39:08 +0000 Subject: [PATCH 02/22] Add base executor objects that can be used by implementors Signed-off-by: Michael Carroll --- rclcpp/CMakeLists.txt | 3 + .../executor_entities_collection.hpp | 137 +++++++ .../executors/executor_entities_collector.hpp | 151 ++++++++ .../executors/executor_notify_waitable.hpp | 117 ++++++ .../executor_entities_collection.cpp | 356 ++++++++++++++++++ .../executors/executor_entities_collector.cpp | 287 ++++++++++++++ .../executors/executor_notify_waitable.cpp | 108 ++++++ rclcpp/test/rclcpp/CMakeLists.txt | 9 + .../test_executor_notify_waitable.cpp | 95 +++++ 9 files changed, 1263 insertions(+) create mode 100644 rclcpp/include/rclcpp/executors/executor_entities_collection.hpp create mode 100644 rclcpp/include/rclcpp/executors/executor_entities_collector.hpp create mode 100644 rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp create mode 100644 rclcpp/src/rclcpp/executors/executor_entities_collection.cpp create mode 100644 rclcpp/src/rclcpp/executors/executor_entities_collector.cpp create mode 100644 rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp create mode 100644 rclcpp/test/rclcpp/executors/test_executor_notify_waitable.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 6979d980f8..324a53c518 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -54,6 +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/multi_threaded_executor.cpp src/rclcpp/executors/single_threaded_executor.cpp src/rclcpp/executors/static_executor_entities_collector.cpp diff --git a/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp b/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp new file mode 100644 index 0000000000..baf178d720 --- /dev/null +++ b/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp @@ -0,0 +1,137 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTION_HPP_ +#define RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTION_HPP_ + +#include +#include +#include + +#include "rcpputils/thread_safety_annotations.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace rclcpp +{ +namespace executors +{ + +struct ExecutorEntitiesCollection +{ + struct TimerEntry + { + rclcpp::TimerBase::WeakPtr timer; + rclcpp::CallbackGroup::WeakPtr callback_group; + }; + using TimerCollection = std::unordered_map; + + struct SubscriptionEntry + { + rclcpp::SubscriptionBase::WeakPtr subscription; + rclcpp::CallbackGroup::WeakPtr callback_group; + }; + using SubscriptionCollection = std::unordered_map; + + struct ClientEntry + { + rclcpp::ClientBase::WeakPtr client; + rclcpp::CallbackGroup::WeakPtr callback_group; + }; + using ClientCollection = std::unordered_map; + + struct ServiceEntry + { + rclcpp::ServiceBase::WeakPtr service; + rclcpp::CallbackGroup::WeakPtr callback_group; + }; + using ServiceCollection = std::unordered_map; + + struct WaitableEntry + { + rclcpp::Waitable::WeakPtr waitable; + rclcpp::CallbackGroup::WeakPtr callback_group; + }; + using WaitableCollection = std::unordered_map; + + using GuardConditionCollection = std::unordered_map; + + TimerCollection timers; + SubscriptionCollection subscriptions; + ClientCollection clients; + ServiceCollection services; + GuardConditionCollection guard_conditions; + WaitableCollection waitables; + + void clear(); + + using TimerUpdatedFunc = std::function; + void update_timers( + const ExecutorEntitiesCollection::TimerCollection & other, + TimerUpdatedFunc timer_added, + TimerUpdatedFunc timer_removed); + + using SubscriptionUpdatedFunc = std::function; + void update_subscriptions( + const ExecutorEntitiesCollection::SubscriptionCollection & other, + SubscriptionUpdatedFunc subscription_added, + SubscriptionUpdatedFunc subscription_removed); + + using ClientUpdatedFunc = std::function; + void update_clients( + const ExecutorEntitiesCollection::ClientCollection & other, + ClientUpdatedFunc client_added, + ClientUpdatedFunc client_removed); + + using ServiceUpdatedFunc = std::function; + void update_services( + const ExecutorEntitiesCollection::ServiceCollection & other, + ServiceUpdatedFunc service_added, + ServiceUpdatedFunc service_removed); + + using GuardConditionUpdatedFunc = std::function; + void update_guard_conditions( + const ExecutorEntitiesCollection::GuardConditionCollection & other, + GuardConditionUpdatedFunc guard_condition_added, + GuardConditionUpdatedFunc guard_condition_removed); + + using WaitableUpdatedFunc = std::function; + void update_waitables( + const ExecutorEntitiesCollection::WaitableCollection & other, + WaitableUpdatedFunc waitable_added, + WaitableUpdatedFunc waitable_removed); +}; + +void +build_entities_collection( + const std::vector & callback_groups, + ExecutorEntitiesCollection & collection); + +std::deque +ready_executables( + const ExecutorEntitiesCollection & collection, + rclcpp::WaitResult & wait_result +); + +} // namespace executors +} // namespace rclcpp + +#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 new file mode 100644 index 0000000000..623ec64ac5 --- /dev/null +++ b/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp @@ -0,0 +1,151 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTOR_HPP_ +#define RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTOR_HPP_ + +#include +#include +#include +#include + +#include "rcpputils/thread_safety_annotations.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace rclcpp +{ +namespace executors +{ + +class ExecutorEntitiesCollector +{ +public: + RCLCPP_PUBLIC + ExecutorEntitiesCollector(); + + RCLCPP_PUBLIC + ~ExecutorEntitiesCollector(); + + /// Add a node to the entity collector + /** + * \param[in] node_ptr a shared pointer that points to a node base interface + * \throw std::runtime_error if the node is associated with an executor + */ + RCLCPP_PUBLIC + void + add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr); + + /// Remove a node from the entity collector + /** + * \param[in] node_ptr a shared pointer that points to a node base interface + * \throw std::runtime_error if the node is associated with an executor + * \throw std::runtime_error if the node is associated with this executor + */ + RCLCPP_PUBLIC + void + remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr); + + RCLCPP_PUBLIC + bool + has_node(const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr); + + /// Add a callback group to the entity collector + /** + * \param[in] group_ptr a shared pointer that points to a callback group + * \throw std::runtime_error if the callback_group is associated with an executor + */ + RCLCPP_PUBLIC + void + add_callback_group(rclcpp::CallbackGroup::SharedPtr group_ptr); + + /// Remove a callback group from the entity collector + /** + * \param[in] group_ptr a shared pointer that points to a callback group + * \throw std::runtime_error if the callback_group is not associated with an executor + * \throw std::runtime_error if the callback_group is not associated with this executor + */ + RCLCPP_PUBLIC + void + remove_callback_group(rclcpp::CallbackGroup::SharedPtr group_ptr); + + RCLCPP_PUBLIC + std::vector + get_all_callback_groups(); + + RCLCPP_PUBLIC + std::vector + get_manually_added_callback_groups(); + + RCLCPP_PUBLIC + std::vector + get_automatically_added_callback_groups(); + + RCLCPP_PUBLIC + void + update_collections(); + + RCLCPP_PUBLIC + std::shared_ptr + get_executor_notify_waitable(); + +protected: + 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_); + + RCLCPP_PUBLIC + void + remove_callback_group_from_collection( + rclcpp::CallbackGroup::SharedPtr group_ptr, + CallbackGroupCollection & collection) RCPPUTILS_TSA_REQUIRES(mutex_); + + RCLCPP_PUBLIC + void + add_automatically_associated_callback_groups() RCPPUTILS_TSA_REQUIRES(mutex_); + + RCLCPP_PUBLIC + void + prune_invalid_nodes_and_groups() RCPPUTILS_TSA_REQUIRES(mutex_); + + mutable std::mutex mutex_; + + CallbackGroupCollection + manually_added_groups_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + + CallbackGroupCollection + automatically_added_groups_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + + /// nodes that are associated with the executor + std::list + weak_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + + std::shared_ptr notify_waitable_ RCPPUTILS_TSA_GUARDED_BY(mutex_); +}; +} // namespace executors +} // namespace rclcpp + +#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 new file mode 100644 index 0000000000..44b8513813 --- /dev/null +++ b/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp @@ -0,0 +1,117 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__EXECUTORS__EXECUTOR_NOTIFY_WAITABLE_HPP_ +#define RCLCPP__EXECUTORS__EXECUTOR_NOTIFY_WAITABLE_HPP_ + +#include +#include + +#include "rclcpp/guard_condition.hpp" +#include "rclcpp/waitable.hpp" + +namespace rclcpp +{ +namespace executors +{ + +/// Maintain a collection of guard conditions from associated nodes and callback groups +/// to signal to the executor when associated entities have changed. +class ExecutorNotifyWaitable : public rclcpp::Waitable +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(ExecutorNotifyWaitable) + + // Constructor + /** + * \param[in] on_execute_callback Callback to execute when one of the conditions + * of this waitable has signaled the wait_set. + */ + RCLCPP_PUBLIC + explicit ExecutorNotifyWaitable(std::function on_execute_callback = {}); + + // Destructor + RCLCPP_PUBLIC + ~ExecutorNotifyWaitable() override = default; + + /// Add conditions to the wait set + /** + * \param[inout] wait_set structure that conditions will be added to + */ + RCLCPP_PUBLIC + void + add_to_wait_set(rcl_wait_set_t * wait_set) override; + + /// Check conditions against the wait set + /** + * \param[inout] wait_set structure that internal elements will be checked against. + * \return true if this waitable is ready to be executed, false otherwise. + */ + RCLCPP_PUBLIC + bool + is_ready(rcl_wait_set_t * wait_set) override; + + /// Perform work associated with the waitable. + /** + * This will call the callback provided in the constructor. + * \param[in] data Data to be use for the execute, if available, else nullptr. + */ + RCLCPP_PUBLIC + void + execute(std::shared_ptr & data) override; + + /// Retrieve data to be used in the next execute call. + /** + * \return If available, data to be used, otherwise nullptr + */ + RCLCPP_PUBLIC + std::shared_ptr + take_data() override; + + /// Add a guard condition to be waited on. + /** + * \param[in] guard_condition The guard condition to add. + */ + RCLCPP_PUBLIC + void + add_guard_condition(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); + + /// Get the number of ready guard_conditions + /** + * \return The number of guard_conditions associated with the Waitable. + */ + RCLCPP_PUBLIC + size_t + get_number_of_ready_guard_conditions() override; + +private: + /// Callback to run when waitable executes + std::function execute_callback_; + + /// The collection of guard conditions to be waited on. + std::list notify_guard_conditions_; +}; + +} // namespace executors +} // namespace rclcpp + +#endif // RCLCPP__EXECUTORS__EXECUTOR_NOTIFY_WAITABLE_HPP_ diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp new file mode 100644 index 0000000000..f42ed0249b --- /dev/null +++ b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp @@ -0,0 +1,356 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/executors/executor_entities_collection.hpp" + +namespace rclcpp +{ +namespace executors +{ + +void ExecutorEntitiesCollection::clear() +{ + subscriptions.clear(); + timers.clear(); + guard_conditions.clear(); + clients.clear(); + services.clear(); + waitables.clear(); +} + +void ExecutorEntitiesCollection::update_timers( + const ExecutorEntitiesCollection::TimerCollection & other, + TimerUpdatedFunc timer_added, + TimerUpdatedFunc timer_removed) +{ + for (auto it = this->timers.begin(); it != this->timers.end(); ) { + if (other.count(it->first) == 0) { + auto timer = it->second.timer.lock(); + if (timer) { + timer_removed(timer); + } + it = this->timers.erase(it); + } else { + ++it; + } + } + for (auto it = other.begin(); it != other.end(); ++it) { + if (this->timers.count(it->first) == 0) { + auto timer = it->second.timer.lock(); + if (timer) { + timer_added(timer); + } + this->timers.insert(*it); + } + } +} + +void ExecutorEntitiesCollection::update_subscriptions( + const ExecutorEntitiesCollection::SubscriptionCollection & other, + SubscriptionUpdatedFunc subscription_added, + SubscriptionUpdatedFunc subscription_removed) +{ + for (auto it = this->subscriptions.begin(); it != this->subscriptions.end(); ) { + if (other.count(it->first) == 0) { + auto subscription = it->second.subscription.lock(); + if (subscription) { + subscription_removed(subscription); + } + it = this->subscriptions.erase(it); + } else { + ++it; + } + } + for (auto it = other.begin(); it != other.end(); ++it) { + if (this->subscriptions.count(it->first) == 0) { + auto subscription = it->second.subscription.lock(); + if (subscription) { + subscription_added(subscription); + } + this->subscriptions.insert(*it); + } + } +} + +void ExecutorEntitiesCollection::update_clients( + const ExecutorEntitiesCollection::ClientCollection & other, + ClientUpdatedFunc client_added, + ClientUpdatedFunc client_removed) +{ + for (auto it = this->clients.begin(); it != this->clients.end(); ) { + if (other.count(it->first) == 0) { + auto client = it->second.client.lock(); + if (client) { + client_removed(client); + } + it = this->clients.erase(it); + } else { + ++it; + } + } + for (auto it = other.begin(); it != other.end(); ++it) { + if (this->clients.count(it->first) == 0) { + auto client = it->second.client.lock(); + if (client) { + client_added(client); + } + this->clients.insert(*it); + } + } +} + +void ExecutorEntitiesCollection::update_services( + const ExecutorEntitiesCollection::ServiceCollection & other, + ServiceUpdatedFunc service_added, + ServiceUpdatedFunc service_removed) +{ + for (auto it = this->services.begin(); it != this->services.end(); ) { + if (other.count(it->first) == 0) { + auto service = it->second.service.lock(); + if (service) { + service_removed(service); + } + it = this->services.erase(it); + } else { + ++it; + } + } + for (auto it = other.begin(); it != other.end(); ++it) { + if (this->services.count(it->first) == 0) { + auto service = it->second.service.lock(); + if (service) { + service_added(service); + } + this->services.insert(*it); + } + } +} + +void ExecutorEntitiesCollection::update_guard_conditions( + const ExecutorEntitiesCollection::GuardConditionCollection & other, + GuardConditionUpdatedFunc guard_condition_added, + GuardConditionUpdatedFunc guard_condition_removed) +{ + for (auto it = this->guard_conditions.begin(); it != this->guard_conditions.end(); ) { + if (other.count(it->first) == 0) { + auto guard_condition = it->second.lock(); + if (guard_condition) { + guard_condition_removed(guard_condition); + } + it = this->guard_conditions.erase(it); + } else { + ++it; + } + } + for (auto it = other.begin(); it != other.end(); ++it) { + if (this->guard_conditions.count(it->first) == 0) { + auto guard_condition = it->second.lock(); + if (guard_condition) { + guard_condition_added(guard_condition); + } + this->guard_conditions.insert(*it); + } + } +} + +void ExecutorEntitiesCollection::update_waitables( + const ExecutorEntitiesCollection::WaitableCollection & other, + WaitableUpdatedFunc waitable_added, + WaitableUpdatedFunc waitable_removed) +{ + for (auto it = this->waitables.begin(); it != this->waitables.end(); ) { + if (other.count(it->first) == 0) { + auto waitable = it->second.waitable.lock(); + if (waitable) { + waitable_removed(waitable); + } + it = this->waitables.erase(it); + } else { + ++it; + } + } + for (auto it = other.begin(); it != other.end(); ++it) { + if (this->waitables.count(it->first) == 0) { + auto waitable = it->second.waitable.lock(); + if (waitable) { + waitable_added(waitable); + } + this->waitables.insert({it->first, it->second}); + } + } +} + + +void +build_entities_collection( + const std::vector & callback_groups, + ExecutorEntitiesCollection & collection) +{ + collection.clear(); + + for (auto weak_group_ptr : callback_groups) { + auto group_ptr = weak_group_ptr.lock(); + if (!group_ptr) { + 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} + }); + } + ); + } +} + +std::deque +ready_executables( + const ExecutorEntitiesCollection & collection, + rclcpp::WaitResult & wait_result +) +{ + std::deque ret; + + if (wait_result.kind() != rclcpp::WaitResultKind::Ready) { + return ret; + } + + auto rcl_wait_set = wait_result.get_wait_set().get_rcl_wait_set(); + + for (size_t ii = 0; ii < rcl_wait_set.size_of_timers; ++ii) { + if (rcl_wait_set.timers[ii]) { + auto timer_iter = collection.timers.find(rcl_wait_set.timers[ii]); + if (timer_iter != collection.timers.end()) { + auto timer = timer_iter->second.timer.lock(); + auto group = timer_iter->second.callback_group.lock(); + if (!timer || !group || !group->can_be_taken_from().load()) { + continue; + } + + if (!timer->call()) { + continue; + } + + rclcpp::AnyExecutable exec; + exec.timer = timer; + exec.callback_group = group; + ret.push_back(exec); + } + } + } + + for (size_t ii = 0; ii < rcl_wait_set.size_of_subscriptions; ++ii) { + if (rcl_wait_set.subscriptions[ii]) { + auto subscription_iter = collection.subscriptions.find(rcl_wait_set.subscriptions[ii]); + if (subscription_iter != collection.subscriptions.end()) { + auto subscription = subscription_iter->second.subscription.lock(); + auto group = subscription_iter->second.callback_group.lock(); + if (!subscription || !group || !group->can_be_taken_from().load()) { + continue; + } + + rclcpp::AnyExecutable exec; + exec.subscription = subscription; + exec.callback_group = group; + ret.push_back(exec); + } + } + } + + for (size_t ii = 0; ii < rcl_wait_set.size_of_services; ++ii) { + if (rcl_wait_set.services[ii]) { + auto service_iter = collection.services.find(rcl_wait_set.services[ii]); + if (service_iter != collection.services.end()) { + auto service = service_iter->second.service.lock(); + auto group = service_iter->second.callback_group.lock(); + if (!service || !group || !group->can_be_taken_from().load()) { + continue; + } + + rclcpp::AnyExecutable exec; + exec.service = service; + exec.callback_group = group; + ret.push_back(exec); + } + } + } + + for (size_t ii = 0; ii < rcl_wait_set.size_of_clients; ++ii) { + if (rcl_wait_set.clients[ii]) { + auto client_iter = collection.clients.find(rcl_wait_set.clients[ii]); + if (client_iter != collection.clients.end()) { + auto client = client_iter->second.client.lock(); + auto group = client_iter->second.callback_group.lock(); + if (!client || !group || !group->can_be_taken_from().load()) { + continue; + } + + rclcpp::AnyExecutable exec; + exec.client = client; + exec.callback_group = group; + ret.push_back(exec); + } + } + } + + for (auto & [handle, entry] : collection.waitables) { + auto waitable = entry.waitable.lock(); + if (waitable && waitable->is_ready(&rcl_wait_set)) { + auto group = entry.callback_group.lock(); + if (!group || !group->can_be_taken_from().load()) { + continue; + } + + rclcpp::AnyExecutable exec; + exec.waitable = waitable; + exec.callback_group = group; + ret.push_back(exec); + } + } + return ret; +} + +} // namespace executors +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp new file mode 100644 index 0000000000..6ee8b2390b --- /dev/null +++ b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp @@ -0,0 +1,287 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "rclcpp/executors/executor_entities_collector.hpp" +#include "rclcpp/executors/executor_notify_waitable.hpp" + +namespace rclcpp +{ +namespace executors +{ + +ExecutorEntitiesCollector::ExecutorEntitiesCollector() +: notify_waitable_(std::make_shared()) +{ +} + +ExecutorEntitiesCollector::~ExecutorEntitiesCollector() +{ + std::lock_guard guard{mutex_}; + + // Disassociate each node from this executor collector + std::for_each( + weak_nodes_.begin(), weak_nodes_.end(), [] + (rclcpp::node_interfaces::NodeBaseInterface::WeakPtr weak_node_ptr) { + auto shared_node_ptr = weak_node_ptr.lock(); + if (shared_node_ptr) { + std::atomic_bool & has_executor = shared_node_ptr->get_associated_with_executor_atomic(); + has_executor.store(false); + } + }); + 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) { + auto group_ptr = weak_group_ptr.lock(); + if (group_ptr) { + std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic(); + has_executor.store(false); + } + }); + 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) { + auto group_ptr = weak_group_ptr.lock(); + if (group_ptr) { + std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic(); + has_executor.store(false); + } + }); + manually_added_groups_.clear(); +} + +void +ExecutorEntitiesCollector::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) +{ + std::lock_guard guard{mutex_}; + // If the node already has an executor + std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); + if (has_executor.exchange(true)) { + throw std::runtime_error( + std::string("Node '") + node_ptr->get_fully_qualified_name() + + "' has already been added to an executor."); + } + weak_nodes_.push_back(node_ptr); + this->notify_waitable_->add_guard_condition(&node_ptr->get_notify_guard_condition()); + this->add_automatically_associated_callback_groups(); +} + +void +ExecutorEntitiesCollector::remove_node( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) +{ + std::lock_guard guard{mutex_}; + if (!node_ptr->get_associated_with_executor_atomic().load()) { + throw std::runtime_error("Node needs to be associated with an executor."); + } + + bool found_node = false; + auto node_it = weak_nodes_.begin(); + while (node_it != weak_nodes_.end()) { + bool matched = (node_it->lock() == node_ptr); + if (matched) { + found_node = true; + node_it = weak_nodes_.erase(node_it); + } else { + ++node_it; + } + } + if (!found_node) { + throw std::runtime_error("Node needs to be associated with this executor."); + } + + for (auto group_it = automatically_added_groups_.begin(); + group_it != automatically_added_groups_.end(); ) + { + auto group_ptr = group_it->lock(); + if (node_ptr->callback_group_in_node(group_ptr)) { + std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic(); + has_executor.store(false); + group_it = automatically_added_groups_.erase(group_it); + } else { + group_it++; + } + } + + this->notify_waitable_->remove_guard_condition(&node_ptr->get_notify_guard_condition()); + std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); + has_executor.store(false); +} + +void +ExecutorEntitiesCollector::add_callback_group(rclcpp::CallbackGroup::SharedPtr group_ptr) +{ + std::lock_guard guard{mutex_}; + this->add_callback_group_to_collection(group_ptr, manually_added_groups_); +} + +void +ExecutorEntitiesCollector::remove_callback_group(rclcpp::CallbackGroup::SharedPtr group_ptr) +{ + std::lock_guard guard{mutex_}; + this->remove_callback_group_from_collection(group_ptr, manually_added_groups_); +} + +std::vector +ExecutorEntitiesCollector::get_all_callback_groups() +{ + std::vector groups; + std::lock_guard guard{mutex_}; + + this->update_collections(); + + for (const auto & group_ptr : manually_added_groups_) { + groups.push_back(group_ptr); + } + for (auto const & group_ptr : automatically_added_groups_) { + groups.push_back(group_ptr); + } + return groups; +} + +std::vector +ExecutorEntitiesCollector::get_manually_added_callback_groups() +{ + std::vector groups; + std::lock_guard guard{mutex_}; + for (const auto & group_ptr : manually_added_groups_) { + groups.push_back(group_ptr); + } + return groups; +} + +std::vector +ExecutorEntitiesCollector::get_automatically_added_callback_groups() +{ + std::vector groups; + std::lock_guard guard{mutex_}; + for (auto const & group_ptr : automatically_added_groups_) { + groups.push_back(group_ptr); + } + return groups; +} + +void +ExecutorEntitiesCollector::update_collections() +{ + this->add_automatically_associated_callback_groups(); + this->prune_invalid_nodes_and_groups(); +} + +std::shared_ptr +ExecutorEntitiesCollector::get_executor_notify_waitable() +{ + return this->notify_waitable_; +} + +void +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)) { + throw std::runtime_error("Callback group has already been added to an executor."); + } + auto iter = collection.insert(group_ptr); + if (iter.second == false) { + throw std::runtime_error("Callback group has already been added to this executor."); + } + this->notify_waitable_->add_guard_condition(group_ptr->get_notify_guard_condition().get()); +} + +void +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); + if (iter != collection.end()) { + // Check that the group hasn't been orphaned from it's respective node + if (!group_ptr->has_valid_node()) { + throw std::runtime_error("Node must not be deleted before its callback group(s)."); + } + + // Disassociate the callback group with the executor + std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic(); + has_executor.store(false); + collection.erase(iter); + this->notify_waitable_->remove_guard_condition(group_ptr->get_notify_guard_condition().get()); + } else { + throw std::runtime_error("Callback group needs to be associated with executor."); + } +} + +void +ExecutorEntitiesCollector::add_automatically_associated_callback_groups() +{ + for (auto & weak_node : weak_nodes_) { + auto node = weak_node.lock(); + if (node) { + node->for_each_callback_group( + [this, node](rclcpp::CallbackGroup::SharedPtr group_ptr) + { + if (!group_ptr->get_associated_with_executor_atomic().load() && + group_ptr->automatically_add_to_executor_with_node()) + { + this->add_callback_group_to_collection(group_ptr, this->automatically_added_groups_); + } + }); + } + } +} + +void +ExecutorEntitiesCollector::prune_invalid_nodes_and_groups() +{ + for (auto node_it = weak_nodes_.begin(); + node_it != weak_nodes_.end(); ) + { + if (node_it->expired()) { + node_it = weak_nodes_.erase(node_it); + } else { + node_it++; + } + } + + for (auto group_it = automatically_added_groups_.begin(); + group_it != automatically_added_groups_.end(); ) + { + if (group_it->expired()) { + group_it = automatically_added_groups_.erase(group_it); + } else { + group_it++; + } + } + for (auto group_it = manually_added_groups_.begin(); + group_it != manually_added_groups_.end(); ) + { + if (group_it->expired()) { + group_it = manually_added_groups_.erase(group_it); + } else { + group_it++; + } + } +} + +} // namespace executors +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp new file mode 100644 index 0000000000..6663cd9731 --- /dev/null +++ b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp @@ -0,0 +1,108 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "rclcpp/executors/executor_notify_waitable.hpp" +#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp" + +namespace rclcpp +{ +namespace executors +{ + +ExecutorNotifyWaitable::ExecutorNotifyWaitable(std::function on_execute_callback) +: execute_callback_(on_execute_callback) +{ +} + +void +ExecutorNotifyWaitable::add_to_wait_set(rcl_wait_set_t * wait_set) +{ + for (auto guard_condition : this->notify_guard_conditions_) { + detail::add_guard_condition_to_rcl_wait_set(*wait_set, *guard_condition); + } +} + +bool +ExecutorNotifyWaitable::is_ready(rcl_wait_set_t * wait_set) +{ + for (size_t ii = 0; ii < wait_set->size_of_guard_conditions; ++ii) { + auto rcl_guard_condition = wait_set->guard_conditions[ii]; + + if (nullptr == rcl_guard_condition) { + continue; + } + + for (auto guard_condition : this->notify_guard_conditions_) { + if (&guard_condition->get_rcl_guard_condition() == rcl_guard_condition) { + return true; + } + } + } + + return false; +} + +void +ExecutorNotifyWaitable::execute(std::shared_ptr & data) +{ + (void) data; + this->execute_callback_(); +} + +std::shared_ptr +ExecutorNotifyWaitable::take_data() +{ + return nullptr; +} + +void +ExecutorNotifyWaitable::add_guard_condition(rclcpp::GuardCondition * guard_condition) +{ + if (guard_condition == nullptr) { + throw std::runtime_error("Attempting to add null guard condition."); + } + + for (const auto & existing_guard_condition : notify_guard_conditions_) { + if (existing_guard_condition == guard_condition) { + return; + } + } + notify_guard_conditions_.push_back(guard_condition); +} + +void +ExecutorNotifyWaitable::remove_guard_condition(rclcpp::GuardCondition * guard_condition) +{ + if (guard_condition == nullptr) { + throw std::runtime_error("Attempting to remove null guard condition."); + } + + for (auto it = notify_guard_conditions_.begin(); it != notify_guard_conditions_.end(); ++it) { + if (*it == guard_condition) { + notify_guard_conditions_.erase(it); + break; + } + } +} + +size_t +ExecutorNotifyWaitable::get_number_of_ready_guard_conditions() +{ + return notify_guard_conditions_.size(); +} + +} // namespace executors +} // namespace rclcpp diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index d4da759c02..256ddc65eb 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -688,6 +688,15 @@ if(TARGET test_static_executor_entities_collector) target_link_libraries(test_static_executor_entities_collector ${PROJECT_NAME} mimick) endif() +ament_add_gtest(test_executor_notify_waitable executors/test_executor_notify_waitable.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120) +if(TARGET test_executor_notify_waitable) + ament_target_dependencies(test_executor_notify_waitable + "rcl" + "test_msgs") + target_link_libraries(test_executor_notify_waitable ${PROJECT_NAME} mimick) +endif() + ament_add_gtest(test_guard_condition test_guard_condition.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_guard_condition) diff --git a/rclcpp/test/rclcpp/executors/test_executor_notify_waitable.cpp b/rclcpp/test/rclcpp/executors/test_executor_notify_waitable.cpp new file mode 100644 index 0000000000..8c9aaad819 --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_executor_notify_waitable.cpp @@ -0,0 +1,95 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include + +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "rcpputils/scope_exit.hpp" + +#include "rclcpp/executors/executor_notify_waitable.hpp" + +#include "../../utils/rclcpp_gtest_macros.hpp" + + +class TestExecutorNotifyWaitable : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestExecutorNotifyWaitable, construct_destruct) { + { + auto waitable = std::make_shared(); + waitable.reset(); + } + { + auto on_execute_callback = []() {}; + auto waitable = + std::make_shared(on_execute_callback); + waitable.reset(); + } +} + +TEST_F(TestExecutorNotifyWaitable, add_remove_guard_conditions) { + auto on_execute_callback = []() {}; + auto waitable = + std::make_shared(on_execute_callback); + + auto node = std::make_shared("my_node", "/ns"); + auto & notify_guard_condition = node->get_node_base_interface()->get_notify_guard_condition(); + + EXPECT_NO_THROW(waitable->add_guard_condition(¬ify_guard_condition)); + EXPECT_NO_THROW(waitable->remove_guard_condition(¬ify_guard_condition)); +} + +TEST_F(TestExecutorNotifyWaitable, wait) { + int on_execute_calls = 0; + auto on_execute_callback = [&on_execute_calls]() {on_execute_calls++;}; + + auto waitable = + std::make_shared(on_execute_callback); + + auto node = std::make_shared("my_node", "/ns"); + auto & notify_guard_condition = node->get_node_base_interface()->get_notify_guard_condition(); + EXPECT_NO_THROW(waitable->add_guard_condition(¬ify_guard_condition)); + + auto default_cbg = node->get_node_base_interface()->get_default_callback_group(); + ASSERT_NE(nullptr, default_cbg->get_notify_guard_condition()); + + auto waitables = node->get_node_waitables_interface(); + waitables->add_waitable(std::static_pointer_cast(waitable), default_cbg); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + executor.spin_all(std::chrono::seconds(1)); + EXPECT_EQ(1u, on_execute_calls); + + // on_execute_callback doesn't change if the topology doesn't change + executor.spin_all(std::chrono::seconds(1)); + EXPECT_EQ(1u, on_execute_calls); +} From 2426056b9c22d2f83a26aecedcdd3b17b0125883 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 29 Mar 2023 19:42:48 +0000 Subject: [PATCH 03/22] Template common operations Signed-off-by: Michael Carroll --- .../executor_entities_collection.hpp | 140 ++++---- .../executor_entities_collection.cpp | 311 ++++-------------- 2 files changed, 142 insertions(+), 309 deletions(-) diff --git a/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp b/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp index baf178d720..4754020217 100644 --- a/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp +++ b/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp @@ -15,12 +15,11 @@ #ifndef RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTION_HPP_ #define RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTION_HPP_ +#include "rclcpp/subscription_base.hpp" #include #include #include -#include "rcpputils/thread_safety_annotations.hpp" - #include #include #include @@ -34,45 +33,82 @@ namespace rclcpp namespace executors { -struct ExecutorEntitiesCollection +template +struct CollectionEntry { + typename EntityValueType::WeakPtr entity; + rclcpp::CallbackGroup::WeakPtr callback_group; +}; + +template +void update_entities( + const CollectionType & update_from, + CollectionType update_to, + std::function on_added, + std::function on_removed +) { - struct TimerEntry + for (auto it = update_to.begin(); it != update_to.end(); ) { + if (update_from.count(it->first) == 0) { + auto entity = it->second.entity.lock(); + if (entity) { + on_removed(entity); + } + it = update_to.erase(it); + } else { + ++it; + } + } + for (auto it = update_from.begin(); it != update_from.end(); ++it) { + if (update_to.count(it->first) == 0) { + auto entity = it->entity.lock(); + if (entity) { + on_added(entity); + } + update_to.insert(*it); + } + } +} +template +class EntityCollection: + public std::unordered_map> +{ +public: + using Key = const EntityKeyType *; + using EntityWeakPtr = typename EntityValueType::WeakPtr; + using EntitySharedPtr = typename EntityValueType::SharedPtr; + + void update(const EntityCollection & other, + std::function on_added, + std::function on_removed) { - rclcpp::TimerBase::WeakPtr timer; - rclcpp::CallbackGroup::WeakPtr callback_group; - }; - using TimerCollection = std::unordered_map; + update_entities(*this, other, on_added, on_removed); + } +}; - struct SubscriptionEntry - { - rclcpp::SubscriptionBase::WeakPtr subscription; - rclcpp::CallbackGroup::WeakPtr callback_group; - }; - using SubscriptionCollection = std::unordered_map; +/// Represent the total set of entities for a single executor +/** + * This allows the entities to be stored from ExecutorEntitiesCollector. + * The structure also makes in convenient to re-evaluate when entities have been added or removed. + */ +struct ExecutorEntitiesCollection +{ + /// Entity entries for timers + using TimerCollection = EntityCollection; - struct ClientEntry - { - rclcpp::ClientBase::WeakPtr client; - rclcpp::CallbackGroup::WeakPtr callback_group; - }; - using ClientCollection = std::unordered_map; + /// Entity entries for subscriptions + using SubscriptionCollection = EntityCollection; - struct ServiceEntry - { - rclcpp::ServiceBase::WeakPtr service; - rclcpp::CallbackGroup::WeakPtr callback_group; - }; - using ServiceCollection = std::unordered_map; + /// Entity entries for clients + using ClientCollection = EntityCollection; - struct WaitableEntry - { - rclcpp::Waitable::WeakPtr waitable; - rclcpp::CallbackGroup::WeakPtr callback_group; - }; - using WaitableCollection = std::unordered_map; + /// Entity entries for services + using ServiceCollection = EntityCollection; + + /// Entity entries for waitables + using WaitableCollection = EntityCollection; - using GuardConditionCollection = std::unordered_map; + /// Entity entries for guard conditions + using GuardConditionCollection = EntityCollection; TimerCollection timers; SubscriptionCollection subscriptions; @@ -82,42 +118,6 @@ struct ExecutorEntitiesCollection WaitableCollection waitables; void clear(); - - using TimerUpdatedFunc = std::function; - void update_timers( - const ExecutorEntitiesCollection::TimerCollection & other, - TimerUpdatedFunc timer_added, - TimerUpdatedFunc timer_removed); - - using SubscriptionUpdatedFunc = std::function; - void update_subscriptions( - const ExecutorEntitiesCollection::SubscriptionCollection & other, - SubscriptionUpdatedFunc subscription_added, - SubscriptionUpdatedFunc subscription_removed); - - using ClientUpdatedFunc = std::function; - void update_clients( - const ExecutorEntitiesCollection::ClientCollection & other, - ClientUpdatedFunc client_added, - ClientUpdatedFunc client_removed); - - using ServiceUpdatedFunc = std::function; - void update_services( - const ExecutorEntitiesCollection::ServiceCollection & other, - ServiceUpdatedFunc service_added, - ServiceUpdatedFunc service_removed); - - using GuardConditionUpdatedFunc = std::function; - void update_guard_conditions( - const ExecutorEntitiesCollection::GuardConditionCollection & other, - GuardConditionUpdatedFunc guard_condition_added, - GuardConditionUpdatedFunc guard_condition_removed); - - using WaitableUpdatedFunc = std::function; - void update_waitables( - const ExecutorEntitiesCollection::WaitableCollection & other, - WaitableUpdatedFunc waitable_added, - WaitableUpdatedFunc waitable_removed); }; void diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp index f42ed0249b..a8e4714d0c 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp @@ -29,169 +29,6 @@ void ExecutorEntitiesCollection::clear() waitables.clear(); } -void ExecutorEntitiesCollection::update_timers( - const ExecutorEntitiesCollection::TimerCollection & other, - TimerUpdatedFunc timer_added, - TimerUpdatedFunc timer_removed) -{ - for (auto it = this->timers.begin(); it != this->timers.end(); ) { - if (other.count(it->first) == 0) { - auto timer = it->second.timer.lock(); - if (timer) { - timer_removed(timer); - } - it = this->timers.erase(it); - } else { - ++it; - } - } - for (auto it = other.begin(); it != other.end(); ++it) { - if (this->timers.count(it->first) == 0) { - auto timer = it->second.timer.lock(); - if (timer) { - timer_added(timer); - } - this->timers.insert(*it); - } - } -} - -void ExecutorEntitiesCollection::update_subscriptions( - const ExecutorEntitiesCollection::SubscriptionCollection & other, - SubscriptionUpdatedFunc subscription_added, - SubscriptionUpdatedFunc subscription_removed) -{ - for (auto it = this->subscriptions.begin(); it != this->subscriptions.end(); ) { - if (other.count(it->first) == 0) { - auto subscription = it->second.subscription.lock(); - if (subscription) { - subscription_removed(subscription); - } - it = this->subscriptions.erase(it); - } else { - ++it; - } - } - for (auto it = other.begin(); it != other.end(); ++it) { - if (this->subscriptions.count(it->first) == 0) { - auto subscription = it->second.subscription.lock(); - if (subscription) { - subscription_added(subscription); - } - this->subscriptions.insert(*it); - } - } -} - -void ExecutorEntitiesCollection::update_clients( - const ExecutorEntitiesCollection::ClientCollection & other, - ClientUpdatedFunc client_added, - ClientUpdatedFunc client_removed) -{ - for (auto it = this->clients.begin(); it != this->clients.end(); ) { - if (other.count(it->first) == 0) { - auto client = it->second.client.lock(); - if (client) { - client_removed(client); - } - it = this->clients.erase(it); - } else { - ++it; - } - } - for (auto it = other.begin(); it != other.end(); ++it) { - if (this->clients.count(it->first) == 0) { - auto client = it->second.client.lock(); - if (client) { - client_added(client); - } - this->clients.insert(*it); - } - } -} - -void ExecutorEntitiesCollection::update_services( - const ExecutorEntitiesCollection::ServiceCollection & other, - ServiceUpdatedFunc service_added, - ServiceUpdatedFunc service_removed) -{ - for (auto it = this->services.begin(); it != this->services.end(); ) { - if (other.count(it->first) == 0) { - auto service = it->second.service.lock(); - if (service) { - service_removed(service); - } - it = this->services.erase(it); - } else { - ++it; - } - } - for (auto it = other.begin(); it != other.end(); ++it) { - if (this->services.count(it->first) == 0) { - auto service = it->second.service.lock(); - if (service) { - service_added(service); - } - this->services.insert(*it); - } - } -} - -void ExecutorEntitiesCollection::update_guard_conditions( - const ExecutorEntitiesCollection::GuardConditionCollection & other, - GuardConditionUpdatedFunc guard_condition_added, - GuardConditionUpdatedFunc guard_condition_removed) -{ - for (auto it = this->guard_conditions.begin(); it != this->guard_conditions.end(); ) { - if (other.count(it->first) == 0) { - auto guard_condition = it->second.lock(); - if (guard_condition) { - guard_condition_removed(guard_condition); - } - it = this->guard_conditions.erase(it); - } else { - ++it; - } - } - for (auto it = other.begin(); it != other.end(); ++it) { - if (this->guard_conditions.count(it->first) == 0) { - auto guard_condition = it->second.lock(); - if (guard_condition) { - guard_condition_added(guard_condition); - } - this->guard_conditions.insert(*it); - } - } -} - -void ExecutorEntitiesCollection::update_waitables( - const ExecutorEntitiesCollection::WaitableCollection & other, - WaitableUpdatedFunc waitable_added, - WaitableUpdatedFunc waitable_removed) -{ - for (auto it = this->waitables.begin(); it != this->waitables.end(); ) { - if (other.count(it->first) == 0) { - auto waitable = it->second.waitable.lock(); - if (waitable) { - waitable_removed(waitable); - } - it = this->waitables.erase(it); - } else { - ++it; - } - } - for (auto it = other.begin(); it != other.end(); ++it) { - if (this->waitables.count(it->first) == 0) { - auto waitable = it->second.waitable.lock(); - if (waitable) { - waitable_added(waitable); - } - this->waitables.insert({it->first, it->second}); - } - } -} - - void build_entities_collection( const std::vector & callback_groups, @@ -245,98 +82,94 @@ build_entities_collection( } } -std::deque -ready_executables( - const ExecutorEntitiesCollection & collection, - rclcpp::WaitResult & wait_result -) +template +void check_ready( + EntityCollectionType & collection, + std::deque & executables, + size_t size_of_waited_entities, + typename EntityCollectionType::Key * waited_entities, + std::function fill_executable) { - std::deque ret; - - if (wait_result.kind() != rclcpp::WaitResultKind::Ready) { - return ret; - } - - auto rcl_wait_set = wait_result.get_wait_set().get_rcl_wait_set(); - - for (size_t ii = 0; ii < rcl_wait_set.size_of_timers; ++ii) { - if (rcl_wait_set.timers[ii]) { - auto timer_iter = collection.timers.find(rcl_wait_set.timers[ii]); - if (timer_iter != collection.timers.end()) { - auto timer = timer_iter->second.timer.lock(); - auto group = timer_iter->second.callback_group.lock(); - if (!timer || !group || !group->can_be_taken_from().load()) { - continue; - } - - if (!timer->call()) { + 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()) { continue; } rclcpp::AnyExecutable exec; - exec.timer = timer; - exec.callback_group = group; - ret.push_back(exec); - } - } - } - - for (size_t ii = 0; ii < rcl_wait_set.size_of_subscriptions; ++ii) { - if (rcl_wait_set.subscriptions[ii]) { - auto subscription_iter = collection.subscriptions.find(rcl_wait_set.subscriptions[ii]); - if (subscription_iter != collection.subscriptions.end()) { - auto subscription = subscription_iter->second.subscription.lock(); - auto group = subscription_iter->second.callback_group.lock(); - if (!subscription || !group || !group->can_be_taken_from().load()) { - continue; + exec.callback_group = callback_group; + if (fill_executable(exec, entity)) { + executables.push_back(exec); } - - rclcpp::AnyExecutable exec; - exec.subscription = subscription; - exec.callback_group = group; - ret.push_back(exec); } } } +} - for (size_t ii = 0; ii < rcl_wait_set.size_of_services; ++ii) { - if (rcl_wait_set.services[ii]) { - auto service_iter = collection.services.find(rcl_wait_set.services[ii]); - if (service_iter != collection.services.end()) { - auto service = service_iter->second.service.lock(); - auto group = service_iter->second.callback_group.lock(); - if (!service || !group || !group->can_be_taken_from().load()) { - continue; - } +std::deque +ready_executables( + const ExecutorEntitiesCollection & collection, + rclcpp::WaitResult & wait_result +) +{ + std::deque ret; - rclcpp::AnyExecutable exec; - exec.service = service; - exec.callback_group = group; - ret.push_back(exec); - } - } + if (wait_result.kind() != rclcpp::WaitResultKind::Ready) { + return ret; } - for (size_t ii = 0; ii < rcl_wait_set.size_of_clients; ++ii) { - if (rcl_wait_set.clients[ii]) { - auto client_iter = collection.clients.find(rcl_wait_set.clients[ii]); - if (client_iter != collection.clients.end()) { - auto client = client_iter->second.client.lock(); - auto group = client_iter->second.callback_group.lock(); - if (!client || !group || !group->can_be_taken_from().load()) { - continue; - } + auto rcl_wait_set = wait_result.get_wait_set().get_rcl_wait_set(); - rclcpp::AnyExecutable exec; - exec.client = client; - exec.callback_group = group; - ret.push_back(exec); - } - } - } + check_ready( + collection.timers, + ret, + rcl_wait_set.size_of_timers, + rcl_wait_set.timers, + [](auto exec, auto timer){ + if (!timer->call()) { + return false; + } + exec.timer = timer; + return true; + }); + + check_ready( + collection.subscriptions, + ret, + rcl_wait_set.size_of_subscriptions, + rcl_wait_set.subscriptions, + [](auto 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){ + exec.service = service; + return true; + }); + + check_ready( + collection.clients, + ret, + rcl_wait_set.size_of_clients, + rcl_wait_set.clients, + [](auto exec, auto client){ + exec.client = client; + return true; + }); for (auto & [handle, entry] : collection.waitables) { - auto waitable = entry.waitable.lock(); + 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()) { From 173ffd686f0a7353533c5c379bf16d6dcebf0f05 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 29 Mar 2023 20:04:35 +0000 Subject: [PATCH 04/22] Address reviewer feedback: * Add callback to EntitiesCollector constructor * Make function to check automatically added callback groups take a list Signed-off-by: Michael Carroll --- .../executors/executor_entities_collector.hpp | 59 +++++++++++++++++-- .../executors/executor_entities_collector.cpp | 26 ++++---- 2 files changed, 66 insertions(+), 19 deletions(-) diff --git a/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp b/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp index 623ec64ac5..a08b522862 100644 --- a/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp +++ b/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp @@ -35,12 +35,37 @@ namespace rclcpp namespace executors { +/// Class to monitor a set of nodes and callback groups for changes in entity membership +/** + * This is to be used with an executor to track the membership of various nodes, groups, + * and entities (timers, subscriptions, clients, services, etc) and report status to the + * executor. + * + * In general, users will add either nodes or callback groups to an executor. + * Each node may have callback groups that are automatically associated with executors, + * or callback groups that must be manually associated with an executor. + * + * This object tracks both types of callback groups as well as nodes that have been + * previously added to the executor. + * When a new callback group is added/removed or new entities are added/removed, the + * corresponding node or callback group will signal this to the executor so that the + * entity collection may be rebuilt according to that executor's implementation. + * + */ class ExecutorEntitiesCollector { public: + /// Constructor + /** + * \param[in] on_notify_waitable Callback to execute when one of the associated + * nodes or callback groups trigger their guard condition, indicating that their + * corresponding entities have changed. + */ RCLCPP_PUBLIC - ExecutorEntitiesCollector(); + explicit ExecutorEntitiesCollector( + std::function on_notify_waitable_callback = {}); + /// Destructor RCLCPP_PUBLIC ~ExecutorEntitiesCollector(); @@ -63,6 +88,11 @@ class ExecutorEntitiesCollector void remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr); + /// Check if a node is associated with this executor/collector. + /** + * \param[in] node_ptr a shared pointer that points to a node base interface + * \return true if the node is tracked in this collector, false otherwise + */ RCLCPP_PUBLIC bool has_node(const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr); @@ -86,26 +116,42 @@ class ExecutorEntitiesCollector void remove_callback_group(rclcpp::CallbackGroup::SharedPtr group_ptr); + /// Get all callback groups known to this entity collector + /** + * This will include manually added and automatically added (node associated) groups + * \return vector of all callback groups + */ RCLCPP_PUBLIC std::vector get_all_callback_groups(); + /// Get manually-added callback groups known to this entity collector + /** + * This will include callback groups that have been added via add_callback_group + * \return vector of manually-added callback groups + */ RCLCPP_PUBLIC std::vector get_manually_added_callback_groups(); + /// Get automatically-added callback groups known to this entity collector + /** + * This will include callback groups that are associated with nodes added via add_node + * \return vector of automatically-added callback groups + */ RCLCPP_PUBLIC std::vector get_automatically_added_callback_groups(); + /// Update the underlying collections + /** + * This will prune nodes and callback groups that are no longer valid as well + * as adding new callback groups from any associated nodes. + */ RCLCPP_PUBLIC void update_collections(); - RCLCPP_PUBLIC - std::shared_ptr - get_executor_notify_waitable(); - protected: using CallbackGroupCollection = std::set< rclcpp::CallbackGroup::WeakPtr, @@ -125,7 +171,8 @@ class ExecutorEntitiesCollector RCLCPP_PUBLIC void - add_automatically_associated_callback_groups() RCPPUTILS_TSA_REQUIRES(mutex_); + add_automatically_associated_callback_groups( + std::list nodes_to_check) RCPPUTILS_TSA_REQUIRES(mutex_); RCLCPP_PUBLIC void diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp index 6ee8b2390b..7647d6946e 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp @@ -16,15 +16,18 @@ #include "rclcpp/executors/executor_entities_collector.hpp" #include "rclcpp/executors/executor_notify_waitable.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" namespace rclcpp { namespace executors { -ExecutorEntitiesCollector::ExecutorEntitiesCollector() -: notify_waitable_(std::make_shared()) -{ +ExecutorEntitiesCollector::ExecutorEntitiesCollector( + std::function on_notify_waitable_callback) { + notify_waitable_ = std::make_shared( + [on_notify_waitable_callback](){on_notify_waitable_callback();}); + } ExecutorEntitiesCollector::~ExecutorEntitiesCollector() @@ -81,7 +84,8 @@ ExecutorEntitiesCollector::add_node(rclcpp::node_interfaces::NodeBaseInterface:: } weak_nodes_.push_back(node_ptr); this->notify_waitable_->add_guard_condition(&node_ptr->get_notify_guard_condition()); - this->add_automatically_associated_callback_groups(); + + this->add_automatically_associated_callback_groups({node_ptr}); } void @@ -182,16 +186,10 @@ ExecutorEntitiesCollector::get_automatically_added_callback_groups() void ExecutorEntitiesCollector::update_collections() { - this->add_automatically_associated_callback_groups(); + this->add_automatically_associated_callback_groups(this->weak_nodes_); this->prune_invalid_nodes_and_groups(); } -std::shared_ptr -ExecutorEntitiesCollector::get_executor_notify_waitable() -{ - return this->notify_waitable_; -} - void ExecutorEntitiesCollector::add_callback_group_to_collection( rclcpp::CallbackGroup::SharedPtr group_ptr, @@ -232,9 +230,11 @@ ExecutorEntitiesCollector::remove_callback_group_from_collection( } void -ExecutorEntitiesCollector::add_automatically_associated_callback_groups() +ExecutorEntitiesCollector::add_automatically_associated_callback_groups( + std::list nodes_to_check +) { - for (auto & weak_node : weak_nodes_) { + for (auto & weak_node : nodes_to_check) { auto node = weak_node.lock(); if (node) { node->for_each_callback_group( From a524bf016ad3c8f7a557d5de5eec44108fe2f3f9 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 29 Mar 2023 20:13:01 +0000 Subject: [PATCH 05/22] Lint Signed-off-by: Michael Carroll --- .../executor_entities_collection.hpp | 25 ++++++++++--------- .../executors/executor_entities_collector.hpp | 5 ++-- .../executor_entities_collection.cpp | 13 +++++----- .../executors/executor_entities_collector.cpp | 7 +++--- 4 files changed, 26 insertions(+), 24 deletions(-) diff --git a/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp b/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp index 4754020217..894550f958 100644 --- a/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp +++ b/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp @@ -15,7 +15,6 @@ #ifndef RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTION_HPP_ #define RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTION_HPP_ -#include "rclcpp/subscription_base.hpp" #include #include #include @@ -33,18 +32,19 @@ namespace rclcpp namespace executors { -template -struct CollectionEntry { +template +struct CollectionEntry +{ typename EntityValueType::WeakPtr entity; rclcpp::CallbackGroup::WeakPtr callback_group; }; -template +template void update_entities( const CollectionType & update_from, CollectionType update_to, - std::function on_added, - std::function on_removed + std::function on_added, + std::function on_removed ) { for (auto it = update_to.begin(); it != update_to.end(); ) { @@ -68,18 +68,19 @@ void update_entities( } } } -template -class EntityCollection: - public std::unordered_map> +template +class EntityCollection + : public std::unordered_map> { public: using Key = const EntityKeyType *; using EntityWeakPtr = typename EntityValueType::WeakPtr; using EntitySharedPtr = typename EntityValueType::SharedPtr; - void update(const EntityCollection & other, - std::function on_added, - std::function on_removed) + void update( + const EntityCollection & other, + std::function on_added, + std::function on_removed) { update_entities(*this, other, on_added, on_removed); } diff --git a/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp b/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp index a08b522862..5edb89e9b4 100644 --- a/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp +++ b/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp @@ -63,7 +63,7 @@ class ExecutorEntitiesCollector */ RCLCPP_PUBLIC explicit ExecutorEntitiesCollector( - std::function on_notify_waitable_callback = {}); + std::function on_notify_waitable_callback = {}); /// Destructor RCLCPP_PUBLIC @@ -172,7 +172,8 @@ class ExecutorEntitiesCollector RCLCPP_PUBLIC void add_automatically_associated_callback_groups( - std::list nodes_to_check) RCPPUTILS_TSA_REQUIRES(mutex_); + std::list nodes_to_check) + RCPPUTILS_TSA_REQUIRES(mutex_); RCLCPP_PUBLIC void diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp index a8e4714d0c..6a46280746 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp @@ -82,13 +82,14 @@ build_entities_collection( } } -template +template void check_ready( EntityCollectionType & collection, std::deque & executables, size_t size_of_waited_entities, typename EntityCollectionType::Key * waited_entities, - std::function fill_executable) + std::function fill_executable) { for (size_t ii = 0; ii < size_of_waited_entities; ++ii) { if (waited_entities[ii]) { @@ -130,7 +131,7 @@ ready_executables( ret, rcl_wait_set.size_of_timers, rcl_wait_set.timers, - [](auto exec, auto timer){ + [](auto exec, auto timer) { if (!timer->call()) { return false; } @@ -143,7 +144,7 @@ ready_executables( ret, rcl_wait_set.size_of_subscriptions, rcl_wait_set.subscriptions, - [](auto exec, auto subscription){ + [](auto exec, auto subscription) { exec.subscription = subscription; return true; }); @@ -153,7 +154,7 @@ ready_executables( ret, rcl_wait_set.size_of_services, rcl_wait_set.services, - [](auto exec, auto service){ + [](auto exec, auto service) { exec.service = service; return true; }); @@ -163,7 +164,7 @@ ready_executables( ret, rcl_wait_set.size_of_clients, rcl_wait_set.clients, - [](auto exec, auto client){ + [](auto exec, auto client) { exec.client = client; return true; }); diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp index 7647d6946e..c45d0dd191 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp @@ -24,10 +24,9 @@ namespace executors { ExecutorEntitiesCollector::ExecutorEntitiesCollector( - std::function on_notify_waitable_callback) { - notify_waitable_ = std::make_shared( - [on_notify_waitable_callback](){on_notify_waitable_callback();}); - + std::function on_notify_waitable_callback) +: notify_waitable_(std::make_shared(on_notify_waitable_callback)) +{ } ExecutorEntitiesCollector::~ExecutorEntitiesCollector() From 89f210687d5302e7609d90ae9903424ed09e9fae Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Thu, 30 Mar 2023 19:20:07 +0000 Subject: [PATCH 06/22] Address reviewer feedback and fix templates Signed-off-by: Michael Carroll --- rclcpp/CMakeLists.txt | 4 +- .../executor_entities_collection.hpp | 16 +- .../executors/executor_entities_collector.hpp | 39 +++- .../executors/executor_notify_waitable.hpp | 13 +- rclcpp/src/rclcpp/callback_group.cpp | 3 + .../executor_entities_collection.cpp | 114 ++++++----- .../executors/executor_entities_collector.cpp | 183 +++++++++-------- .../executors/executor_notify_waitable.cpp | 62 ++++-- .../src/rclcpp/node_interfaces/node_base.cpp | 17 +- rclcpp/test/rclcpp/CMakeLists.txt | 9 + .../executors/test_entities_collector.cpp | 185 ++++++++++++++++++ 11 files changed, 470 insertions(+), 175 deletions(-) create mode 100644 rclcpp/test/rclcpp/executors/test_entities_collector.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 324a53c518..3fba0d7c08 100644 --- a/rclcpp/CMakeLists.txt +++ b/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 diff --git a/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp b/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp index 894550f958..a5be2d6b7e 100644 --- a/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp +++ b/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp @@ -35,16 +35,19 @@ namespace executors template struct CollectionEntry { - typename EntityValueType::WeakPtr entity; + using EntityWeakPtr = typename EntityValueType::WeakPtr; + using EntitySharedPtr = typename EntityValueType::SharedPtr; + + EntityWeakPtr entity; rclcpp::CallbackGroup::WeakPtr callback_group; }; template void update_entities( const CollectionType & update_from, - CollectionType update_to, - std::function on_added, - std::function on_removed + CollectionType & update_to, + std::function on_added, + std::function 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 on_added, std::function 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(); }; diff --git a/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp b/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp index 5edb89e9b4..6ca01d0c13 100644 --- a/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp +++ b/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp @@ -143,6 +143,9 @@ class ExecutorEntitiesCollector std::vector get_automatically_added_callback_groups(); + RCLCPP_PUBLIC + std::shared_ptr 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>; + using CallbackGroupCollection = std::set< rclcpp::CallbackGroup::WeakPtr, std::owner_less>; + using WeakNodesToGuardConditionsMap = std::map< + rclcpp::node_interfaces::NodeBaseInterface::WeakPtr, + const rclcpp::GuardCondition *, + std::owner_less>; + + using WeakGroupsToGuardConditionsMap = std::map< + rclcpp::CallbackGroup::WeakPtr, + const rclcpp::GuardCondition *, + std::owner_less>; + + 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 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 + 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 notify_waitable_ RCPPUTILS_TSA_GUARDED_BY(mutex_); }; } // namespace executors } // namespace rclcpp - +// #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 44b8513813..5fb2532dec 100644 --- a/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp +++ b/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp @@ -85,7 +85,7 @@ 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. /** @@ -93,7 +93,7 @@ class ExecutorNotifyWaitable : public rclcpp::Waitable */ 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 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 notify_guard_conditions_; + + /// The collection of guard conditions to be waited on. + std::list to_add_; + + /// The collection of guard conditions to be waited on. + std::list to_remove_; }; } // namespace executors diff --git a/rclcpp/src/rclcpp/callback_group.cpp b/rclcpp/src/rclcpp/callback_group.cpp index 2a41c4c446..8e3b62da3b 100644 --- a/rclcpp/src/rclcpp/callback_group.cpp +++ b/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 lock(notify_guard_condition_mutex_); auto context_ptr = this->get_context_(); if (context_ptr && context_ptr->is_valid()) { std::lock_guard 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; } diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp index 6a46280746..7418b32a16 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp +++ b/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 & 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 & executables, size_t size_of_waited_entities, typename EntityCollectionType::Key * waited_entities, - std::function fill_executable) + std::function 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; } diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp index c45d0dd191..0447e776e3 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp @@ -33,47 +33,24 @@ ExecutorEntitiesCollector::~ExecutorEntitiesCollector() { std::lock_guard guard{mutex_}; - // Disassociate each node from this executor collector - std::for_each( - weak_nodes_.begin(), weak_nodes_.end(), [] - (rclcpp::node_interfaces::NodeBaseInterface::WeakPtr weak_node_ptr) { - auto shared_node_ptr = weak_node_ptr.lock(); - if (shared_node_ptr) { - std::atomic_bool & has_executor = shared_node_ptr->get_associated_with_executor_atomic(); - has_executor.store(false); - } - }); - 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) { - auto group_ptr = weak_group_ptr.lock(); - if (group_ptr) { - std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic(); - has_executor.store(false); - } - }); - 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) { - auto group_ptr = weak_group_ptr.lock(); - if (group_ptr) { - std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic(); - has_executor.store(false); - } - }); - manually_added_groups_.clear(); + for (auto weak_node_it = weak_nodes_.begin(); weak_node_it != weak_nodes_.end();) { + weak_node_it = remove_weak_node(weak_node_it); + } + + for (auto weak_group_it = automatically_added_groups_.begin(); weak_group_it != automatically_added_groups_.end(); ) { + weak_group_it= remove_weak_callback_group(weak_group_it, automatically_added_groups_); + } + + for (auto weak_group_it = manually_added_groups_.begin(); weak_group_it != manually_added_groups_.end(); ) { + weak_group_it= remove_weak_callback_group(weak_group_it, manually_added_groups_); + } } void ExecutorEntitiesCollector::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) { std::lock_guard guard{mutex_}; + // If the node already has an executor std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); if (has_executor.exchange(true)) { @@ -81,10 +58,14 @@ ExecutorEntitiesCollector::add_node(rclcpp::node_interfaces::NodeBaseInterface:: std::string("Node '") + node_ptr->get_fully_qualified_name() + "' has already been added to an executor."); } - weak_nodes_.push_back(node_ptr); - this->notify_waitable_->add_guard_condition(&node_ptr->get_notify_guard_condition()); - + weak_nodes_.insert(node_ptr); this->add_automatically_associated_callback_groups({node_ptr}); + + // Store node guard condition in map and add it to the notify waitable + rclcpp::GuardCondition * node_guard_condition = &node_ptr->get_notify_guard_condition(); + weak_nodes_to_guard_conditions_.insert({node_ptr, node_guard_condition}); + + this->notify_waitable_->add_guard_condition(node_guard_condition); } void @@ -92,41 +73,29 @@ ExecutorEntitiesCollector::remove_node( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) { std::lock_guard guard{mutex_}; + if (!node_ptr->get_associated_with_executor_atomic().load()) { throw std::runtime_error("Node needs to be associated with an executor."); } - bool found_node = false; - auto node_it = weak_nodes_.begin(); - while (node_it != weak_nodes_.end()) { - bool matched = (node_it->lock() == node_ptr); - if (matched) { - found_node = true; - node_it = weak_nodes_.erase(node_it); - } else { - ++node_it; - } - } - if (!found_node) { + auto node_it = weak_nodes_.find(node_ptr); + if (node_it != weak_nodes_.end()) { + remove_weak_node(node_it); + } else { throw std::runtime_error("Node needs to be associated with this executor."); } for (auto group_it = automatically_added_groups_.begin(); - group_it != automatically_added_groups_.end(); ) + group_it != automatically_added_groups_.end();) { auto group_ptr = group_it->lock(); if (node_ptr->callback_group_in_node(group_ptr)) { - std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic(); - has_executor.store(false); - group_it = automatically_added_groups_.erase(group_it); - } else { - group_it++; + group_it = remove_weak_callback_group(group_it, automatically_added_groups_); + } + else { + ++group_it; } } - - this->notify_waitable_->remove_guard_condition(&node_ptr->get_notify_guard_condition()); - std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); - has_executor.store(false); } void @@ -149,8 +118,6 @@ ExecutorEntitiesCollector::get_all_callback_groups() std::vector groups; std::lock_guard guard{mutex_}; - this->update_collections(); - for (const auto & group_ptr : manually_added_groups_) { groups.push_back(group_ptr); } @@ -182,13 +149,71 @@ ExecutorEntitiesCollector::get_automatically_added_callback_groups() return groups; } +std::shared_ptr +ExecutorEntitiesCollector::get_notify_waitable() +{ + return this->notify_waitable_; +} + void ExecutorEntitiesCollector::update_collections() { + std::lock_guard guard{mutex_}; this->add_automatically_associated_callback_groups(this->weak_nodes_); this->prune_invalid_nodes_and_groups(); } +ExecutorEntitiesCollector::NodeCollection::iterator +ExecutorEntitiesCollector::remove_weak_node(NodeCollection::iterator weak_node) +{ + // Disassociate the guard condition from the executor notify waitable + auto guard_condition_it = weak_nodes_to_guard_conditions_.find(*weak_node); + if (guard_condition_it != weak_nodes_to_guard_conditions_.end()) + { + this->notify_waitable_->remove_guard_condition(guard_condition_it->second); + weak_nodes_to_guard_conditions_.erase(guard_condition_it); + } + + // Mark the node as disassociated (if the node is still valid) + auto node_ptr = weak_node->lock(); + if (node_ptr) { + std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); + has_executor.store(false); + } + + // Remove the node from tracked nodes + return weak_nodes_.erase(weak_node); +} + +ExecutorEntitiesCollector::CallbackGroupCollection::iterator +ExecutorEntitiesCollector::remove_weak_callback_group( + CallbackGroupCollection::iterator weak_group_it, + CallbackGroupCollection & collection +) +{ + // Disassociate the guard condition from the executor notify waitable + auto guard_condition_it = weak_groups_to_guard_conditions_.find(*weak_group_it); + if (guard_condition_it != weak_groups_to_guard_conditions_.end()) + { + this->notify_waitable_->remove_guard_condition(guard_condition_it->second); + weak_groups_to_guard_conditions_.erase(guard_condition_it); + } + + // Mark the node as disassociated (if the group is still valid) + auto group_ptr = weak_group_it->lock(); + + if (group_ptr) { + if (!group_ptr->has_valid_node()) { + throw std::runtime_error("Node must not be deleted before its callback group(s)."); + } + std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic(); + has_executor.store(false); + } + + // Remove the node from tracked nodes + return collection.erase(weak_group_it); +} + void ExecutorEntitiesCollector::add_callback_group_to_collection( rclcpp::CallbackGroup::SharedPtr group_ptr, @@ -202,7 +227,11 @@ ExecutorEntitiesCollector::add_callback_group_to_collection( if (iter.second == false) { throw std::runtime_error("Callback group has already been added to this executor."); } - this->notify_waitable_->add_guard_condition(group_ptr->get_notify_guard_condition().get()); + + // Store node guard condition in map and add it to the notify waitable + rclcpp::GuardCondition * group_guard_condition = group_ptr->get_notify_guard_condition().get(); + weak_groups_to_guard_conditions_.insert({group_ptr, group_guard_condition}); + this->notify_waitable_->add_guard_condition(group_guard_condition); } void @@ -210,28 +239,13 @@ 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); - if (iter != collection.end()) { - // Check that the group hasn't been orphaned from it's respective node - if (!group_ptr->has_valid_node()) { - throw std::runtime_error("Node must not be deleted before its callback group(s)."); - } - - // Disassociate the callback group with the executor - std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic(); - has_executor.store(false); - collection.erase(iter); - this->notify_waitable_->remove_guard_condition(group_ptr->get_notify_guard_condition().get()); - } else { - throw std::runtime_error("Callback group needs to be associated with executor."); - } + auto group_it = collection.find(group_ptr); + remove_weak_callback_group(group_it, collection); } void ExecutorEntitiesCollector::add_automatically_associated_callback_groups( - std::list nodes_to_check -) + const NodeCollection & nodes_to_check) { for (auto & weak_node : nodes_to_check) { auto node = weak_node.lock(); @@ -256,17 +270,16 @@ ExecutorEntitiesCollector::prune_invalid_nodes_and_groups() node_it != weak_nodes_.end(); ) { if (node_it->expired()) { - node_it = weak_nodes_.erase(node_it); + node_it = remove_weak_node(node_it); } else { node_it++; } } - for (auto group_it = automatically_added_groups_.begin(); group_it != automatically_added_groups_.end(); ) { if (group_it->expired()) { - group_it = automatically_added_groups_.erase(group_it); + group_it = remove_weak_callback_group(group_it, automatically_added_groups_); } else { group_it++; } @@ -275,7 +288,7 @@ ExecutorEntitiesCollector::prune_invalid_nodes_and_groups() group_it != manually_added_groups_.end(); ) { if (group_it->expired()) { - group_it = manually_added_groups_.erase(group_it); + group_it = remove_weak_callback_group(group_it, manually_added_groups_); } else { group_it++; } diff --git a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp index 6663cd9731..89788bc2a5 100644 --- a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp +++ b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp @@ -14,8 +14,8 @@ #include +#include "rclcpp/exceptions.hpp" #include "rclcpp/executors/executor_notify_waitable.hpp" -#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp" namespace rclcpp { @@ -30,14 +30,25 @@ ExecutorNotifyWaitable::ExecutorNotifyWaitable(std::function on_exec void ExecutorNotifyWaitable::add_to_wait_set(rcl_wait_set_t * wait_set) { + std::lock_guard lock(guard_condition_mutex_); for (auto guard_condition : this->notify_guard_conditions_) { - detail::add_guard_condition_to_rcl_wait_set(*wait_set, *guard_condition); + auto rcl_guard_condition = guard_condition->get_rcl_guard_condition(); + + rcl_ret_t ret = rcl_wait_set_add_guard_condition( + wait_set, + &rcl_guard_condition, NULL); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "failed to add guard condition to wait set"); + } } } bool ExecutorNotifyWaitable::is_ready(rcl_wait_set_t * wait_set) { + std::lock_guard lock(guard_condition_mutex_); for (size_t ii = 0; ii < wait_set->size_of_guard_conditions; ++ii) { auto rcl_guard_condition = wait_set->guard_conditions[ii]; @@ -51,7 +62,6 @@ ExecutorNotifyWaitable::is_ready(rcl_wait_set_t * wait_set) } } } - return false; } @@ -69,38 +79,52 @@ ExecutorNotifyWaitable::take_data() } void -ExecutorNotifyWaitable::add_guard_condition(rclcpp::GuardCondition * guard_condition) +ExecutorNotifyWaitable::add_guard_condition(const rclcpp::GuardCondition * guard_condition) { if (guard_condition == nullptr) { throw std::runtime_error("Attempting to add null guard condition."); } - - for (const auto & existing_guard_condition : notify_guard_conditions_) { - if (existing_guard_condition == guard_condition) { - return; - } - } - notify_guard_conditions_.push_back(guard_condition); + std::lock_guard lock(guard_condition_mutex_); + to_add_.push_back(guard_condition); } void -ExecutorNotifyWaitable::remove_guard_condition(rclcpp::GuardCondition * guard_condition) +ExecutorNotifyWaitable::remove_guard_condition(const rclcpp::GuardCondition * guard_condition) { if (guard_condition == nullptr) { throw std::runtime_error("Attempting to remove null guard condition."); } - - for (auto it = notify_guard_conditions_.begin(); it != notify_guard_conditions_.end(); ++it) { - if (*it == guard_condition) { - notify_guard_conditions_.erase(it); - break; - } - } + std::lock_guard lock(guard_condition_mutex_); + to_remove_.push_back(guard_condition); } size_t ExecutorNotifyWaitable::get_number_of_ready_guard_conditions() { + std::lock_guard lock(guard_condition_mutex_); + + for (auto add_guard_condition: to_add_) + { + auto guard_it = std::find(notify_guard_conditions_.begin(), + notify_guard_conditions_.end(), + add_guard_condition); + if (guard_it == notify_guard_conditions_.end()) { + notify_guard_conditions_.push_back(add_guard_condition); + } + } + to_add_.clear(); + + for (auto remove_guard_condition: to_remove_) + { + auto guard_it = std::find(notify_guard_conditions_.begin(), + notify_guard_conditions_.end(), + remove_guard_condition); + if (guard_it != notify_guard_conditions_.end()){ + notify_guard_conditions_.erase(guard_it); + } + } + to_remove_.clear(); + return notify_guard_conditions_.size(); } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index e2100851d6..7b4a70d411 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -129,13 +129,6 @@ NodeBase::NodeBase( delete node; }); - // Create the default callback group, if needed. - if (nullptr == default_callback_group_) { - using rclcpp::CallbackGroupType; - default_callback_group_ = - NodeBase::create_callback_group(CallbackGroupType::MutuallyExclusive); - } - // Indicate the notify_guard_condition is now valid. notify_guard_condition_is_valid_ = true; } @@ -205,8 +198,7 @@ NodeBase::create_callback_group( auto weak_ptr = this->weak_from_this(); auto group = std::make_shared( group_type, - [this]() -> rclcpp::Context::SharedPtr { - auto weak_ptr = this->weak_from_this(); + [weak_ptr]() -> rclcpp::Context::SharedPtr { auto node_ptr = weak_ptr.lock(); if (node_ptr) { return node_ptr->get_context(); @@ -222,6 +214,13 @@ NodeBase::create_callback_group( rclcpp::CallbackGroup::SharedPtr NodeBase::get_default_callback_group() { + // Create the default callback group, if needed. + if (nullptr == default_callback_group_) { + using rclcpp::CallbackGroupType; + default_callback_group_ = + NodeBase::create_callback_group(CallbackGroupType::MutuallyExclusive); + } + return default_callback_group_; } diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 256ddc65eb..ddb219b558 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -688,6 +688,15 @@ if(TARGET test_static_executor_entities_collector) target_link_libraries(test_static_executor_entities_collector ${PROJECT_NAME} mimick) endif() +ament_add_gtest(test_entities_collector executors/test_entities_collector.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120) +if(TARGET test_entities_collector) + ament_target_dependencies(test_entities_collector + "rcl" + "test_msgs") + target_link_libraries(test_entities_collector ${PROJECT_NAME} mimick) +endif() + ament_add_gtest(test_executor_notify_waitable executors/test_executor_notify_waitable.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120) if(TARGET test_executor_notify_waitable) diff --git a/rclcpp/test/rclcpp/executors/test_entities_collector.cpp b/rclcpp/test/rclcpp/executors/test_entities_collector.cpp new file mode 100644 index 0000000000..defa647999 --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_entities_collector.cpp @@ -0,0 +1,185 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/executors/executor_entities_collector.hpp" + +#include "../../utils/rclcpp_gtest_macros.hpp" + +class TestExecutorEntitiesCollector : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestExecutorEntitiesCollector, add_remove_node) { + auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(); + + auto node1 = std::make_shared("node1", "ns"); + auto node2 = std::make_shared("node2", "ns"); + + // Add a node + EXPECT_NO_THROW(entities_collector.add_node(node1->get_node_base_interface())); + + // Add the same node a second time + RCLCPP_EXPECT_THROW_EQ( + entities_collector.add_node(node1->get_node_base_interface()), + std::runtime_error("Node '/ns/node1' has already been added to an executor.")); + + // Remove a node before adding + RCLCPP_EXPECT_THROW_EQ( + entities_collector.remove_node(node2->get_node_base_interface()), + std::runtime_error("Node needs to be associated with an executor.")); + + // Simulate node being associated somewhere else + auto & has_executor = node2->get_node_base_interface()->get_associated_with_executor_atomic(); + has_executor.store(true); + + // Add an already-associated node + RCLCPP_EXPECT_THROW_EQ( + entities_collector.remove_node(node2->get_node_base_interface()), + std::runtime_error("Node needs to be associated with this executor.")); + + has_executor.store(false); + + // Add the now-disassociated node + EXPECT_NO_THROW(entities_collector.add_node(node2->get_node_base_interface())); + + // Remove an existing node + EXPECT_NO_THROW(entities_collector.remove_node(node1->get_node_base_interface())); + // Remove the same node a second time + RCLCPP_EXPECT_THROW_EQ( + entities_collector.remove_node(node1->get_node_base_interface()), + std::runtime_error("Node needs to be associated with an executor.")); + + // Remove an existing node + EXPECT_NO_THROW(entities_collector.remove_node(node2->get_node_base_interface())); +} + +TEST_F(TestExecutorEntitiesCollector, add_callback_group) { + auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(); + + auto node = std::make_shared("node1", "ns"); + rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + entities_collector.add_callback_group(cb_group); + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 1u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 1u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); +} + +TEST_F(TestExecutorEntitiesCollector, add_node_default_callback_group) { + auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(); + + auto node = std::make_shared("node1", "ns"); + entities_collector.add_node(node->get_node_base_interface()); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 1u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 1u); +} + +TEST_F(TestExecutorEntitiesCollector, add_callback_group_after_add_node) { + auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(); + + auto node = std::make_shared("node1", "ns"); + rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + entities_collector.add_node(node->get_node_base_interface()); + RCLCPP_EXPECT_THROW_EQ( + entities_collector.add_callback_group(cb_group), + std::runtime_error("Callback group has already been added to an executor.")); +} + +TEST_F(TestExecutorEntitiesCollector, add_callback_group_twice) { + auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(); + + auto node = std::make_shared("node1", "ns"); + rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + entities_collector.add_callback_group(cb_group); + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 1u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 1u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); + + cb_group->get_associated_with_executor_atomic().exchange(false); + RCLCPP_EXPECT_THROW_EQ( + entities_collector.add_callback_group(cb_group), + std::runtime_error("Callback group has already been added to this executor.")); +} + +TEST_F(TestExecutorEntitiesCollector, remove_callback_group_after_node) { + auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(); + + auto node = std::make_shared("node1", "ns"); + rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + entities_collector.add_callback_group(cb_group); + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 1u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 1u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); + + node.reset(); + + ASSERT_FALSE(cb_group->has_valid_node()); + + RCLCPP_EXPECT_THROW_EQ( + entities_collector.remove_callback_group(cb_group), + std::runtime_error("Node must not be deleted before its callback group(s).")); +} + +TEST_F(TestExecutorEntitiesCollector, remove_callback_group_twice) { + auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(); + + auto node = std::make_shared("node1", "ns"); + rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + entities_collector.add_callback_group(cb_group); + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 1u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 1u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); + + entities_collector.remove_callback_group(cb_group); + + RCLCPP_EXPECT_THROW_EQ( + entities_collector.remove_callback_group(cb_group), + std::runtime_error("Callback group needs to be associated with executor.")); +} + +TEST_F(TestExecutorEntitiesCollector, remove_node_opposite_order) { + auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(); + + auto node1 = std::make_shared("node1", "ns"); + EXPECT_NO_THROW(entities_collector.add_node(node1->get_node_base_interface())); + + auto node2 = std::make_shared("node2", "ns"); + EXPECT_NO_THROW(entities_collector.add_node(node2->get_node_base_interface())); + + EXPECT_NO_THROW(entities_collector.remove_node(node2->get_node_base_interface())); +} From e173e5a62a39bd3b0a5970230cada749b9c6ae4d Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Thu, 30 Mar 2023 19:23:36 +0000 Subject: [PATCH 07/22] Lint and docs Signed-off-by: Michael Carroll --- .../executor_entities_collection.hpp | 81 +++++++++++++++++-- .../executors/executor_entities_collector.hpp | 1 + .../executor_entities_collection.cpp | 18 ++--- .../executors/executor_entities_collector.cpp | 25 +++--- .../executors/executor_notify_waitable.cpp | 22 ++--- 5 files changed, 108 insertions(+), 39 deletions(-) diff --git a/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp b/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp index a5be2d6b7e..e808fd3760 100644 --- a/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp +++ b/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp @@ -32,16 +32,37 @@ namespace rclcpp namespace executors { +/// Structure to represent a single entity's entry in a collection template struct CollectionEntry { + /// Weak pointer to entity type using EntityWeakPtr = typename EntityValueType::WeakPtr; + /// Shared pointer to entity type using EntitySharedPtr = typename EntityValueType::SharedPtr; + /// The entity EntityWeakPtr entity; + + /// If relevant, the entity's corresponding callback_group rclcpp::CallbackGroup::WeakPtr callback_group; }; +/// Update a collection based on another collection +/* + * Iterates update_from and update_to to see which entities have been added/removed between + * the two collections. + * + * For each new entry (in update_from, but not in update_to), + * add the entity and fire the on_added callback + * For each removed entry (in update_to, but not in update_from), + * remove the entity and fire the on_removed callback. + * + * \param[in] update_from The collection representing the next iteration's state + * \param[inout] update_to The collection representing the current iteration's state + * \param[in] on_added Callback fired when a new entity is detected + * \param[in] on_removed Callback fired when an entity is removed + */ template void update_entities( const CollectionType & update_from, @@ -71,15 +92,31 @@ void update_entities( } } } + +/// A collection of entities, indexed by their corresponding handles template class EntityCollection : public std::unordered_map> { public: + /// Key type of the map using Key = const EntityKeyType *; + + /// Weak pointer to entity type using EntityWeakPtr = typename EntityValueType::WeakPtr; + + /// Shared pointer to entity type using EntitySharedPtr = typename EntityValueType::SharedPtr; + /// Update this collection based on the contents of another collection + /** + * Update the internal state of this collection, firing callbacks when entities have been + * added or removed. + * + * \param[in] other Collection to compare to + * \param[in] on_added Callback for when entities have been added + * \param[in] on_removed Callback for when entities have been removed + */ void update( const EntityCollection & other, std::function on_added, @@ -96,40 +133,72 @@ class EntityCollection */ struct ExecutorEntitiesCollection { - /// Entity entries for timers + /// Collection type for timer entities using TimerCollection = EntityCollection; - /// Entity entries for subscriptions + /// Collection type for subscription entities using SubscriptionCollection = EntityCollection; - /// Entity entries for clients + /// Collection type for client entities using ClientCollection = EntityCollection; - /// Entity entries for services + /// Collection type for service entities using ServiceCollection = EntityCollection; - /// Entity entries for waitables + /// Collection type for waitable entities using WaitableCollection = EntityCollection; - /// Entity entries for guard conditions + /// Collection type for guard condition entities using GuardConditionCollection = EntityCollection; + /// Collection of timers currently in use by the executor. TimerCollection timers; + + /// Collection of subscriptions currently in use by the executor. SubscriptionCollection subscriptions; + + /// Collection of clients currently in use by the executor. ClientCollection clients; + + /// Collection of services currently in use by the executor. ServiceCollection services; + + /// Collection of guard conditions currently in use by the executor. GuardConditionCollection guard_conditions; + + /// Collection of waitables currently in use by the executor. WaitableCollection waitables; + /// Check if the entities collection is empty + /** + * \return true if all member collections are empty, false otherwise + */ bool empty() const; + + /// Clear the entities collection void clear(); }; +/// Build an entities collection from callback groups +/** + * Iterates a list of callback groups and adds entities from each valid group + * + * \param[in] callback_groups List of callback groups to check for entities + * \param[inout] colletion Entities collection to populate with found entities + */ void build_entities_collection( const std::vector & callback_groups, ExecutorEntitiesCollection & collection); +/// Build a queue of executables ready to be executed +/** + * Iterates a list of entities and adds them to a queue if they are ready. + * + * \param[in] collection Collection of entities corresponding to the current wait set. + * \param[in] wait_result Result of rclcpp::WaitSet::wait corresponding to the collection. + * \return A queue of executables that have been marked ready by the waitset. + */ std::deque ready_executables( const ExecutorEntitiesCollection & collection, diff --git a/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp b/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp index 6ca01d0c13..cb3e4aa265 100644 --- a/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp +++ b/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp @@ -16,6 +16,7 @@ #define RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTOR_HPP_ #include +#include #include #include #include diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp index 7418b32a16..2dce369b08 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp @@ -20,12 +20,12 @@ namespace executors { bool ExecutorEntitiesCollection::empty() const { - return (subscriptions.empty() && - timers.empty() && - guard_conditions.empty() && - clients.empty() && - services.empty() && - waitables.empty()); + return subscriptions.empty() && + timers.empty() && + guard_conditions.empty() && + clients.empty() && + services.empty() && + waitables.empty(); } void ExecutorEntitiesCollection::clear() @@ -52,8 +52,7 @@ build_entities_collection( continue; } - if (group_ptr->can_be_taken_from().load()) - { + 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( @@ -102,7 +101,7 @@ void check_ready( size_t size_of_waited_entities, typename EntityCollectionType::Key * waited_entities, std::function fill_executable) + typename EntityCollectionType::EntitySharedPtr &)> fill_executable) { for (size_t ii = 0; ii < size_of_waited_entities; ++ii) { if (waited_entities[ii]) { @@ -122,7 +121,6 @@ void check_ready( exec.callback_group = callback_group; if (fill_executable(exec, entity)) { executables.push_back(exec); - } else { } } } diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp index 0447e776e3..014c525016 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp @@ -33,16 +33,20 @@ ExecutorEntitiesCollector::~ExecutorEntitiesCollector() { std::lock_guard guard{mutex_}; - for (auto weak_node_it = weak_nodes_.begin(); weak_node_it != weak_nodes_.end();) { + for (auto weak_node_it = weak_nodes_.begin(); weak_node_it != weak_nodes_.end(); ) { weak_node_it = remove_weak_node(weak_node_it); } - for (auto weak_group_it = automatically_added_groups_.begin(); weak_group_it != automatically_added_groups_.end(); ) { - weak_group_it= remove_weak_callback_group(weak_group_it, automatically_added_groups_); + for (auto weak_group_it = automatically_added_groups_.begin(); + weak_group_it != automatically_added_groups_.end(); ) + { + weak_group_it = remove_weak_callback_group(weak_group_it, automatically_added_groups_); } - for (auto weak_group_it = manually_added_groups_.begin(); weak_group_it != manually_added_groups_.end(); ) { - weak_group_it= remove_weak_callback_group(weak_group_it, manually_added_groups_); + for (auto weak_group_it = manually_added_groups_.begin(); + weak_group_it != manually_added_groups_.end(); ) + { + weak_group_it = remove_weak_callback_group(weak_group_it, manually_added_groups_); } } @@ -86,13 +90,12 @@ ExecutorEntitiesCollector::remove_node( } for (auto group_it = automatically_added_groups_.begin(); - group_it != automatically_added_groups_.end();) + group_it != automatically_added_groups_.end(); ) { auto group_ptr = group_it->lock(); if (node_ptr->callback_group_in_node(group_ptr)) { group_it = remove_weak_callback_group(group_it, automatically_added_groups_); - } - else { + } else { ++group_it; } } @@ -168,8 +171,7 @@ ExecutorEntitiesCollector::remove_weak_node(NodeCollection::iterator weak_node) { // Disassociate the guard condition from the executor notify waitable auto guard_condition_it = weak_nodes_to_guard_conditions_.find(*weak_node); - if (guard_condition_it != weak_nodes_to_guard_conditions_.end()) - { + if (guard_condition_it != weak_nodes_to_guard_conditions_.end()) { this->notify_waitable_->remove_guard_condition(guard_condition_it->second); weak_nodes_to_guard_conditions_.erase(guard_condition_it); } @@ -193,8 +195,7 @@ ExecutorEntitiesCollector::remove_weak_callback_group( { // Disassociate the guard condition from the executor notify waitable auto guard_condition_it = weak_groups_to_guard_conditions_.find(*weak_group_it); - if (guard_condition_it != weak_groups_to_guard_conditions_.end()) - { + if (guard_condition_it != weak_groups_to_guard_conditions_.end()) { this->notify_waitable_->remove_guard_condition(guard_condition_it->second); weak_groups_to_guard_conditions_.erase(guard_condition_it); } diff --git a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp index 89788bc2a5..78e2db91a2 100644 --- a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp +++ b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp @@ -103,23 +103,23 @@ ExecutorNotifyWaitable::get_number_of_ready_guard_conditions() { std::lock_guard lock(guard_condition_mutex_); - for (auto add_guard_condition: to_add_) - { - auto guard_it = std::find(notify_guard_conditions_.begin(), - notify_guard_conditions_.end(), - add_guard_condition); + for (auto add_guard_condition : to_add_) { + auto guard_it = std::find( + notify_guard_conditions_.begin(), + notify_guard_conditions_.end(), + add_guard_condition); if (guard_it == notify_guard_conditions_.end()) { notify_guard_conditions_.push_back(add_guard_condition); } } to_add_.clear(); - for (auto remove_guard_condition: to_remove_) - { - auto guard_it = std::find(notify_guard_conditions_.begin(), - notify_guard_conditions_.end(), - remove_guard_condition); - if (guard_it != notify_guard_conditions_.end()){ + for (auto remove_guard_condition : to_remove_) { + auto guard_it = std::find( + notify_guard_conditions_.begin(), + notify_guard_conditions_.end(), + remove_guard_condition); + if (guard_it != notify_guard_conditions_.end()) { notify_guard_conditions_.erase(guard_it); } } From 653d1a38687d1e2be683c7a6a818a90e33f82ab3 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Fri, 31 Mar 2023 01:45:19 +0000 Subject: [PATCH 08/22] Make executor own the notify waitable Signed-off-by: Michael Carroll --- .../executors/executor_entities_collector.hpp | 7 ++---- .../executors/executor_entities_collector.cpp | 10 ++------ .../executors/test_entities_collector.cpp | 25 +++++++++++++------ 3 files changed, 21 insertions(+), 21 deletions(-) diff --git a/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp b/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp index cb3e4aa265..43884388ed 100644 --- a/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp +++ b/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp @@ -64,7 +64,7 @@ class ExecutorEntitiesCollector */ RCLCPP_PUBLIC explicit ExecutorEntitiesCollector( - std::function on_notify_waitable_callback = {}); + std::shared_ptr notify_waitable); /// Destructor RCLCPP_PUBLIC @@ -144,9 +144,6 @@ class ExecutorEntitiesCollector std::vector get_automatically_added_callback_groups(); - RCLCPP_PUBLIC - std::shared_ptr get_notify_waitable(); - /// Update the underlying collections /** * This will prune nodes and callback groups that are no longer valid as well @@ -225,7 +222,7 @@ class ExecutorEntitiesCollector WeakGroupsToGuardConditionsMap weak_groups_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_); - std::shared_ptr notify_waitable_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + std::shared_ptr notify_waitable_; }; } // namespace executors } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp index 014c525016..7c17f68939 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp @@ -24,8 +24,8 @@ namespace executors { ExecutorEntitiesCollector::ExecutorEntitiesCollector( - std::function on_notify_waitable_callback) -: notify_waitable_(std::make_shared(on_notify_waitable_callback)) + std::shared_ptr notify_waitable) +: notify_waitable_(notify_waitable) { } @@ -152,12 +152,6 @@ ExecutorEntitiesCollector::get_automatically_added_callback_groups() return groups; } -std::shared_ptr -ExecutorEntitiesCollector::get_notify_waitable() -{ - return this->notify_waitable_; -} - void ExecutorEntitiesCollector::update_collections() { diff --git a/rclcpp/test/rclcpp/executors/test_entities_collector.cpp b/rclcpp/test/rclcpp/executors/test_entities_collector.cpp index defa647999..69414cf2db 100644 --- a/rclcpp/test/rclcpp/executors/test_entities_collector.cpp +++ b/rclcpp/test/rclcpp/executors/test_entities_collector.cpp @@ -14,6 +14,7 @@ #include +#include "rclcpp/executors/executor_notify_waitable.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp/executors/executor_entities_collector.hpp" @@ -34,7 +35,8 @@ class TestExecutorEntitiesCollector : public ::testing::Test }; TEST_F(TestExecutorEntitiesCollector, add_remove_node) { - auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(); + auto notify_waitable = std::make_shared(); + auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(notify_waitable); auto node1 = std::make_shared("node1", "ns"); auto node2 = std::make_shared("node2", "ns"); @@ -78,7 +80,8 @@ TEST_F(TestExecutorEntitiesCollector, add_remove_node) { } TEST_F(TestExecutorEntitiesCollector, add_callback_group) { - auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(); + auto notify_waitable = std::make_shared(); + auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(notify_waitable); auto node = std::make_shared("node1", "ns"); rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( @@ -91,7 +94,8 @@ TEST_F(TestExecutorEntitiesCollector, add_callback_group) { } TEST_F(TestExecutorEntitiesCollector, add_node_default_callback_group) { - auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(); + auto notify_waitable = std::make_shared(); + auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(notify_waitable); auto node = std::make_shared("node1", "ns"); entities_collector.add_node(node->get_node_base_interface()); @@ -102,7 +106,8 @@ TEST_F(TestExecutorEntitiesCollector, add_node_default_callback_group) { } TEST_F(TestExecutorEntitiesCollector, add_callback_group_after_add_node) { - auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(); + auto notify_waitable = std::make_shared(); + auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(notify_waitable); auto node = std::make_shared("node1", "ns"); rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( @@ -115,7 +120,8 @@ TEST_F(TestExecutorEntitiesCollector, add_callback_group_after_add_node) { } TEST_F(TestExecutorEntitiesCollector, add_callback_group_twice) { - auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(); + auto notify_waitable = std::make_shared(); + auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(notify_waitable); auto node = std::make_shared("node1", "ns"); rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( @@ -133,7 +139,8 @@ TEST_F(TestExecutorEntitiesCollector, add_callback_group_twice) { } TEST_F(TestExecutorEntitiesCollector, remove_callback_group_after_node) { - auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(); + auto notify_waitable = std::make_shared(); + auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(notify_waitable); auto node = std::make_shared("node1", "ns"); rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( @@ -154,7 +161,8 @@ TEST_F(TestExecutorEntitiesCollector, remove_callback_group_after_node) { } TEST_F(TestExecutorEntitiesCollector, remove_callback_group_twice) { - auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(); + auto notify_waitable = std::make_shared(); + auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(notify_waitable); auto node = std::make_shared("node1", "ns"); rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( @@ -173,7 +181,8 @@ TEST_F(TestExecutorEntitiesCollector, remove_callback_group_twice) { } TEST_F(TestExecutorEntitiesCollector, remove_node_opposite_order) { - auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(); + auto notify_waitable = std::make_shared(); + auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(notify_waitable); auto node1 = std::make_shared("node1", "ns"); EXPECT_NO_THROW(entities_collector.add_node(node1->get_node_base_interface())); From a6c4c1b43599dacc6bda536d5b33e360a37f564e Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Mon, 3 Apr 2023 17:01:17 +0000 Subject: [PATCH 09/22] Add pending queue to collector, remove from waitable Also change node's get_guard_condition to return shared_ptr Signed-off-by: Michael Carroll --- .../executors/executor_entities_collector.hpp | 43 ++-- .../executors/executor_notify_waitable.hpp | 23 ++- .../rclcpp/node_interfaces/node_base.hpp | 8 +- .../node_interfaces/node_base_interface.hpp | 7 +- .../executors/executor_entities_collector.cpp | 187 ++++++++++++++---- .../executors/executor_notify_waitable.cpp | 88 ++++----- .../src/rclcpp/node_interfaces/node_base.cpp | 14 +- .../src/rclcpp/node_interfaces/node_graph.cpp | 3 +- .../rclcpp/node_interfaces/node_services.cpp | 6 +- .../rclcpp/node_interfaces/node_timers.cpp | 3 +- .../rclcpp/node_interfaces/node_topics.cpp | 6 +- .../rclcpp/node_interfaces/node_waitables.cpp | 3 +- .../test_executor_notify_waitable.cpp | 10 +- 13 files changed, 265 insertions(+), 136 deletions(-) diff --git a/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp b/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp index 43884388ed..d60e01bfdb 100644 --- a/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp +++ b/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp @@ -70,6 +70,8 @@ class ExecutorEntitiesCollector RCLCPP_PUBLIC ~ExecutorEntitiesCollector(); + bool has_pending(); + /// Add a node to the entity collector /** * \param[in] node_ptr a shared pointer that points to a node base interface @@ -164,63 +166,66 @@ class ExecutorEntitiesCollector using WeakNodesToGuardConditionsMap = std::map< rclcpp::node_interfaces::NodeBaseInterface::WeakPtr, - const rclcpp::GuardCondition *, + rclcpp::GuardCondition::WeakPtr, std::owner_less>; using WeakGroupsToGuardConditionsMap = std::map< rclcpp::CallbackGroup::WeakPtr, - const rclcpp::GuardCondition *, + rclcpp::GuardCondition::WeakPtr, std::owner_less>; RCLCPP_PUBLIC NodeCollection::iterator - remove_weak_node(NodeCollection::iterator weak_node) RCPPUTILS_TSA_REQUIRES(mutex_); + remove_weak_node(NodeCollection::iterator weak_node); RCLCPP_PUBLIC CallbackGroupCollection::iterator remove_weak_callback_group( CallbackGroupCollection::iterator weak_group_it, - CallbackGroupCollection & collection) RCPPUTILS_TSA_REQUIRES(mutex_); + CallbackGroupCollection & collection); RCLCPP_PUBLIC void add_callback_group_to_collection( rclcpp::CallbackGroup::SharedPtr group_ptr, - CallbackGroupCollection & collection) RCPPUTILS_TSA_REQUIRES(mutex_); + CallbackGroupCollection & collection); RCLCPP_PUBLIC void remove_callback_group_from_collection( rclcpp::CallbackGroup::SharedPtr group_ptr, - CallbackGroupCollection & collection) RCPPUTILS_TSA_REQUIRES(mutex_); + CallbackGroupCollection & collection); RCLCPP_PUBLIC void - add_automatically_associated_callback_groups( - const NodeCollection & nodes_to_check) - RCPPUTILS_TSA_REQUIRES(mutex_); + process_queues(); RCLCPP_PUBLIC void - prune_invalid_nodes_and_groups() RCPPUTILS_TSA_REQUIRES(mutex_); + add_automatically_associated_callback_groups( + const NodeCollection & nodes_to_check); - mutable std::mutex mutex_; + RCLCPP_PUBLIC + void + prune_invalid_nodes_and_groups(); /// Callback groups that were added via `add_callback_group` - CallbackGroupCollection - manually_added_groups_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + CallbackGroupCollection manually_added_groups_; /// Callback groups that were added by their association with added nodes - CallbackGroupCollection - automatically_added_groups_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + CallbackGroupCollection automatically_added_groups_; /// nodes that are associated with the executor - NodeCollection - weak_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + NodeCollection weak_nodes_; - WeakNodesToGuardConditionsMap weak_nodes_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + std::mutex pending_mutex_; + NodeCollection pending_added_nodes_; + NodeCollection pending_removed_nodes_; + CallbackGroupCollection pending_manually_added_groups_; + CallbackGroupCollection pending_manually_removed_groups_; - WeakGroupsToGuardConditionsMap weak_groups_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + WeakNodesToGuardConditionsMap weak_nodes_to_guard_conditions_; + WeakGroupsToGuardConditionsMap weak_groups_to_guard_conditions_; std::shared_ptr notify_waitable_; }; diff --git a/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp b/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp index 5fb2532dec..aac0820dbe 100644 --- a/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp +++ b/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp @@ -17,6 +17,7 @@ #include #include +#include #include "rclcpp/guard_condition.hpp" #include "rclcpp/waitable.hpp" @@ -33,6 +34,7 @@ class ExecutorNotifyWaitable : public rclcpp::Waitable public: RCLCPP_SMART_PTR_DEFINITIONS(ExecutorNotifyWaitable) + // Constructor /** * \param[in] on_execute_callback Callback to execute when one of the conditions @@ -45,6 +47,13 @@ class ExecutorNotifyWaitable : public rclcpp::Waitable RCLCPP_PUBLIC ~ExecutorNotifyWaitable() override = default; + RCLCPP_PUBLIC + ExecutorNotifyWaitable(const ExecutorNotifyWaitable & other); + + + RCLCPP_PUBLIC + ExecutorNotifyWaitable & operator=(const ExecutorNotifyWaitable & other); + /// Add conditions to the wait set /** * \param[inout] wait_set structure that conditions will be added to @@ -85,7 +94,7 @@ class ExecutorNotifyWaitable : public rclcpp::Waitable */ RCLCPP_PUBLIC void - add_guard_condition(const rclcpp::GuardCondition * guard_condition); + add_guard_condition(rclcpp::GuardCondition::WeakPtr guard_condition); /// Remove a guard condition from being waited on. /** @@ -93,7 +102,7 @@ class ExecutorNotifyWaitable : public rclcpp::Waitable */ RCLCPP_PUBLIC void - remove_guard_condition(const rclcpp::GuardCondition * guard_condition); + remove_guard_condition(rclcpp::GuardCondition::WeakPtr guard_condition); /// Get the number of ready guard_conditions /** @@ -107,17 +116,11 @@ class ExecutorNotifyWaitable : public rclcpp::Waitable /// Callback to run when waitable executes std::function 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 notify_guard_conditions_; - - /// The collection of guard conditions to be waited on. - std::list to_add_; - - /// The collection of guard conditions to be waited on. - std::list to_remove_; + std::set> notify_guard_conditions_; }; } // namespace executors diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp index a6c84e4aa0..6131681b80 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp @@ -122,9 +122,13 @@ class NodeBase : public NodeBaseInterface, public std::enable_shared_from_this notify_guard_condition_; bool notify_guard_condition_is_valid_; }; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp index fd4f64b22b..eb87771eab 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp @@ -152,9 +152,14 @@ class NodeBaseInterface */ RCLCPP_PUBLIC virtual - rclcpp::GuardCondition & + rclcpp::GuardCondition::SharedPtr get_notify_guard_condition() = 0; + RCLCPP_PUBLIC + virtual + void + trigger_notify_guard_condition() = 0; + /// Return the default preference for using intra process communication. RCLCPP_PUBLIC virtual diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp index 7c17f68939..4afdeb3a2b 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp @@ -31,8 +31,6 @@ ExecutorEntitiesCollector::ExecutorEntitiesCollector( ExecutorEntitiesCollector::~ExecutorEntitiesCollector() { - std::lock_guard guard{mutex_}; - for (auto weak_node_it = weak_nodes_.begin(); weak_node_it != weak_nodes_.end(); ) { weak_node_it = remove_weak_node(weak_node_it); } @@ -48,13 +46,40 @@ ExecutorEntitiesCollector::~ExecutorEntitiesCollector() { weak_group_it = remove_weak_callback_group(weak_group_it, manually_added_groups_); } + + std::lock_guard pending_lock(pending_mutex_); + + for (auto weak_node_ptr : pending_added_nodes_) { + auto node_ptr = weak_node_ptr.lock(); + if (node_ptr) { + node_ptr->get_associated_with_executor_atomic().store(false); + } + } + pending_added_nodes_.clear(); + pending_removed_nodes_.clear(); + + for (auto weak_group_ptr : pending_manually_added_groups_) { + auto group_ptr = weak_group_ptr.lock(); + if (group_ptr) { + group_ptr->get_associated_with_executor_atomic().store(false); + } + } + pending_manually_added_groups_.clear(); + pending_manually_removed_groups_.clear(); +} + +bool ExecutorEntitiesCollector::has_pending() +{ + std::lock_guard pending_lock(pending_mutex_); + return pending_manually_added_groups_.size() != 0 || + pending_manually_removed_groups_.size() != 0 || + pending_added_nodes_.size() != 0 || + pending_removed_nodes_.size() != 0; } void ExecutorEntitiesCollector::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) { - std::lock_guard guard{mutex_}; - // If the node already has an executor std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); if (has_executor.exchange(true)) { @@ -62,64 +87,88 @@ ExecutorEntitiesCollector::add_node(rclcpp::node_interfaces::NodeBaseInterface:: std::string("Node '") + node_ptr->get_fully_qualified_name() + "' has already been added to an executor."); } - weak_nodes_.insert(node_ptr); - this->add_automatically_associated_callback_groups({node_ptr}); - // Store node guard condition in map and add it to the notify waitable - rclcpp::GuardCondition * node_guard_condition = &node_ptr->get_notify_guard_condition(); - weak_nodes_to_guard_conditions_.insert({node_ptr, node_guard_condition}); + std::lock_guard pending_lock(pending_mutex_); + bool associated = weak_nodes_.count(node_ptr) != 0; + bool add_queued = pending_added_nodes_.count(node_ptr) != 0; + bool remove_queued = pending_removed_nodes_.count(node_ptr) != 0; + + if ((associated || add_queued) && !remove_queued) { + throw std::runtime_error( + std::string("Node '") + node_ptr->get_fully_qualified_name() + + "' has already been added to this executor."); + } - this->notify_waitable_->add_guard_condition(node_guard_condition); + this->pending_added_nodes_.insert(node_ptr); } void ExecutorEntitiesCollector::remove_node( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) { - std::lock_guard guard{mutex_}; - if (!node_ptr->get_associated_with_executor_atomic().load()) { throw std::runtime_error("Node needs to be associated with an executor."); } - auto node_it = weak_nodes_.find(node_ptr); - if (node_it != weak_nodes_.end()) { - remove_weak_node(node_it); - } else { - throw std::runtime_error("Node needs to be associated with this executor."); - } + std::lock_guard pending_lock(pending_mutex_); + bool associated = weak_nodes_.count(node_ptr) != 0; + bool add_queued = pending_added_nodes_.count(node_ptr) != 0; + bool remove_queued = pending_removed_nodes_.count(node_ptr) != 0; - for (auto group_it = automatically_added_groups_.begin(); - group_it != automatically_added_groups_.end(); ) - { - auto group_ptr = group_it->lock(); - if (node_ptr->callback_group_in_node(group_ptr)) { - group_it = remove_weak_callback_group(group_it, automatically_added_groups_); - } else { - ++group_it; - } + if (!(associated || add_queued) || remove_queued) { + throw std::runtime_error( + std::string("Node '") + node_ptr->get_fully_qualified_name() + + "' needs to be associated with this executor."); } + + this->pending_removed_nodes_.insert(node_ptr); } void ExecutorEntitiesCollector::add_callback_group(rclcpp::CallbackGroup::SharedPtr group_ptr) { - std::lock_guard guard{mutex_}; - this->add_callback_group_to_collection(group_ptr, manually_added_groups_); + std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic(); + if (has_executor.exchange(true)) { + throw std::runtime_error("Callback group has already been added to an executor."); + } + + std::lock_guard pending_lock(pending_mutex_); + bool associated = manually_added_groups_.count(group_ptr) != 0; + bool add_queued = pending_manually_added_groups_.count(group_ptr) != 0; + bool remove_queued = pending_manually_removed_groups_.count(group_ptr) != 0; + + if ((associated || add_queued) && !remove_queued) { + throw std::runtime_error("Callback group has already been added to this executor."); + } + + this->pending_manually_added_groups_.insert(group_ptr); } void ExecutorEntitiesCollector::remove_callback_group(rclcpp::CallbackGroup::SharedPtr group_ptr) { - std::lock_guard guard{mutex_}; - this->remove_callback_group_from_collection(group_ptr, manually_added_groups_); + if (!group_ptr->get_associated_with_executor_atomic().load()) { + throw std::runtime_error("Callback group needs to be associated with an executor."); + } + + auto weak_group_ptr = rclcpp::CallbackGroup::WeakPtr(group_ptr); + + std::lock_guard pending_lock(pending_mutex_); + bool associated = manually_added_groups_.count(group_ptr) != 0; + bool add_queued = pending_manually_added_groups_.count(group_ptr) != 0; + bool remove_queued = pending_manually_removed_groups_.count(group_ptr) != 0; + + if ((associated || add_queued) && !remove_queued) { + throw std::runtime_error("Callback group needs to be associated with this executor."); + } + + this->pending_manually_removed_groups_.insert(group_ptr); } std::vector ExecutorEntitiesCollector::get_all_callback_groups() { std::vector groups; - std::lock_guard guard{mutex_}; for (const auto & group_ptr : manually_added_groups_) { groups.push_back(group_ptr); @@ -134,7 +183,6 @@ std::vector ExecutorEntitiesCollector::get_manually_added_callback_groups() { std::vector groups; - std::lock_guard guard{mutex_}; for (const auto & group_ptr : manually_added_groups_) { groups.push_back(group_ptr); } @@ -145,7 +193,6 @@ std::vector ExecutorEntitiesCollector::get_automatically_added_callback_groups() { std::vector groups; - std::lock_guard guard{mutex_}; for (auto const & group_ptr : automatically_added_groups_) { groups.push_back(group_ptr); } @@ -155,7 +202,7 @@ ExecutorEntitiesCollector::get_automatically_added_callback_groups() void ExecutorEntitiesCollector::update_collections() { - std::lock_guard guard{mutex_}; + this->process_queues(); this->add_automatically_associated_callback_groups(this->weak_nodes_); this->prune_invalid_nodes_and_groups(); } @@ -224,7 +271,7 @@ ExecutorEntitiesCollector::add_callback_group_to_collection( } // Store node guard condition in map and add it to the notify waitable - rclcpp::GuardCondition * group_guard_condition = group_ptr->get_notify_guard_condition().get(); + auto group_guard_condition = group_ptr->get_notify_guard_condition(); weak_groups_to_guard_conditions_.insert({group_ptr, group_guard_condition}); this->notify_waitable_->add_guard_condition(group_guard_condition); } @@ -235,7 +282,73 @@ ExecutorEntitiesCollector::remove_callback_group_from_collection( CallbackGroupCollection & collection) { auto group_it = collection.find(group_ptr); - remove_weak_callback_group(group_it, collection); + + if (group_it != collection.end()) { + remove_weak_callback_group(group_it, collection); + } else { + throw std::runtime_error("Attempting to remove a callback group not added to this executor."); + } +} + +void +ExecutorEntitiesCollector::process_queues() +{ + std::lock_guard pending_lock(pending_mutex_); + + for (auto weak_node_ptr: pending_added_nodes_) { + auto node_ptr = weak_node_ptr.lock(); + if (!node_ptr) { + continue; + } + weak_nodes_.insert(weak_node_ptr); + this->add_automatically_associated_callback_groups({weak_node_ptr}); + + // Store node guard condition in map and add it to the notify waitable + auto node_guard_condition = node_ptr->get_notify_guard_condition(); + weak_nodes_to_guard_conditions_.insert({weak_node_ptr, node_guard_condition}); + this->notify_waitable_->add_guard_condition(node_guard_condition); + } + pending_added_nodes_.clear(); + + 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); + } else { + throw std::runtime_error("Node needs to be associated with this executor."); + } + + auto node_ptr = weak_node_ptr.lock(); + if (node_ptr) { + for (auto group_it = automatically_added_groups_.begin(); + group_it != automatically_added_groups_.end(); ) + { + auto group_ptr = group_it->lock(); + if (node_ptr->callback_group_in_node(group_ptr)) { + group_it = remove_weak_callback_group(group_it, automatically_added_groups_); + } else { + ++group_it; + } + } + } + } + pending_removed_nodes_.clear(); + + 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_) { + auto group_ptr = weak_group_ptr.lock(); + if (group_ptr) { + this->remove_callback_group_from_collection(group_ptr, manually_added_groups_); + } + } + pending_manually_removed_groups_.clear(); } void diff --git a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp index 78e2db91a2..11ff07a732 100644 --- a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp +++ b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp @@ -27,20 +27,39 @@ ExecutorNotifyWaitable::ExecutorNotifyWaitable(std::function on_exec { } +ExecutorNotifyWaitable::ExecutorNotifyWaitable(const ExecutorNotifyWaitable & other) +: ExecutorNotifyWaitable(other.execute_callback_) +{ + this->notify_guard_conditions_ = other.notify_guard_conditions_; +} + +ExecutorNotifyWaitable & ExecutorNotifyWaitable::operator=(const ExecutorNotifyWaitable & other) +{ + if (this != &other) { + this->execute_callback_ = other.execute_callback_; + this->notify_guard_conditions_ = other.notify_guard_conditions_; + } + return *this; +} + void ExecutorNotifyWaitable::add_to_wait_set(rcl_wait_set_t * wait_set) { std::lock_guard lock(guard_condition_mutex_); - for (auto guard_condition : this->notify_guard_conditions_) { - auto rcl_guard_condition = guard_condition->get_rcl_guard_condition(); - rcl_ret_t ret = rcl_wait_set_add_guard_condition( - wait_set, - &rcl_guard_condition, NULL); + for (auto weak_guard_condition : this->notify_guard_conditions_) { + auto guard_condition = weak_guard_condition.lock(); + if (guard_condition) { + auto rcl_guard_condition = &guard_condition->get_rcl_guard_condition(); + + rcl_ret_t ret = rcl_wait_set_add_guard_condition( + wait_set, + rcl_guard_condition, NULL); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error( - ret, "failed to add guard condition to wait set"); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "failed to add guard condition to wait set"); + } } } } @@ -49,20 +68,22 @@ bool ExecutorNotifyWaitable::is_ready(rcl_wait_set_t * wait_set) { std::lock_guard lock(guard_condition_mutex_); + + bool any_ready = false; for (size_t ii = 0; ii < wait_set->size_of_guard_conditions; ++ii) { auto rcl_guard_condition = wait_set->guard_conditions[ii]; if (nullptr == rcl_guard_condition) { continue; } - - for (auto guard_condition : this->notify_guard_conditions_) { - if (&guard_condition->get_rcl_guard_condition() == rcl_guard_condition) { - return true; + for (auto weak_guard_condition : this->notify_guard_conditions_) { + auto guard_condition = weak_guard_condition.lock(); + if (guard_condition && &guard_condition->get_rcl_guard_condition() == rcl_guard_condition) { + any_ready = true; } } } - return false; + return any_ready; } void @@ -79,52 +100,27 @@ ExecutorNotifyWaitable::take_data() } void -ExecutorNotifyWaitable::add_guard_condition(const rclcpp::GuardCondition * guard_condition) +ExecutorNotifyWaitable::add_guard_condition(rclcpp::GuardCondition::WeakPtr guard_condition) { - if (guard_condition == nullptr) { - throw std::runtime_error("Attempting to add null guard condition."); - } std::lock_guard lock(guard_condition_mutex_); - to_add_.push_back(guard_condition); + if (notify_guard_conditions_.count(guard_condition) == 0) { + notify_guard_conditions_.insert(guard_condition); + } } void -ExecutorNotifyWaitable::remove_guard_condition(const rclcpp::GuardCondition * guard_condition) +ExecutorNotifyWaitable::remove_guard_condition(rclcpp::GuardCondition::WeakPtr guard_condition) { - if (guard_condition == nullptr) { - throw std::runtime_error("Attempting to remove null guard condition."); - } std::lock_guard lock(guard_condition_mutex_); - to_remove_.push_back(guard_condition); + if (notify_guard_conditions_.count(guard_condition) != 0) { + notify_guard_conditions_.erase(guard_condition); + } } size_t ExecutorNotifyWaitable::get_number_of_ready_guard_conditions() { std::lock_guard lock(guard_condition_mutex_); - - for (auto add_guard_condition : to_add_) { - auto guard_it = std::find( - notify_guard_conditions_.begin(), - notify_guard_conditions_.end(), - add_guard_condition); - if (guard_it == notify_guard_conditions_.end()) { - notify_guard_conditions_.push_back(add_guard_condition); - } - } - to_add_.clear(); - - for (auto remove_guard_condition : to_remove_) { - auto guard_it = std::find( - notify_guard_conditions_.begin(), - notify_guard_conditions_.end(), - remove_guard_condition); - if (guard_it != notify_guard_conditions_.end()) { - notify_guard_conditions_.erase(guard_it); - } - } - to_remove_.clear(); - return notify_guard_conditions_.size(); } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index 7b4a70d411..2b08b746b6 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -45,7 +45,7 @@ NodeBase::NodeBase( node_handle_(nullptr), default_callback_group_(default_callback_group), associated_with_executor_(false), - notify_guard_condition_(context), + notify_guard_condition_(std::make_shared(context)), notify_guard_condition_is_valid_(false) { // Create the rcl node and store it in a shared_ptr with a custom destructor. @@ -254,7 +254,7 @@ NodeBase::get_associated_with_executor_atomic() return associated_with_executor_; } -rclcpp::GuardCondition & +rclcpp::GuardCondition::SharedPtr NodeBase::get_notify_guard_condition() { std::lock_guard notify_condition_lock(notify_guard_condition_mutex_); @@ -264,6 +264,16 @@ NodeBase::get_notify_guard_condition() return notify_guard_condition_; } +void +NodeBase::trigger_notify_guard_condition() +{ + std::lock_guard notify_condition_lock(notify_guard_condition_mutex_); + if (!notify_guard_condition_is_valid_) { + throw std::runtime_error("failed to trigger notify guard condition because it is invalid"); + } + notify_guard_condition_->trigger(); +} + bool NodeBase::get_use_intra_process_default() const { diff --git a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp index e13ec7cd4c..bf47a1e3b5 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp @@ -533,9 +533,8 @@ NodeGraph::notify_graph_change() } } graph_cv_.notify_all(); - auto & node_gc = node_base_->get_notify_guard_condition(); try { - node_gc.trigger(); + node_base_->trigger_notify_guard_condition(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( std::string("failed to notify wait set on graph change: ") + ex.what()); diff --git a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp index 2f1afd3224..e9c4a5266e 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp @@ -42,9 +42,8 @@ NodeServices::add_service( group->add_service(service_base_ptr); // Notify the executor that a new service was created using the parent Node. - auto & node_gc = node_base_->get_notify_guard_condition(); try { - node_gc.trigger(); + node_base_->trigger_notify_guard_condition(); group->trigger_notify_guard_condition(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( @@ -69,9 +68,8 @@ NodeServices::add_client( group->add_client(client_base_ptr); // Notify the executor that a new client was created using the parent Node. - auto & node_gc = node_base_->get_notify_guard_condition(); try { - node_gc.trigger(); + node_base_->trigger_notify_guard_condition(); group->trigger_notify_guard_condition(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( diff --git a/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp b/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp index d2e821a9e6..96097def22 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp @@ -42,9 +42,8 @@ NodeTimers::add_timer( } callback_group->add_timer(timer); - auto & node_gc = node_base_->get_notify_guard_condition(); try { - node_gc.trigger(); + node_base_->trigger_notify_guard_condition(); callback_group->trigger_notify_guard_condition(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( diff --git a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp index 167a35f35d..659129d62c 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp @@ -70,9 +70,8 @@ NodeTopics::add_publisher( } // Notify the executor that a new publisher was created using the parent Node. - auto & node_gc = node_base_->get_notify_guard_condition(); try { - node_gc.trigger(); + node_base_->trigger_notify_guard_condition(); callback_group->trigger_notify_guard_condition(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( @@ -119,9 +118,8 @@ NodeTopics::add_subscription( } // Notify the executor that a new subscription was created using the parent Node. - auto & node_gc = node_base_->get_notify_guard_condition(); try { - node_gc.trigger(); + node_base_->trigger_notify_guard_condition(); callback_group->trigger_notify_guard_condition(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( diff --git a/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp b/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp index 1d1fe2ce59..02a9de82b0 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp @@ -42,9 +42,8 @@ NodeWaitables::add_waitable( group->add_waitable(waitable_ptr); // Notify the executor that a new waitable was created using the parent Node. - auto & node_gc = node_base_->get_notify_guard_condition(); try { - node_gc.trigger(); + node_base_->trigger_notify_guard_condition(); group->trigger_notify_guard_condition(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( diff --git a/rclcpp/test/rclcpp/executors/test_executor_notify_waitable.cpp b/rclcpp/test/rclcpp/executors/test_executor_notify_waitable.cpp index 8c9aaad819..a286de5fa8 100644 --- a/rclcpp/test/rclcpp/executors/test_executor_notify_waitable.cpp +++ b/rclcpp/test/rclcpp/executors/test_executor_notify_waitable.cpp @@ -61,10 +61,10 @@ TEST_F(TestExecutorNotifyWaitable, add_remove_guard_conditions) { std::make_shared(on_execute_callback); auto node = std::make_shared("my_node", "/ns"); - auto & notify_guard_condition = node->get_node_base_interface()->get_notify_guard_condition(); + auto notify_guard_condition = node->get_node_base_interface()->get_notify_guard_condition(); - EXPECT_NO_THROW(waitable->add_guard_condition(¬ify_guard_condition)); - EXPECT_NO_THROW(waitable->remove_guard_condition(¬ify_guard_condition)); + EXPECT_NO_THROW(waitable->add_guard_condition(notify_guard_condition)); + EXPECT_NO_THROW(waitable->remove_guard_condition(notify_guard_condition)); } TEST_F(TestExecutorNotifyWaitable, wait) { @@ -75,8 +75,8 @@ TEST_F(TestExecutorNotifyWaitable, wait) { std::make_shared(on_execute_callback); auto node = std::make_shared("my_node", "/ns"); - auto & notify_guard_condition = node->get_node_base_interface()->get_notify_guard_condition(); - EXPECT_NO_THROW(waitable->add_guard_condition(¬ify_guard_condition)); + auto notify_guard_condition = node->get_node_base_interface()->get_notify_guard_condition(); + EXPECT_NO_THROW(waitable->add_guard_condition(notify_guard_condition)); auto default_cbg = node->get_node_base_interface()->get_default_callback_group(); ASSERT_NE(nullptr, default_cbg->get_notify_guard_condition()); From 9dd48ce6c2d306ed8008ca401805514c1f4601c2 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Mon, 3 Apr 2023 17:55:36 +0000 Subject: [PATCH 10/22] Change interrupt guard condition to shared_ptr Check if guard condition is valid before adding it to the waitable Signed-off-by: Michael Carroll --- rclcpp/include/rclcpp/executor.hpp | 3 ++- rclcpp/src/rclcpp/callback_group.cpp | 2 +- rclcpp/src/rclcpp/executor.cpp | 25 ++++++++----------- .../executors/executor_notify_waitable.cpp | 7 +++--- .../static_executor_entities_collector.cpp | 3 +-- .../static_single_threaded_executor.cpp | 8 +++--- 6 files changed, 23 insertions(+), 25 deletions(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 94a8488557..43e15eda59 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -637,8 +637,9 @@ class Executor std::atomic_bool spinning; /// Guard condition for signaling the rmw layer to wake up for special events. - rclcpp::GuardCondition interrupt_guard_condition_; + std::shared_ptr interrupt_guard_condition_; + /// 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/src/rclcpp/callback_group.cpp b/rclcpp/src/rclcpp/callback_group.cpp index 8e3b62da3b..f8c7bfc35f 100644 --- a/rclcpp/src/rclcpp/callback_group.cpp +++ b/rclcpp/src/rclcpp/callback_group.cpp @@ -43,8 +43,8 @@ CallbackGroup::~CallbackGroup() { trigger_notify_guard_condition(); } - bool + CallbackGroup::has_valid_node() { return nullptr != this->get_context_(); diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 197822e087..747b784878 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -38,14 +38,11 @@ using namespace std::chrono_literals; using rclcpp::exceptions::throw_from_rcl_error; -using rclcpp::AnyExecutable; using rclcpp::Executor; -using rclcpp::ExecutorOptions; -using rclcpp::FutureReturnCode; Executor::Executor(const rclcpp::ExecutorOptions & options) : spinning(false), - interrupt_guard_condition_(options.context), + interrupt_guard_condition_(std::make_shared(options.context)), shutdown_guard_condition_(std::make_shared(options.context)), memory_strategy_(options.memory_strategy) { @@ -65,7 +62,7 @@ Executor::Executor(const rclcpp::ExecutorOptions & options) memory_strategy_->add_guard_condition(*shutdown_guard_condition_.get()); // Put the executor's guard condition in - memory_strategy_->add_guard_condition(interrupt_guard_condition_); + memory_strategy_->add_guard_condition(*interrupt_guard_condition_.get()); rcl_allocator_t allocator = memory_strategy_->get_allocator(); rcl_ret_t ret = rcl_wait_set_init( @@ -127,7 +124,7 @@ Executor::~Executor() } // Remove and release the sigint guard condition memory_strategy_->remove_guard_condition(shutdown_guard_condition_.get()); - memory_strategy_->remove_guard_condition(&interrupt_guard_condition_); + memory_strategy_->remove_guard_condition(interrupt_guard_condition_.get()); // Remove shutdown callback handle registered to Context if (!context_->remove_on_shutdown_callback(shutdown_callback_handle_)) { @@ -231,7 +228,7 @@ Executor::add_callback_group_to_map( if (notify) { // Interrupt waiting to handle new node try { - interrupt_guard_condition_.trigger(); + interrupt_guard_condition_->trigger(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( std::string( @@ -279,10 +276,10 @@ Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_pt } }); - const auto & gc = node_ptr->get_notify_guard_condition(); - weak_nodes_to_guard_conditions_[node_ptr] = &gc; + const auto gc = node_ptr->get_notify_guard_condition(); + weak_nodes_to_guard_conditions_[node_ptr] = gc.get(); // Add the node's notify condition to the guard condition handles - memory_strategy_->add_guard_condition(gc); + memory_strategy_->add_guard_condition(*gc); weak_nodes_.push_back(node_ptr); } @@ -319,7 +316,7 @@ Executor::remove_callback_group_from_map( if (notify) { try { - interrupt_guard_condition_.trigger(); + interrupt_guard_condition_->trigger(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( std::string( @@ -387,7 +384,7 @@ Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node } } - memory_strategy_->remove_guard_condition(&node_ptr->get_notify_guard_condition()); + memory_strategy_->remove_guard_condition(node_ptr->get_notify_guard_condition().get()); weak_nodes_to_guard_conditions_.erase(node_ptr); std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); @@ -500,7 +497,7 @@ Executor::cancel() { spinning.store(false); try { - interrupt_guard_condition_.trigger(); + interrupt_guard_condition_->trigger(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( std::string("Failed to trigger guard condition in cancel: ") + ex.what()); @@ -549,7 +546,7 @@ Executor::execute_any_executable(AnyExecutable & any_exec) // Wake the wait, because it may need to be recalculated or work that // was previously blocked is now available. try { - interrupt_guard_condition_.trigger(); + interrupt_guard_condition_->trigger(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( std::string( diff --git a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp index 11ff07a732..c0ad8a25a4 100644 --- a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp +++ b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp @@ -100,11 +100,12 @@ ExecutorNotifyWaitable::take_data() } void -ExecutorNotifyWaitable::add_guard_condition(rclcpp::GuardCondition::WeakPtr guard_condition) +ExecutorNotifyWaitable::add_guard_condition(rclcpp::GuardCondition::WeakPtr weak_guard_condition) { std::lock_guard lock(guard_condition_mutex_); - if (notify_guard_conditions_.count(guard_condition) == 0) { - notify_guard_conditions_.insert(guard_condition); + auto guard_condition = weak_guard_condition.lock(); + if (guard_condition && notify_guard_conditions_.count(weak_guard_condition) == 0) { + notify_guard_conditions_.insert(weak_guard_condition); } } diff --git a/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp index bf50e062f3..fc737562ab 100644 --- a/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp @@ -109,8 +109,7 @@ StaticExecutorEntitiesCollector::execute(std::shared_ptr & data) std::lock_guard guard{new_nodes_mutex_}; for (const auto & weak_node : new_nodes_) { if (auto node_ptr = weak_node.lock()) { - const auto & gc = node_ptr->get_notify_guard_condition(); - weak_nodes_to_guard_conditions_[node_ptr] = &gc; + weak_nodes_to_guard_conditions_[node_ptr] = node_ptr->get_notify_guard_condition().get(); } } new_nodes_.clear(); diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp index 209fcde556..3c14b37b45 100644 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp @@ -139,7 +139,7 @@ StaticSingleThreadedExecutor::add_callback_group( bool is_new_node = entities_collector_->add_callback_group(group_ptr, node_ptr); if (is_new_node && notify) { // Interrupt waiting to handle new node - interrupt_guard_condition_.trigger(); + interrupt_guard_condition_->trigger(); } } @@ -150,7 +150,7 @@ StaticSingleThreadedExecutor::add_node( bool is_new_node = entities_collector_->add_node(node_ptr); if (is_new_node && notify) { // Interrupt waiting to handle new node - interrupt_guard_condition_.trigger(); + interrupt_guard_condition_->trigger(); } } @@ -167,7 +167,7 @@ StaticSingleThreadedExecutor::remove_callback_group( bool node_removed = entities_collector_->remove_callback_group(group_ptr); // If the node was matched and removed, interrupt waiting if (node_removed && notify) { - interrupt_guard_condition_.trigger(); + interrupt_guard_condition_->trigger(); } } @@ -181,7 +181,7 @@ StaticSingleThreadedExecutor::remove_node( } // If the node was matched and removed, interrupt waiting if (notify) { - interrupt_guard_condition_.trigger(); + interrupt_guard_condition_->trigger(); } } From 6267741212c2370cf2f109ea692ab7458f2b2097 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Mon, 3 Apr 2023 18:20:10 +0000 Subject: [PATCH 11/22] 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_); From 1b1a9154d5b3f951bc9f8e5d284cc6bf495925e1 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Mon, 3 Apr 2023 19:30:12 +0000 Subject: [PATCH 12/22] Don't exchange atomic twice Signed-off-by: Michael Carroll --- rclcpp/src/rclcpp/executors/executor_entities_collector.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp index fc9af38dc2..e086ec5f7c 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp @@ -261,10 +261,6 @@ 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)) { - throw std::runtime_error("Callback group has already been added to an executor."); - } auto iter = collection.insert(group_ptr); if (iter.second == false) { throw std::runtime_error("Callback group has already been added to this executor."); From 0a9c9a6403efa6271401e6440b6b425c538e1bda Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Mon, 3 Apr 2023 20:16:37 +0000 Subject: [PATCH 13/22] Fix add_node and add more tests Signed-off-by: Michael Carroll --- .../executors/executor_entities_collector.cpp | 14 +- .../executors/test_entities_collector.cpp | 166 +++++++++++++++--- 2 files changed, 156 insertions(+), 24 deletions(-) diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp index e086ec5f7c..acc79b0bbe 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp @@ -107,7 +107,9 @@ ExecutorEntitiesCollector::remove_node( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) { if (!node_ptr->get_associated_with_executor_atomic().load()) { - throw std::runtime_error("Node needs to be associated with an executor."); + throw std::runtime_error( + std::string("Node '") + node_ptr->get_fully_qualified_name() + + "' needs to be associated with an executor."); } std::lock_guard pending_lock(pending_mutex_); @@ -151,6 +153,10 @@ ExecutorEntitiesCollector::remove_callback_group(rclcpp::CallbackGroup::SharedPt throw std::runtime_error("Callback group needs to be associated with an executor."); } + if (!group_ptr->has_valid_node()) { + throw std::runtime_error("Node must not be deleted before its callback group(s)."); + } + auto weak_group_ptr = rclcpp::CallbackGroup::WeakPtr(group_ptr); std::lock_guard pending_lock(pending_mutex_); @@ -158,7 +164,7 @@ ExecutorEntitiesCollector::remove_callback_group(rclcpp::CallbackGroup::SharedPt bool add_queued = pending_manually_added_groups_.count(group_ptr) != 0; bool remove_queued = pending_manually_removed_groups_.count(group_ptr) != 0; - if ((associated || add_queued) && !remove_queued) { + if (!(associated || add_queued) || remove_queued) { throw std::runtime_error("Callback group needs to be associated with this executor."); } @@ -360,6 +366,10 @@ ExecutorEntitiesCollector::add_automatically_associated_callback_groups( if (!group_ptr->get_associated_with_executor_atomic().load() && group_ptr->automatically_add_to_executor_with_node()) { + std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic(); + if (has_executor.exchange(true)) { + throw std::runtime_error("Callback group has already been added to an executor."); + } this->add_callback_group_to_collection(group_ptr, this->automatically_added_groups_); } }); diff --git a/rclcpp/test/rclcpp/executors/test_entities_collector.cpp b/rclcpp/test/rclcpp/executors/test_entities_collector.cpp index 69414cf2db..7e9e060999 100644 --- a/rclcpp/test/rclcpp/executors/test_entities_collector.cpp +++ b/rclcpp/test/rclcpp/executors/test_entities_collector.cpp @@ -26,57 +26,94 @@ class TestExecutorEntitiesCollector : public ::testing::Test void SetUp() { rclcpp::init(0, nullptr); + + notify_waitable = std::make_shared(); + entities_collector = std::make_shared(notify_waitable); } void TearDown() { rclcpp::shutdown(); } + + std::shared_ptr notify_waitable; + std::shared_ptr entities_collector; }; TEST_F(TestExecutorEntitiesCollector, add_remove_node) { - auto notify_waitable = std::make_shared(); - auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(notify_waitable); auto node1 = std::make_shared("node1", "ns"); - auto node2 = std::make_shared("node2", "ns"); // Add a node - EXPECT_NO_THROW(entities_collector.add_node(node1->get_node_base_interface())); + EXPECT_NO_THROW(entities_collector->add_node(node1->get_node_base_interface())); + EXPECT_NO_THROW(entities_collector->update_collections()); + + // Remove a node + EXPECT_NO_THROW(entities_collector->remove_node(node1->get_node_base_interface())); + EXPECT_NO_THROW(entities_collector->update_collections()); +} + +TEST_F(TestExecutorEntitiesCollector, add_node_twice) { + + auto node1 = std::make_shared("node1", "ns"); + + EXPECT_NO_THROW(entities_collector->add_node(node1->get_node_base_interface())); - // Add the same node a second time RCLCPP_EXPECT_THROW_EQ( - entities_collector.add_node(node1->get_node_base_interface()), + entities_collector->add_node(node1->get_node_base_interface()), std::runtime_error("Node '/ns/node1' has already been added to an executor.")); - // Remove a node before adding - RCLCPP_EXPECT_THROW_EQ( - entities_collector.remove_node(node2->get_node_base_interface()), - std::runtime_error("Node needs to be associated with an executor.")); + EXPECT_NO_THROW(entities_collector->update_collections()); +} + +TEST_F(TestExecutorEntitiesCollector, add_associated_node) { + + auto node1 = std::make_shared("node1", "ns"); // Simulate node being associated somewhere else - auto & has_executor = node2->get_node_base_interface()->get_associated_with_executor_atomic(); + auto & has_executor = node1->get_node_base_interface()->get_associated_with_executor_atomic(); has_executor.store(true); // Add an already-associated node RCLCPP_EXPECT_THROW_EQ( - entities_collector.remove_node(node2->get_node_base_interface()), - std::runtime_error("Node needs to be associated with this executor.")); + entities_collector->remove_node(node1->get_node_base_interface()), + std::runtime_error("Node '/ns/node1' needs to be associated with this executor.")); has_executor.store(false); +} - // Add the now-disassociated node - EXPECT_NO_THROW(entities_collector.add_node(node2->get_node_base_interface())); +TEST_F(TestExecutorEntitiesCollector, remove_unassociated_node) { - // Remove an existing node - EXPECT_NO_THROW(entities_collector.remove_node(node1->get_node_base_interface())); - // Remove the same node a second time + auto node1 = std::make_shared("node1", "ns"); + + // Add an already-associated node RCLCPP_EXPECT_THROW_EQ( - entities_collector.remove_node(node1->get_node_base_interface()), - std::runtime_error("Node needs to be associated with an executor.")); + entities_collector->remove_node(node1->get_node_base_interface()), + std::runtime_error("Node '/ns/node1' needs to be associated with an executor.")); + + // Simulate node being associated somewhere else + auto & has_executor = node1->get_node_base_interface()->get_associated_with_executor_atomic(); + has_executor.store(true); - // Remove an existing node + // Add an already-associated node + RCLCPP_EXPECT_THROW_EQ( + entities_collector->remove_node(node1->get_node_base_interface()), + std::runtime_error("Node '/ns/node1' needs to be associated with this executor.")); +} + +TEST_F(TestExecutorEntitiesCollector, add_remove_node_before_update) { + auto notify_waitable = std::make_shared(); + auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(notify_waitable); + + auto node1 = std::make_shared("node1", "ns"); + auto node2 = std::make_shared("node2", "ns"); + + // Add and remove nodes without running updatenode + EXPECT_NO_THROW(entities_collector.add_node(node1->get_node_base_interface())); + EXPECT_NO_THROW(entities_collector.add_node(node2->get_node_base_interface())); + EXPECT_NO_THROW(entities_collector.remove_node(node1->get_node_base_interface())); EXPECT_NO_THROW(entities_collector.remove_node(node2->get_node_base_interface())); + EXPECT_NO_THROW(entities_collector.update_collections()); } TEST_F(TestExecutorEntitiesCollector, add_callback_group) { @@ -87,10 +124,31 @@ TEST_F(TestExecutorEntitiesCollector, add_callback_group) { rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive); + // Add a callback group and update entities_collector.add_callback_group(cb_group); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); + + entities_collector.update_collections(); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 1u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 1u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); + + // Remove callback group and update + entities_collector.remove_callback_group(cb_group); + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 1u); ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 1u); ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); + + entities_collector.update_collections(); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); } TEST_F(TestExecutorEntitiesCollector, add_node_default_callback_group) { @@ -100,6 +158,12 @@ TEST_F(TestExecutorEntitiesCollector, add_node_default_callback_group) { auto node = std::make_shared("node1", "ns"); entities_collector.add_node(node->get_node_base_interface()); + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); + + entities_collector.update_collections(); + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 1u); ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 0u); ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 1u); @@ -114,6 +178,17 @@ TEST_F(TestExecutorEntitiesCollector, add_callback_group_after_add_node) { rclcpp::CallbackGroupType::MutuallyExclusive); entities_collector.add_node(node->get_node_base_interface()); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); + + entities_collector.update_collections(); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 2u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 2u); + RCLCPP_EXPECT_THROW_EQ( entities_collector.add_callback_group(cb_group), std::runtime_error("Callback group has already been added to an executor.")); @@ -128,6 +203,13 @@ TEST_F(TestExecutorEntitiesCollector, add_callback_group_twice) { rclcpp::CallbackGroupType::MutuallyExclusive); entities_collector.add_callback_group(cb_group); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); + + entities_collector.update_collections(); + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 1u); ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 1u); ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); @@ -147,6 +229,13 @@ TEST_F(TestExecutorEntitiesCollector, remove_callback_group_after_node) { rclcpp::CallbackGroupType::MutuallyExclusive); entities_collector.add_callback_group(cb_group); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); + + entities_collector.update_collections(); + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 1u); ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 1u); ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); @@ -160,6 +249,36 @@ TEST_F(TestExecutorEntitiesCollector, remove_callback_group_after_node) { std::runtime_error("Node must not be deleted before its callback group(s).")); } +TEST_F(TestExecutorEntitiesCollector, remove_callback_group_after_node2) { + auto notify_waitable = std::make_shared(); + auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(notify_waitable); + + auto node = std::make_shared("node1", "ns"); + rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + entities_collector.add_callback_group(cb_group); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); + + entities_collector.update_collections(); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 1u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 1u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); + + EXPECT_NO_THROW(entities_collector.remove_callback_group(cb_group)); + + node.reset(); + ASSERT_FALSE(cb_group->has_valid_node()); + + RCLCPP_EXPECT_THROW_EQ( + entities_collector.update_collections(), + std::runtime_error("Node must not be deleted before its callback group(s).")); +} + TEST_F(TestExecutorEntitiesCollector, remove_callback_group_twice) { auto notify_waitable = std::make_shared(); auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(notify_waitable); @@ -169,15 +288,18 @@ TEST_F(TestExecutorEntitiesCollector, remove_callback_group_twice) { rclcpp::CallbackGroupType::MutuallyExclusive); entities_collector.add_callback_group(cb_group); + entities_collector.update_collections(); + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 1u); ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 1u); ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); entities_collector.remove_callback_group(cb_group); + entities_collector.update_collections(); RCLCPP_EXPECT_THROW_EQ( entities_collector.remove_callback_group(cb_group), - std::runtime_error("Callback group needs to be associated with executor.")); + std::runtime_error("Callback group needs to be associated with an executor.")); } TEST_F(TestExecutorEntitiesCollector, remove_node_opposite_order) { From 0ae0bea1faa6640a659cdc03ffb0022d49d1ba10 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Mon, 3 Apr 2023 20:40:18 +0000 Subject: [PATCH 14/22] Make get_notify_guard_condition follow API tick-tock Signed-off-by: Michael Carroll --- .../rclcpp/node_interfaces/node_base.hpp | 7 ++++++- .../node_interfaces/node_base_interface.hpp | 19 +++++++++++++++++-- rclcpp/src/rclcpp/executor.cpp | 4 ++-- .../executors/executor_entities_collector.cpp | 2 +- .../static_executor_entities_collector.cpp | 3 ++- .../src/rclcpp/node_interfaces/node_base.cpp | 12 +++++++++++- .../executors/test_entities_collector.cpp | 7 ++----- .../test_executor_notify_waitable.cpp | 6 ++++-- 8 files changed, 45 insertions(+), 15 deletions(-) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp index 6131681b80..6173a08d50 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp @@ -121,10 +121,15 @@ class NodeBase : public NodeBaseInterface, public std::enable_shared_from_thisget_notify_guard_condition(); + const auto gc = node_ptr->get_shared_notify_guard_condition(); weak_nodes_to_guard_conditions_[node_ptr] = gc.get(); // Add the node's notify condition to the guard condition handles memory_strategy_->add_guard_condition(*gc); @@ -384,7 +384,7 @@ Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node } } - memory_strategy_->remove_guard_condition(node_ptr->get_notify_guard_condition().get()); + memory_strategy_->remove_guard_condition(node_ptr->get_shared_notify_guard_condition().get()); weak_nodes_to_guard_conditions_.erase(node_ptr); std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp index acc79b0bbe..ee9b621402 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp @@ -306,7 +306,7 @@ ExecutorEntitiesCollector::process_queues() this->add_automatically_associated_callback_groups({weak_node_ptr}); // Store node guard condition in map and add it to the notify waitable - auto node_guard_condition = node_ptr->get_notify_guard_condition(); + auto node_guard_condition = node_ptr->get_shared_notify_guard_condition(); weak_nodes_to_guard_conditions_.insert({weak_node_ptr, node_guard_condition}); this->notify_waitable_->add_guard_condition(node_guard_condition); } diff --git a/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp index fc737562ab..6fd0b56a85 100644 --- a/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp @@ -109,7 +109,8 @@ StaticExecutorEntitiesCollector::execute(std::shared_ptr & data) std::lock_guard guard{new_nodes_mutex_}; for (const auto & weak_node : new_nodes_) { if (auto node_ptr = weak_node.lock()) { - weak_nodes_to_guard_conditions_[node_ptr] = node_ptr->get_notify_guard_condition().get(); + weak_nodes_to_guard_conditions_[node_ptr] = + node_ptr->get_shared_notify_guard_condition().get(); } } new_nodes_.clear(); diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index 2b08b746b6..4575d705a8 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -254,13 +254,23 @@ NodeBase::get_associated_with_executor_atomic() return associated_with_executor_; } -rclcpp::GuardCondition::SharedPtr +rclcpp::GuardCondition & NodeBase::get_notify_guard_condition() { std::lock_guard notify_condition_lock(notify_guard_condition_mutex_); if (!notify_guard_condition_is_valid_) { throw std::runtime_error("failed to get notify guard condition because it is invalid"); } + return *notify_guard_condition_; +} + +rclcpp::GuardCondition::SharedPtr +NodeBase::get_shared_notify_guard_condition() +{ + std::lock_guard notify_condition_lock(notify_guard_condition_mutex_); + if (!notify_guard_condition_is_valid_) { + return nullptr; + } return notify_guard_condition_; } diff --git a/rclcpp/test/rclcpp/executors/test_entities_collector.cpp b/rclcpp/test/rclcpp/executors/test_entities_collector.cpp index 7e9e060999..1f54159896 100644 --- a/rclcpp/test/rclcpp/executors/test_entities_collector.cpp +++ b/rclcpp/test/rclcpp/executors/test_entities_collector.cpp @@ -28,7 +28,8 @@ class TestExecutorEntitiesCollector : public ::testing::Test rclcpp::init(0, nullptr); notify_waitable = std::make_shared(); - entities_collector = std::make_shared(notify_waitable); + entities_collector = std::make_shared( + notify_waitable); } void TearDown() @@ -41,7 +42,6 @@ class TestExecutorEntitiesCollector : public ::testing::Test }; TEST_F(TestExecutorEntitiesCollector, add_remove_node) { - auto node1 = std::make_shared("node1", "ns"); // Add a node @@ -54,7 +54,6 @@ TEST_F(TestExecutorEntitiesCollector, add_remove_node) { } TEST_F(TestExecutorEntitiesCollector, add_node_twice) { - auto node1 = std::make_shared("node1", "ns"); EXPECT_NO_THROW(entities_collector->add_node(node1->get_node_base_interface())); @@ -67,7 +66,6 @@ TEST_F(TestExecutorEntitiesCollector, add_node_twice) { } TEST_F(TestExecutorEntitiesCollector, add_associated_node) { - auto node1 = std::make_shared("node1", "ns"); // Simulate node being associated somewhere else @@ -83,7 +81,6 @@ TEST_F(TestExecutorEntitiesCollector, add_associated_node) { } TEST_F(TestExecutorEntitiesCollector, remove_unassociated_node) { - auto node1 = std::make_shared("node1", "ns"); // Add an already-associated node diff --git a/rclcpp/test/rclcpp/executors/test_executor_notify_waitable.cpp b/rclcpp/test/rclcpp/executors/test_executor_notify_waitable.cpp index a286de5fa8..ab7f730a2e 100644 --- a/rclcpp/test/rclcpp/executors/test_executor_notify_waitable.cpp +++ b/rclcpp/test/rclcpp/executors/test_executor_notify_waitable.cpp @@ -61,8 +61,9 @@ TEST_F(TestExecutorNotifyWaitable, add_remove_guard_conditions) { std::make_shared(on_execute_callback); auto node = std::make_shared("my_node", "/ns"); - auto notify_guard_condition = node->get_node_base_interface()->get_notify_guard_condition(); + auto notify_guard_condition = + node->get_node_base_interface()->get_shared_notify_guard_condition(); EXPECT_NO_THROW(waitable->add_guard_condition(notify_guard_condition)); EXPECT_NO_THROW(waitable->remove_guard_condition(notify_guard_condition)); } @@ -75,7 +76,8 @@ TEST_F(TestExecutorNotifyWaitable, wait) { std::make_shared(on_execute_callback); auto node = std::make_shared("my_node", "/ns"); - auto notify_guard_condition = node->get_node_base_interface()->get_notify_guard_condition(); + auto notify_guard_condition = + node->get_node_base_interface()->get_shared_notify_guard_condition(); EXPECT_NO_THROW(waitable->add_guard_condition(notify_guard_condition)); auto default_cbg = node->get_node_base_interface()->get_default_callback_group(); From 87f41bff1d15720345785bbf1a1629305b197f3e Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Mon, 3 Apr 2023 20:44:42 +0000 Subject: [PATCH 15/22] Improve callback group tick-tocking Signed-off-by: Michael Carroll --- rclcpp/include/rclcpp/callback_group.hpp | 42 +++++++++++++++++++++++- rclcpp/src/rclcpp/callback_group.cpp | 9 ++--- 2 files changed, 46 insertions(+), 5 deletions(-) diff --git a/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp index 279c642c0e..bb87aa7df6 100644 --- a/rclcpp/include/rclcpp/callback_group.hpp +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -59,6 +59,46 @@ class CallbackGroup public: RCLCPP_SMART_PTR_DEFINITIONS(CallbackGroup) + /// Constructor for CallbackGroup. + /** + * Callback Groups have a type, either 'Mutually Exclusive' or 'Reentrant' + * and when creating one the type must be specified. + * + * Callbacks in Reentrant Callback Groups must be able to: + * - run at the same time as themselves (reentrant) + * - run at the same time as other callbacks in their group + * - run at the same time as other callbacks in other groups + * + * Callbacks in Mutually Exclusive Callback Groups: + * - will not be run multiple times simultaneously (non-reentrant) + * - will not be run at the same time as other callbacks in their group + * - but must run at the same time as callbacks in other groups + * + * Additionally, callback groups have a property which determines whether or + * not they are added to an executor with their associated node automatically. + * When creating a callback group the automatically_add_to_executor_with_node + * argument determines this behavior, and if true it will cause the newly + * created callback group to be added to an executor with the node when the + * Executor::add_node method is used. + * If false, this callback group will not be added automatically and would + * have to be added to an executor manually using the + * Executor::add_callback_group method. + * + * Whether the node was added to the executor before creating the callback + * group, or after, is irrelevant; the callback group will be automatically + * added to the executor in either case. + * + * \param[in] group_type The type of the callback group. + * \param[in] automatically_add_to_executor_with_node A boolean that + * determines whether a callback group is automatically added to an executor + * with the node with which it is associated. + */ + [[deprecated("Use CallbackGroup constructor with context function argument")]] + RCLCPP_PUBLIC + explicit CallbackGroup( + CallbackGroupType group_type, + bool automatically_add_to_executor_with_node = true); + /// Constructor for CallbackGroup. /** * Callback Groups have a type, either 'Mutually Exclusive' or 'Reentrant' @@ -143,7 +183,7 @@ class CallbackGroup /// Return if the node that created this callback group still exists /** * As nodes can share ownership of callback groups with an executor, this - * may be used to ensures that the executor doesn't operate on a callback + * may be used to ensure that the executor doesn't operate on a callback * group that has outlived it's creating node. * * \return true if the creating node still exists, otherwise false diff --git a/rclcpp/src/rclcpp/callback_group.cpp b/rclcpp/src/rclcpp/callback_group.cpp index f8c7bfc35f..23a9803b10 100644 --- a/rclcpp/src/rclcpp/callback_group.cpp +++ b/rclcpp/src/rclcpp/callback_group.cpp @@ -142,6 +142,11 @@ rclcpp::GuardCondition::SharedPtr CallbackGroup::get_notify_guard_condition() { std::lock_guard lock(notify_guard_condition_mutex_); + + if (!this->get_context_) { + throw std::runtime_error("Callback group was created without context and not passed context"); + } + auto context_ptr = this->get_context_(); if (context_ptr && context_ptr->is_valid()) { std::lock_guard lock(notify_guard_condition_mutex_); @@ -151,14 +156,10 @@ CallbackGroup::get_notify_guard_condition() } notify_guard_condition_ = nullptr; } - if (!notify_guard_condition_) { notify_guard_condition_ = std::make_shared(context_ptr); } - return notify_guard_condition_; - } else { - throw std::runtime_error("Couldn't get guard condition from invalid context"); } return nullptr; } From 58093288f8dbd14290342568653237186c95e519 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Mon, 3 Apr 2023 20:47:38 +0000 Subject: [PATCH 16/22] Don't lock twice Signed-off-by: Michael Carroll --- rclcpp/src/rclcpp/callback_group.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/rclcpp/src/rclcpp/callback_group.cpp b/rclcpp/src/rclcpp/callback_group.cpp index 23a9803b10..3569a6bada 100644 --- a/rclcpp/src/rclcpp/callback_group.cpp +++ b/rclcpp/src/rclcpp/callback_group.cpp @@ -146,10 +146,8 @@ CallbackGroup::get_notify_guard_condition() if (!this->get_context_) { throw std::runtime_error("Callback group was created without context and not passed context"); } - auto context_ptr = this->get_context_(); if (context_ptr && context_ptr->is_valid()) { - std::lock_guard lock(notify_guard_condition_mutex_); if (notify_guard_condition_ && context_ptr != notify_guard_condition_->get_context()) { if (associated_with_executor_) { trigger_notify_guard_condition(); From debe396b71850b3783c024be95fe50214e4bc8c0 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 4 Apr 2023 13:13:14 +0000 Subject: [PATCH 17/22] Address reviewer feedback Signed-off-by: Michael Carroll --- .../executor_entities_collection.hpp | 9 ++-- .../executors/executor_entities_collector.hpp | 48 ++++++++----------- .../executors/executor_notify_waitable.hpp | 3 +- .../executor_entities_collection.cpp | 31 ++++++------ .../executors/executor_entities_collector.cpp | 9 ++-- 5 files changed, 48 insertions(+), 52 deletions(-) diff --git a/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp b/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp index e808fd3760..98a92ccdd8 100644 --- a/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp +++ b/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp @@ -16,6 +16,7 @@ #define RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTION_HPP_ #include +#include #include #include @@ -67,8 +68,8 @@ template void update_entities( const CollectionType & update_from, CollectionType & update_to, - std::function on_added, - std::function on_removed + std::function on_added, + std::function on_removed ) { for (auto it = update_to.begin(); it != update_to.end(); ) { @@ -119,8 +120,8 @@ class EntityCollection */ void update( const EntityCollection & other, - std::function on_added, - std::function on_removed) + std::function on_added, + std::function on_removed) { update_entities(other, *this, on_added, on_removed); } diff --git a/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp b/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp index 36fe9d5906..2428677492 100644 --- a/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp +++ b/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp @@ -15,9 +15,9 @@ #ifndef RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTOR_HPP_ #define RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTOR_HPP_ -#include #include #include +#include #include #include @@ -58,9 +58,8 @@ class ExecutorEntitiesCollector public: /// Constructor /** - * \param[in] on_notify_waitable Callback to execute when one of the associated - * nodes or callback groups trigger their guard condition, indicating that their - * corresponding entities have changed. + * \param[in] notify_waitable Waitable that is used to signal to the executor + * when nodes or callback groups have been added or removed. */ RCLCPP_PUBLIC explicit ExecutorEntitiesCollector( @@ -70,7 +69,11 @@ class ExecutorEntitiesCollector RCLCPP_PUBLIC ~ExecutorEntitiesCollector(); - bool has_pending(); + /// Indicate if the entities collector has pending additions or removals. + /** + * \return true if there are pending additions or removals + */ + bool has_pending() const; /// Add a node to the entity collector /** @@ -91,15 +94,6 @@ class ExecutorEntitiesCollector void remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr); - /// Check if a node is associated with this executor/collector. - /** - * \param[in] node_ptr a shared pointer that points to a node base interface - * \return true if the node is tracked in this collector, false otherwise - */ - RCLCPP_PUBLIC - bool - has_node(const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr); - /// Add a callback group to the entity collector /** * \param[in] group_ptr a shared pointer that points to a callback group @@ -126,7 +120,7 @@ class ExecutorEntitiesCollector */ RCLCPP_PUBLIC std::vector - get_all_callback_groups(); + get_all_callback_groups() const; /// Get manually-added callback groups known to this entity collector /** @@ -135,7 +129,7 @@ class ExecutorEntitiesCollector */ RCLCPP_PUBLIC std::vector - get_manually_added_callback_groups(); + get_manually_added_callback_groups() const; /// Get automatically-added callback groups known to this entity collector /** @@ -144,12 +138,12 @@ class ExecutorEntitiesCollector */ RCLCPP_PUBLIC std::vector - get_automatically_added_callback_groups(); + get_automatically_added_callback_groups() const; /// Update the underlying collections /** * This will prune nodes and callback groups that are no longer valid as well - * as adding new callback groups from any associated nodes. + * as add new callback groups from any associated nodes. */ RCLCPP_PUBLIC void @@ -190,7 +184,7 @@ class ExecutorEntitiesCollector NodeCollection::iterator remove_weak_node(NodeCollection::iterator weak_node); - /// Implementation of removing a callback gruop from the collector. + /// Implementation of removing a callback group from the collector. /** * This will disassociate the callback group from the collector * @@ -248,6 +242,9 @@ class ExecutorEntitiesCollector void prune_invalid_nodes_and_groups(); + /// mutex to protect pending queues + mutable std::mutex pending_mutex_; + /// Callback groups that were added via `add_callback_group` CallbackGroupCollection manually_added_groups_; @@ -257,8 +254,11 @@ class ExecutorEntitiesCollector /// nodes that are associated with the executor NodeCollection weak_nodes_; - /// mutex to protect pending queues - std::mutex pending_mutex_; + /// 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_; /// nodes that have been added since the last update. NodeCollection pending_added_nodes_; @@ -272,12 +272,6 @@ class ExecutorEntitiesCollector /// 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_; }; diff --git a/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp b/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp index aac0820dbe..9fb44e78c6 100644 --- a/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp +++ b/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp @@ -15,8 +15,9 @@ #ifndef RCLCPP__EXECUTORS__EXECUTOR_NOTIFY_WAITABLE_HPP_ #define RCLCPP__EXECUTORS__EXECUTOR_NOTIFY_WAITABLE_HPP_ -#include +#include #include +#include #include #include "rclcpp/guard_condition.hpp" diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp index 2dce369b08..567b28014e 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp @@ -104,24 +104,23 @@ void check_ready( 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(); - if (!entity) { - continue; - } + if (!waited_entities[ii]) {continue;} + auto entity_iter = collection.find(waited_entities[ii]); + if (entity_iter != collection.end()) { + auto entity = entity_iter->second.entity.lock(); + 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; + 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); - } + exec.callback_group = callback_group; + if (fill_executable(exec, entity)) { + executables.push_back(exec); } } } diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp index ee9b621402..fe259abc57 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp @@ -68,7 +68,8 @@ ExecutorEntitiesCollector::~ExecutorEntitiesCollector() pending_manually_removed_groups_.clear(); } -bool ExecutorEntitiesCollector::has_pending() +bool +ExecutorEntitiesCollector::has_pending() const { std::lock_guard pending_lock(pending_mutex_); return pending_manually_added_groups_.size() != 0 || @@ -172,7 +173,7 @@ ExecutorEntitiesCollector::remove_callback_group(rclcpp::CallbackGroup::SharedPt } std::vector -ExecutorEntitiesCollector::get_all_callback_groups() +ExecutorEntitiesCollector::get_all_callback_groups() const { std::vector groups; @@ -186,7 +187,7 @@ ExecutorEntitiesCollector::get_all_callback_groups() } std::vector -ExecutorEntitiesCollector::get_manually_added_callback_groups() +ExecutorEntitiesCollector::get_manually_added_callback_groups() const { std::vector groups; for (const auto & group_ptr : manually_added_groups_) { @@ -196,7 +197,7 @@ ExecutorEntitiesCollector::get_manually_added_callback_groups() } std::vector -ExecutorEntitiesCollector::get_automatically_added_callback_groups() +ExecutorEntitiesCollector::get_automatically_added_callback_groups() const { std::vector groups; for (auto const & group_ptr : automatically_added_groups_) { From c4b658935fccaa5e2ddd01da2aa2936e5c2cd29e Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 4 Apr 2023 14:11:15 +0000 Subject: [PATCH 18/22] Add thread safety annotations and make locks consistent Signed-off-by: Michael Carroll --- .../executors/executor_entities_collector.hpp | 45 +++++++------------ .../executors/executor_entities_collector.cpp | 41 +++++++---------- 2 files changed, 33 insertions(+), 53 deletions(-) diff --git a/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp b/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp index 2428677492..ad9bc84fad 100644 --- a/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp +++ b/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp @@ -182,7 +182,7 @@ class ExecutorEntitiesCollector */ RCLCPP_PUBLIC NodeCollection::iterator - remove_weak_node(NodeCollection::iterator weak_node); + remove_weak_node(NodeCollection::iterator weak_node) RCPPUTILS_TSA_REQUIRES(mutex_); /// Implementation of removing a callback group from the collector. /** @@ -201,7 +201,7 @@ class ExecutorEntitiesCollector CallbackGroupCollection::iterator remove_weak_callback_group( CallbackGroupCollection::iterator weak_group_it, - CallbackGroupCollection & collection); + CallbackGroupCollection & collection) RCPPUTILS_TSA_REQUIRES(mutex_); /// Implementation of adding a callback group /** @@ -212,65 +212,54 @@ class ExecutorEntitiesCollector 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); + CallbackGroupCollection & collection) RCPPUTILS_TSA_REQUIRES(mutex_); /// Iterate over queued added/remove nodes and callback_groups RCLCPP_PUBLIC void - process_queues(); + process_queues() RCPPUTILS_TSA_REQUIRES(mutex_); /// 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); + const NodeCollection & nodes_to_check) RCPPUTILS_TSA_REQUIRES(mutex_); /// Check all nodes and group for expired weak pointers and remove them. RCLCPP_PUBLIC void - prune_invalid_nodes_and_groups(); + prune_invalid_nodes_and_groups() RCPPUTILS_TSA_REQUIRES(mutex_); - /// mutex to protect pending queues - mutable std::mutex pending_mutex_; + /// mutex to protect collections and pending queues + mutable std::mutex mutex_; /// Callback groups that were added via `add_callback_group` - CallbackGroupCollection manually_added_groups_; + CallbackGroupCollection manually_added_groups_ RCPPUTILS_TSA_GUARDED_BY(mutex_); /// Callback groups that were added by their association with added nodes - CallbackGroupCollection automatically_added_groups_; + CallbackGroupCollection automatically_added_groups_ RCPPUTILS_TSA_GUARDED_BY(mutex_); /// nodes that are associated with the executor - NodeCollection weak_nodes_; + NodeCollection weak_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_); /// Track guard conditions associated with added nodes - WeakNodesToGuardConditionsMap weak_nodes_to_guard_conditions_; + WeakNodesToGuardConditionsMap weak_nodes_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_); /// Track guard conditions associated with added callback groups - WeakGroupsToGuardConditionsMap weak_groups_to_guard_conditions_; + WeakGroupsToGuardConditionsMap weak_groups_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_); /// nodes that have been added since the last update. - NodeCollection pending_added_nodes_; + NodeCollection pending_added_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_); /// nodes that have been removed since the last update. - NodeCollection pending_removed_nodes_; + NodeCollection pending_removed_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_); /// callback groups that have been added since the last update. - CallbackGroupCollection pending_manually_added_groups_; + CallbackGroupCollection pending_manually_added_groups_ RCPPUTILS_TSA_GUARDED_BY(mutex_); /// callback groups that have been removed since the last update. - CallbackGroupCollection pending_manually_removed_groups_; + CallbackGroupCollection pending_manually_removed_groups_ RCPPUTILS_TSA_GUARDED_BY(mutex_); /// Waitable to add guard conditions to std::shared_ptr notify_waitable_; diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp index fe259abc57..d4261b1083 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp @@ -47,8 +47,6 @@ ExecutorEntitiesCollector::~ExecutorEntitiesCollector() weak_group_it = remove_weak_callback_group(weak_group_it, manually_added_groups_); } - std::lock_guard pending_lock(pending_mutex_); - for (auto weak_node_ptr : pending_added_nodes_) { auto node_ptr = weak_node_ptr.lock(); if (node_ptr) { @@ -71,7 +69,7 @@ ExecutorEntitiesCollector::~ExecutorEntitiesCollector() bool ExecutorEntitiesCollector::has_pending() const { - std::lock_guard pending_lock(pending_mutex_); + std::lock_guard lock(mutex_); return pending_manually_added_groups_.size() != 0 || pending_manually_removed_groups_.size() != 0 || pending_added_nodes_.size() != 0 || @@ -89,7 +87,7 @@ ExecutorEntitiesCollector::add_node(rclcpp::node_interfaces::NodeBaseInterface:: "' has already been added to an executor."); } - std::lock_guard pending_lock(pending_mutex_); + std::lock_guard lock(mutex_); bool associated = weak_nodes_.count(node_ptr) != 0; bool add_queued = pending_added_nodes_.count(node_ptr) != 0; bool remove_queued = pending_removed_nodes_.count(node_ptr) != 0; @@ -113,7 +111,7 @@ ExecutorEntitiesCollector::remove_node( "' needs to be associated with an executor."); } - std::lock_guard pending_lock(pending_mutex_); + std::lock_guard lock(mutex_); bool associated = weak_nodes_.count(node_ptr) != 0; bool add_queued = pending_added_nodes_.count(node_ptr) != 0; bool remove_queued = pending_removed_nodes_.count(node_ptr) != 0; @@ -135,7 +133,7 @@ ExecutorEntitiesCollector::add_callback_group(rclcpp::CallbackGroup::SharedPtr g throw std::runtime_error("Callback group has already been added to an executor."); } - std::lock_guard pending_lock(pending_mutex_); + std::lock_guard lock(mutex_); bool associated = manually_added_groups_.count(group_ptr) != 0; bool add_queued = pending_manually_added_groups_.count(group_ptr) != 0; bool remove_queued = pending_manually_removed_groups_.count(group_ptr) != 0; @@ -160,7 +158,7 @@ ExecutorEntitiesCollector::remove_callback_group(rclcpp::CallbackGroup::SharedPt auto weak_group_ptr = rclcpp::CallbackGroup::WeakPtr(group_ptr); - std::lock_guard pending_lock(pending_mutex_); + std::lock_guard lock(mutex_); bool associated = manually_added_groups_.count(group_ptr) != 0; bool add_queued = pending_manually_added_groups_.count(group_ptr) != 0; bool remove_queued = pending_manually_removed_groups_.count(group_ptr) != 0; @@ -176,7 +174,7 @@ std::vector ExecutorEntitiesCollector::get_all_callback_groups() const { std::vector groups; - + std::lock_guard lock(mutex_); for (const auto & group_ptr : manually_added_groups_) { groups.push_back(group_ptr); } @@ -190,6 +188,7 @@ std::vector ExecutorEntitiesCollector::get_manually_added_callback_groups() const { std::vector groups; + std::lock_guard lock(mutex_); for (const auto & group_ptr : manually_added_groups_) { groups.push_back(group_ptr); } @@ -200,6 +199,7 @@ std::vector ExecutorEntitiesCollector::get_automatically_added_callback_groups() const { std::vector groups; + std::lock_guard lock(mutex_); for (auto const & group_ptr : automatically_added_groups_) { groups.push_back(group_ptr); } @@ -209,6 +209,7 @@ ExecutorEntitiesCollector::get_automatically_added_callback_groups() const void ExecutorEntitiesCollector::update_collections() { + std::lock_guard lock(mutex_); this->process_queues(); this->add_automatically_associated_callback_groups(this->weak_nodes_); this->prune_invalid_nodes_and_groups(); @@ -279,25 +280,9 @@ ExecutorEntitiesCollector::add_callback_group_to_collection( this->notify_waitable_->add_guard_condition(group_guard_condition); } -void -ExecutorEntitiesCollector::remove_callback_group_from_collection( - rclcpp::CallbackGroup::SharedPtr group_ptr, - CallbackGroupCollection & collection) -{ - auto group_it = collection.find(group_ptr); - - if (group_it != collection.end()) { - remove_weak_callback_group(group_it, collection); - } else { - throw std::runtime_error("Attempting to remove a callback group not added to this executor."); - } -} - void ExecutorEntitiesCollector::process_queues() { - std::lock_guard pending_lock(pending_mutex_); - for (auto weak_node_ptr : pending_added_nodes_) { auto node_ptr = weak_node_ptr.lock(); if (!node_ptr) { @@ -348,7 +333,13 @@ ExecutorEntitiesCollector::process_queues() 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_); + auto group_it = manually_added_groups_.find(group_ptr); + if (group_it != manually_added_groups_.end()) { + remove_weak_callback_group(group_it, manually_added_groups_); + } else { + throw std::runtime_error( + "Attempting to remove a callback group not added to this executor."); + } } } pending_manually_removed_groups_.clear(); From cd7aaba5ca02d7e0c0e0b6768e2da39123251003 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 11 Apr 2023 10:15:43 -0500 Subject: [PATCH 19/22] Address reviewer feedback Signed-off-by: Michael Carroll --- rclcpp/include/rclcpp/callback_group.hpp | 2 +- rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp | 1 - rclcpp/src/rclcpp/callback_group.cpp | 2 +- rclcpp/src/rclcpp/executors/executor_entities_collector.cpp | 2 +- 4 files changed, 3 insertions(+), 4 deletions(-) diff --git a/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp index bb87aa7df6..e1a9594048 100644 --- a/rclcpp/include/rclcpp/callback_group.hpp +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -190,7 +190,7 @@ class CallbackGroup */ RCLCPP_PUBLIC bool - has_valid_node(); + has_valid_node() const; RCLCPP_PUBLIC std::atomic_bool & diff --git a/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp b/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp index 9fb44e78c6..88158952d9 100644 --- a/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp +++ b/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp @@ -35,7 +35,6 @@ class ExecutorNotifyWaitable : public rclcpp::Waitable public: RCLCPP_SMART_PTR_DEFINITIONS(ExecutorNotifyWaitable) - // Constructor /** * \param[in] on_execute_callback Callback to execute when one of the conditions diff --git a/rclcpp/src/rclcpp/callback_group.cpp b/rclcpp/src/rclcpp/callback_group.cpp index 3569a6bada..c337f57af5 100644 --- a/rclcpp/src/rclcpp/callback_group.cpp +++ b/rclcpp/src/rclcpp/callback_group.cpp @@ -45,7 +45,7 @@ CallbackGroup::~CallbackGroup() } bool -CallbackGroup::has_valid_node() +CallbackGroup::has_valid_node() const { return nullptr != this->get_context_(); } diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp index d4261b1083..bfb01ed2a6 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp @@ -338,7 +338,7 @@ ExecutorEntitiesCollector::process_queues() remove_weak_callback_group(group_it, manually_added_groups_); } else { throw std::runtime_error( - "Attempting to remove a callback group not added to this executor."); + "Attempting to remove a callback group not added to this executor."); } } } From 6379f0cfa0940a69a54f59652116f859f1e11a16 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 12 Apr 2023 18:11:19 -0500 Subject: [PATCH 20/22] Remove the "add_valid_node" API Signed-off-by: Michael Carroll --- rclcpp/include/rclcpp/callback_group.hpp | 12 ------- rclcpp/src/rclcpp/callback_group.cpp | 7 ----- .../executors/executor_entities_collector.cpp | 17 ++++++++-- .../src/rclcpp/node_interfaces/node_base.cpp | 31 ++++++++++--------- .../executors/test_entities_collector.cpp | 15 ++++++--- 5 files changed, 41 insertions(+), 41 deletions(-) diff --git a/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp index e1a9594048..97579fcf8c 100644 --- a/rclcpp/include/rclcpp/callback_group.hpp +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -180,18 +180,6 @@ class CallbackGroup return _find_ptrs_if_impl(func, waitable_ptrs_); } - /// Return if the node that created this callback group still exists - /** - * As nodes can share ownership of callback groups with an executor, this - * may be used to ensure that the executor doesn't operate on a callback - * group that has outlived it's creating node. - * - * \return true if the creating node still exists, otherwise false - */ - RCLCPP_PUBLIC - bool - has_valid_node() const; - RCLCPP_PUBLIC std::atomic_bool & can_be_taken_from(); diff --git a/rclcpp/src/rclcpp/callback_group.cpp b/rclcpp/src/rclcpp/callback_group.cpp index c337f57af5..753a441332 100644 --- a/rclcpp/src/rclcpp/callback_group.cpp +++ b/rclcpp/src/rclcpp/callback_group.cpp @@ -43,12 +43,6 @@ CallbackGroup::~CallbackGroup() { trigger_notify_guard_condition(); } -bool - -CallbackGroup::has_valid_node() const -{ - return nullptr != this->get_context_(); -} std::atomic_bool & CallbackGroup::can_be_taken_from() @@ -142,7 +136,6 @@ rclcpp::GuardCondition::SharedPtr CallbackGroup::get_notify_guard_condition() { std::lock_guard lock(notify_guard_condition_mutex_); - if (!this->get_context_) { throw std::runtime_error("Callback group was created without context and not passed context"); } diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp index bfb01ed2a6..84ada64925 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp @@ -151,13 +151,18 @@ ExecutorEntitiesCollector::remove_callback_group(rclcpp::CallbackGroup::SharedPt if (!group_ptr->get_associated_with_executor_atomic().load()) { throw std::runtime_error("Callback group needs to be associated with an executor."); } - + /** + * TODO(mjcarroll): The callback groups, being created by a node, should never outlive + * the node. Since we haven't historically enforced this, turning this on may cause + * previously-functional code to fail. + * Consider re-enablng this check (along with corresponding CallbackGroup::has_valid_node), + * when we can guarantee node/group lifetimes. if (!group_ptr->has_valid_node()) { throw std::runtime_error("Node must not be deleted before its callback group(s)."); } + */ auto weak_group_ptr = rclcpp::CallbackGroup::WeakPtr(group_ptr); - std::lock_guard lock(mutex_); bool associated = manually_added_groups_.count(group_ptr) != 0; bool add_queued = pending_manually_added_groups_.count(group_ptr) != 0; @@ -251,11 +256,17 @@ ExecutorEntitiesCollector::remove_weak_callback_group( // Mark the node as disassociated (if the group is still valid) auto group_ptr = weak_group_it->lock(); - if (group_ptr) { + /** + * TODO(mjcarroll): The callback groups, being created by a node, should never outlive + * the node. Since we haven't historically enforced this, turning this on may cause + * previously-functional code to fail. + * Consider re-enablng this check (along with corresponding CallbackGroup::has_valid_node), + * when we can guarantee node/group lifetimes. if (!group_ptr->has_valid_node()) { throw std::runtime_error("Node must not be deleted before its callback group(s)."); } + */ std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic(); has_executor.store(false); } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index 4575d705a8..cfc5adcfd8 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -129,6 +129,15 @@ NodeBase::NodeBase( delete node; }); + // Create the default callback group, if needed. + if (nullptr == default_callback_group_) { + using rclcpp::CallbackGroupType; + // Default callback group is mutually exclusive and automatically associated with + // any executors that this node is added to. + default_callback_group_ = + NodeBase::create_callback_group(CallbackGroupType::MutuallyExclusive, true); + } + // Indicate the notify_guard_condition is now valid. notify_guard_condition_is_valid_ = true; } @@ -195,16 +204,15 @@ NodeBase::create_callback_group( rclcpp::CallbackGroupType group_type, bool automatically_add_to_executor_with_node) { - auto weak_ptr = this->weak_from_this(); + auto weak_context = this->get_context()->weak_from_this(); + + auto get_node_context = [weak_context]() -> rclcpp::Context::SharedPtr { + return weak_context.lock(); + }; + auto group = std::make_shared( group_type, - [weak_ptr]() -> rclcpp::Context::SharedPtr { - auto node_ptr = weak_ptr.lock(); - if (node_ptr) { - return node_ptr->get_context(); - } - return nullptr; - }, + get_node_context, automatically_add_to_executor_with_node); std::lock_guard lock(callback_groups_mutex_); callback_groups_.push_back(group); @@ -214,13 +222,6 @@ NodeBase::create_callback_group( rclcpp::CallbackGroup::SharedPtr NodeBase::get_default_callback_group() { - // Create the default callback group, if needed. - if (nullptr == default_callback_group_) { - using rclcpp::CallbackGroupType; - default_callback_group_ = - NodeBase::create_callback_group(CallbackGroupType::MutuallyExclusive); - } - return default_callback_group_; } diff --git a/rclcpp/test/rclcpp/executors/test_entities_collector.cpp b/rclcpp/test/rclcpp/executors/test_entities_collector.cpp index 1f54159896..930dc68aff 100644 --- a/rclcpp/test/rclcpp/executors/test_entities_collector.cpp +++ b/rclcpp/test/rclcpp/executors/test_entities_collector.cpp @@ -239,11 +239,14 @@ TEST_F(TestExecutorEntitiesCollector, remove_callback_group_after_node) { node.reset(); - ASSERT_FALSE(cb_group->has_valid_node()); - + /** + * TODO(mjcarroll): Assert this when we are enforcing that nodes must be destroyed + * after their created callback groups. RCLCPP_EXPECT_THROW_EQ( entities_collector.remove_callback_group(cb_group), std::runtime_error("Node must not be deleted before its callback group(s).")); + */ + EXPECT_NO_THROW(entities_collector.update_collections()); } TEST_F(TestExecutorEntitiesCollector, remove_callback_group_after_node2) { @@ -269,11 +272,15 @@ TEST_F(TestExecutorEntitiesCollector, remove_callback_group_after_node2) { EXPECT_NO_THROW(entities_collector.remove_callback_group(cb_group)); node.reset(); - ASSERT_FALSE(cb_group->has_valid_node()); + /** + * TODO(mjcarroll): Assert this when we are enforcing that nodes must be destroyed + * after their created callback groups. RCLCPP_EXPECT_THROW_EQ( - entities_collector.update_collections(), + entities_collector.remove_callback_group(cb_group), std::runtime_error("Node must not be deleted before its callback group(s).")); + */ + EXPECT_NO_THROW(entities_collector.update_collections()); } TEST_F(TestExecutorEntitiesCollector, remove_callback_group_twice) { From 855c64dc3f83701cdedc24268f430857ffb8da07 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 12 Apr 2023 18:51:46 -0500 Subject: [PATCH 21/22] Only notify if the trigger condition is valid Signed-off-by: Michael Carroll --- rclcpp/src/rclcpp/node_interfaces/node_base.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index cfc5adcfd8..2af5bd7e55 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -216,6 +216,13 @@ NodeBase::create_callback_group( automatically_add_to_executor_with_node); std::lock_guard lock(callback_groups_mutex_); callback_groups_.push_back(group); + + // If this is creating the default callback group, then the + // notify guard condition won't be ready or needed yet. + if (notify_guard_condition_is_valid_) { + this->trigger_notify_guard_condition(); + } + return group; } From d9a92061c5f07e40a3d090f66e5ed8b02b5eb20a Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 12 Apr 2023 19:24:09 -0500 Subject: [PATCH 22/22] Only trigger if valid and needed Signed-off-by: Michael Carroll --- rclcpp/src/rclcpp/node_interfaces/node_base.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index 2af5bd7e55..6544d69214 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -205,7 +205,6 @@ NodeBase::create_callback_group( bool automatically_add_to_executor_with_node) { auto weak_context = this->get_context()->weak_from_this(); - auto get_node_context = [weak_context]() -> rclcpp::Context::SharedPtr { return weak_context.lock(); }; @@ -217,12 +216,15 @@ NodeBase::create_callback_group( std::lock_guard lock(callback_groups_mutex_); callback_groups_.push_back(group); - // If this is creating the default callback group, then the - // notify guard condition won't be ready or needed yet. - if (notify_guard_condition_is_valid_) { + // This guard condition is generally used to signal to this node's executor that a callback + // group has been added that should be considered for new entities. + // If this is creating the default callback group, then the notify guard condition won't be + // ready or needed yet, as the node is not done being constructed and therefore cannot be added. + // If the callback group is not automatically associated with this node's executors, then + // triggering the guard condition is also unnecessary, it will be manually added to an exector. + if (notify_guard_condition_is_valid_ && automatically_add_to_executor_with_node) { this->trigger_notify_guard_condition(); } - return group; }