From 2bf88de912e0bb3011cbccbc5c9f466f704f0b94 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 29 Mar 2023 15:06:17 +0000 Subject: [PATCH 01/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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/74] 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 974e8455824dd1b95a6478c950859549e231a7fd Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 29 Mar 2023 17:59:14 +0000 Subject: [PATCH 12/74] Utilize rclcpp::WaitSet as part of the executors Signed-off-by: Michael Carroll --- rclcpp/CMakeLists.txt | 2 - rclcpp/include/rclcpp/executor.hpp | 169 +---- rclcpp/include/rclcpp/executors.hpp | 1 - .../detail/storage_policy_common.hpp | 4 + .../thread_safe_synchronization.hpp | 3 + rclcpp/include/rclcpp/wait_set_template.hpp | 1 + rclcpp/src/rclcpp/executor.cpp | 663 +++++------------- .../executor_entities_collection.cpp | 1 + .../executors/executor_notify_waitable.cpp | 2 + rclcpp/test/benchmark/benchmark_executor.cpp | 127 ---- rclcpp/test/rclcpp/CMakeLists.txt | 17 - .../test/rclcpp/executors/test_executors.cpp | 10 +- .../test_add_callback_groups_to_executor.cpp | 10 +- rclcpp/test/rclcpp/test_executor.cpp | 109 +-- 14 files changed, 207 insertions(+), 912 deletions(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 3fba0d7c08..72fb24de3b 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -59,8 +59,6 @@ set(${PROJECT_NAME}_SRCS 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 - src/rclcpp/executors/static_single_threaded_executor.cpp src/rclcpp/expand_topic_or_service_name.cpp src/rclcpp/future_return_code.cpp src/rclcpp/generic_publisher.cpp diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 3e654faa54..115d755626 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -29,26 +30,24 @@ #include "rcl/guard_condition.h" #include "rcl/wait.h" +#include "rclcpp/executors/executor_notify_waitable.hpp" #include "rcpputils/scope_exit.hpp" #include "rclcpp/context.hpp" #include "rclcpp/contexts/default_context.hpp" #include "rclcpp/guard_condition.hpp" #include "rclcpp/executor_options.hpp" +#include "rclcpp/executors/executor_entities_collection.hpp" +#include "rclcpp/executors/executor_entities_collector.hpp" #include "rclcpp/future_return_code.hpp" -#include "rclcpp/memory_strategies.hpp" -#include "rclcpp/memory_strategy.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/visibility_control.hpp" +#include "rclcpp/wait_set.hpp" namespace rclcpp { -typedef std::map> WeakCallbackGroupsToNodesMap; - // Forward declaration is used in convenience method signature. class Node; @@ -402,17 +401,6 @@ class Executor void cancel(); - /// Support dynamic switching of the memory strategy. - /** - * Switching the memory strategy while the executor is spinning in another threading could have - * unintended consequences. - * \param[in] memory_strategy Shared pointer to the memory strategy to set. - * \throws std::runtime_error if memory_strategy is null - */ - RCLCPP_PUBLIC - void - set_memory_strategy(memory_strategy::MemoryStrategy::SharedPtr memory_strategy); - /// Returns true if the executor is currently spinning. /** * This function can be called asynchronously from any thread. @@ -497,6 +485,10 @@ class Executor static void execute_client(rclcpp::ClientBase::SharedPtr client); + RCLCPP_PUBLIC + void + collect_entities(); + /// Block until more work becomes avilable or timeout is reached. /** * Builds a set of waitable entities, which are passed to the middleware. @@ -508,62 +500,6 @@ class Executor void wait_for_work(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); - /// Find node associated with a callback group - /** - * \param[in] weak_groups_to_nodes map of callback groups to nodes - * \param[in] group callback group to find assocatiated node - * \return Pointer to associated node if found, else nullptr - */ - RCLCPP_PUBLIC - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr - get_node_by_group( - const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes, - rclcpp::CallbackGroup::SharedPtr group); - - /// Return true if the node has been added to this executor. - /** - * \param[in] node_ptr a shared pointer that points to a node base interface - * \param[in] weak_groups_to_nodes map to nodes to lookup - * \return true if the node is associated with the executor, otherwise false - */ - RCLCPP_PUBLIC - bool - has_node( - const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) const; - - /// Find the callback group associated with a timer - /** - * \param[in] timer Timer to find associated callback group - * \return Pointer to callback group node if found, else nullptr - */ - RCLCPP_PUBLIC - rclcpp::CallbackGroup::SharedPtr - get_group_by_timer(rclcpp::TimerBase::SharedPtr timer); - - /// Add a callback group to an executor - /** - * \see rclcpp::Executor::add_callback_group - */ - RCLCPP_PUBLIC - virtual void - add_callback_group_to_map( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - WeakCallbackGroupsToNodesMap & weak_groups_to_nodes, - bool notify = true) RCPPUTILS_TSA_REQUIRES(mutex_); - - /// Remove a callback group from the executor. - /** - * \see rclcpp::Executor::remove_callback_group - */ - RCLCPP_PUBLIC - virtual void - remove_callback_group_from_map( - rclcpp::CallbackGroup::SharedPtr group_ptr, - WeakCallbackGroupsToNodesMap & weak_groups_to_nodes, - bool notify = true) RCPPUTILS_TSA_REQUIRES(mutex_); - /// Check for executable in ready state and populate union structure. /** * \param[out] any_executable populated union structure of ready executable @@ -574,33 +510,6 @@ class Executor bool get_next_ready_executable(AnyExecutable & any_executable); - /// Check for executable in ready state and populate union structure. - /** - * This is the implementation of get_next_ready_executable that takes into - * account the current state of callback groups' association with nodes and - * executors. - * - * This checks in a particular order for available work: - * * Timers - * * Subscriptions - * * Services - * * Clients - * * Waitable - * - * If the next executable is not associated with this executor/node pair, - * then this method will return false. - * - * \param[out] any_executable populated union structure of ready executable - * \param[in] weak_groups_to_nodes mapping of callback groups to nodes - * \return true if an executable was ready and any_executable was populated, - * otherwise false - */ - RCLCPP_PUBLIC - bool - get_next_ready_executable_from_map( - AnyExecutable & any_executable, - const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); - /// Wait for executable in ready state and populate union structure. /** * If an executable is ready, it will return immediately, otherwise @@ -618,21 +527,6 @@ class Executor AnyExecutable & any_executable, std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); - /// Add all callback groups that can be automatically added from associated nodes. - /** - * The executor, before collecting entities, verifies if any callback group from - * nodes associated with the executor, which is not already associated to an executor, - * can be automatically added to this executor. - * This takes care of any callback group that has been added to a node but not explicitly added - * to the executor. - * It is important to note that in order for the callback groups to be automatically added to an - * executor through this function, the node of the callback groups needs to have been added - * through the `add_node` method. - */ - RCLCPP_PUBLIC - virtual void - add_callback_groups_from_nodes_associated_to_executor() RCPPUTILS_TSA_REQUIRES(mutex_); - /// Spinning state, used to prevent multi threaded calls to spin and to cancel blocking spins. std::atomic_bool spinning; @@ -642,16 +536,8 @@ class Executor /// 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. - rcl_wait_set_t wait_set_ = rcl_get_zero_initialized_wait_set(); - - // Mutex to protect the subsequent memory_strategy_. mutable std::mutex mutex_; - /// The memory strategy: an interface for handling user-defined memory allocation strategies. - memory_strategy::MemoryStrategy::SharedPtr - memory_strategy_ RCPPUTILS_TSA_PT_GUARDED_BY(mutex_); - /// The context associated with this executor. std::shared_ptr context_; @@ -661,39 +547,14 @@ class Executor virtual void spin_once_impl(std::chrono::nanoseconds timeout); - typedef std::map> - WeakNodesToGuardConditionsMap; - - typedef std::map> - WeakCallbackGroupsToGuardConditionsMap; - - /// maps nodes to guard conditions - WeakNodesToGuardConditionsMap - weak_nodes_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_); - - /// maps callback groups to guard conditions - WeakCallbackGroupsToGuardConditionsMap - weak_groups_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_); - - /// maps callback groups associated to nodes - WeakCallbackGroupsToNodesMap - weak_groups_associated_with_executor_to_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_); - - /// maps callback groups to nodes associated with executor - WeakCallbackGroupsToNodesMap - weak_groups_to_nodes_associated_with_executor_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + std::shared_ptr notify_waitable_; + rclcpp::executors::ExecutorEntitiesCollector collector_; - /// maps all callback groups to nodes - WeakCallbackGroupsToNodesMap - weak_groups_to_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + rclcpp::executors::ExecutorEntitiesCollection current_collection_; + std::shared_ptr current_notify_waitable_; - /// nodes that are associated with the executor - std::list - weak_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + std::shared_ptr wait_set_; + std::deque ready_executables_; /// shutdown callback handle registered to Context rclcpp::OnShutdownCallbackHandle shutdown_callback_handle_; diff --git a/rclcpp/include/rclcpp/executors.hpp b/rclcpp/include/rclcpp/executors.hpp index 36fb0d63cf..85220a8899 100644 --- a/rclcpp/include/rclcpp/executors.hpp +++ b/rclcpp/include/rclcpp/executors.hpp @@ -20,7 +20,6 @@ #include "rclcpp/executors/multi_threaded_executor.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" -#include "rclcpp/executors/static_single_threaded_executor.hpp" #include "rclcpp/node.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/visibility_control.hpp" diff --git a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp index 2db3ff630a..69b744877c 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP__WAIT_SET_POLICIES__DETAIL__STORAGE_POLICY_COMMON_HPP_ #define RCLCPP__WAIT_SET_POLICIES__DETAIL__STORAGE_POLICY_COMMON_HPP_ +#include #include #include #include @@ -79,6 +80,7 @@ class StoragePolicyCommon throw std::runtime_error("unexpected condition, fixed storage policy needs pruning"); } // Flag for pruning. + std::cout << "waitable needs pruning" << std::endl; needs_pruning_ = true; continue; } @@ -86,6 +88,8 @@ class StoragePolicyCommon rclcpp::Waitable & waitable = *waitable_ptr_pair.second; subscriptions_from_waitables += waitable.get_number_of_ready_subscriptions(); guard_conditions_from_waitables += waitable.get_number_of_ready_guard_conditions(); + std::cout << "guard_conditions_from_waitables: " << guard_conditions_from_waitables << + std::endl; timers_from_waitables += waitable.get_number_of_ready_timers(); clients_from_waitables += waitable.get_number_of_ready_clients(); services_from_waitables += waitable.get_number_of_ready_services(); diff --git a/rclcpp/include/rclcpp/wait_set_policies/thread_safe_synchronization.hpp b/rclcpp/include/rclcpp/wait_set_policies/thread_safe_synchronization.hpp index 4a4fb16547..a7bd5ecf80 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/thread_safe_synchronization.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/thread_safe_synchronization.hpp @@ -279,9 +279,11 @@ class ThreadSafeSynchronization : public detail::SynchronizationPolicyCommon // storage wait sets since they cannot be changed after construction. // This will also clear the wait set and re-add all the entities, which // prepares it to be waited on again. + std::cout << "rebuild_rcl_wait_set" << std::endl; rebuild_rcl_wait_set(); } + std::cout << "get_rcl_wait_set" << std::endl; rcl_wait_set_t & rcl_wait_set = get_rcl_wait_set(); // Wait unconditionally until timeout condition occurs since we assume @@ -297,6 +299,7 @@ class ThreadSafeSynchronization : public detail::SynchronizationPolicyCommon // It is ok to wait while not having the lock acquired, because the state // in the rcl wait set will not be updated until this method calls // rebuild_rcl_wait_set(). + std::cout << "rcl_wait" << std::endl; rcl_ret_t ret = rcl_wait(&rcl_wait_set, time_left_to_wait_ns.count()); if (RCL_RET_OK == ret) { // Something has become ready in the wait set, first check if it was diff --git a/rclcpp/include/rclcpp/wait_set_template.hpp b/rclcpp/include/rclcpp/wait_set_template.hpp index b3f41f8ed5..c139913be8 100644 --- a/rclcpp/include/rclcpp/wait_set_template.hpp +++ b/rclcpp/include/rclcpp/wait_set_template.hpp @@ -661,6 +661,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli this->storage_acquire_ownerships(); RCPPUTILS_SCOPE_EXIT({this->storage_release_ownerships();}); + // this method comes from the SynchronizationPolicy return this->template sync_wait>( // pass along the time_to_wait duration as nanoseconds diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 747b784878..29a30d64e2 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include #include @@ -22,12 +23,13 @@ #include "rcl/allocator.h" #include "rcl/error_handling.h" +#include "rclcpp/executors/executor_notify_waitable.hpp" +#include "rclcpp/subscription_wait_set_mask.hpp" #include "rcpputils/scope_exit.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/executor.hpp" #include "rclcpp/guard_condition.hpp" -#include "rclcpp/memory_strategy.hpp" #include "rclcpp/node.hpp" #include "rclcpp/utilities.hpp" @@ -37,14 +39,21 @@ using namespace std::chrono_literals; -using rclcpp::exceptions::throw_from_rcl_error; using rclcpp::Executor; +static constexpr rclcpp::SubscriptionWaitSetMask kDefaultSubscriptionMask = {true, false, false}; + Executor::Executor(const rclcpp::ExecutorOptions & options) : spinning(false), interrupt_guard_condition_(std::make_shared(options.context)), shutdown_guard_condition_(std::make_shared(options.context)), - memory_strategy_(options.memory_strategy) + notify_waitable_(std::make_shared( + [this]() { + this->collect_entities(); + })), + collector_(notify_waitable_), + current_notify_waitable_(notify_waitable_), + wait_set_(std::make_shared()) { // Store the context for later use. context_ = options.context; @@ -57,74 +66,50 @@ Executor::Executor(const rclcpp::ExecutorOptions & options) } }); - // The number of guard conditions is always at least 2: 1 for the ctrl-c guard cond, - // and one for the executor's guard cond (interrupt_guard_condition_) - memory_strategy_->add_guard_condition(*shutdown_guard_condition_.get()); - - // Put the executor's guard condition in - 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( - &wait_set_, - 0, 2, 0, 0, 0, 0, - context_->get_rcl_context().get(), - allocator); - if (RCL_RET_OK != ret) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "failed to create wait set: %s", rcl_get_error_string().str); - rcl_reset_error(); - throw_from_rcl_error(ret, "Failed to create wait set in Executor constructor"); - } + notify_waitable_->add_guard_condition(interrupt_guard_condition_); + notify_waitable_->add_guard_condition(shutdown_guard_condition_); } Executor::~Executor() { - // Disassociate all callback groups. - for (auto & pair : weak_groups_to_nodes_) { - auto group = pair.first.lock(); - if (group) { - std::atomic_bool & has_executor = group->get_associated_with_executor_atomic(); - has_executor.store(false); - } - } - // Disassociate all nodes. - 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); - } + std::lock_guard lock(mutex_); + + notify_waitable_->remove_guard_condition(interrupt_guard_condition_); + notify_waitable_->remove_guard_condition(shutdown_guard_condition_); + + current_collection_.timers.update( + {}, + [this](auto timer) {wait_set_->add_timer(timer);}, + [this](auto timer) {wait_set_->remove_timer(timer);}); + + current_collection_.subscriptions.update( + {}, + [this](auto subscription) { + wait_set_->add_subscription(subscription, kDefaultSubscriptionMask); + }, + [this](auto subscription) { + wait_set_->remove_subscription(subscription, kDefaultSubscriptionMask); }); - weak_nodes_.clear(); - weak_groups_associated_with_executor_to_nodes_.clear(); - weak_groups_to_nodes_associated_with_executor_.clear(); - weak_groups_to_nodes_.clear(); - for (const auto & pair : weak_groups_to_guard_conditions_) { - auto guard_condition = pair.second; - memory_strategy_->remove_guard_condition(guard_condition); - } - weak_groups_to_guard_conditions_.clear(); - for (const auto & pair : weak_nodes_to_guard_conditions_) { - auto guard_condition = pair.second; - memory_strategy_->remove_guard_condition(guard_condition); - } - weak_nodes_to_guard_conditions_.clear(); + current_collection_.clients.update( + {}, + [this](auto client) {wait_set_->add_client(client);}, + [this](auto client) {wait_set_->remove_client(client);}); - // Finalize the wait set. - if (rcl_wait_set_fini(&wait_set_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "failed to destroy wait set: %s", rcl_get_error_string().str); - rcl_reset_error(); - } - // Remove and release the sigint guard condition - memory_strategy_->remove_guard_condition(shutdown_guard_condition_.get()); - memory_strategy_->remove_guard_condition(interrupt_guard_condition_.get()); + current_collection_.services.update( + {}, + [this](auto service) {wait_set_->add_service(service);}, + [this](auto service) {wait_set_->remove_service(service);}); + + current_collection_.guard_conditions.update( + {}, + [this](auto guard_condition) {wait_set_->add_guard_condition(guard_condition);}, + [this](auto guard_condition) {wait_set_->remove_guard_condition(guard_condition);}); + + current_collection_.waitables.update( + {}, + [this](auto waitable) {wait_set_->add_waitable(waitable);}, + [this](auto waitable) {wait_set_->remove_waitable(waitable);}); // Remove shutdown callback handle registered to Context if (!context_->remove_on_shutdown_callback(shutdown_callback_handle_)) { @@ -138,95 +123,38 @@ Executor::~Executor() std::vector Executor::get_all_callback_groups() { - std::vector groups; - std::lock_guard guard{mutex_}; - for (const auto & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) { - groups.push_back(group_node_ptr.first); - } - for (auto const & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) { - groups.push_back(group_node_ptr.first); - } - return groups; + std::lock_guard lock(mutex_); + return this->collector_.get_all_callback_groups(); } std::vector Executor::get_manually_added_callback_groups() { - std::vector groups; - std::lock_guard guard{mutex_}; - for (auto const & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) { - groups.push_back(group_node_ptr.first); - } - return groups; + std::lock_guard lock(mutex_); + return this->collector_.get_manually_added_callback_groups(); } std::vector Executor::get_automatically_added_callback_groups_from_nodes() { - std::vector groups; - std::lock_guard guard{mutex_}; - for (auto const & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) { - groups.push_back(group_node_ptr.first); - } - return groups; -} - -void -Executor::add_callback_groups_from_nodes_associated_to_executor() -{ - for (auto & weak_node : weak_nodes_) { - auto node = weak_node.lock(); - if (node) { - node->for_each_callback_group( - [this, node](rclcpp::CallbackGroup::SharedPtr shared_group_ptr) - { - if ( - shared_group_ptr->automatically_add_to_executor_with_node() && - !shared_group_ptr->get_associated_with_executor_atomic().load()) - { - this->add_callback_group_to_map( - shared_group_ptr, - node, - weak_groups_to_nodes_associated_with_executor_, - true); - } - }); - } - } + std::lock_guard lock(mutex_); + return this->collector_.get_automatically_added_callback_groups(); } void -Executor::add_callback_group_to_map( +Executor::add_callback_group( rclcpp::CallbackGroup::SharedPtr group_ptr, rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & weak_groups_to_nodes, bool notify) { - // If the callback_group already has an executor - 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."); - } - - rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr; - auto insert_info = - weak_groups_to_nodes.insert(std::make_pair(weak_group_ptr, node_ptr)); - bool was_inserted = insert_info.second; - if (!was_inserted) { - throw std::runtime_error("Callback group was already added to executor."); - } - // Also add to the map that contains all callback groups - 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(); - 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); + (void) node_ptr; + { + std::lock_guard lock(mutex_); + this->collector_.add_callback_group(group_ptr); } if (notify) { - // Interrupt waiting to handle new node + // Interrupt waiting to handle removed callback group try { interrupt_guard_condition_->trigger(); } catch (const rclcpp::exceptions::RCLError & ex) { @@ -237,91 +165,22 @@ Executor::add_callback_group_to_map( } } -void -Executor::add_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - bool notify) -{ - std::lock_guard guard{mutex_}; - this->add_callback_group_to_map( - group_ptr, - node_ptr, - weak_groups_associated_with_executor_to_nodes_, - notify); -} - void Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) { - // 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."); + { + std::lock_guard lock(mutex_); + this->collector_.add_node(node_ptr); } - std::lock_guard guard{mutex_}; - node_ptr->for_each_callback_group( - [this, node_ptr, notify](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_map( - group_ptr, - node_ptr, - weak_groups_to_nodes_associated_with_executor_, - notify); - } - }); - 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); - weak_nodes_.push_back(node_ptr); -} - -void -Executor::remove_callback_group_from_map( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & weak_groups_to_nodes, - bool notify) -{ - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr; - rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr; - auto iter = weak_groups_to_nodes.find(weak_group_ptr); - if (iter != weak_groups_to_nodes.end()) { - node_ptr = iter->second.lock(); - if (node_ptr == nullptr) { - throw std::runtime_error("Node must not be deleted before its callback group(s)."); - } - weak_groups_to_nodes.erase(iter); - weak_groups_to_nodes_.erase(group_ptr); - std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic(); - has_executor.store(false); - } else { - throw std::runtime_error("Callback group needs to be associated with executor."); - } - // If the node was matched and removed, interrupt waiting. - if (!has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_) && - !has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_)) - { - auto iter = weak_groups_to_guard_conditions_.find(weak_group_ptr); - if (iter != weak_groups_to_guard_conditions_.end()) { - memory_strategy_->remove_guard_condition(iter->second); - } - weak_groups_to_guard_conditions_.erase(weak_group_ptr); - - if (notify) { - try { - interrupt_guard_condition_->trigger(); - } catch (const rclcpp::exceptions::RCLError & ex) { - throw std::runtime_error( - std::string( - "Failed to trigger guard condition on callback group remove: ") + ex.what()); - } + if (notify) { + // Interrupt waiting to handle removed callback group + try { + interrupt_guard_condition_->trigger(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string( + "Failed to trigger guard condition on callback group remove: ") + ex.what()); } } } @@ -331,11 +190,21 @@ Executor::remove_callback_group( rclcpp::CallbackGroup::SharedPtr group_ptr, bool notify) { - std::lock_guard guard{mutex_}; - this->remove_callback_group_from_map( - group_ptr, - weak_groups_associated_with_executor_to_nodes_, - notify); + { + std::lock_guard lock(mutex_); + this->collector_.remove_callback_group(group_ptr); + } + + if (notify) { + // Interrupt waiting to handle removed callback group + try { + interrupt_guard_condition_->trigger(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string( + "Failed to trigger guard condition on callback group remove: ") + ex.what()); + } + } } void @@ -347,48 +216,21 @@ Executor::add_node(std::shared_ptr node_ptr, bool notify) void Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) { - if (!node_ptr->get_associated_with_executor_atomic().load()) { - throw std::runtime_error("Node needs to be associated with an executor."); - } - - std::lock_guard guard{mutex_}; - 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."); + { + std::lock_guard lock(mutex_); + this->collector_.remove_node(node_ptr); } - for (auto it = weak_groups_to_nodes_associated_with_executor_.begin(); - it != weak_groups_to_nodes_associated_with_executor_.end(); ) - { - auto weak_node_ptr = it->second; - auto shared_node_ptr = weak_node_ptr.lock(); - auto group_ptr = it->first.lock(); - - // Increment iterator before removing in case it's invalidated - it++; - if (shared_node_ptr == node_ptr) { - remove_callback_group_from_map( - group_ptr, - weak_groups_to_nodes_associated_with_executor_, - notify); + if (notify) { + // Interrupt waiting to handle removed callback group + try { + interrupt_guard_condition_->trigger(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string( + "Failed to trigger guard condition on callback group add: ") + ex.what()); } } - - 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(); - has_executor.store(false); } void @@ -504,22 +346,13 @@ Executor::cancel() } } -void -Executor::set_memory_strategy(rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy) -{ - if (memory_strategy == nullptr) { - throw std::runtime_error("Received NULL memory strategy in executor."); - } - std::lock_guard guard{mutex_}; - memory_strategy_ = memory_strategy; -} - void Executor::execute_any_executable(AnyExecutable & any_exec) { if (!spinning.load()) { return; } + if (any_exec.timer) { TRACEPOINT( rclcpp_executor_execute, @@ -541,16 +374,10 @@ Executor::execute_any_executable(AnyExecutable & any_exec) if (any_exec.waitable) { any_exec.waitable->execute(any_exec.data); } + // Reset the callback_group, regardless of type - any_exec.callback_group->can_be_taken_from().store(true); - // Wake the wait, because it may need to be recalculated or work that - // was previously blocked is now available. - try { - interrupt_guard_condition_->trigger(); - } catch (const rclcpp::exceptions::RCLError & ex) { - throw std::runtime_error( - std::string( - "Failed to trigger guard condition from execute_any_executable: ") + ex.what()); + if (any_exec.callback_group) { + any_exec.callback_group->can_be_taken_from().store(true); } } @@ -691,227 +518,99 @@ Executor::execute_client( } void -Executor::wait_for_work(std::chrono::nanoseconds timeout) +Executor::collect_entities() { - TRACEPOINT(rclcpp_executor_wait_for_work, timeout.count()); - { - std::lock_guard guard(mutex_); - - // Check weak_nodes_ to find any callback group that is not owned - // by an executor and add it to the list of callbackgroups for - // collect entities. Also exchange to false so it is not - // allowed to add to another executor - add_callback_groups_from_nodes_associated_to_executor(); - - // Collect the subscriptions and timers to be waited on - memory_strategy_->clear_handles(); - bool has_invalid_weak_groups_or_nodes = - memory_strategy_->collect_entities(weak_groups_to_nodes_); - - if (has_invalid_weak_groups_or_nodes) { - std::vector invalid_group_ptrs; - for (auto pair : weak_groups_to_nodes_) { - auto weak_group_ptr = pair.first; - auto weak_node_ptr = pair.second; - if (weak_group_ptr.expired() || weak_node_ptr.expired()) { - invalid_group_ptrs.push_back(weak_group_ptr); - auto node_guard_pair = weak_nodes_to_guard_conditions_.find(weak_node_ptr); - if (node_guard_pair != weak_nodes_to_guard_conditions_.end()) { - auto guard_condition = node_guard_pair->second; - weak_nodes_to_guard_conditions_.erase(weak_node_ptr); - memory_strategy_->remove_guard_condition(guard_condition); - } - } - } - std::for_each( - invalid_group_ptrs.begin(), invalid_group_ptrs.end(), - [this](rclcpp::CallbackGroup::WeakPtr group_ptr) { - if (weak_groups_to_nodes_associated_with_executor_.find(group_ptr) != - weak_groups_to_nodes_associated_with_executor_.end()) - { - weak_groups_to_nodes_associated_with_executor_.erase(group_ptr); - } - if (weak_groups_associated_with_executor_to_nodes_.find(group_ptr) != - weak_groups_associated_with_executor_to_nodes_.end()) - { - weak_groups_associated_with_executor_to_nodes_.erase(group_ptr); - } - auto callback_guard_pair = weak_groups_to_guard_conditions_.find(group_ptr); - if (callback_guard_pair != weak_groups_to_guard_conditions_.end()) { - auto guard_condition = callback_guard_pair->second; - weak_groups_to_guard_conditions_.erase(group_ptr); - memory_strategy_->remove_guard_condition(guard_condition); - } - weak_groups_to_nodes_.erase(group_ptr); - }); - } - - // clear wait set - rcl_ret_t ret = rcl_wait_set_clear(&wait_set_); - if (ret != RCL_RET_OK) { - throw_from_rcl_error(ret, "Couldn't clear wait set"); - } + std::lock_guard guard(mutex_); + rclcpp::executors::ExecutorEntitiesCollection collection; + this->collector_.update_collections(); + auto callback_groups = this->collector_.get_all_callback_groups(); + rclcpp::executors::build_entities_collection(callback_groups, collection); + + if (notify_waitable_) { + // Make a copy of notify waitable so we can continue to mutate the original + // one outside of the execute loop. + *current_notify_waitable_ = *notify_waitable_; + auto notify_waitable = std::static_pointer_cast(current_notify_waitable_); + collection.waitables.insert({notify_waitable.get(), {notify_waitable, {}}}); + } + + current_collection_.timers.update( + collection.timers, + [this](auto timer) {wait_set_->add_timer(timer);}, + [this](auto timer) {wait_set_->remove_timer(timer);}); + + current_collection_.subscriptions.update( + collection.subscriptions, + [this](auto subscription) { + wait_set_->add_subscription(subscription, kDefaultSubscriptionMask); + }, + [this](auto subscription) { + wait_set_->remove_subscription(subscription, kDefaultSubscriptionMask); + }); - // The size of waitables are accounted for in size of the other entities - ret = rcl_wait_set_resize( - &wait_set_, memory_strategy_->number_of_ready_subscriptions(), - memory_strategy_->number_of_guard_conditions(), memory_strategy_->number_of_ready_timers(), - memory_strategy_->number_of_ready_clients(), memory_strategy_->number_of_ready_services(), - memory_strategy_->number_of_ready_events()); - if (RCL_RET_OK != ret) { - throw_from_rcl_error(ret, "Couldn't resize the wait set"); - } + current_collection_.clients.update( + collection.clients, + [this](auto client) {wait_set_->add_client(client);}, + [this](auto client) {wait_set_->remove_client(client);}); - if (!memory_strategy_->add_handles_to_wait_set(&wait_set_)) { - throw std::runtime_error("Couldn't fill wait set"); - } - } + current_collection_.services.update( + collection.services, + [this](auto service) {wait_set_->add_service(service);}, + [this](auto service) {wait_set_->remove_service(service);}); - rcl_ret_t status = - rcl_wait(&wait_set_, std::chrono::duration_cast(timeout).count()); - if (status == RCL_RET_WAIT_SET_EMPTY) { - RCUTILS_LOG_WARN_NAMED( - "rclcpp", - "empty wait set received in rcl_wait(). This should never happen."); - } else if (status != RCL_RET_OK && status != RCL_RET_TIMEOUT) { - using rclcpp::exceptions::throw_from_rcl_error; - throw_from_rcl_error(status, "rcl_wait() failed"); - } + current_collection_.guard_conditions.update( + collection.guard_conditions, + [this](auto guard_condition) {wait_set_->add_guard_condition(guard_condition);}, + [this](auto guard_condition) {wait_set_->remove_guard_condition(guard_condition);}); - // check the null handles in the wait set and remove them from the handles in memory strategy - // for callback-based entities - std::lock_guard guard(mutex_); - memory_strategy_->remove_null_handles(&wait_set_); + current_collection_.waitables.update( + collection.waitables, + [this](auto waitable) {wait_set_->add_waitable(waitable);}, + [this](auto waitable) {wait_set_->remove_waitable(waitable);}); } -rclcpp::node_interfaces::NodeBaseInterface::SharedPtr -Executor::get_node_by_group( - const rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & - weak_groups_to_nodes, - rclcpp::CallbackGroup::SharedPtr group) +void +Executor::wait_for_work(std::chrono::nanoseconds timeout) { - if (!group) { - return nullptr; - } - rclcpp::CallbackGroup::WeakPtr weak_group_ptr(group); - const auto finder = weak_groups_to_nodes.find(weak_group_ptr); - if (finder != weak_groups_to_nodes.end()) { - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr = finder->second.lock(); - return node_ptr; - } - return nullptr; -} + TRACEPOINT(rclcpp_executor_wait_for_work, timeout.count()); -rclcpp::CallbackGroup::SharedPtr -Executor::get_group_by_timer(rclcpp::TimerBase::SharedPtr timer) -{ - std::lock_guard guard{mutex_}; - for (const auto & pair : weak_groups_associated_with_executor_to_nodes_) { - auto group = pair.first.lock(); - if (!group) { - continue; - } - auto timer_ref = group->find_timer_ptrs_if( - [timer](const rclcpp::TimerBase::SharedPtr & timer_ptr) -> bool { - return timer_ptr == timer; - }); - if (timer_ref) { - return group; - } + if (current_collection_.empty() || this->collector_.has_pending()) { + this->collect_entities(); } - for (const auto & pair : weak_groups_to_nodes_associated_with_executor_) { - auto group = pair.first.lock(); - if (!group) { - continue; - } - auto timer_ref = group->find_timer_ptrs_if( - [timer](const rclcpp::TimerBase::SharedPtr & timer_ptr) -> bool { - return timer_ptr == timer; - }); - if (timer_ref) { - return group; - } + auto wait_result = wait_set_->wait(timeout); + + if (wait_result.kind() == WaitResultKind::Empty) { + RCUTILS_LOG_WARN_NAMED( + "rclcpp", + "empty wait set received in wait(). This should never happen."); } - return nullptr; -} -bool -Executor::get_next_ready_executable(AnyExecutable & any_executable) -{ - bool success = get_next_ready_executable_from_map(any_executable, weak_groups_to_nodes_); - return success; + std::lock_guard guard{mutex_}; + ready_executables_ = rclcpp::executors::ready_executables(current_collection_, wait_result); } bool -Executor::get_next_ready_executable_from_map( - AnyExecutable & any_executable, - const rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & - weak_groups_to_nodes) +Executor::get_next_ready_executable(AnyExecutable & any_executable) { TRACEPOINT(rclcpp_executor_get_next_ready); - bool success = false; std::lock_guard guard{mutex_}; - // Check the timers to see if there are any that are ready - memory_strategy_->get_next_timer(any_executable, weak_groups_to_nodes); - if (any_executable.timer) { - success = true; - } - if (!success) { - // Check the subscriptions to see if there are any that are ready - memory_strategy_->get_next_subscription(any_executable, weak_groups_to_nodes); - if (any_executable.subscription) { - success = true; - } - } - if (!success) { - // Check the services to see if there are any that are ready - memory_strategy_->get_next_service(any_executable, weak_groups_to_nodes); - if (any_executable.service) { - success = true; - } - } - if (!success) { - // Check the clients to see if there are any that are ready - memory_strategy_->get_next_client(any_executable, weak_groups_to_nodes); - if (any_executable.client) { - success = true; - } - } - if (!success) { - // Check the waitables to see if there are any that are ready - memory_strategy_->get_next_waitable(any_executable, weak_groups_to_nodes); - if (any_executable.waitable) { - any_executable.data = any_executable.waitable->take_data(); - success = true; - } - } - // At this point any_executable should be valid with either a valid subscription - // or a valid timer, or it should be a null shared_ptr - if (success) { - rclcpp::CallbackGroup::WeakPtr weak_group_ptr = any_executable.callback_group; - auto iter = weak_groups_to_nodes.find(weak_group_ptr); - if (iter == weak_groups_to_nodes.end()) { - success = false; - } + + if (ready_executables_.size() == 0) { + return false; } - if (success) { - // If it is valid, check to see if the group is mutually exclusive or - // not, then mark it accordingly ..Check if the callback_group belongs to this executor - if (any_executable.callback_group && any_executable.callback_group->type() == \ - CallbackGroupType::MutuallyExclusive) - { - // It should not have been taken otherwise - assert(any_executable.callback_group->can_be_taken_from().load()); - // Set to false to indicate something is being run from this group - // This is reset to true either when the any_executable is executed or when the - // any_executable is destructued - any_executable.callback_group->can_be_taken_from().store(false); - } + any_executable = ready_executables_.front(); + ready_executables_.pop_front(); + + if (any_executable.callback_group && + any_executable.callback_group->type() == CallbackGroupType::MutuallyExclusive) + { + assert(any_executable.callback_group->can_be_taken_from().load()); + any_executable.callback_group->can_be_taken_from().store(false); } - // If there is no ready executable, return false - return success; + + return true; } bool @@ -934,22 +633,6 @@ Executor::get_next_executable(AnyExecutable & any_executable, std::chrono::nanos return success; } -// Returns true iff the weak_groups_to_nodes map has node_ptr as the value in any of its entry. -bool -Executor::has_node( - const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - const rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & - weak_groups_to_nodes) const -{ - return std::find_if( - weak_groups_to_nodes.begin(), - weak_groups_to_nodes.end(), - [&](const WeakCallbackGroupsToNodesMap::value_type & other) -> bool { - auto other_ptr = other.second.lock(); - return other_ptr == node_ptr; - }) != weak_groups_to_nodes.end(); -} - bool Executor::is_spinning() { diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp index 2dce369b08..a10e8d694f 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp @@ -121,6 +121,7 @@ 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_notify_waitable.cpp b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp index c0ad8a25a4..6d9fc01361 100644 --- a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp +++ b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp @@ -67,6 +67,7 @@ ExecutorNotifyWaitable::add_to_wait_set(rcl_wait_set_t * wait_set) bool ExecutorNotifyWaitable::is_ready(rcl_wait_set_t * wait_set) { + std::cout << "ExecutorNotifyWaitable::is_ready" << std::endl; std::lock_guard lock(guard_condition_mutex_); bool any_ready = false; @@ -89,6 +90,7 @@ ExecutorNotifyWaitable::is_ready(rcl_wait_set_t * wait_set) void ExecutorNotifyWaitable::execute(std::shared_ptr & data) { + std::cout << "ExecutorNotifyWaitable::execute" << std::endl; (void) data; this->execute_callback_(); } diff --git a/rclcpp/test/benchmark/benchmark_executor.cpp b/rclcpp/test/benchmark/benchmark_executor.cpp index 652007b589..c2b42752f2 100644 --- a/rclcpp/test/benchmark/benchmark_executor.cpp +++ b/rclcpp/test/benchmark/benchmark_executor.cpp @@ -189,71 +189,6 @@ BENCHMARK_F(PerformanceTestExecutorSimple, multi_thread_executor_remove_node)(be } } -BENCHMARK_F( - PerformanceTestExecutorSimple, - static_single_thread_executor_add_node)(benchmark::State & st) -{ - rclcpp::executors::StaticSingleThreadedExecutor executor; - for (auto _ : st) { - (void)_; - executor.add_node(node); - st.PauseTiming(); - executor.remove_node(node); - st.ResumeTiming(); - } -} - -BENCHMARK_F( - PerformanceTestExecutorSimple, - static_single_thread_executor_remove_node)(benchmark::State & st) -{ - rclcpp::executors::StaticSingleThreadedExecutor executor; - for (auto _ : st) { - (void)_; - st.PauseTiming(); - executor.add_node(node); - st.ResumeTiming(); - executor.remove_node(node); - } -} - -BENCHMARK_F( - PerformanceTestExecutorSimple, - static_single_thread_executor_spin_until_future_complete)(benchmark::State & st) -{ - rclcpp::executors::StaticSingleThreadedExecutor executor; - // test success of an immediately finishing future - std::promise promise; - std::future future = promise.get_future(); - promise.set_value(true); - auto shared_future = future.share(); - - auto ret = executor.spin_until_future_complete(shared_future, 100ms); - if (ret != rclcpp::FutureReturnCode::SUCCESS) { - st.SkipWithError(rcutils_get_error_string().str); - } - - reset_heap_counters(); - - for (auto _ : st) { - (void)_; - // static_single_thread_executor has a special design. We need to add/remove the node each - // time you call spin - st.PauseTiming(); - executor.add_node(node); - st.ResumeTiming(); - - ret = executor.spin_until_future_complete(shared_future, 100ms); - if (ret != rclcpp::FutureReturnCode::SUCCESS) { - st.SkipWithError(rcutils_get_error_string().str); - break; - } - st.PauseTiming(); - executor.remove_node(node); - st.ResumeTiming(); - } -} - BENCHMARK_F( PerformanceTestExecutorSimple, single_thread_executor_spin_node_until_future_complete)(benchmark::State & st) @@ -314,29 +249,6 @@ BENCHMARK_F( } } -BENCHMARK_F( - PerformanceTestExecutorSimple, - static_single_thread_executor_spin_node_until_future_complete)(benchmark::State & st) -{ - rclcpp::executors::StaticSingleThreadedExecutor executor; - // test success of an immediately finishing future - std::promise promise; - std::future future = promise.get_future(); - promise.set_value(true); - auto shared_future = future.share(); - - reset_heap_counters(); - - for (auto _ : st) { - (void)_; - auto ret = rclcpp::executors::spin_node_until_future_complete( - executor, node, shared_future, 1s); - if (ret != rclcpp::FutureReturnCode::SUCCESS) { - st.SkipWithError(rcutils_get_error_string().str); - break; - } - } -} BENCHMARK_F(PerformanceTestExecutorSimple, spin_until_future_complete)(benchmark::State & st) { @@ -362,42 +274,3 @@ BENCHMARK_F(PerformanceTestExecutorSimple, spin_until_future_complete)(benchmark } } } - -BENCHMARK_F( - PerformanceTestExecutorSimple, - static_executor_entities_collector_execute)(benchmark::State & st) -{ - rclcpp::executors::StaticExecutorEntitiesCollector::SharedPtr entities_collector_ = - std::make_shared(); - entities_collector_->add_node(node->get_node_base_interface()); - - rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); - rcl_allocator_t allocator = rcl_get_default_allocator(); - auto shared_context = node->get_node_base_interface()->get_context(); - rcl_context_t * context = shared_context->get_rcl_context().get(); - rcl_ret_t ret = rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator); - if (ret != RCL_RET_OK) { - st.SkipWithError(rcutils_get_error_string().str); - } - RCPPUTILS_SCOPE_EXIT( - { - rcl_ret_t ret = rcl_wait_set_fini(&wait_set); - if (ret != RCL_RET_OK) { - st.SkipWithError(rcutils_get_error_string().str); - } - }); - - auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); - rclcpp::GuardCondition guard_condition(shared_context); - - entities_collector_->init(&wait_set, memory_strategy); - RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); - - reset_heap_counters(); - - for (auto _ : st) { - (void)_; - std::shared_ptr data = entities_collector_->take_data(); - entities_collector_->execute(data); - } -} diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index ddb219b558..b822e6e24f 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -663,14 +663,6 @@ if(TARGET test_executors) target_link_libraries(test_executors ${PROJECT_NAME}) endif() -ament_add_gtest(test_static_single_threaded_executor executors/test_static_single_threaded_executor.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") -if(TARGET test_static_single_threaded_executor) - ament_target_dependencies(test_static_single_threaded_executor - "test_msgs") - target_link_libraries(test_static_single_threaded_executor ${PROJECT_NAME} mimick) -endif() - ament_add_gtest(test_multi_threaded_executor executors/test_multi_threaded_executor.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_multi_threaded_executor) @@ -679,15 +671,6 @@ if(TARGET test_multi_threaded_executor) target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME}) endif() -ament_add_gtest(test_static_executor_entities_collector executors/test_static_executor_entities_collector.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120) -if(TARGET test_static_executor_entities_collector) - ament_target_dependencies(test_static_executor_entities_collector - "rcl" - "test_msgs") - 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) diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 1fa2cbb4dd..1c22b17c24 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -91,8 +91,7 @@ class TestExecutorsStable : public TestExecutors {}; using ExecutorTypes = ::testing::Types< rclcpp::executors::SingleThreadedExecutor, - rclcpp::executors::MultiThreadedExecutor, - rclcpp::executors::StaticSingleThreadedExecutor>; + rclcpp::executors::MultiThreadedExecutor>; class ExecutorTypeNames { @@ -109,10 +108,6 @@ class ExecutorTypeNames return "MultiThreadedExecutor"; } - if (std::is_same()) { - return "StaticSingleThreadedExecutor"; - } - return ""; } }; @@ -135,6 +130,7 @@ TYPED_TEST(TestExecutors, detachOnDestruction) { { ExecutorType executor; executor.add_node(this->node); + std::cout << "here" << std::endl; } { ExecutorType executor; @@ -156,7 +152,7 @@ TYPED_TEST(TestExecutorsStable, addTemporaryNode) { } // Sleep for a short time to verify executor.spin() is going, and didn't throw. - std::thread spinner([&]() {EXPECT_NO_THROW(executor.spin());}); + std::thread spinner([&]() {executor.spin();}); std::this_thread::sleep_for(50ms); executor.cancel(); diff --git a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp index 02fa0b7a94..b53f47ffd5 100644 --- a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp +++ b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp @@ -49,8 +49,7 @@ class TestAddCallbackGroupsToExecutor : public ::testing::Test using ExecutorTypes = ::testing::Types< rclcpp::executors::SingleThreadedExecutor, - rclcpp::executors::MultiThreadedExecutor, - rclcpp::executors::StaticSingleThreadedExecutor>; + rclcpp::executors::MultiThreadedExecutor>; class ExecutorTypeNames { @@ -67,10 +66,6 @@ class ExecutorTypeNames return "MultiThreadedExecutor"; } - if (std::is_same()) { - return "StaticSingleThreadedExecutor"; - } - return ""; } }; @@ -137,9 +132,11 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, remove_callback_groups) { 2s, timer_callback, cb_grp); ExecutorType executor; executor.add_callback_group(cb_grp, node->get_node_base_interface()); + const rclcpp::QoS qos(10); auto options = rclcpp::SubscriptionOptions(); auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; + rclcpp::CallbackGroup::SharedPtr cb_grp2 = node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive); options.callback_group = cb_grp2; @@ -147,6 +144,7 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, remove_callback_groups) { node->create_subscription("topic_name", qos, callback, options); executor.add_callback_group(cb_grp2, node->get_node_base_interface()); + executor.remove_callback_group(cb_grp); ASSERT_EQ(executor.get_all_callback_groups().size(), 1u); executor.remove_callback_group(cb_grp2); diff --git a/rclcpp/test/rclcpp/test_executor.cpp b/rclcpp/test/rclcpp/test_executor.cpp index bdbb0a1079..e36b060d0b 100644 --- a/rclcpp/test/rclcpp/test_executor.cpp +++ b/rclcpp/test/rclcpp/test_executor.cpp @@ -46,23 +46,6 @@ class DummyExecutor : public rclcpp::Executor { spin_node_once_nanoseconds(node, std::chrono::milliseconds(100)); } - - rclcpp::memory_strategy::MemoryStrategy * memory_strategy_ptr() - { - return memory_strategy_.get(); - } - - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr local_get_node_by_group( - rclcpp::CallbackGroup::SharedPtr group) - { - std::lock_guard guard_{mutex_}; // only to make the TSA happy - return get_node_by_group(weak_groups_to_nodes_, group); - } - - rclcpp::CallbackGroup::SharedPtr local_get_group_by_timer(rclcpp::TimerBase::SharedPtr timer) - { - return get_group_by_timer(timer); - } }; class TestExecutor : public ::testing::Test @@ -126,13 +109,6 @@ TEST_F(TestExecutor, constructor_bad_guard_condition_init) { rclcpp::exceptions::RCLError); } -TEST_F(TestExecutor, constructor_bad_wait_set_init) { - auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_init, RCL_RET_ERROR); - RCLCPP_EXPECT_THROW_EQ( - static_cast(std::make_unique()), - std::runtime_error("Failed to create wait set in Executor constructor: error not set")); -} - TEST_F(TestExecutor, add_callback_group_twice) { DummyExecutor dummy; auto node = std::make_shared("node", "ns"); @@ -142,7 +118,7 @@ TEST_F(TestExecutor, add_callback_group_twice) { cb_group->get_associated_with_executor_atomic().exchange(false); RCLCPP_EXPECT_THROW_EQ( dummy.add_callback_group(cb_group, node->get_node_base_interface(), false), - std::runtime_error("Callback group was already added to executor.")); + std::runtime_error("Callback group has already been added to this executor.")); } TEST_F(TestExecutor, add_callback_group_failed_trigger_guard_condition) { @@ -328,24 +304,6 @@ TEST_F(TestExecutor, cancel_failed_trigger_guard_condition) { std::runtime_error("Failed to trigger guard condition in cancel: error not set")); } -TEST_F(TestExecutor, set_memory_strategy_nullptr) { - DummyExecutor dummy; - - RCLCPP_EXPECT_THROW_EQ( - dummy.set_memory_strategy(nullptr), - std::runtime_error("Received NULL memory strategy in executor.")); -} - -TEST_F(TestExecutor, set_memory_strategy) { - DummyExecutor dummy; - rclcpp::memory_strategy::MemoryStrategy::SharedPtr strategy = - std::make_shared< - rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy<>>(); - - dummy.set_memory_strategy(strategy); - EXPECT_EQ(dummy.memory_strategy_ptr(), strategy.get()); -} - TEST_F(TestExecutor, spin_once_failed_trigger_guard_condition) { DummyExecutor dummy; auto node = std::make_shared("node", "ns"); @@ -417,71 +375,6 @@ TEST_F(TestExecutor, spin_some_fail_wait) { std::runtime_error("rcl_wait() failed: error not set")); } -TEST_F(TestExecutor, get_node_by_group_null_group) { - DummyExecutor dummy; - ASSERT_EQ(nullptr, dummy.local_get_node_by_group(nullptr)); -} - -TEST_F(TestExecutor, get_node_by_group) { - DummyExecutor dummy; - auto node = std::make_shared("node", "ns"); - rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - dummy.add_callback_group(cb_group, node->get_node_base_interface(), false); - ASSERT_EQ(node->get_node_base_interface().get(), dummy.local_get_node_by_group(cb_group).get()); -} - -TEST_F(TestExecutor, get_node_by_group_not_found) { - DummyExecutor dummy; - auto node = std::make_shared("node", "ns"); - rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - ASSERT_EQ(nullptr, dummy.local_get_node_by_group(cb_group).get()); -} - -TEST_F(TestExecutor, get_group_by_timer_nullptr) { - DummyExecutor dummy; - ASSERT_EQ(nullptr, dummy.local_get_group_by_timer(nullptr)); -} - -TEST_F(TestExecutor, get_group_by_timer) { - DummyExecutor dummy; - auto node = std::make_shared("node", "ns"); - rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - auto timer = - node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}, cb_group); - dummy.add_node(node); - - ASSERT_EQ(cb_group.get(), dummy.local_get_group_by_timer(timer).get()); -} - -TEST_F(TestExecutor, get_group_by_timer_with_deleted_group) { - DummyExecutor dummy; - auto node = std::make_shared("node", "ns"); - rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - auto timer = - node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}, cb_group); - dummy.add_node(node); - - cb_group.reset(); - - ASSERT_EQ(nullptr, dummy.local_get_group_by_timer(timer).get()); -} - -TEST_F(TestExecutor, get_group_by_timer_add_callback_group) { - DummyExecutor dummy; - auto node = std::make_shared("node", "ns"); - rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - auto timer = - node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}, cb_group); - dummy.add_callback_group(cb_group, node->get_node_base_interface(), false); - - ASSERT_EQ(cb_group.get(), dummy.local_get_group_by_timer(timer).get()); -} - TEST_F(TestExecutor, spin_until_future_complete_in_spin_until_future_complete) { DummyExecutor dummy; auto node = std::make_shared("node", "ns"); From 1b1a9154d5b3f951bc9f8e5d284cc6bf495925e1 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Mon, 3 Apr 2023 19:30:12 +0000 Subject: [PATCH 13/74] 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 14/74] 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 15/74] 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 16/74] 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 17/74] 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 18/74] 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 19/74] 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 0c912b6a6abbbd3bb5ba3484973132d9e77095e7 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 4 Apr 2023 15:10:02 +0000 Subject: [PATCH 20/74] @wip Signed-off-by: Michael Carroll --- rclcpp/include/rclcpp/executor.hpp | 2 +- rclcpp/include/rclcpp/executors.hpp | 4 +- .../static_executor_entities_collector.hpp | 357 ------------ .../static_single_threaded_executor.hpp | 179 +----- .../detail/storage_policy_common.hpp | 3 - .../thread_safe_synchronization.hpp | 3 - rclcpp/src/rclcpp/executor.cpp | 81 ++- .../executor_entities_collection.cpp | 1 + .../executors/executor_entities_collector.cpp | 5 +- .../executors/executor_notify_waitable.cpp | 3 - .../static_executor_entities_collector.cpp | 524 ------------------ .../static_single_threaded_executor.cpp | 280 ---------- .../test/rclcpp/executors/test_executors.cpp | 1 - 13 files changed, 46 insertions(+), 1397 deletions(-) delete mode 100644 rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp delete mode 100644 rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp delete mode 100644 rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 115d755626..616dd85a41 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -553,7 +553,7 @@ class Executor rclcpp::executors::ExecutorEntitiesCollection current_collection_; std::shared_ptr current_notify_waitable_; - std::shared_ptr wait_set_; + rclcpp::WaitSet wait_set_; std::deque ready_executables_; /// shutdown callback handle registered to Context diff --git a/rclcpp/include/rclcpp/executors.hpp b/rclcpp/include/rclcpp/executors.hpp index 85220a8899..bf822cab62 100644 --- a/rclcpp/include/rclcpp/executors.hpp +++ b/rclcpp/include/rclcpp/executors.hpp @@ -20,6 +20,7 @@ #include "rclcpp/executors/multi_threaded_executor.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/executors/static_single_threaded_executor.hpp" #include "rclcpp/node.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/visibility_control.hpp" @@ -50,9 +51,6 @@ spin(rclcpp::Node::SharedPtr node_ptr); namespace executors { -using rclcpp::executors::MultiThreadedExecutor; -using rclcpp::executors::SingleThreadedExecutor; - /// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted. /** * \param[in] executor The executor which will spin the node. diff --git a/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp b/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp deleted file mode 100644 index f9fd2ff672..0000000000 --- a/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp +++ /dev/null @@ -1,357 +0,0 @@ -// Copyright 2020 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__STATIC_EXECUTOR_ENTITIES_COLLECTOR_HPP_ -#define RCLCPP__EXECUTORS__STATIC_EXECUTOR_ENTITIES_COLLECTOR_HPP_ - -#include -#include -#include -#include -#include - -#include "rcl/guard_condition.h" -#include "rcl/wait.h" - -#include "rclcpp/experimental/executable_list.hpp" -#include "rclcpp/macros.hpp" -#include "rclcpp/memory_strategy.hpp" -#include "rclcpp/visibility_control.hpp" -#include "rclcpp/waitable.hpp" - -namespace rclcpp -{ -namespace executors -{ -typedef std::map> WeakCallbackGroupsToNodesMap; - -class StaticExecutorEntitiesCollector final - : public rclcpp::Waitable, - public std::enable_shared_from_this -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS(StaticExecutorEntitiesCollector) - - // Constructor - RCLCPP_PUBLIC - StaticExecutorEntitiesCollector() = default; - - // Destructor - RCLCPP_PUBLIC - ~StaticExecutorEntitiesCollector(); - - /// Initialize StaticExecutorEntitiesCollector - /** - * \param p_wait_set A reference to the wait set to be used in the executor - * \param memory_strategy Shared pointer to the memory strategy to set. - * \throws std::runtime_error if memory strategy is null - */ - RCLCPP_PUBLIC - void - init( - rcl_wait_set_t * p_wait_set, - rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy); - - /// Finalize StaticExecutorEntitiesCollector to clear resources - RCLCPP_PUBLIC - bool - is_init() {return initialized_;} - - RCLCPP_PUBLIC - void - fini(); - - /// Execute the waitable. - RCLCPP_PUBLIC - void - execute(std::shared_ptr & data) override; - - /// Take the data so that it can be consumed with `execute`. - /** - * For `StaticExecutorEntitiesCollector`, this always return `nullptr`. - * \sa rclcpp::Waitable::take_data() - */ - RCLCPP_PUBLIC - std::shared_ptr - take_data() override; - - /// Function to add_handles_to_wait_set and wait for work and - /** - * block until the wait set is ready or until the timeout has been exceeded. - * \throws std::runtime_error if wait set couldn't be cleared or filled. - * \throws any rcl errors from rcl_wait, \see rclcpp::exceptions::throw_from_rcl_error() - */ - RCLCPP_PUBLIC - void - refresh_wait_set(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); - - /** - * \throws std::runtime_error if it couldn't add guard condition to wait set - */ - RCLCPP_PUBLIC - void - add_to_wait_set(rcl_wait_set_t * wait_set) override; - - RCLCPP_PUBLIC - size_t - get_number_of_ready_guard_conditions() override; - - /// Add a callback group to an executor. - /** - * \see rclcpp::Executor::add_callback_group - */ - RCLCPP_PUBLIC - bool - add_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr); - - /// Add a callback group to an executor. - /** - * \see rclcpp::Executor::add_callback_group - * \return boolean whether the node from the callback group is new - */ - RCLCPP_PUBLIC - bool - add_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); - - /// Remove a callback group from the executor. - /** - * \see rclcpp::Executor::remove_callback_group - */ - RCLCPP_PUBLIC - bool - remove_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr); - - /// Remove a callback group from the executor. - /** - * \see rclcpp::Executor::remove_callback_group_from_map - */ - RCLCPP_PUBLIC - bool - remove_callback_group_from_map( - rclcpp::CallbackGroup::SharedPtr group_ptr, - WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); - - /** - * \see rclcpp::Executor::add_node() - * \throw std::runtime_error if node was already added - */ - RCLCPP_PUBLIC - bool - add_node( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr); - - /** - * \see rclcpp::Executor::remove_node() - * \throw std::runtime_error if no guard condition is associated with node. - */ - RCLCPP_PUBLIC - bool - remove_node( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr); - - RCLCPP_PUBLIC - std::vector - get_all_callback_groups(); - - /// Get callback groups that belong to executor. - /** - * \see rclcpp::Executor::get_manually_added_callback_groups() - */ - RCLCPP_PUBLIC - std::vector - get_manually_added_callback_groups(); - - /// Get callback groups that belong to executor. - /** - * \see rclcpp::Executor::get_automatically_added_callback_groups_from_nodes() - */ - RCLCPP_PUBLIC - std::vector - get_automatically_added_callback_groups_from_nodes(); - - /// Complete all available queued work without blocking. - /** - * This function checks if after the guard condition was triggered - * (or a spurious wakeup happened) we are really ready to execute - * i.e. re-collect entities - */ - RCLCPP_PUBLIC - bool - is_ready(rcl_wait_set_t * wait_set) override; - - /// Return number of timers - /** - * \return number of timers - */ - RCLCPP_PUBLIC - size_t - get_number_of_timers() {return exec_list_.number_of_timers;} - - /// Return number of subscriptions - /** - * \return number of subscriptions - */ - RCLCPP_PUBLIC - size_t - get_number_of_subscriptions() {return exec_list_.number_of_subscriptions;} - - /// Return number of services - /** - * \return number of services - */ - RCLCPP_PUBLIC - size_t - get_number_of_services() {return exec_list_.number_of_services;} - - /// Return number of clients - /** - * \return number of clients - */ - RCLCPP_PUBLIC - size_t - get_number_of_clients() {return exec_list_.number_of_clients;} - - /// Return number of waitables - /** - * \return number of waitables - */ - RCLCPP_PUBLIC - size_t - get_number_of_waitables() {return exec_list_.number_of_waitables;} - - /** Return a SubscritionBase Sharedptr by index. - * \param[in] i The index of the SubscritionBase - * \return a SubscritionBase shared pointer - * \throws std::out_of_range if the argument is higher than the size of the structrue. - */ - RCLCPP_PUBLIC - rclcpp::SubscriptionBase::SharedPtr - get_subscription(size_t i) {return exec_list_.subscription[i];} - - /** Return a TimerBase Sharedptr by index. - * \param[in] i The index of the TimerBase - * \return a TimerBase shared pointer - * \throws std::out_of_range if the argument is higher than the size. - */ - RCLCPP_PUBLIC - rclcpp::TimerBase::SharedPtr - get_timer(size_t i) {return exec_list_.timer[i];} - - /** Return a ServiceBase Sharedptr by index. - * \param[in] i The index of the ServiceBase - * \return a ServiceBase shared pointer - * \throws std::out_of_range if the argument is higher than the size. - */ - RCLCPP_PUBLIC - rclcpp::ServiceBase::SharedPtr - get_service(size_t i) {return exec_list_.service[i];} - - /** Return a ClientBase Sharedptr by index - * \param[in] i The index of the ClientBase - * \return a ClientBase shared pointer - * \throws std::out_of_range if the argument is higher than the size. - */ - RCLCPP_PUBLIC - rclcpp::ClientBase::SharedPtr - get_client(size_t i) {return exec_list_.client[i];} - - /** Return a Waitable Sharedptr by index - * \param[in] i The index of the Waitable - * \return a Waitable shared pointer - * \throws std::out_of_range if the argument is higher than the size. - */ - RCLCPP_PUBLIC - rclcpp::Waitable::SharedPtr - get_waitable(size_t i) {return exec_list_.waitable[i];} - -private: - /// Function to reallocate space for entities in the wait set. - /** - * \throws std::runtime_error if wait set couldn't be cleared or resized. - */ - void - prepare_wait_set(); - - void - fill_executable_list(); - - void - fill_memory_strategy(); - - /// Return true if the node belongs to the collector - /** - * \param[in] node_ptr a node base interface shared pointer - * \param[in] weak_groups_to_nodes map to nodes to lookup - * \return boolean whether a node belongs the collector - */ - bool - has_node( - const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) const; - - /// Add all callback groups that can be automatically added by any executor - /// and is not already associated with an executor from nodes - /// that are associated with executor - /** - * \see rclcpp::Executor::add_callback_groups_from_nodes_associated_to_executor() - */ - void - add_callback_groups_from_nodes_associated_to_executor(); - - void - fill_executable_list_from_map(const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); - - /// Memory strategy: an interface for handling user-defined memory allocation strategies. - rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy_; - - // maps callback groups to nodes. - WeakCallbackGroupsToNodesMap weak_groups_associated_with_executor_to_nodes_; - // maps callback groups to nodes. - WeakCallbackGroupsToNodesMap weak_groups_to_nodes_associated_with_executor_; - - typedef std::map> - WeakNodesToGuardConditionsMap; - WeakNodesToGuardConditionsMap weak_nodes_to_guard_conditions_; - - /// List of weak nodes registered in the static executor - std::list weak_nodes_; - - // Mutex to protect vector of new nodes. - std::mutex new_nodes_mutex_; - std::vector new_nodes_; - - /// Wait set for managing entities that the rmw layer waits on. - rcl_wait_set_t * p_wait_set_ = nullptr; - - /// Executable list: timers, subscribers, clients, services and waitables - rclcpp::experimental::ExecutableList exec_list_; - - /// Bool to check if the entities collector has been initialized - bool initialized_ = false; -}; - -} // namespace executors -} // namespace rclcpp - -#endif // RCLCPP__EXECUTORS__STATIC_EXECUTOR_ENTITIES_COLLECTOR_HPP_ diff --git a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp index 5294605eaf..23c7ee85cd 100644 --- a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp @@ -15,24 +15,8 @@ #ifndef RCLCPP__EXECUTORS__STATIC_SINGLE_THREADED_EXECUTOR_HPP_ #define RCLCPP__EXECUTORS__STATIC_SINGLE_THREADED_EXECUTOR_HPP_ -#include -#include -#include -#include -#include -#include - -#include "rmw/rmw.h" - #include "rclcpp/executor.hpp" -#include "rclcpp/executors/static_executor_entities_collector.hpp" -#include "rclcpp/experimental/executable_list.hpp" -#include "rclcpp/macros.hpp" -#include "rclcpp/memory_strategies.hpp" -#include "rclcpp/node.hpp" -#include "rclcpp/rate.hpp" -#include "rclcpp/utilities.hpp" -#include "rclcpp/visibility_control.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" namespace rclcpp { @@ -55,167 +39,8 @@ namespace executors * exec.spin(); * exec.remove_node(node); */ -class StaticSingleThreadedExecutor : public rclcpp::Executor -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS(StaticSingleThreadedExecutor) - - /// Default constructor. See the default constructor for Executor. - RCLCPP_PUBLIC - explicit StaticSingleThreadedExecutor( - const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions()); - - /// Default destrcutor. - RCLCPP_PUBLIC - virtual ~StaticSingleThreadedExecutor(); - - /// Static executor implementation of spin. - /** - * This function will block until work comes in, execute it, and keep blocking. - * It will only be interrupted by a CTRL-C (managed by the global signal handler). - * \throws std::runtime_error when spin() called while already spinning - */ - RCLCPP_PUBLIC - void - spin() override; - - /// Static executor implementation of spin some - /** - * This non-blocking function will execute entities that - * were ready when this API was called, until timeout or no - * more work available. Entities that got ready while - * executing work, won't be taken into account here. - * - * Example: - * while(condition) { - * spin_some(); - * sleep(); // User should have some sync work or - * // sleep to avoid a 100% CPU usage - * } - */ - RCLCPP_PUBLIC - void - spin_some(std::chrono::nanoseconds max_duration = std::chrono::nanoseconds(0)) override; - - /// Static executor implementation of spin all - /** - * This non-blocking function will execute entities until timeout (must be >= 0) - * or no more work available. - * If timeout is `0`, potentially it blocks forever until no more work is available. - * If new entities get ready while executing work available, they will be executed - * as long as the timeout hasn't expired. - * - * Example: - * while(condition) { - * spin_all(); - * sleep(); // User should have some sync work or - * // sleep to avoid a 100% CPU usage - * } - */ - RCLCPP_PUBLIC - void - spin_all(std::chrono::nanoseconds max_duration) override; - - /// Add a callback group to an executor. - /** - * \sa rclcpp::Executor::add_callback_group - */ - RCLCPP_PUBLIC - void - add_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - bool notify = true) override; - - /// Remove callback group from the executor - /** - * \sa rclcpp::Executor::remove_callback_group - */ - RCLCPP_PUBLIC - void - remove_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr, - bool notify = true) override; - - /// Add a node to the executor. - /** - * \sa rclcpp::Executor::add_node - */ - RCLCPP_PUBLIC - void - add_node( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - bool notify = true) override; - - /// Convenience function which takes Node and forwards NodeBaseInterface. - /** - * \sa rclcpp::StaticSingleThreadedExecutor::add_node - */ - RCLCPP_PUBLIC - void - add_node(std::shared_ptr node_ptr, bool notify = true) override; - - /// Remove a node from the executor. - /** - * \sa rclcpp::Executor::remove_node - */ - RCLCPP_PUBLIC - void - remove_node( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - bool notify = true) override; - - /// Convenience function which takes Node and forwards NodeBaseInterface. - /** - * \sa rclcpp::Executor::remove_node - */ - RCLCPP_PUBLIC - void - remove_node(std::shared_ptr node_ptr, bool notify = true) override; - - RCLCPP_PUBLIC - std::vector - get_all_callback_groups() override; - - /// Get callback groups that belong to executor. - /** - * \sa rclcpp::Executor::get_manually_added_callback_groups() - */ - RCLCPP_PUBLIC - std::vector - get_manually_added_callback_groups() override; - - /// Get callback groups that belong to executor. - /** - * \sa rclcpp::Executor::get_automatically_added_callback_groups_from_nodes() - */ - RCLCPP_PUBLIC - std::vector - get_automatically_added_callback_groups_from_nodes() override; - -protected: - /** - * @brief Executes ready executables from wait set. - * @param spin_once if true executes only the first ready executable. - * @return true if any executable was ready. - */ - RCLCPP_PUBLIC - bool - execute_ready_executables(bool spin_once = false); - - RCLCPP_PUBLIC - void - spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive); - - RCLCPP_PUBLIC - void - spin_once_impl(std::chrono::nanoseconds timeout) override; - -private: - RCLCPP_DISABLE_COPY(StaticSingleThreadedExecutor) - StaticExecutorEntitiesCollector::SharedPtr entities_collector_; -}; +using StaticSingleThreadedExecutor = SingleThreadedExecutor; } // namespace executors } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp index 69b744877c..738657e85c 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp @@ -80,7 +80,6 @@ class StoragePolicyCommon throw std::runtime_error("unexpected condition, fixed storage policy needs pruning"); } // Flag for pruning. - std::cout << "waitable needs pruning" << std::endl; needs_pruning_ = true; continue; } @@ -88,8 +87,6 @@ class StoragePolicyCommon rclcpp::Waitable & waitable = *waitable_ptr_pair.second; subscriptions_from_waitables += waitable.get_number_of_ready_subscriptions(); guard_conditions_from_waitables += waitable.get_number_of_ready_guard_conditions(); - std::cout << "guard_conditions_from_waitables: " << guard_conditions_from_waitables << - std::endl; timers_from_waitables += waitable.get_number_of_ready_timers(); clients_from_waitables += waitable.get_number_of_ready_clients(); services_from_waitables += waitable.get_number_of_ready_services(); diff --git a/rclcpp/include/rclcpp/wait_set_policies/thread_safe_synchronization.hpp b/rclcpp/include/rclcpp/wait_set_policies/thread_safe_synchronization.hpp index a7bd5ecf80..4a4fb16547 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/thread_safe_synchronization.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/thread_safe_synchronization.hpp @@ -279,11 +279,9 @@ class ThreadSafeSynchronization : public detail::SynchronizationPolicyCommon // storage wait sets since they cannot be changed after construction. // This will also clear the wait set and re-add all the entities, which // prepares it to be waited on again. - std::cout << "rebuild_rcl_wait_set" << std::endl; rebuild_rcl_wait_set(); } - std::cout << "get_rcl_wait_set" << std::endl; rcl_wait_set_t & rcl_wait_set = get_rcl_wait_set(); // Wait unconditionally until timeout condition occurs since we assume @@ -299,7 +297,6 @@ class ThreadSafeSynchronization : public detail::SynchronizationPolicyCommon // It is ok to wait while not having the lock acquired, because the state // in the rcl wait set will not be updated until this method calls // rebuild_rcl_wait_set(). - std::cout << "rcl_wait" << std::endl; rcl_ret_t ret = rcl_wait(&rcl_wait_set, time_left_to_wait_ns.count()); if (RCL_RET_OK == ret) { // Something has become ready in the wait set, first check if it was diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 29a30d64e2..07f5766e24 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -53,7 +53,7 @@ Executor::Executor(const rclcpp::ExecutorOptions & options) })), collector_(notify_waitable_), current_notify_waitable_(notify_waitable_), - wait_set_(std::make_shared()) + wait_set_({}, {}, {}, {}, {}, {}, options.context) { // Store the context for later use. context_ = options.context; @@ -79,37 +79,37 @@ Executor::~Executor() current_collection_.timers.update( {}, - [this](auto timer) {wait_set_->add_timer(timer);}, - [this](auto timer) {wait_set_->remove_timer(timer);}); + [this](auto timer) {wait_set_.add_timer(timer);}, + [this](auto timer) {wait_set_.remove_timer(timer);}); current_collection_.subscriptions.update( {}, [this](auto subscription) { - wait_set_->add_subscription(subscription, kDefaultSubscriptionMask); + wait_set_.add_subscription(subscription, kDefaultSubscriptionMask); }, [this](auto subscription) { - wait_set_->remove_subscription(subscription, kDefaultSubscriptionMask); + wait_set_.remove_subscription(subscription, kDefaultSubscriptionMask); }); current_collection_.clients.update( {}, - [this](auto client) {wait_set_->add_client(client);}, - [this](auto client) {wait_set_->remove_client(client);}); + [this](auto client) {wait_set_.add_client(client);}, + [this](auto client) {wait_set_.remove_client(client);}); current_collection_.services.update( {}, - [this](auto service) {wait_set_->add_service(service);}, - [this](auto service) {wait_set_->remove_service(service);}); + [this](auto service) {wait_set_.add_service(service);}, + [this](auto service) {wait_set_.remove_service(service);}); current_collection_.guard_conditions.update( {}, - [this](auto guard_condition) {wait_set_->add_guard_condition(guard_condition);}, - [this](auto guard_condition) {wait_set_->remove_guard_condition(guard_condition);}); + [this](auto guard_condition) {wait_set_.add_guard_condition(guard_condition);}, + [this](auto guard_condition) {wait_set_.remove_guard_condition(guard_condition);}); current_collection_.waitables.update( {}, - [this](auto waitable) {wait_set_->add_waitable(waitable);}, - [this](auto waitable) {wait_set_->remove_waitable(waitable);}); + [this](auto waitable) {wait_set_.add_waitable(waitable);}, + [this](auto waitable) {wait_set_.remove_waitable(waitable);}); // Remove shutdown callback handle registered to Context if (!context_->remove_on_shutdown_callback(shutdown_callback_handle_)) { @@ -124,6 +124,7 @@ std::vector Executor::get_all_callback_groups() { std::lock_guard lock(mutex_); + this->collector_.update_collections(); return this->collector_.get_all_callback_groups(); } @@ -131,6 +132,7 @@ std::vector Executor::get_manually_added_callback_groups() { std::lock_guard lock(mutex_); + this->collector_.update_collections(); return this->collector_.get_manually_added_callback_groups(); } @@ -138,6 +140,7 @@ std::vector Executor::get_automatically_added_callback_groups_from_nodes() { std::lock_guard lock(mutex_); + this->collector_.update_collections(); return this->collector_.get_automatically_added_callback_groups(); } @@ -148,10 +151,7 @@ Executor::add_callback_group( bool notify) { (void) node_ptr; - { - std::lock_guard lock(mutex_); - this->collector_.add_callback_group(group_ptr); - } + this->collector_.add_callback_group(group_ptr); if (notify) { // Interrupt waiting to handle removed callback group @@ -168,10 +168,7 @@ Executor::add_callback_group( void Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) { - { - std::lock_guard lock(mutex_); - this->collector_.add_node(node_ptr); - } + this->collector_.add_node(node_ptr); if (notify) { // Interrupt waiting to handle removed callback group @@ -190,10 +187,7 @@ Executor::remove_callback_group( rclcpp::CallbackGroup::SharedPtr group_ptr, bool notify) { - { - std::lock_guard lock(mutex_); - this->collector_.remove_callback_group(group_ptr); - } + this->collector_.remove_callback_group(group_ptr); if (notify) { // Interrupt waiting to handle removed callback group @@ -216,10 +210,8 @@ Executor::add_node(std::shared_ptr node_ptr, bool notify) void Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) { - { - std::lock_guard lock(mutex_); - this->collector_.remove_node(node_ptr); - } + this->collector_.remove_node(node_ptr); + this->collect_entities(); if (notify) { // Interrupt waiting to handle removed callback group @@ -529,44 +521,47 @@ Executor::collect_entities() if (notify_waitable_) { // Make a copy of notify waitable so we can continue to mutate the original // one outside of the execute loop. - *current_notify_waitable_ = *notify_waitable_; + *current_notify_waitable_.get() = *notify_waitable_.get(); + auto notify_waitable = std::static_pointer_cast(current_notify_waitable_); collection.waitables.insert({notify_waitable.get(), {notify_waitable, {}}}); } current_collection_.timers.update( collection.timers, - [this](auto timer) {wait_set_->add_timer(timer);}, - [this](auto timer) {wait_set_->remove_timer(timer);}); + [this](auto timer) {wait_set_.add_timer(timer);}, + [this](auto timer) {wait_set_.remove_timer(timer);}); current_collection_.subscriptions.update( collection.subscriptions, [this](auto subscription) { - wait_set_->add_subscription(subscription, kDefaultSubscriptionMask); + wait_set_.add_subscription(subscription, kDefaultSubscriptionMask); }, [this](auto subscription) { - wait_set_->remove_subscription(subscription, kDefaultSubscriptionMask); + wait_set_.remove_subscription(subscription, kDefaultSubscriptionMask); }); current_collection_.clients.update( collection.clients, - [this](auto client) {wait_set_->add_client(client);}, - [this](auto client) {wait_set_->remove_client(client);}); + [this](auto client) {wait_set_.add_client(client);}, + [this](auto client) {wait_set_.remove_client(client);}); current_collection_.services.update( collection.services, - [this](auto service) {wait_set_->add_service(service);}, - [this](auto service) {wait_set_->remove_service(service);}); + [this](auto service) {wait_set_.add_service(service);}, + [this](auto service) {wait_set_.remove_service(service);}); current_collection_.guard_conditions.update( collection.guard_conditions, - [this](auto guard_condition) {wait_set_->add_guard_condition(guard_condition);}, - [this](auto guard_condition) {wait_set_->remove_guard_condition(guard_condition);}); + [this](auto guard_condition) {wait_set_.add_guard_condition(guard_condition);}, + [this](auto guard_condition) {wait_set_.remove_guard_condition(guard_condition);}); current_collection_.waitables.update( collection.waitables, - [this](auto waitable) {wait_set_->add_waitable(waitable);}, - [this](auto waitable) {wait_set_->remove_waitable(waitable);}); + [this](auto waitable) { + wait_set_.add_waitable(waitable); + }, + [this](auto waitable) {wait_set_.remove_waitable(waitable);}); } void @@ -578,7 +573,7 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) this->collect_entities(); } - auto wait_result = wait_set_->wait(timeout); + auto wait_result = wait_set_.wait(timeout); if (wait_result.kind() == WaitResultKind::Empty) { RCUTILS_LOG_WARN_NAMED( diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp index 567b28014e..4f796f0e62 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp @@ -193,6 +193,7 @@ ready_executables( rclcpp::AnyExecutable exec; exec.waitable = waitable; exec.callback_group = group; + exec.data = waitable->take_data(); ret.push_back(exec); } } diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp index d4261b1083..525e0e87f0 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp @@ -105,7 +105,8 @@ void ExecutorEntitiesCollector::remove_node( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) { - if (!node_ptr->get_associated_with_executor_atomic().load()) { + std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); + if (!has_executor.exchange(false)) { throw std::runtime_error( std::string("Node '") + node_ptr->get_fully_qualified_name() + "' needs to be associated with an executor."); @@ -338,7 +339,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."); } } } diff --git a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp index 6d9fc01361..bcc432ec87 100644 --- a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp +++ b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp @@ -67,9 +67,7 @@ ExecutorNotifyWaitable::add_to_wait_set(rcl_wait_set_t * wait_set) bool ExecutorNotifyWaitable::is_ready(rcl_wait_set_t * wait_set) { - std::cout << "ExecutorNotifyWaitable::is_ready" << std::endl; 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]; @@ -90,7 +88,6 @@ ExecutorNotifyWaitable::is_ready(rcl_wait_set_t * wait_set) void ExecutorNotifyWaitable::execute(std::shared_ptr & data) { - std::cout << "ExecutorNotifyWaitable::execute" << std::endl; (void) data; this->execute_callback_(); } diff --git a/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp deleted file mode 100644 index 6fd0b56a85..0000000000 --- a/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp +++ /dev/null @@ -1,524 +0,0 @@ -// Copyright 2020 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/static_executor_entities_collector.hpp" - -#include -#include -#include -#include -#include -#include - -#include "rclcpp/memory_strategy.hpp" -#include "rclcpp/executors/static_single_threaded_executor.hpp" -#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp" - -using rclcpp::executors::StaticExecutorEntitiesCollector; - -StaticExecutorEntitiesCollector::~StaticExecutorEntitiesCollector() -{ - // Disassociate all callback groups and thus nodes. - for (const auto & pair : weak_groups_associated_with_executor_to_nodes_) { - auto group = pair.first.lock(); - if (group) { - std::atomic_bool & has_executor = group->get_associated_with_executor_atomic(); - has_executor.store(false); - } - } - for (const auto & pair : weak_groups_to_nodes_associated_with_executor_) { - auto group = pair.first.lock(); - if (group) { - std::atomic_bool & has_executor = group->get_associated_with_executor_atomic(); - has_executor.store(false); - } - } - // Disassociate all nodes - for (const auto & weak_node : weak_nodes_) { - auto node = weak_node.lock(); - if (node) { - std::atomic_bool & has_executor = node->get_associated_with_executor_atomic(); - has_executor.store(false); - } - } - weak_groups_associated_with_executor_to_nodes_.clear(); - weak_groups_to_nodes_associated_with_executor_.clear(); - exec_list_.clear(); - weak_nodes_.clear(); - weak_nodes_to_guard_conditions_.clear(); -} - -void -StaticExecutorEntitiesCollector::init( - rcl_wait_set_t * p_wait_set, - rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy) -{ - // Empty initialize executable list - exec_list_ = rclcpp::experimental::ExecutableList(); - // Get executor's wait_set_ pointer - p_wait_set_ = p_wait_set; - // Get executor's memory strategy ptr - if (memory_strategy == nullptr) { - throw std::runtime_error("Received NULL memory strategy in executor waitable."); - } - memory_strategy_ = memory_strategy; - - // Get memory strategy and executable list. Prepare wait_set_ - std::shared_ptr shared_ptr; - execute(shared_ptr); - - // The entities collector is now initialized - initialized_ = true; -} - -void -StaticExecutorEntitiesCollector::fini() -{ - memory_strategy_->clear_handles(); - exec_list_.clear(); -} - -std::shared_ptr -StaticExecutorEntitiesCollector::take_data() -{ - return nullptr; -} - -void -StaticExecutorEntitiesCollector::execute(std::shared_ptr & data) -{ - (void) data; - // Fill memory strategy with entities coming from weak_nodes_ - fill_memory_strategy(); - // Fill exec_list_ with entities coming from weak_nodes_ (same as memory strategy) - fill_executable_list(); - // Resize the wait_set_ based on memory_strategy handles (rcl_wait_set_resize) - prepare_wait_set(); - // Add new nodes guard conditions to map - 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_shared_notify_guard_condition().get(); - } - } - new_nodes_.clear(); -} - -void -StaticExecutorEntitiesCollector::fill_memory_strategy() -{ - memory_strategy_->clear_handles(); - bool has_invalid_weak_groups_or_nodes = - memory_strategy_->collect_entities(weak_groups_to_nodes_associated_with_executor_); - // Clean up any invalid nodes, if they were detected - if (has_invalid_weak_groups_or_nodes) { - std::vector invalid_group_ptrs; - for (const auto & pair : weak_groups_to_nodes_associated_with_executor_) { - auto & weak_group_ptr = pair.first; - auto & weak_node_ptr = pair.second; - if (weak_group_ptr.expired() || weak_node_ptr.expired()) { - invalid_group_ptrs.push_back(weak_group_ptr); - } - } - std::for_each( - invalid_group_ptrs.begin(), invalid_group_ptrs.end(), - [this](rclcpp::CallbackGroup::WeakPtr group_ptr) { - weak_groups_to_nodes_associated_with_executor_.erase(group_ptr); - }); - } - has_invalid_weak_groups_or_nodes = - memory_strategy_->collect_entities(weak_groups_associated_with_executor_to_nodes_); - // Clean up any invalid nodes, if they were detected - if (has_invalid_weak_groups_or_nodes) { - std::vector invalid_group_ptrs; - for (const auto & pair : weak_groups_associated_with_executor_to_nodes_) { - auto & weak_group_ptr = pair.first; - const auto & weak_node_ptr = pair.second; - if (weak_group_ptr.expired() || weak_node_ptr.expired()) { - invalid_group_ptrs.push_back(weak_group_ptr); - } - } - std::for_each( - invalid_group_ptrs.begin(), invalid_group_ptrs.end(), - [this](rclcpp::CallbackGroup::WeakPtr group_ptr) { - weak_groups_associated_with_executor_to_nodes_.erase(group_ptr); - }); - } - - // Add the static executor waitable to the memory strategy - memory_strategy_->add_waitable_handle(this->shared_from_this()); -} - -void -StaticExecutorEntitiesCollector::fill_executable_list() -{ - exec_list_.clear(); - add_callback_groups_from_nodes_associated_to_executor(); - fill_executable_list_from_map(weak_groups_associated_with_executor_to_nodes_); - fill_executable_list_from_map(weak_groups_to_nodes_associated_with_executor_); - // Add the executor's waitable to the executable list - exec_list_.add_waitable(shared_from_this()); -} -void -StaticExecutorEntitiesCollector::fill_executable_list_from_map( - const rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & - weak_groups_to_nodes) -{ - for (const auto & pair : weak_groups_to_nodes) { - auto group = pair.first.lock(); - auto node = pair.second.lock(); - if (!node || !group || !group->can_be_taken_from().load()) { - continue; - } - group->find_timer_ptrs_if( - [this](const rclcpp::TimerBase::SharedPtr & timer) { - if (timer) { - exec_list_.add_timer(timer); - } - return false; - }); - group->find_subscription_ptrs_if( - [this](const rclcpp::SubscriptionBase::SharedPtr & subscription) { - if (subscription) { - exec_list_.add_subscription(subscription); - } - return false; - }); - group->find_service_ptrs_if( - [this](const rclcpp::ServiceBase::SharedPtr & service) { - if (service) { - exec_list_.add_service(service); - } - return false; - }); - group->find_client_ptrs_if( - [this](const rclcpp::ClientBase::SharedPtr & client) { - if (client) { - exec_list_.add_client(client); - } - return false; - }); - group->find_waitable_ptrs_if( - [this](const rclcpp::Waitable::SharedPtr & waitable) { - if (waitable) { - exec_list_.add_waitable(waitable); - } - return false; - }); - } -} - -void -StaticExecutorEntitiesCollector::prepare_wait_set() -{ - // clear wait set - if (rcl_wait_set_clear(p_wait_set_) != RCL_RET_OK) { - throw std::runtime_error("Couldn't clear wait set"); - } - - // The size of waitables are accounted for in size of the other entities - rcl_ret_t ret = rcl_wait_set_resize( - p_wait_set_, memory_strategy_->number_of_ready_subscriptions(), - memory_strategy_->number_of_guard_conditions(), memory_strategy_->number_of_ready_timers(), - memory_strategy_->number_of_ready_clients(), memory_strategy_->number_of_ready_services(), - memory_strategy_->number_of_ready_events()); - - if (RCL_RET_OK != ret) { - throw std::runtime_error( - std::string("Couldn't resize the wait set: ") + rcl_get_error_string().str); - } -} - -void -StaticExecutorEntitiesCollector::refresh_wait_set(std::chrono::nanoseconds timeout) -{ - // clear wait set (memset to '0' all wait_set_ entities - // but keeps the wait_set_ number of entities) - if (rcl_wait_set_clear(p_wait_set_) != RCL_RET_OK) { - throw std::runtime_error("Couldn't clear wait set"); - } - - if (!memory_strategy_->add_handles_to_wait_set(p_wait_set_)) { - throw std::runtime_error("Couldn't fill wait set"); - } - - rcl_ret_t status = - rcl_wait(p_wait_set_, std::chrono::duration_cast(timeout).count()); - - if (status == RCL_RET_WAIT_SET_EMPTY) { - RCUTILS_LOG_WARN_NAMED( - "rclcpp", - "empty wait set received in rcl_wait(). This should never happen."); - } else if (status != RCL_RET_OK && status != RCL_RET_TIMEOUT) { - using rclcpp::exceptions::throw_from_rcl_error; - throw_from_rcl_error(status, "rcl_wait() failed"); - } -} - -void -StaticExecutorEntitiesCollector::add_to_wait_set(rcl_wait_set_t * wait_set) -{ - // Add waitable guard conditions (one for each registered node) into the wait set. - for (const auto & pair : weak_nodes_to_guard_conditions_) { - auto & gc = pair.second; - detail::add_guard_condition_to_rcl_wait_set(*wait_set, *gc); - } -} - -size_t StaticExecutorEntitiesCollector::get_number_of_ready_guard_conditions() -{ - std::lock_guard guard{new_nodes_mutex_}; - return weak_nodes_to_guard_conditions_.size() + new_nodes_.size(); -} - -bool -StaticExecutorEntitiesCollector::add_node( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) -{ - bool is_new_node = false; - // 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("Node has already been added to an executor."); - } - node_ptr->for_each_callback_group( - [this, node_ptr, &is_new_node](rclcpp::CallbackGroup::SharedPtr group_ptr) - { - if ( - !group_ptr->get_associated_with_executor_atomic().load() && - group_ptr->automatically_add_to_executor_with_node()) - { - is_new_node = (add_callback_group( - group_ptr, - node_ptr, - weak_groups_to_nodes_associated_with_executor_) || - is_new_node); - } - }); - weak_nodes_.push_back(node_ptr); - return is_new_node; -} - -bool -StaticExecutorEntitiesCollector::add_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) -{ - // If the callback_group already has an executor - 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."); - } - bool is_new_node = !has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_) && - !has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_); - rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr; - auto insert_info = weak_groups_to_nodes.insert( - std::make_pair(weak_group_ptr, node_ptr)); - bool was_inserted = insert_info.second; - if (!was_inserted) { - throw std::runtime_error("Callback group was already added to executor."); - } - if (is_new_node) { - std::lock_guard guard{new_nodes_mutex_}; - new_nodes_.push_back(node_ptr); - return true; - } - return false; -} - -bool -StaticExecutorEntitiesCollector::add_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) -{ - return add_callback_group(group_ptr, node_ptr, weak_groups_associated_with_executor_to_nodes_); -} - -bool -StaticExecutorEntitiesCollector::remove_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr) -{ - return this->remove_callback_group_from_map( - group_ptr, - weak_groups_associated_with_executor_to_nodes_); -} - -bool -StaticExecutorEntitiesCollector::remove_callback_group_from_map( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) -{ - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr; - rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr; - auto iter = weak_groups_to_nodes.find(weak_group_ptr); - if (iter != weak_groups_to_nodes.end()) { - node_ptr = iter->second.lock(); - if (node_ptr == nullptr) { - throw std::runtime_error("Node must not be deleted before its callback group(s)."); - } - weak_groups_to_nodes.erase(iter); - } else { - throw std::runtime_error("Callback group needs to be associated with executor."); - } - // If the node was matched and removed, interrupt waiting. - if (!has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_) && - !has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_)) - { - rclcpp::node_interfaces::NodeBaseInterface::WeakPtr node_weak_ptr(node_ptr); - weak_nodes_to_guard_conditions_.erase(node_weak_ptr); - return true; - } - return false; -} - -bool -StaticExecutorEntitiesCollector::remove_node( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) -{ - if (!node_ptr->get_associated_with_executor_atomic().load()) { - return false; - } - bool node_found = false; - auto node_it = weak_nodes_.begin(); - while (node_it != weak_nodes_.end()) { - bool matched = (node_it->lock() == node_ptr); - if (matched) { - weak_nodes_.erase(node_it); - node_found = true; - break; - } - ++node_it; - } - if (!node_found) { - return false; - } - std::vector found_group_ptrs; - std::for_each( - weak_groups_to_nodes_associated_with_executor_.begin(), - weak_groups_to_nodes_associated_with_executor_.end(), - [&found_group_ptrs, node_ptr](std::pair key_value_pair) { - auto & weak_node_ptr = key_value_pair.second; - auto shared_node_ptr = weak_node_ptr.lock(); - auto group_ptr = key_value_pair.first.lock(); - if (shared_node_ptr == node_ptr) { - found_group_ptrs.push_back(group_ptr); - } - }); - std::for_each( - found_group_ptrs.begin(), found_group_ptrs.end(), [this] - (rclcpp::CallbackGroup::SharedPtr group_ptr) { - this->remove_callback_group_from_map( - group_ptr, - weak_groups_to_nodes_associated_with_executor_); - }); - std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); - has_executor.store(false); - return true; -} - -bool -StaticExecutorEntitiesCollector::is_ready(rcl_wait_set_t * p_wait_set) -{ - // Check wait_set guard_conditions for added/removed entities to/from a node - for (size_t i = 0; i < p_wait_set->size_of_guard_conditions; ++i) { - if (p_wait_set->guard_conditions[i] != NULL) { - auto found_guard_condition = std::find_if( - weak_nodes_to_guard_conditions_.begin(), weak_nodes_to_guard_conditions_.end(), - [&](std::pair pair) -> bool { - const rcl_guard_condition_t & rcl_gc = pair.second->get_rcl_guard_condition(); - return &rcl_gc == p_wait_set->guard_conditions[i]; - }); - if (found_guard_condition != weak_nodes_to_guard_conditions_.end()) { - return true; - } - } - } - // None of the guard conditions triggered belong to a registered node - return false; -} - -// Returns true iff the weak_groups_to_nodes map has node_ptr as the value in any of its entry. -bool -StaticExecutorEntitiesCollector::has_node( - const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - const rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & - weak_groups_to_nodes) const -{ - return std::find_if( - weak_groups_to_nodes.begin(), - weak_groups_to_nodes.end(), - [&](const WeakCallbackGroupsToNodesMap::value_type & other) -> bool { - auto other_ptr = other.second.lock(); - return other_ptr == node_ptr; - }) != weak_groups_to_nodes.end(); -} - -void -StaticExecutorEntitiesCollector::add_callback_groups_from_nodes_associated_to_executor() -{ - for (const auto & weak_node : weak_nodes_) { - auto node = weak_node.lock(); - if (node) { - node->for_each_callback_group( - [this, node](rclcpp::CallbackGroup::SharedPtr shared_group_ptr) - { - if (shared_group_ptr->automatically_add_to_executor_with_node() && - !shared_group_ptr->get_associated_with_executor_atomic().load()) - { - add_callback_group( - shared_group_ptr, - node, - weak_groups_to_nodes_associated_with_executor_); - } - }); - } - } -} - -std::vector -StaticExecutorEntitiesCollector::get_all_callback_groups() -{ - std::vector groups; - for (const auto & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) { - groups.push_back(group_node_ptr.first); - } - for (const auto & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) { - groups.push_back(group_node_ptr.first); - } - return groups; -} - -std::vector -StaticExecutorEntitiesCollector::get_manually_added_callback_groups() -{ - std::vector groups; - for (const auto & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) { - groups.push_back(group_node_ptr.first); - } - return groups; -} - -std::vector -StaticExecutorEntitiesCollector::get_automatically_added_callback_groups_from_nodes() -{ - std::vector groups; - for (const auto & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) { - groups.push_back(group_node_ptr.first); - } - return groups; -} diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp deleted file mode 100644 index 3c14b37b45..0000000000 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ /dev/null @@ -1,280 +0,0 @@ -// Copyright 2019 Nobleo Technology -// -// 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/static_single_threaded_executor.hpp" - -#include -#include -#include -#include - -#include "rcpputils/scope_exit.hpp" - -using rclcpp::executors::StaticSingleThreadedExecutor; -using rclcpp::experimental::ExecutableList; - -StaticSingleThreadedExecutor::StaticSingleThreadedExecutor( - const rclcpp::ExecutorOptions & options) -: rclcpp::Executor(options) -{ - entities_collector_ = std::make_shared(); -} - -StaticSingleThreadedExecutor::~StaticSingleThreadedExecutor() -{ - if (entities_collector_->is_init()) { - entities_collector_->fini(); - } -} - -void -StaticSingleThreadedExecutor::spin() -{ - if (spinning.exchange(true)) { - throw std::runtime_error("spin() called while already spinning"); - } - RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); - - // Set memory_strategy_ and exec_list_ based on weak_nodes_ - // Prepare wait_set_ based on memory_strategy_ - entities_collector_->init(&wait_set_, memory_strategy_); - - while (rclcpp::ok(this->context_) && spinning.load()) { - // Refresh wait set and wait for work - entities_collector_->refresh_wait_set(); - execute_ready_executables(); - } -} - -void -StaticSingleThreadedExecutor::spin_some(std::chrono::nanoseconds max_duration) -{ - // In this context a 0 input max_duration means no duration limit - if (std::chrono::nanoseconds(0) == max_duration) { - max_duration = std::chrono::nanoseconds::max(); - } - - return this->spin_some_impl(max_duration, false); -} - -void -StaticSingleThreadedExecutor::spin_all(std::chrono::nanoseconds max_duration) -{ - if (max_duration < std::chrono::nanoseconds(0)) { - throw std::invalid_argument("max_duration must be greater than or equal to 0"); - } - return this->spin_some_impl(max_duration, true); -} - -void -StaticSingleThreadedExecutor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive) -{ - // Make sure the entities collector has been initialized - if (!entities_collector_->is_init()) { - entities_collector_->init(&wait_set_, memory_strategy_); - } - - auto start = std::chrono::steady_clock::now(); - auto max_duration_not_elapsed = [max_duration, start]() { - if (std::chrono::nanoseconds(0) == max_duration) { - // told to spin forever if need be - return true; - } else if (std::chrono::steady_clock::now() - start < max_duration) { - // told to spin only for some maximum amount of time - return true; - } - // spun too long - return false; - }; - - if (spinning.exchange(true)) { - throw std::runtime_error("spin_some() called while already spinning"); - } - RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); - - while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) { - // Get executables that are ready now - entities_collector_->refresh_wait_set(std::chrono::milliseconds::zero()); - // Execute ready executables - bool work_available = execute_ready_executables(); - if (!work_available || !exhaustive) { - break; - } - } -} - -void -StaticSingleThreadedExecutor::spin_once_impl(std::chrono::nanoseconds timeout) -{ - // Make sure the entities collector has been initialized - if (!entities_collector_->is_init()) { - entities_collector_->init(&wait_set_, memory_strategy_); - } - - if (rclcpp::ok(context_) && spinning.load()) { - // Wait until we have a ready entity or timeout expired - entities_collector_->refresh_wait_set(timeout); - // Execute ready executables - execute_ready_executables(true); - } -} - -void -StaticSingleThreadedExecutor::add_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - bool notify) -{ - 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(); - } -} - -void -StaticSingleThreadedExecutor::add_node( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) -{ - 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(); - } -} - -void -StaticSingleThreadedExecutor::add_node(std::shared_ptr node_ptr, bool notify) -{ - this->add_node(node_ptr->get_node_base_interface(), notify); -} - -void -StaticSingleThreadedExecutor::remove_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr, bool notify) -{ - 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(); - } -} - -void -StaticSingleThreadedExecutor::remove_node( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) -{ - bool node_removed = entities_collector_->remove_node(node_ptr); - if (!node_removed) { - throw std::runtime_error("Node needs to be associated with this executor."); - } - // If the node was matched and removed, interrupt waiting - if (notify) { - interrupt_guard_condition_->trigger(); - } -} - -std::vector -StaticSingleThreadedExecutor::get_all_callback_groups() -{ - return entities_collector_->get_all_callback_groups(); -} - -std::vector -StaticSingleThreadedExecutor::get_manually_added_callback_groups() -{ - return entities_collector_->get_manually_added_callback_groups(); -} - -std::vector -StaticSingleThreadedExecutor::get_automatically_added_callback_groups_from_nodes() -{ - return entities_collector_->get_automatically_added_callback_groups_from_nodes(); -} - -void -StaticSingleThreadedExecutor::remove_node(std::shared_ptr node_ptr, bool notify) -{ - this->remove_node(node_ptr->get_node_base_interface(), notify); -} - -bool -StaticSingleThreadedExecutor::execute_ready_executables(bool spin_once) -{ - bool any_ready_executable = false; - - // Execute all the ready subscriptions - for (size_t i = 0; i < wait_set_.size_of_subscriptions; ++i) { - if (i < entities_collector_->get_number_of_subscriptions()) { - if (wait_set_.subscriptions[i]) { - execute_subscription(entities_collector_->get_subscription(i)); - if (spin_once) { - return true; - } - any_ready_executable = true; - } - } - } - // Execute all the ready timers - for (size_t i = 0; i < wait_set_.size_of_timers; ++i) { - if (i < entities_collector_->get_number_of_timers()) { - if (wait_set_.timers[i] && entities_collector_->get_timer(i)->is_ready()) { - auto timer = entities_collector_->get_timer(i); - timer->call(); - execute_timer(std::move(timer)); - if (spin_once) { - return true; - } - any_ready_executable = true; - } - } - } - // Execute all the ready services - for (size_t i = 0; i < wait_set_.size_of_services; ++i) { - if (i < entities_collector_->get_number_of_services()) { - if (wait_set_.services[i]) { - execute_service(entities_collector_->get_service(i)); - if (spin_once) { - return true; - } - any_ready_executable = true; - } - } - } - // Execute all the ready clients - for (size_t i = 0; i < wait_set_.size_of_clients; ++i) { - if (i < entities_collector_->get_number_of_clients()) { - if (wait_set_.clients[i]) { - execute_client(entities_collector_->get_client(i)); - if (spin_once) { - return true; - } - any_ready_executable = true; - } - } - } - // Execute all the ready waitables - for (size_t i = 0; i < entities_collector_->get_number_of_waitables(); ++i) { - auto waitable = entities_collector_->get_waitable(i); - if (waitable->is_ready(&wait_set_)) { - auto data = waitable->take_data(); - waitable->execute(data); - if (spin_once) { - return true; - } - any_ready_executable = true; - } - } - return any_ready_executable; -} diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 1c22b17c24..1fee748b78 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -130,7 +130,6 @@ TYPED_TEST(TestExecutors, detachOnDestruction) { { ExecutorType executor; executor.add_node(this->node); - std::cout << "here" << std::endl; } { ExecutorType executor; From ae9a845620f43d12df16bb7956a1af4ae17a187a Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Thu, 6 Apr 2023 15:16:06 +0000 Subject: [PATCH 21/74] Reset callback groups for multithreaded executor Signed-off-by: Michael Carroll --- rclcpp/include/rclcpp/callback_group.hpp | 3 ++ rclcpp/src/rclcpp/callback_group.cpp | 9 +++- rclcpp/src/rclcpp/executor.cpp | 45 +++++++++---------- .../executors/multi_threaded_executor.cpp | 13 ++++++ 4 files changed, 44 insertions(+), 26 deletions(-) diff --git a/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp index bb87aa7df6..51e0714c16 100644 --- a/rclcpp/include/rclcpp/callback_group.hpp +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -192,6 +192,9 @@ class CallbackGroup bool has_valid_node(); + RCLCPP_PUBLIC + size_t size() 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 3569a6bada..7ed3895b4a 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 +bool CallbackGroup::has_valid_node() { return nullptr != this->get_context_(); @@ -62,6 +62,13 @@ CallbackGroup::type() const return type_; } +size_t +CallbackGroup::size() const +{ + return subscription_ptrs_.size() + service_ptrs_.size() + client_ptrs_.size() + timer_ptrs_.size() + waitable_ptrs_.size(); + +} + void CallbackGroup::collect_all_ptrs( std::function sub_func, std::function service_func, diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 07f5766e24..74bdbbc842 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -78,37 +78,29 @@ Executor::~Executor() notify_waitable_->remove_guard_condition(shutdown_guard_condition_); current_collection_.timers.update( - {}, - [this](auto timer) {wait_set_.add_timer(timer);}, + {}, {}, [this](auto timer) {wait_set_.remove_timer(timer);}); current_collection_.subscriptions.update( - {}, - [this](auto subscription) { - wait_set_.add_subscription(subscription, kDefaultSubscriptionMask); - }, + {}, {}, [this](auto subscription) { wait_set_.remove_subscription(subscription, kDefaultSubscriptionMask); }); current_collection_.clients.update( - {}, - [this](auto client) {wait_set_.add_client(client);}, + {}, {}, [this](auto client) {wait_set_.remove_client(client);}); current_collection_.services.update( - {}, - [this](auto service) {wait_set_.add_service(service);}, + {}, {}, [this](auto service) {wait_set_.remove_service(service);}); current_collection_.guard_conditions.update( - {}, - [this](auto guard_condition) {wait_set_.add_guard_condition(guard_condition);}, + {}, {}, [this](auto guard_condition) {wait_set_.remove_guard_condition(guard_condition);}); current_collection_.waitables.update( - {}, - [this](auto waitable) {wait_set_.add_waitable(waitable);}, + {}, {}, [this](auto waitable) {wait_set_.remove_waitable(waitable);}); // Remove shutdown callback handle registered to Context @@ -153,8 +145,9 @@ Executor::add_callback_group( (void) node_ptr; this->collector_.add_callback_group(group_ptr); - if (notify) { - // Interrupt waiting to handle removed callback group + if (!spinning.load()) { + this->collect_entities(); + } else if (notify ){ try { interrupt_guard_condition_->trigger(); } catch (const rclcpp::exceptions::RCLError & ex) { @@ -170,14 +163,15 @@ Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_pt { this->collector_.add_node(node_ptr); - if (notify) { - // Interrupt waiting to handle removed callback group + if (!spinning.load()) { + this->collect_entities(); + } else if (notify ){ try { interrupt_guard_condition_->trigger(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( std::string( - "Failed to trigger guard condition on callback group remove: ") + ex.what()); + "Failed to trigger guard condition on callback group add: ") + ex.what()); } } } @@ -189,14 +183,15 @@ Executor::remove_callback_group( { this->collector_.remove_callback_group(group_ptr); - if (notify) { - // Interrupt waiting to handle removed callback group + if (!spinning.load()) { + this->collect_entities(); + } else if (notify ){ try { interrupt_guard_condition_->trigger(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( std::string( - "Failed to trigger guard condition on callback group remove: ") + ex.what()); + "Failed to trigger guard condition on callback group add: ") + ex.what()); } } } @@ -211,10 +206,10 @@ void Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) { this->collector_.remove_node(node_ptr); - this->collect_entities(); - if (notify) { - // Interrupt waiting to handle removed callback group + if (!spinning.load()) { + this->collect_entities(); + } else if (notify ){ try { interrupt_guard_condition_->trigger(); } catch (const rclcpp::exceptions::RCLError & ex) { diff --git a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp index 507d47f913..ee5dcdadad 100644 --- a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp @@ -99,6 +99,19 @@ MultiThreadedExecutor::run(size_t this_thread_number) execute_any_executable(any_exec); + if (any_exec.callback_group && + any_exec.callback_group->type() == CallbackGroupType::MutuallyExclusive && + any_exec.callback_group->size() > 1) + { + try { + interrupt_guard_condition_->trigger(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string( + "Failed to trigger guard condition on callback group change: ") + ex.what()); + } + } + // Clear the callback_group to prevent the AnyExecutable destructor from // resetting the callback group `can_be_taken_from` any_exec.callback_group.reset(); From 3db897ad2fc185d5ad1b91c9ed751785f35b8d22 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Thu, 6 Apr 2023 16:17:45 +0000 Subject: [PATCH 22/74] Avoid many small function calls when building executables Signed-off-by: Michael Carroll --- rclcpp/include/rclcpp/executor.hpp | 3 + .../executor_entities_collection.hpp | 5 +- rclcpp/src/rclcpp/executor.cpp | 17 +- .../executor_entities_collection.cpp | 156 ++++++++++-------- 4 files changed, 99 insertions(+), 82 deletions(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 616dd85a41..4520d6f8c0 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -553,7 +553,10 @@ class Executor rclcpp::executors::ExecutorEntitiesCollection current_collection_; std::shared_ptr current_notify_waitable_; + mutable std::mutex wait_set_mutex_; rclcpp::WaitSet wait_set_; + + mutable std::mutex ready_executables_mutex_; std::deque ready_executables_; /// shutdown callback handle registered to Context diff --git a/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp b/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp index 98a92ccdd8..78eef5d962 100644 --- a/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp +++ b/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp @@ -200,10 +200,11 @@ build_entities_collection( * \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 +size_t ready_executables( const ExecutorEntitiesCollection & collection, - rclcpp::WaitResult & wait_result + rclcpp::WaitResult & wait_result, + std::deque & executables ); } // namespace executors diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 74bdbbc842..4f643e9e6c 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -72,7 +72,7 @@ Executor::Executor(const rclcpp::ExecutorOptions & options) Executor::~Executor() { - std::lock_guard lock(mutex_); + std::lock_guard lock(wait_set_mutex_); notify_waitable_->remove_guard_condition(interrupt_guard_condition_); notify_waitable_->remove_guard_condition(shutdown_guard_condition_); @@ -115,7 +115,6 @@ Executor::~Executor() std::vector Executor::get_all_callback_groups() { - std::lock_guard lock(mutex_); this->collector_.update_collections(); return this->collector_.get_all_callback_groups(); } @@ -123,7 +122,6 @@ Executor::get_all_callback_groups() std::vector Executor::get_manually_added_callback_groups() { - std::lock_guard lock(mutex_); this->collector_.update_collections(); return this->collector_.get_manually_added_callback_groups(); } @@ -131,7 +129,6 @@ Executor::get_manually_added_callback_groups() std::vector Executor::get_automatically_added_callback_groups_from_nodes() { - std::lock_guard lock(mutex_); this->collector_.update_collections(); return this->collector_.get_automatically_added_callback_groups(); } @@ -507,7 +504,6 @@ Executor::execute_client( void Executor::collect_entities() { - std::lock_guard guard(mutex_); rclcpp::executors::ExecutorEntitiesCollection collection; this->collector_.update_collections(); auto callback_groups = this->collector_.get_all_callback_groups(); @@ -522,6 +518,8 @@ Executor::collect_entities() collection.waitables.insert({notify_waitable.get(), {notify_waitable, {}}}); } + std::lock_guard waitset_guard{wait_set_mutex_}; + current_collection_.timers.update( collection.timers, [this](auto timer) {wait_set_.add_timer(timer);}, @@ -564,10 +562,11 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) { TRACEPOINT(rclcpp_executor_wait_for_work, timeout.count()); - if (current_collection_.empty() || this->collector_.has_pending()) { + if (current_collection_.empty()) { this->collect_entities(); } + std::lock_guard waitset_guard{wait_set_mutex_}; auto wait_result = wait_set_.wait(timeout); if (wait_result.kind() == WaitResultKind::Empty) { @@ -576,15 +575,15 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) "empty wait set received in wait(). This should never happen."); } - std::lock_guard guard{mutex_}; - ready_executables_ = rclcpp::executors::ready_executables(current_collection_, wait_result); + std::lock_guard guard{ready_executables_mutex_}; + rclcpp::executors::ready_executables(current_collection_, wait_result, ready_executables_); } bool Executor::get_next_ready_executable(AnyExecutable & any_executable) { TRACEPOINT(rclcpp_executor_get_next_ready); - std::lock_guard guard{mutex_}; + std::lock_guard guard{ready_executables_mutex_}; if (ready_executables_.size() == 0) { return false; diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp index 4f796f0e62..43e01597c5 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp @@ -94,93 +94,106 @@ build_entities_collection( } } -template -void check_ready( - EntityCollectionType & collection, - std::deque & executables, - size_t size_of_waited_entities, - typename EntityCollectionType::Key * waited_entities, - std::function fill_executable) +size_t +ready_executables( + const ExecutorEntitiesCollection & collection, + rclcpp::WaitResult & wait_result, + std::deque & executables +) { - for (size_t ii = 0; ii < size_of_waited_entities; ++ii) { - if (!waited_entities[ii]) {continue;} - auto entity_iter = collection.find(waited_entities[ii]); - if (entity_iter != collection.end()) { + if (wait_result.kind() != rclcpp::WaitResultKind::Ready) { + return 0; + } + + size_t added = 0; + 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]) {continue;} + auto entity_iter = collection.timers.find(rcl_wait_set.timers[ii]); + if (entity_iter != collection.timers.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; + if (!entity->call()) { + continue; + } + rclcpp::AnyExecutable exec; + exec.timer = entity; exec.callback_group = callback_group; - if (fill_executable(exec, entity)) { - executables.push_back(exec); + executables.push_back(exec); + added++; + } + } + + for (size_t ii = 0; ii < rcl_wait_set.size_of_subscriptions; ++ii) { + if (!rcl_wait_set.subscriptions[ii]) {continue;} + auto entity_iter = collection.subscriptions.find(rcl_wait_set.subscriptions[ii]); + if (entity_iter != collection.subscriptions.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; + exec.subscription = entity; + exec.callback_group = callback_group; + executables.push_back(exec); + added++; } } -} -std::deque -ready_executables( - const ExecutorEntitiesCollection & collection, - rclcpp::WaitResult & wait_result -) -{ - std::deque ret; + for (size_t ii = 0; ii < rcl_wait_set.size_of_services; ++ii) { + if (!rcl_wait_set.services[ii]) {continue;} + auto entity_iter = collection.services.find(rcl_wait_set.services[ii]); + if (entity_iter != collection.services.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; + } - if (wait_result.kind() != rclcpp::WaitResultKind::Ready) { - return ret; + rclcpp::AnyExecutable exec; + exec.service = entity; + exec.callback_group = callback_group; + executables.push_back(exec); + added++; + } } - 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, - [](rclcpp::AnyExecutable & exec, auto timer) { - if (!timer->call()) { - return false; + + for (size_t ii = 0; ii < rcl_wait_set.size_of_clients; ++ii) { + if (!rcl_wait_set.clients[ii]) {continue;} + auto entity_iter = collection.clients.find(rcl_wait_set.clients[ii]); + if (entity_iter != collection.clients.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; } - exec.timer = timer; - return true; - }); - - check_ready( - collection.subscriptions, - ret, - rcl_wait_set.size_of_subscriptions, - rcl_wait_set.subscriptions, - [](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, - [](rclcpp::AnyExecutable & exec, auto service) { - exec.service = service; - return true; - }); - - check_ready( - collection.clients, - ret, - rcl_wait_set.size_of_clients, - rcl_wait_set.clients, - [](rclcpp::AnyExecutable & exec, auto client) { - exec.client = client; - return true; - }); + + rclcpp::AnyExecutable exec; + exec.client = entity; + exec.callback_group = callback_group; + executables.push_back(exec); + added++; + } + } for (auto & [handle, entry] : collection.waitables) { auto waitable = entry.entity.lock(); @@ -194,10 +207,11 @@ ready_executables( exec.waitable = waitable; exec.callback_group = group; exec.data = waitable->take_data(); - ret.push_back(exec); + executables.push_back(exec); + added++; } } - return ret; + return added; } } // namespace executors From 20d3ccaf57d0876197d1569561281b88a602f186 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Thu, 6 Apr 2023 19:47:42 +0000 Subject: [PATCH 23/74] Re-trigger guard condition if buffer has data Signed-off-by: Michael Carroll --- .../rclcpp/experimental/subscription_intra_process.hpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp index 91ea91a7c3..96c9c57d5f 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp @@ -118,6 +118,10 @@ class SubscriptionIntraProcess return nullptr; } } + + if (this->buffer_->has_data()) + this->trigger_guard_condition(); + return std::static_pointer_cast( std::make_shared>( std::pair( From cd7aaba5ca02d7e0c0e0b6768e2da39123251003 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 11 Apr 2023 10:15:43 -0500 Subject: [PATCH 24/74] 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 d8ff831e8f3d4590896a1013ca6c3286aecb7e98 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 11 Apr 2023 08:40:09 -0500 Subject: [PATCH 25/74] Trace points Signed-off-by: Michael Carroll --- rclcpp/CMakeLists.txt | 5 + rclcpp/include/rclcpp/any_executable.hpp | 43 +++- rclcpp/include/rclcpp/executor.hpp | 3 - .../executors/executor_notify_waitable.hpp | 4 +- .../static_single_threaded_executor.hpp | 23 +- .../subscription_intra_process.hpp | 5 +- .../subscription_intra_process_buffer.hpp | 1 + .../detail/storage_policy_common.hpp | 224 ++++++++++-------- .../wait_set_policies/dynamic_storage.hpp | 23 +- .../sequential_synchronization.hpp | 42 ++-- rclcpp/include/rclcpp/wait_set_template.hpp | 6 + rclcpp/package.xml | 1 + rclcpp/src/rclcpp/any_executable.cpp | 56 ++++- rclcpp/src/rclcpp/executor.cpp | 26 +- .../executor_entities_collection.cpp | 115 +++++---- .../executors/executor_notify_waitable.cpp | 33 ++- .../executors/single_threaded_executor.cpp | 27 ++- .../static_single_threaded_executor.cpp | 61 +++++ rclcpp_action/src/server.cpp | 3 + 19 files changed, 484 insertions(+), 217 deletions(-) create mode 100644 rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 72fb24de3b..f35126e9e6 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -21,6 +21,8 @@ find_package(rosidl_typesupport_cpp REQUIRED) find_package(statistics_msgs REQUIRED) find_package(tracetools REQUIRED) +find_package(tracy_vendor REQUIRED) + # TODO(wjwwood): remove this when gtest can build on its own, when using target_compile_features() # Default to C++17 if(NOT CMAKE_CXX_STANDARD) @@ -206,6 +208,7 @@ ament_target_dependencies(${PROJECT_NAME} "rosidl_runtime_cpp" "statistics_msgs" "tracetools" + "tracy_vendor" ) # Causes the visibility macros to use dllexport rather than dllimport, @@ -241,6 +244,8 @@ ament_export_dependencies(rosidl_runtime_cpp) ament_export_dependencies(rcl_yaml_param_parser) ament_export_dependencies(statistics_msgs) ament_export_dependencies(tracetools) +ament_export_dependencies(tracy_vendor) +ament_export_dependencies(Threads) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/rclcpp/include/rclcpp/any_executable.hpp b/rclcpp/include/rclcpp/any_executable.hpp index 5d4064f452..693f1b121c 100644 --- a/rclcpp/include/rclcpp/any_executable.hpp +++ b/rclcpp/include/rclcpp/any_executable.hpp @@ -35,19 +35,46 @@ struct AnyExecutable RCLCPP_PUBLIC AnyExecutable(); + RCLCPP_PUBLIC + AnyExecutable( + const rclcpp::SubscriptionBase::SharedPtr & subscription, + const rclcpp::CallbackGroup::SharedPtr & callback_group); + + RCLCPP_PUBLIC + AnyExecutable( + const rclcpp::TimerBase::SharedPtr & timer, + const rclcpp::CallbackGroup::SharedPtr & callback_group); + + RCLCPP_PUBLIC + AnyExecutable( + const rclcpp::ServiceBase::SharedPtr & service, + const rclcpp::CallbackGroup::SharedPtr & callback_group); + + RCLCPP_PUBLIC + AnyExecutable( + const rclcpp::ClientBase::SharedPtr & client, + const rclcpp::CallbackGroup::SharedPtr & callback_group); + + RCLCPP_PUBLIC + AnyExecutable( + const rclcpp::Waitable::SharedPtr & waitable, + const rclcpp::CallbackGroup::SharedPtr & callback_group, + const std::shared_ptr & data); + + RCLCPP_PUBLIC virtual ~AnyExecutable(); // Only one of the following pointers will be set. - rclcpp::SubscriptionBase::SharedPtr subscription; - rclcpp::TimerBase::SharedPtr timer; - rclcpp::ServiceBase::SharedPtr service; - rclcpp::ClientBase::SharedPtr client; - rclcpp::Waitable::SharedPtr waitable; + rclcpp::SubscriptionBase::SharedPtr subscription = nullptr; + rclcpp::TimerBase::SharedPtr timer = nullptr; + rclcpp::ServiceBase::SharedPtr service = nullptr; + rclcpp::ClientBase::SharedPtr client = nullptr; + rclcpp::Waitable::SharedPtr waitable = nullptr; // These are used to keep the scope on the containing items - rclcpp::CallbackGroup::SharedPtr callback_group; - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base; - std::shared_ptr data; + rclcpp::CallbackGroup::SharedPtr callback_group = nullptr; + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base = nullptr; + std::shared_ptr data = nullptr; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 4520d6f8c0..616dd85a41 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -553,10 +553,7 @@ class Executor rclcpp::executors::ExecutorEntitiesCollection current_collection_; std::shared_ptr current_notify_waitable_; - mutable std::mutex wait_set_mutex_; rclcpp::WaitSet wait_set_; - - mutable std::mutex ready_executables_mutex_; std::deque ready_executables_; /// shutdown callback handle registered to Context diff --git a/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp b/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp index 9fb44e78c6..bf6ae26089 100644 --- a/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp +++ b/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp @@ -120,8 +120,8 @@ class ExecutorNotifyWaitable : public rclcpp::Waitable std::mutex guard_condition_mutex_; /// The collection of guard conditions to be waited on. - std::set> notify_guard_conditions_; + std::set> notify_guard_conditions_; }; } // namespace executors diff --git a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp index 23c7ee85cd..cb1a3df5b6 100644 --- a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp @@ -40,7 +40,28 @@ namespace executors * exec.remove_node(node); */ -using StaticSingleThreadedExecutor = SingleThreadedExecutor; +class StaticSingleThreadedExecutor : public rclcpp::Executor +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(SingleThreadedExecutor) + + /// Default constructor. See the default constructor for Executor. + RCLCPP_PUBLIC + explicit StaticSingleThreadedExecutor( + const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions()); + + /// Default destructor. + RCLCPP_PUBLIC + virtual ~StaticSingleThreadedExecutor(); + + /// Single-threaded implementation of spin. + RCLCPP_PUBLIC + void + spin() override; + +private: + RCLCPP_DISABLE_COPY(StaticSingleThreadedExecutor) +}; } // namespace executors } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp index 96c9c57d5f..ec89ebc5ef 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp @@ -119,8 +119,11 @@ class SubscriptionIntraProcess } } - if (this->buffer_->has_data()) + if (this->buffer_->has_data()) { + // If there is data still to be processed, indicate to the + // executor or waitset by triggering the guard condition. this->trigger_guard_condition(); + } return std::static_pointer_cast( std::make_shared>( diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp index 3c71512677..7577160046 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp @@ -167,6 +167,7 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff void trigger_guard_condition() override { + std::cout << "trigger_guard_condition" << std::endl; this->gc_.trigger(); } diff --git a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp index 738657e85c..de500e2660 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp @@ -28,6 +28,8 @@ #include "rclcpp/visibility_control.hpp" #include "rclcpp/waitable.hpp" +#include "tracy/Tracy.hpp" + namespace rclcpp { namespace wait_set_policies @@ -192,9 +194,8 @@ class StoragePolicyCommon size_t clients_from_waitables = 0; size_t services_from_waitables = 0; size_t events_from_waitables = 0; - for (const auto & waitable_entry : waitables) { - auto waitable_ptr_pair = get_raw_pointer_from_smart_pointer(waitable_entry.waitable); - if (nullptr == waitable_ptr_pair.second) { + for (const auto & waitable_entry: waitables) { + if (!waitable_entry.waitable) { // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. if (HasStrongOwnership) { // This will not happen in fixed sized storage, as it holds @@ -205,13 +206,13 @@ class StoragePolicyCommon needs_pruning_ = true; continue; } - rclcpp::Waitable & waitable = *waitable_ptr_pair.second; - subscriptions_from_waitables += waitable.get_number_of_ready_subscriptions(); - guard_conditions_from_waitables += waitable.get_number_of_ready_guard_conditions(); - timers_from_waitables += waitable.get_number_of_ready_timers(); - clients_from_waitables += waitable.get_number_of_ready_clients(); - services_from_waitables += waitable.get_number_of_ready_services(); - events_from_waitables += waitable.get_number_of_ready_events(); + const auto & waitable = waitable_entry.waitable; + subscriptions_from_waitables += waitable->get_number_of_ready_subscriptions(); + guard_conditions_from_waitables += waitable->get_number_of_ready_guard_conditions(); + timers_from_waitables += waitable->get_number_of_ready_timers(); + clients_from_waitables += waitable->get_number_of_ready_clients(); + services_from_waitables += waitable->get_number_of_ready_services(); + events_from_waitables += waitable->get_number_of_ready_events(); } rcl_ret_t ret = rcl_wait_set_resize( &rcl_wait_set_, @@ -234,6 +235,7 @@ class StoragePolicyCommon needs_resize_ = false; } + // Now clear the wait set, but only if it was not resized, as resizing also // clears the wait set. if (!was_resized) { @@ -244,26 +246,28 @@ class StoragePolicyCommon } // Add subscriptions. - for (const auto & subscription_entry : subscriptions) { - auto subscription_ptr_pair = - get_raw_pointer_from_smart_pointer(subscription_entry.subscription); - if (nullptr == subscription_ptr_pair.second) { - // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. - if (HasStrongOwnership) { - // This will not happen in fixed sized storage, as it holds - // shared ownership the whole time and is never in need of pruning. - throw std::runtime_error("unexpected condition, fixed storage policy needs pruning"); + { + ZoneScopedN("add_subscriptions"); + for (const auto & subscription_entry: subscriptions) { + if (!subscription_entry.subscription) { + // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. + if (HasStrongOwnership) { + // This will not happen in fixed sized storage, as it holds + // shared ownership the whole time and is never in need of pruning. + throw std::runtime_error("unexpected condition, fixed storage policy needs pruning"); + } + // Flag for pruning. + needs_pruning_ = true; + continue; + } + + rcl_ret_t ret = rcl_wait_set_add_subscription( + &rcl_wait_set_, + subscription_entry.subscription->get_subscription_handle().get(), + nullptr); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); } - // Flag for pruning. - needs_pruning_ = true; - continue; - } - rcl_ret_t ret = rcl_wait_set_add_subscription( - &rcl_wait_set_, - subscription_ptr_pair.second->get_subscription_handle().get(), - nullptr); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); } } @@ -272,8 +276,7 @@ class StoragePolicyCommon [this](const auto & inner_guard_conditions) { for (const auto & guard_condition : inner_guard_conditions) { - auto guard_condition_ptr_pair = get_raw_pointer_from_smart_pointer(guard_condition); - if (nullptr == guard_condition_ptr_pair.second) { + if (!guard_condition) { // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. if (HasStrongOwnership) { // This will not happen in fixed sized storage, as it holds @@ -286,7 +289,7 @@ class StoragePolicyCommon } rcl_ret_t ret = rcl_wait_set_add_guard_condition( &rcl_wait_set_, - &guard_condition_ptr_pair.second->get_rcl_guard_condition(), + &guard_condition->get_rcl_guard_condition(), nullptr); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); @@ -294,97 +297,108 @@ class StoragePolicyCommon } }; - // Add guard conditions. - add_guard_conditions(guard_conditions); + { + ZoneScopedN("add_guard_conditions"); + // Add guard conditions. + add_guard_conditions(guard_conditions); - // Add extra guard conditions. - add_guard_conditions(extra_guard_conditions); + // Add extra guard conditions. + add_guard_conditions(extra_guard_conditions); + } // Add timers. - for (const auto & timer : timers) { - auto timer_ptr_pair = get_raw_pointer_from_smart_pointer(timer); - if (nullptr == timer_ptr_pair.second) { - // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. - if (HasStrongOwnership) { - // This will not happen in fixed sized storage, as it holds - // shared ownership the whole time and is never in need of pruning. - throw std::runtime_error("unexpected condition, fixed storage policy needs pruning"); + { + ZoneScopedN("add_timers"); + for (const auto & timer : timers) { + if (!timer) { + // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. + if (HasStrongOwnership) { + // This will not happen in fixed sized storage, as it holds + // shared ownership the whole time and is never in need of pruning. + throw std::runtime_error("unexpected condition, fixed storage policy needs pruning"); + } + // Flag for pruning. + needs_pruning_ = true; + continue; + } + rcl_ret_t ret = rcl_wait_set_add_timer( + &rcl_wait_set_, + timer->get_timer_handle().get(), + nullptr); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); } - // Flag for pruning. - needs_pruning_ = true; - continue; - } - rcl_ret_t ret = rcl_wait_set_add_timer( - &rcl_wait_set_, - timer_ptr_pair.second->get_timer_handle().get(), - nullptr); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); } } // Add clients. - for (const auto & client : clients) { - auto client_ptr_pair = get_raw_pointer_from_smart_pointer(client); - if (nullptr == client_ptr_pair.second) { - // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. - if (HasStrongOwnership) { - // This will not happen in fixed sized storage, as it holds - // shared ownership the whole time and is never in need of pruning. - throw std::runtime_error("unexpected condition, fixed storage policy needs pruning"); + { + ZoneScopedN("add_clients"); + for (const auto & client : clients) { + if (!client) { + // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. + if (HasStrongOwnership) { + // This will not happen in fixed sized storage, as it holds + // shared ownership the whole time and is never in need of pruning. + throw std::runtime_error("unexpected condition, fixed storage policy needs pruning"); + } + // Flag for pruning. + needs_pruning_ = true; + continue; + } + rcl_ret_t ret = rcl_wait_set_add_client( + &rcl_wait_set_, + client->get_client_handle().get(), + nullptr); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); } - // Flag for pruning. - needs_pruning_ = true; - continue; - } - rcl_ret_t ret = rcl_wait_set_add_client( - &rcl_wait_set_, - client_ptr_pair.second->get_client_handle().get(), - nullptr); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); } } - // Add services. - for (const auto & service : services) { - auto service_ptr_pair = get_raw_pointer_from_smart_pointer(service); - if (nullptr == service_ptr_pair.second) { - // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. - if (HasStrongOwnership) { - // This will not happen in fixed sized storage, as it holds - // shared ownership the whole time and is never in need of pruning. - throw std::runtime_error("unexpected condition, fixed storage policy needs pruning"); + + { + ZoneScopedN("add_services"); + // Add services. + for (const auto & service : services) { + if (!service) { + // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. + if (HasStrongOwnership) { + // This will not happen in fixed sized storage, as it holds + // shared ownership the whole time and is never in need of pruning. + throw std::runtime_error("unexpected condition, fixed storage policy needs pruning"); + } + // Flag for pruning. + needs_pruning_ = true; + continue; + } + rcl_ret_t ret = rcl_wait_set_add_service( + &rcl_wait_set_, + service->get_service_handle().get(), + nullptr); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); } - // Flag for pruning. - needs_pruning_ = true; - continue; - } - rcl_ret_t ret = rcl_wait_set_add_service( - &rcl_wait_set_, - service_ptr_pair.second->get_service_handle().get(), - nullptr); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); } } - // Add waitables. - for (auto & waitable_entry : waitables) { - auto waitable_ptr_pair = get_raw_pointer_from_smart_pointer(waitable_entry.waitable); - if (nullptr == waitable_ptr_pair.second) { - // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. - if (HasStrongOwnership) { - // This will not happen in fixed sized storage, as it holds - // shared ownership the whole time and is never in need of pruning. - throw std::runtime_error("unexpected condition, fixed storage policy needs pruning"); + { + ZoneScopedN("add_waitables"); + // Add waitables. + for (auto & waitable_entry: waitables) { + if (!waitable_entry.waitable) { + // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. + if (HasStrongOwnership) { + // This will not happen in fixed sized storage, as it holds + // shared ownership the whole time and is never in need of pruning. + throw std::runtime_error("unexpected condition, fixed storage policy needs pruning"); + } + // Flag for pruning. + needs_pruning_ = true; + continue; } - // Flag for pruning. - needs_pruning_ = true; - continue; + waitable_entry.waitable->add_to_wait_set(&rcl_wait_set_); } - rclcpp::Waitable & waitable = *waitable_ptr_pair.second; - waitable.add_to_wait_set(&rcl_wait_set_); } } diff --git a/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp index 4cec85f39a..9e83e60273 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp @@ -31,6 +31,8 @@ #include "rclcpp/wait_set_policies/detail/storage_policy_common.hpp" #include "rclcpp/waitable.hpp" +#include "tracy/Tracy.hpp" + namespace rclcpp { namespace wait_set_policies @@ -204,15 +206,21 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo void storage_rebuild_rcl_wait_set(const ArrayOfExtraGuardConditions & extra_guard_conditions) { + ZoneScoped; + + this->storage_acquire_ownerships(); + this->storage_rebuild_rcl_wait_set_with_sets( - subscriptions_, - guard_conditions_, + shared_subscriptions_, + shared_guard_conditions_, extra_guard_conditions, - timers_, - clients_, - services_, - waitables_ + shared_timers_, + shared_clients_, + shared_services_, + shared_waitables_ ); + + this->storage_release_ownerships(); } template @@ -394,6 +402,7 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo void storage_acquire_ownerships() { + ZoneScoped; if (++ownership_reference_counter_ > 1) { // Avoid redundant locking. return; @@ -407,6 +416,7 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo } }; // Lock all the weak pointers and hold them until released. + lock_all(subscriptions_, shared_subscriptions_); lock_all(guard_conditions_, shared_guard_conditions_); lock_all(timers_, shared_timers_); lock_all(clients_, shared_clients_); @@ -438,6 +448,7 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo shared_ptr.reset(); } }; + reset_all(shared_subscriptions_); reset_all(shared_guard_conditions_); reset_all(shared_timers_); reset_all(shared_clients_); diff --git a/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp b/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp index be2e569c40..40ca99467c 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp @@ -34,6 +34,8 @@ #include "rclcpp/wait_set_policies/detail/synchronization_policy_common.hpp" #include "rclcpp/waitable.hpp" +#include "tracy/Tracy.hpp" + namespace rclcpp { namespace wait_set_policies @@ -252,6 +254,8 @@ class SequentialSynchronization : public detail::SynchronizationPolicyCommon // which calls this function, by acquiring shared ownership of the entites // for the duration of this function. + ZoneScoped; + // Setup looping predicate. auto start = std::chrono::steady_clock::now(); std::function should_loop = this->create_loop_predicate(time_to_wait_ns, start); @@ -277,20 +281,30 @@ class SequentialSynchronization : public detail::SynchronizationPolicyCommon auto time_left_to_wait_ns = this->calculate_time_left_to_wait(time_to_wait_ns, start); // Then wait for entities to become ready. - rcl_ret_t ret = rcl_wait(&rcl_wait_set, time_left_to_wait_ns.count()); - if (RCL_RET_OK == ret) { - // Something has become ready in the wait set, and since this class - // did not add anything to it, it is a user entity that is ready. - return create_wait_result(WaitResultKind::Ready); - } else if (RCL_RET_TIMEOUT == ret) { - // The wait set timed out, exit the loop. - break; - } else if (RCL_RET_WAIT_SET_EMPTY == ret) { - // Wait set was empty, return Empty. - return create_wait_result(WaitResultKind::Empty); - } else { - // Some other error case, throw. - rclcpp::exceptions::throw_from_rcl_error(ret); + { + rcl_ret_t ret; + { + ZoneScopedN("rcl_wait"); + ret = rcl_wait(&rcl_wait_set, time_left_to_wait_ns.count()); + } + + if (RCL_RET_OK == ret) { + // Something has become ready in the wait set, and since this class + // did not add anything to it, it is a user entity that is ready. + { + ZoneScopedN("create_wait_result"); + return create_wait_result(WaitResultKind::Ready); + } + } else if (RCL_RET_TIMEOUT == ret) { + // The wait set timed out, exit the loop. + break; + } else if (RCL_RET_WAIT_SET_EMPTY == ret) { + // Wait set was empty, return Empty. + return create_wait_result(WaitResultKind::Empty); + } else { + // Some other error case, throw. + rclcpp::exceptions::throw_from_rcl_error(ret); + } } } while (should_loop()); diff --git a/rclcpp/include/rclcpp/wait_set_template.hpp b/rclcpp/include/rclcpp/wait_set_template.hpp index c139913be8..a406c00399 100644 --- a/rclcpp/include/rclcpp/wait_set_template.hpp +++ b/rclcpp/include/rclcpp/wait_set_template.hpp @@ -35,6 +35,8 @@ #include "rclcpp/wait_result.hpp" #include "rclcpp/waitable.hpp" +#include "tracy/Tracy.hpp" + namespace rclcpp { @@ -600,6 +602,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli void prune_deleted_entities() { + ZoneScoped; // this method comes from the SynchronizationPolicy this->sync_prune_deleted_entities( [this]() { @@ -655,6 +658,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli WaitResult wait(std::chrono::duration time_to_wait = std::chrono::duration(-1)) { + ZoneScoped; auto time_to_wait_ns = std::chrono::duration_cast(time_to_wait); // ensure the ownership of the entities in the wait set is shared for the duration of wait @@ -712,6 +716,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli void wait_result_acquire() { + ZoneScoped; if (wait_result_holding_) { throw std::runtime_error("wait_result_acquire() called while already holding"); } @@ -731,6 +736,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli void wait_result_release() { + ZoneScoped; if (!wait_result_holding_) { throw std::runtime_error("wait_result_release() called while not holding"); } diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 38773e2ec2..cf4bd910dd 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -41,6 +41,7 @@ rmw statistics_msgs tracetools + tracy_vendor ament_cmake_gmock ament_cmake_google_benchmark diff --git a/rclcpp/src/rclcpp/any_executable.cpp b/rclcpp/src/rclcpp/any_executable.cpp index 769deacb11..4cee74692f 100644 --- a/rclcpp/src/rclcpp/any_executable.cpp +++ b/rclcpp/src/rclcpp/any_executable.cpp @@ -16,15 +16,57 @@ using rclcpp::AnyExecutable; -AnyExecutable::AnyExecutable() -: subscription(nullptr), - timer(nullptr), - service(nullptr), - client(nullptr), - callback_group(nullptr), - node_base(nullptr) +RCLCPP_PUBLIC +AnyExecutable::AnyExecutable() = default; + +RCLCPP_PUBLIC +AnyExecutable::AnyExecutable( + const rclcpp::SubscriptionBase::SharedPtr & subscription, + const rclcpp::CallbackGroup::SharedPtr & callback_group): + subscription(subscription), + callback_group(callback_group) +{ +} + +RCLCPP_PUBLIC +AnyExecutable::AnyExecutable( + const rclcpp::TimerBase::SharedPtr & timer, + const rclcpp::CallbackGroup::SharedPtr & callback_group): + timer(timer), + callback_group(callback_group) +{ +} + +RCLCPP_PUBLIC +AnyExecutable::AnyExecutable( + const rclcpp::ServiceBase::SharedPtr & service, + const rclcpp::CallbackGroup::SharedPtr & callback_group): + service(service), + callback_group(callback_group) +{ + +} + +RCLCPP_PUBLIC +AnyExecutable::AnyExecutable( + const rclcpp::ClientBase::SharedPtr & client, + const rclcpp::CallbackGroup::SharedPtr & callback_group): + client(client), + callback_group(callback_group) +{ +} + +RCLCPP_PUBLIC +AnyExecutable::AnyExecutable( + const rclcpp::Waitable::SharedPtr & waitable, + const rclcpp::CallbackGroup::SharedPtr & callback_group, + const std::shared_ptr & data): + waitable(waitable), + callback_group(callback_group), + data(data) {} + AnyExecutable::~AnyExecutable() { // Make sure that discarded (taken but not executed) AnyExecutable's have diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 4f643e9e6c..3812611e8a 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -37,6 +37,8 @@ #include "tracetools/tracetools.h" +#include "tracy/Tracy.hpp" + using namespace std::chrono_literals; using rclcpp::Executor; @@ -72,7 +74,7 @@ Executor::Executor(const rclcpp::ExecutorOptions & options) Executor::~Executor() { - std::lock_guard lock(wait_set_mutex_); + std::lock_guard guard(mutex_); notify_waitable_->remove_guard_condition(interrupt_guard_condition_); notify_waitable_->remove_guard_condition(shutdown_guard_condition_); @@ -333,6 +335,7 @@ Executor::cancel() void Executor::execute_any_executable(AnyExecutable & any_exec) { + ZoneScoped; if (!spinning.load()) { return; } @@ -404,6 +407,7 @@ take_and_do_error_handling( void Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) { + ZoneScoped; rclcpp::MessageInfo message_info; message_info.get_rmw_message_info().from_intra_process = false; @@ -473,12 +477,14 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) void Executor::execute_timer(rclcpp::TimerBase::SharedPtr timer) { + ZoneScoped; timer->execute_callback(); } void Executor::execute_service(rclcpp::ServiceBase::SharedPtr service) { + ZoneScoped; auto request_header = service->create_request_header(); std::shared_ptr request = service->create_request(); take_and_do_error_handling( @@ -492,6 +498,7 @@ void Executor::execute_client( rclcpp::ClientBase::SharedPtr client) { + ZoneScoped; auto request_header = client->create_request_header(); std::shared_ptr response = client->create_response(); take_and_do_error_handling( @@ -504,6 +511,8 @@ Executor::execute_client( void Executor::collect_entities() { + ZoneScoped; + rclcpp::executors::ExecutorEntitiesCollection collection; this->collector_.update_collections(); auto callback_groups = this->collector_.get_all_callback_groups(); @@ -518,8 +527,7 @@ Executor::collect_entities() collection.waitables.insert({notify_waitable.get(), {notify_waitable, {}}}); } - std::lock_guard waitset_guard{wait_set_mutex_}; - + std::lock_guard guard(mutex_); current_collection_.timers.update( collection.timers, [this](auto timer) {wait_set_.add_timer(timer);}, @@ -560,31 +568,31 @@ Executor::collect_entities() void Executor::wait_for_work(std::chrono::nanoseconds timeout) { + ZoneScoped; TRACEPOINT(rclcpp_executor_wait_for_work, timeout.count()); if (current_collection_.empty()) { this->collect_entities(); } - std::lock_guard waitset_guard{wait_set_mutex_}; + std::lock_guard guard(mutex_); auto wait_result = wait_set_.wait(timeout); - if (wait_result.kind() == WaitResultKind::Empty) { RCUTILS_LOG_WARN_NAMED( "rclcpp", "empty wait set received in wait(). This should never happen."); } - - std::lock_guard guard{ready_executables_mutex_}; rclcpp::executors::ready_executables(current_collection_, wait_result, ready_executables_); + TracyPlot("added", static_cast(ready_executables_.size())); } bool Executor::get_next_ready_executable(AnyExecutable & any_executable) { + ZoneScoped; TRACEPOINT(rclcpp_executor_get_next_ready); - std::lock_guard guard{ready_executables_mutex_}; + std::lock_guard guard(mutex_); if (ready_executables_.size() == 0) { return false; } @@ -605,6 +613,7 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable) bool Executor::get_next_executable(AnyExecutable & any_executable, std::chrono::nanoseconds timeout) { + ZoneScoped; bool success = false; // Check to see if there are any subscriptions or timers needing service // TODO(wjwwood): improve run to run efficiency of this function @@ -612,6 +621,7 @@ Executor::get_next_executable(AnyExecutable & any_executable, std::chrono::nanos // If there are none if (!success) { // Wait for subscriptions or timers to work on + std::cout << "wait_for_work" << std::endl; wait_for_work(timeout); if (!spinning.load()) { return false; diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp index 43e01597c5..1d33846f99 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp @@ -14,6 +14,8 @@ #include "rclcpp/executors/executor_entities_collection.hpp" +#include "tracy/Tracy.hpp" + namespace rclcpp { namespace executors @@ -44,6 +46,7 @@ build_entities_collection( const std::vector & callback_groups, ExecutorEntitiesCollection & collection) { + ZoneScoped; collection.clear(); for (auto weak_group_ptr : callback_groups) { @@ -101,6 +104,7 @@ ready_executables( std::deque & executables ) { + ZoneScoped; if (wait_result.kind() != rclcpp::WaitResultKind::Ready) { return 0; } @@ -108,108 +112,135 @@ ready_executables( size_t added = 0; auto rcl_wait_set = wait_result.get_wait_set().get_rcl_wait_set(); + struct CachedCallbackGroup + { + rclcpp::CallbackGroup::SharedPtr ptr = nullptr; + bool can_be_taken_from = false; + }; + + std::map> group_map; + auto group_cache = [&group_map](const rclcpp::CallbackGroup::WeakPtr & weak_cbg_ptr) -> const CachedCallbackGroup & + { + if (group_map.count(weak_cbg_ptr) == 0) + { + CachedCallbackGroup temp; + temp.ptr = weak_cbg_ptr.lock(); + if (temp.ptr) + temp.can_be_taken_from = temp.ptr->can_be_taken_from().load(); + group_map.insert({weak_cbg_ptr, temp}); + } + return group_map.find(weak_cbg_ptr)->second; + }; + + + { + ZoneScopedN("timers"); for (size_t ii = 0; ii < rcl_wait_set.size_of_timers; ++ii) { - if (!rcl_wait_set.timers[ii]) {continue;} + if (nullptr == rcl_wait_set.timers[ii]) {continue;} auto entity_iter = collection.timers.find(rcl_wait_set.timers[ii]); if (entity_iter != collection.timers.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()) { + auto group_info = group_cache(entity_iter->second.callback_group); + if (group_info.ptr && !group_info.can_be_taken_from) { continue; } + if (!entity->call()) { continue; } - - rclcpp::AnyExecutable exec; - exec.timer = entity; - exec.callback_group = callback_group; - executables.push_back(exec); + executables.push_back({entity, group_info.ptr}); added++; } } + } + { + ZoneScopedN("subscriptions"); for (size_t ii = 0; ii < rcl_wait_set.size_of_subscriptions; ++ii) { - if (!rcl_wait_set.subscriptions[ii]) {continue;} + if (nullptr == rcl_wait_set.subscriptions[ii]) {continue;} auto entity_iter = collection.subscriptions.find(rcl_wait_set.subscriptions[ii]); if (entity_iter != collection.subscriptions.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()) { + + auto group_info = group_cache(entity_iter->second.callback_group); + if (group_info.ptr && !group_info.can_be_taken_from) { continue; } - rclcpp::AnyExecutable exec; - exec.subscription = entity; - exec.callback_group = callback_group; - executables.push_back(exec); + executables.push_back({entity, group_info.ptr}); added++; } } + } + { + ZoneScopedN("services"); for (size_t ii = 0; ii < rcl_wait_set.size_of_services; ++ii) { - if (!rcl_wait_set.services[ii]) {continue;} + if (nullptr == rcl_wait_set.services[ii]) {continue;} auto entity_iter = collection.services.find(rcl_wait_set.services[ii]); if (entity_iter != collection.services.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()) { + const auto & [callback_group, can_be_taken_from] = group_cache(entity_iter->second.callback_group); + if (callback_group && !can_be_taken_from) { continue; } - - rclcpp::AnyExecutable exec; - exec.service = entity; - exec.callback_group = callback_group; - executables.push_back(exec); - added++; + executables.push_back({entity, callback_group}); } } + } + + { + ZoneScopedN("clients"); for (size_t ii = 0; ii < rcl_wait_set.size_of_clients; ++ii) { - if (!rcl_wait_set.clients[ii]) {continue;} + if (nullptr == rcl_wait_set.clients[ii]) {continue;} auto entity_iter = collection.clients.find(rcl_wait_set.clients[ii]); if (entity_iter != collection.clients.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()) { + + auto group_info = group_cache(entity_iter->second.callback_group); + if (group_info.ptr && !group_info.can_be_taken_from) { continue; } - rclcpp::AnyExecutable exec; - exec.client = entity; - exec.callback_group = callback_group; - executables.push_back(exec); + executables.push_back({entity, group_info.ptr}); added++; } } + } + { + ZoneScopedN("waitables"); for (auto & [handle, entry] : collection.waitables) { 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()) { - continue; - } + if (!waitable) { + continue; + } - rclcpp::AnyExecutable exec; - exec.waitable = waitable; - exec.callback_group = group; - exec.data = waitable->take_data(); - executables.push_back(exec); - added++; + if (!waitable->is_ready(&rcl_wait_set)) { + continue; + } + + auto group_info = group_cache(entry.callback_group); + if (group_info.ptr && !group_info.can_be_taken_from) { + continue; } + + executables.push_back({waitable, group_info.ptr, waitable->take_data()}); + added++; + } } return added; } diff --git a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp index bcc432ec87..9b38ff2fa9 100644 --- a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp +++ b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp @@ -17,6 +17,8 @@ #include "rclcpp/exceptions.hpp" #include "rclcpp/executors/executor_notify_waitable.hpp" +#include "tracy/Tracy.hpp" + namespace rclcpp { namespace executors @@ -45,21 +47,18 @@ ExecutorNotifyWaitable & ExecutorNotifyWaitable::operator=(const ExecutorNotifyW void ExecutorNotifyWaitable::add_to_wait_set(rcl_wait_set_t * wait_set) { + ZoneScoped; 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(); - 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); + 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"); } } } @@ -75,8 +74,7 @@ ExecutorNotifyWaitable::is_ready(rcl_wait_set_t * wait_set) if (nullptr == rcl_guard_condition) { continue; } - for (auto weak_guard_condition : this->notify_guard_conditions_) { - auto guard_condition = weak_guard_condition.lock(); + for (auto guard_condition : this->notify_guard_conditions_) { if (guard_condition && &guard_condition->get_rcl_guard_condition() == rcl_guard_condition) { any_ready = true; } @@ -103,15 +101,16 @@ ExecutorNotifyWaitable::add_guard_condition(rclcpp::GuardCondition::WeakPtr weak { std::lock_guard lock(guard_condition_mutex_); 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); + if (guard_condition && notify_guard_conditions_.count(guard_condition) == 0) { + notify_guard_conditions_.insert(guard_condition); } } void -ExecutorNotifyWaitable::remove_guard_condition(rclcpp::GuardCondition::WeakPtr guard_condition) +ExecutorNotifyWaitable::remove_guard_condition(rclcpp::GuardCondition::WeakPtr weak_guard_condition) { std::lock_guard lock(guard_condition_mutex_); + auto guard_condition = weak_guard_condition.lock(); if (notify_guard_conditions_.count(guard_condition) != 0) { notify_guard_conditions_.erase(guard_condition); } diff --git a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp index e7f311c147..b315087210 100644 --- a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "rclcpp/executors/executor_entities_collection.hpp" #include "rcpputils/scope_exit.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" @@ -31,10 +32,30 @@ SingleThreadedExecutor::spin() throw std::runtime_error("spin() called while already spinning"); } RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); + while (rclcpp::ok(this->context_) && spinning.load()) { - rclcpp::AnyExecutable any_executable; - if (get_next_executable(any_executable)) { - execute_any_executable(any_executable); + + wait_for_work(); + + std::deque to_exec; + + { + std::lock_guard guard(mutex_); + to_exec = this->ready_executables_; + this->ready_executables_.clear(); } + + for (auto & exec: to_exec) + { + if (exec.callback_group && + exec.callback_group->type() == CallbackGroupType::MutuallyExclusive) + { + assert(any_executable.callback_group->can_be_taken_from().load()); + exec.callback_group->can_be_taken_from().store(false); + } + execute_any_executable(exec); + } + + FrameMark; } } diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp new file mode 100644 index 0000000000..b315087210 --- /dev/null +++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp @@ -0,0 +1,61 @@ +// Copyright 2015 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" +#include "rcpputils/scope_exit.hpp" + +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/any_executable.hpp" + +using rclcpp::executors::SingleThreadedExecutor; + +SingleThreadedExecutor::SingleThreadedExecutor(const rclcpp::ExecutorOptions & options) +: rclcpp::Executor(options) {} + +SingleThreadedExecutor::~SingleThreadedExecutor() {} + +void +SingleThreadedExecutor::spin() +{ + if (spinning.exchange(true)) { + throw std::runtime_error("spin() called while already spinning"); + } + RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); + + while (rclcpp::ok(this->context_) && spinning.load()) { + + wait_for_work(); + + std::deque to_exec; + + { + std::lock_guard guard(mutex_); + to_exec = this->ready_executables_; + this->ready_executables_.clear(); + } + + for (auto & exec: to_exec) + { + if (exec.callback_group && + exec.callback_group->type() == CallbackGroupType::MutuallyExclusive) + { + assert(any_executable.callback_group->can_be_taken_from().load()); + exec.callback_group->can_be_taken_from().store(false); + } + execute_any_executable(exec); + } + + FrameMark; + } +} diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index c0fa96a88e..3894565841 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -30,6 +30,8 @@ #include "rclcpp/exceptions.hpp" #include "rclcpp_action/server.hpp" +#include "tracy/Tracy.hpp" + using rclcpp_action::ServerBase; using rclcpp_action::GoalUUID; @@ -167,6 +169,7 @@ ServerBase::get_number_of_ready_guard_conditions() void ServerBase::add_to_wait_set(rcl_wait_set_t * wait_set) { + ZoneScoped; std::lock_guard lock(pimpl_->action_server_reentrant_mutex_); rcl_ret_t ret = rcl_action_wait_set_add_action_server( wait_set, pimpl_->action_server_.get(), NULL); From e52b2420d6e3dc4e26744a2bba6c340c6a37b212 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 11 Apr 2023 10:37:48 -0500 Subject: [PATCH 26/74] Remove tracepoints Signed-off-by: Michael Carroll --- rclcpp/CMakeLists.txt | 5 - .../detail/storage_policy_common.hpp | 190 ++++++++---------- .../wait_set_policies/dynamic_storage.hpp | 5 - .../sequential_synchronization.hpp | 12 +- rclcpp/include/rclcpp/wait_set_template.hpp | 6 - rclcpp/package.xml | 1 - rclcpp/src/rclcpp/executor.cpp | 12 -- .../executor_entities_collection.cpp | 19 -- .../executors/executor_notify_waitable.cpp | 3 - rclcpp_action/src/server.cpp | 3 - 10 files changed, 86 insertions(+), 170 deletions(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index f35126e9e6..72fb24de3b 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -21,8 +21,6 @@ find_package(rosidl_typesupport_cpp REQUIRED) find_package(statistics_msgs REQUIRED) find_package(tracetools REQUIRED) -find_package(tracy_vendor REQUIRED) - # TODO(wjwwood): remove this when gtest can build on its own, when using target_compile_features() # Default to C++17 if(NOT CMAKE_CXX_STANDARD) @@ -208,7 +206,6 @@ ament_target_dependencies(${PROJECT_NAME} "rosidl_runtime_cpp" "statistics_msgs" "tracetools" - "tracy_vendor" ) # Causes the visibility macros to use dllexport rather than dllimport, @@ -244,8 +241,6 @@ ament_export_dependencies(rosidl_runtime_cpp) ament_export_dependencies(rcl_yaml_param_parser) ament_export_dependencies(statistics_msgs) ament_export_dependencies(tracetools) -ament_export_dependencies(tracy_vendor) -ament_export_dependencies(Threads) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp index de500e2660..bfc51a6244 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp @@ -28,8 +28,6 @@ #include "rclcpp/visibility_control.hpp" #include "rclcpp/waitable.hpp" -#include "tracy/Tracy.hpp" - namespace rclcpp { namespace wait_set_policies @@ -246,28 +244,25 @@ class StoragePolicyCommon } // Add subscriptions. - { - ZoneScopedN("add_subscriptions"); - for (const auto & subscription_entry: subscriptions) { - if (!subscription_entry.subscription) { - // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. - if (HasStrongOwnership) { - // This will not happen in fixed sized storage, as it holds - // shared ownership the whole time and is never in need of pruning. - throw std::runtime_error("unexpected condition, fixed storage policy needs pruning"); - } - // Flag for pruning. - needs_pruning_ = true; - continue; + for (const auto & subscription_entry: subscriptions) { + if (!subscription_entry.subscription) { + // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. + if (HasStrongOwnership) { + // This will not happen in fixed sized storage, as it holds + // shared ownership the whole time and is never in need of pruning. + throw std::runtime_error("unexpected condition, fixed storage policy needs pruning"); } + // Flag for pruning. + needs_pruning_ = true; + continue; + } - rcl_ret_t ret = rcl_wait_set_add_subscription( - &rcl_wait_set_, - subscription_entry.subscription->get_subscription_handle().get(), - nullptr); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); - } + rcl_ret_t ret = rcl_wait_set_add_subscription( + &rcl_wait_set_, + subscription_entry.subscription->get_subscription_handle().get(), + nullptr); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); } } @@ -297,108 +292,93 @@ class StoragePolicyCommon } }; - { - ZoneScopedN("add_guard_conditions"); - // Add guard conditions. - add_guard_conditions(guard_conditions); + // Add guard conditions. + add_guard_conditions(guard_conditions); - // Add extra guard conditions. - add_guard_conditions(extra_guard_conditions); - } + // Add extra guard conditions. + add_guard_conditions(extra_guard_conditions); // Add timers. - { - ZoneScopedN("add_timers"); - for (const auto & timer : timers) { - if (!timer) { - // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. - if (HasStrongOwnership) { - // This will not happen in fixed sized storage, as it holds - // shared ownership the whole time and is never in need of pruning. - throw std::runtime_error("unexpected condition, fixed storage policy needs pruning"); - } - // Flag for pruning. - needs_pruning_ = true; - continue; - } - rcl_ret_t ret = rcl_wait_set_add_timer( - &rcl_wait_set_, - timer->get_timer_handle().get(), - nullptr); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); + for (const auto & timer : timers) { + if (!timer) { + // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. + if (HasStrongOwnership) { + // This will not happen in fixed sized storage, as it holds + // shared ownership the whole time and is never in need of pruning. + throw std::runtime_error("unexpected condition, fixed storage policy needs pruning"); } + // Flag for pruning. + needs_pruning_ = true; + continue; + } + rcl_ret_t ret = rcl_wait_set_add_timer( + &rcl_wait_set_, + timer->get_timer_handle().get(), + nullptr); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); } } // Add clients. - { - ZoneScopedN("add_clients"); - for (const auto & client : clients) { - if (!client) { - // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. - if (HasStrongOwnership) { - // This will not happen in fixed sized storage, as it holds - // shared ownership the whole time and is never in need of pruning. - throw std::runtime_error("unexpected condition, fixed storage policy needs pruning"); - } - // Flag for pruning. - needs_pruning_ = true; - continue; - } - rcl_ret_t ret = rcl_wait_set_add_client( - &rcl_wait_set_, - client->get_client_handle().get(), - nullptr); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); + for (const auto & client : clients) { + if (!client) { + // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. + if (HasStrongOwnership) { + // This will not happen in fixed sized storage, as it holds + // shared ownership the whole time and is never in need of pruning. + throw std::runtime_error("unexpected condition, fixed storage policy needs pruning"); } + // Flag for pruning. + needs_pruning_ = true; + continue; + } + rcl_ret_t ret = rcl_wait_set_add_client( + &rcl_wait_set_, + client->get_client_handle().get(), + nullptr); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); } } - { - ZoneScopedN("add_services"); - // Add services. - for (const auto & service : services) { - if (!service) { - // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. - if (HasStrongOwnership) { - // This will not happen in fixed sized storage, as it holds - // shared ownership the whole time and is never in need of pruning. - throw std::runtime_error("unexpected condition, fixed storage policy needs pruning"); - } - // Flag for pruning. - needs_pruning_ = true; - continue; - } - rcl_ret_t ret = rcl_wait_set_add_service( - &rcl_wait_set_, - service->get_service_handle().get(), - nullptr); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); + // Add services. + for (const auto & service : services) { + if (!service) { + // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. + if (HasStrongOwnership) { + // This will not happen in fixed sized storage, as it holds + // shared ownership the whole time and is never in need of pruning. + throw std::runtime_error("unexpected condition, fixed storage policy needs pruning"); } + // Flag for pruning. + needs_pruning_ = true; + continue; + } + rcl_ret_t ret = rcl_wait_set_add_service( + &rcl_wait_set_, + service->get_service_handle().get(), + nullptr); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); } } - { - ZoneScopedN("add_waitables"); - // Add waitables. - for (auto & waitable_entry: waitables) { - if (!waitable_entry.waitable) { - // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. - if (HasStrongOwnership) { - // This will not happen in fixed sized storage, as it holds - // shared ownership the whole time and is never in need of pruning. - throw std::runtime_error("unexpected condition, fixed storage policy needs pruning"); - } - // Flag for pruning. - needs_pruning_ = true; - continue; + // Add waitables. + for (auto & waitable_entry: waitables) { + if (!waitable_entry.waitable) { + // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. + if (HasStrongOwnership) { + // This will not happen in fixed sized storage, as it holds + // shared ownership the whole time and is never in need of pruning. + throw std::runtime_error("unexpected condition, fixed storage policy needs pruning"); } - waitable_entry.waitable->add_to_wait_set(&rcl_wait_set_); + // Flag for pruning. + needs_pruning_ = true; + continue; } + waitable_entry.waitable->add_to_wait_set(&rcl_wait_set_); } } diff --git a/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp index 9e83e60273..7b573e6501 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp @@ -31,8 +31,6 @@ #include "rclcpp/wait_set_policies/detail/storage_policy_common.hpp" #include "rclcpp/waitable.hpp" -#include "tracy/Tracy.hpp" - namespace rclcpp { namespace wait_set_policies @@ -206,8 +204,6 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo void storage_rebuild_rcl_wait_set(const ArrayOfExtraGuardConditions & extra_guard_conditions) { - ZoneScoped; - this->storage_acquire_ownerships(); this->storage_rebuild_rcl_wait_set_with_sets( @@ -402,7 +398,6 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo void storage_acquire_ownerships() { - ZoneScoped; if (++ownership_reference_counter_ > 1) { // Avoid redundant locking. return; diff --git a/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp b/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp index 40ca99467c..435ab6ea9f 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp @@ -34,8 +34,6 @@ #include "rclcpp/wait_set_policies/detail/synchronization_policy_common.hpp" #include "rclcpp/waitable.hpp" -#include "tracy/Tracy.hpp" - namespace rclcpp { namespace wait_set_policies @@ -254,8 +252,6 @@ class SequentialSynchronization : public detail::SynchronizationPolicyCommon // which calls this function, by acquiring shared ownership of the entites // for the duration of this function. - ZoneScoped; - // Setup looping predicate. auto start = std::chrono::steady_clock::now(); std::function should_loop = this->create_loop_predicate(time_to_wait_ns, start); @@ -282,17 +278,11 @@ class SequentialSynchronization : public detail::SynchronizationPolicyCommon // Then wait for entities to become ready. { - rcl_ret_t ret; - { - ZoneScopedN("rcl_wait"); - ret = rcl_wait(&rcl_wait_set, time_left_to_wait_ns.count()); - } - + rcl_ret_t ret = rcl_wait(&rcl_wait_set, time_left_to_wait_ns.count()); if (RCL_RET_OK == ret) { // Something has become ready in the wait set, and since this class // did not add anything to it, it is a user entity that is ready. { - ZoneScopedN("create_wait_result"); return create_wait_result(WaitResultKind::Ready); } } else if (RCL_RET_TIMEOUT == ret) { diff --git a/rclcpp/include/rclcpp/wait_set_template.hpp b/rclcpp/include/rclcpp/wait_set_template.hpp index a406c00399..c139913be8 100644 --- a/rclcpp/include/rclcpp/wait_set_template.hpp +++ b/rclcpp/include/rclcpp/wait_set_template.hpp @@ -35,8 +35,6 @@ #include "rclcpp/wait_result.hpp" #include "rclcpp/waitable.hpp" -#include "tracy/Tracy.hpp" - namespace rclcpp { @@ -602,7 +600,6 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli void prune_deleted_entities() { - ZoneScoped; // this method comes from the SynchronizationPolicy this->sync_prune_deleted_entities( [this]() { @@ -658,7 +655,6 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli WaitResult wait(std::chrono::duration time_to_wait = std::chrono::duration(-1)) { - ZoneScoped; auto time_to_wait_ns = std::chrono::duration_cast(time_to_wait); // ensure the ownership of the entities in the wait set is shared for the duration of wait @@ -716,7 +712,6 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli void wait_result_acquire() { - ZoneScoped; if (wait_result_holding_) { throw std::runtime_error("wait_result_acquire() called while already holding"); } @@ -736,7 +731,6 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli void wait_result_release() { - ZoneScoped; if (!wait_result_holding_) { throw std::runtime_error("wait_result_release() called while not holding"); } diff --git a/rclcpp/package.xml b/rclcpp/package.xml index cf4bd910dd..38773e2ec2 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -41,7 +41,6 @@ rmw statistics_msgs tracetools - tracy_vendor ament_cmake_gmock ament_cmake_google_benchmark diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 3812611e8a..891fa4005e 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -37,8 +37,6 @@ #include "tracetools/tracetools.h" -#include "tracy/Tracy.hpp" - using namespace std::chrono_literals; using rclcpp::Executor; @@ -335,7 +333,6 @@ Executor::cancel() void Executor::execute_any_executable(AnyExecutable & any_exec) { - ZoneScoped; if (!spinning.load()) { return; } @@ -407,7 +404,6 @@ take_and_do_error_handling( void Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) { - ZoneScoped; rclcpp::MessageInfo message_info; message_info.get_rmw_message_info().from_intra_process = false; @@ -477,14 +473,12 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) void Executor::execute_timer(rclcpp::TimerBase::SharedPtr timer) { - ZoneScoped; timer->execute_callback(); } void Executor::execute_service(rclcpp::ServiceBase::SharedPtr service) { - ZoneScoped; auto request_header = service->create_request_header(); std::shared_ptr request = service->create_request(); take_and_do_error_handling( @@ -498,7 +492,6 @@ void Executor::execute_client( rclcpp::ClientBase::SharedPtr client) { - ZoneScoped; auto request_header = client->create_request_header(); std::shared_ptr response = client->create_response(); take_and_do_error_handling( @@ -511,7 +504,6 @@ Executor::execute_client( void Executor::collect_entities() { - ZoneScoped; rclcpp::executors::ExecutorEntitiesCollection collection; this->collector_.update_collections(); @@ -568,7 +560,6 @@ Executor::collect_entities() void Executor::wait_for_work(std::chrono::nanoseconds timeout) { - ZoneScoped; TRACEPOINT(rclcpp_executor_wait_for_work, timeout.count()); if (current_collection_.empty()) { @@ -583,13 +574,11 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) "empty wait set received in wait(). This should never happen."); } rclcpp::executors::ready_executables(current_collection_, wait_result, ready_executables_); - TracyPlot("added", static_cast(ready_executables_.size())); } bool Executor::get_next_ready_executable(AnyExecutable & any_executable) { - ZoneScoped; TRACEPOINT(rclcpp_executor_get_next_ready); std::lock_guard guard(mutex_); @@ -613,7 +602,6 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable) bool Executor::get_next_executable(AnyExecutable & any_executable, std::chrono::nanoseconds timeout) { - ZoneScoped; bool success = false; // Check to see if there are any subscriptions or timers needing service // TODO(wjwwood): improve run to run efficiency of this function diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp index 1d33846f99..b77c3d8683 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp @@ -14,8 +14,6 @@ #include "rclcpp/executors/executor_entities_collection.hpp" -#include "tracy/Tracy.hpp" - namespace rclcpp { namespace executors @@ -46,7 +44,6 @@ build_entities_collection( const std::vector & callback_groups, ExecutorEntitiesCollection & collection) { - ZoneScoped; collection.clear(); for (auto weak_group_ptr : callback_groups) { @@ -104,7 +101,6 @@ ready_executables( std::deque & executables ) { - ZoneScoped; if (wait_result.kind() != rclcpp::WaitResultKind::Ready) { return 0; } @@ -133,8 +129,6 @@ ready_executables( }; - { - ZoneScopedN("timers"); for (size_t ii = 0; ii < rcl_wait_set.size_of_timers; ++ii) { if (nullptr == rcl_wait_set.timers[ii]) {continue;} auto entity_iter = collection.timers.find(rcl_wait_set.timers[ii]); @@ -155,10 +149,7 @@ ready_executables( added++; } } - } - { - ZoneScopedN("subscriptions"); for (size_t ii = 0; ii < rcl_wait_set.size_of_subscriptions; ++ii) { if (nullptr == rcl_wait_set.subscriptions[ii]) {continue;} auto entity_iter = collection.subscriptions.find(rcl_wait_set.subscriptions[ii]); @@ -177,10 +168,7 @@ ready_executables( added++; } } - } - { - ZoneScopedN("services"); for (size_t ii = 0; ii < rcl_wait_set.size_of_services; ++ii) { if (nullptr == rcl_wait_set.services[ii]) {continue;} auto entity_iter = collection.services.find(rcl_wait_set.services[ii]); @@ -196,11 +184,8 @@ ready_executables( executables.push_back({entity, callback_group}); } } - } - { - ZoneScopedN("clients"); for (size_t ii = 0; ii < rcl_wait_set.size_of_clients; ++ii) { if (nullptr == rcl_wait_set.clients[ii]) {continue;} auto entity_iter = collection.clients.find(rcl_wait_set.clients[ii]); @@ -219,10 +204,7 @@ ready_executables( added++; } } - } - { - ZoneScopedN("waitables"); for (auto & [handle, entry] : collection.waitables) { auto waitable = entry.entity.lock(); if (!waitable) { @@ -241,7 +223,6 @@ ready_executables( executables.push_back({waitable, group_info.ptr, waitable->take_data()}); added++; } - } return added; } diff --git a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp index 9b38ff2fa9..9b9445b270 100644 --- a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp +++ b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp @@ -17,8 +17,6 @@ #include "rclcpp/exceptions.hpp" #include "rclcpp/executors/executor_notify_waitable.hpp" -#include "tracy/Tracy.hpp" - namespace rclcpp { namespace executors @@ -47,7 +45,6 @@ ExecutorNotifyWaitable & ExecutorNotifyWaitable::operator=(const ExecutorNotifyW void ExecutorNotifyWaitable::add_to_wait_set(rcl_wait_set_t * wait_set) { - ZoneScoped; 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(); diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 3894565841..c0fa96a88e 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -30,8 +30,6 @@ #include "rclcpp/exceptions.hpp" #include "rclcpp_action/server.hpp" -#include "tracy/Tracy.hpp" - using rclcpp_action::ServerBase; using rclcpp_action::GoalUUID; @@ -169,7 +167,6 @@ ServerBase::get_number_of_ready_guard_conditions() void ServerBase::add_to_wait_set(rcl_wait_set_t * wait_set) { - ZoneScoped; std::lock_guard lock(pimpl_->action_server_reentrant_mutex_); rcl_ret_t ret = rcl_action_wait_set_add_action_server( wait_set, pimpl_->action_server_.get(), NULL); From 0c3c8999a66dff8dd3ef7442ab9cdc96f7096130 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 11 Apr 2023 12:04:19 -0500 Subject: [PATCH 27/74] Reducing diff Signed-off-by: Michael Carroll --- rclcpp/CMakeLists.txt | 1 + rclcpp/include/rclcpp/any_executable.hpp | 43 +--- rclcpp/include/rclcpp/callback_group.hpp | 25 ++ rclcpp/include/rclcpp/executor.hpp | 24 +- .../executor_entities_collection.hpp | 6 +- .../static_single_threaded_executor.hpp | 70 +++++- .../sequential_synchronization.hpp | 32 ++- rclcpp/include/rclcpp/wait_set_template.hpp | 1 - rclcpp/src/rclcpp/any_executable.cpp | 56 +---- rclcpp/src/rclcpp/executor.cpp | 23 +- .../executor_entities_collection.cpp | 82 +++---- .../executors/single_threaded_executor.cpp | 4 +- .../static_single_threaded_executor.cpp | 223 ++++++++++++++++-- 13 files changed, 405 insertions(+), 185 deletions(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 72fb24de3b..4d3f25aa36 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -59,6 +59,7 @@ set(${PROJECT_NAME}_SRCS 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_single_threaded_executor.cpp src/rclcpp/expand_topic_or_service_name.cpp src/rclcpp/future_return_code.cpp src/rclcpp/generic_publisher.cpp diff --git a/rclcpp/include/rclcpp/any_executable.hpp b/rclcpp/include/rclcpp/any_executable.hpp index 693f1b121c..5d4064f452 100644 --- a/rclcpp/include/rclcpp/any_executable.hpp +++ b/rclcpp/include/rclcpp/any_executable.hpp @@ -35,46 +35,19 @@ struct AnyExecutable RCLCPP_PUBLIC AnyExecutable(); - RCLCPP_PUBLIC - AnyExecutable( - const rclcpp::SubscriptionBase::SharedPtr & subscription, - const rclcpp::CallbackGroup::SharedPtr & callback_group); - - RCLCPP_PUBLIC - AnyExecutable( - const rclcpp::TimerBase::SharedPtr & timer, - const rclcpp::CallbackGroup::SharedPtr & callback_group); - - RCLCPP_PUBLIC - AnyExecutable( - const rclcpp::ServiceBase::SharedPtr & service, - const rclcpp::CallbackGroup::SharedPtr & callback_group); - - RCLCPP_PUBLIC - AnyExecutable( - const rclcpp::ClientBase::SharedPtr & client, - const rclcpp::CallbackGroup::SharedPtr & callback_group); - - RCLCPP_PUBLIC - AnyExecutable( - const rclcpp::Waitable::SharedPtr & waitable, - const rclcpp::CallbackGroup::SharedPtr & callback_group, - const std::shared_ptr & data); - - RCLCPP_PUBLIC virtual ~AnyExecutable(); // Only one of the following pointers will be set. - rclcpp::SubscriptionBase::SharedPtr subscription = nullptr; - rclcpp::TimerBase::SharedPtr timer = nullptr; - rclcpp::ServiceBase::SharedPtr service = nullptr; - rclcpp::ClientBase::SharedPtr client = nullptr; - rclcpp::Waitable::SharedPtr waitable = nullptr; + rclcpp::SubscriptionBase::SharedPtr subscription; + rclcpp::TimerBase::SharedPtr timer; + rclcpp::ServiceBase::SharedPtr service; + rclcpp::ClientBase::SharedPtr client; + rclcpp::Waitable::SharedPtr waitable; // These are used to keep the scope on the containing items - rclcpp::CallbackGroup::SharedPtr callback_group = nullptr; - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base = nullptr; - std::shared_ptr data = nullptr; + rclcpp::CallbackGroup::SharedPtr callback_group; + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base; + std::shared_ptr data; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp index 51e0714c16..366fcf242c 100644 --- a/rclcpp/include/rclcpp/callback_group.hpp +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -192,17 +192,42 @@ class CallbackGroup bool has_valid_node(); + /// Get the total number of entities in this callback group. + /** + * \return the number of entities in the callback group. + */ RCLCPP_PUBLIC size_t size() const; + /// Return a reference to the 'can be taken' atomic boolean. + /** + * The resulting bool will be true in the case that no executor is currently + * using an executable entity from this group. + * The resulting bool will be false in the case that an executor is currently + * using an executable entity from this group, and the group policy doesn't + * allow a second take (eg mutual exclusion) + * \return a reference to the flag + */ RCLCPP_PUBLIC std::atomic_bool & can_be_taken_from(); + /// Get the group type. + /** + * \return the group type + */ RCLCPP_PUBLIC const CallbackGroupType & type() const; + /// Collect all of the entity pointers contained in this callback group. + /** + * \param[in] sub_func Function to execute for each subscription + * \param[in] service_func Function to execute for each service + * \param[in] client_func Function to execute for each client + * \param[in] timer_func Function to execute for each timer + * \param[in] waitable_fuinc Function to execute for each waitable + */ RCLCPP_PUBLIC void collect_all_ptrs( std::function sub_func, diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 616dd85a41..1b94411fb9 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -485,6 +485,7 @@ class Executor static void execute_client(rclcpp::ClientBase::SharedPtr client); + /// Gather all of the waitable entities from associated nodes and callback groups. RCLCPP_PUBLIC void collect_entities(); @@ -547,14 +548,29 @@ class Executor virtual void spin_once_impl(std::chrono::nanoseconds timeout); + /// Waitable containing guard conditions controlling the executor flow. + /** + * This waitable contains the interrupt and shutdown guard condition, as well + * as the guard condition associated with each node and callback group. + * By default, if any change is detected in the monitored entities, the notify + * waitable will awake the executor and rebuild the collections. + */ std::shared_ptr notify_waitable_; + + /// Collector used to associate executable entities from nodes and guard conditions rclcpp::executors::ExecutorEntitiesCollector collector_; - rclcpp::executors::ExecutorEntitiesCollection current_collection_; - std::shared_ptr current_notify_waitable_; + /// Waitset to be waited on. + rclcpp::WaitSet wait_set_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + + /// Hold the current state of the collection being waited on by the waitset + rclcpp::executors::ExecutorEntitiesCollection current_collection_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + + /// Hold the current state of the notify waitable being waited on by the waitset + std::shared_ptr current_notify_waitable_ RCPPUTILS_TSA_GUARDED_BY(mutex_); - rclcpp::WaitSet wait_set_; - std::deque ready_executables_; + /// Hold the list of executables currently available to be executed. + std::deque ready_executables_ RCPPUTILS_TSA_GUARDED_BY(mutex_); /// shutdown callback handle registered to Context rclcpp::OnShutdownCallbackHandle shutdown_callback_handle_; diff --git a/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp b/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp index 78eef5d962..2829140bd2 100644 --- a/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp +++ b/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp @@ -200,12 +200,10 @@ build_entities_collection( * \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. */ -size_t +std::deque ready_executables( const ExecutorEntitiesCollection & collection, - rclcpp::WaitResult & wait_result, - std::deque & executables -); + rclcpp::WaitResult & wait_result); } // namespace executors } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp index cb1a3df5b6..b4932c8f02 100644 --- a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp @@ -54,13 +54,81 @@ class StaticSingleThreadedExecutor : public rclcpp::Executor RCLCPP_PUBLIC virtual ~StaticSingleThreadedExecutor(); - /// Single-threaded implementation of spin. + /// Static executor implementation of spin. + /** + * This function will block until work comes in, execute it, and keep blocking. + * It will only be interrupted by a CTRL-C (managed by the global signal handler). + * \throws std::runtime_error when spin() called while already spinning + */ RCLCPP_PUBLIC void spin() override; + /// Static executor implementation of spin some + /** + * This non-blocking function will execute entities that + * were ready when this API was called, until timeout or no + * more work available. Entities that got ready while + * executing work, won't be taken into account here. + * + * Example: + * while(condition) { + * spin_some(); + * sleep(); // User should have some sync work or + * // sleep to avoid a 100% CPU usage + * } + */ + RCLCPP_PUBLIC + void + spin_some(std::chrono::nanoseconds max_duration = std::chrono::nanoseconds(0)) override; + + /// Static executor implementation of spin all + /** + * This non-blocking function will execute entities until timeout (must be >= 0) + * or no more work available. + * If timeout is `0`, potentially it blocks forever until no more work is available. + * If new entities get ready while executing work available, they will be executed + * as long as the timeout hasn't expired. + * + * Example: + * while(condition) { + * spin_all(); + * sleep(); // User should have some sync work or + * // sleep to avoid a 100% CPU usage + * } + */ + RCLCPP_PUBLIC + void + spin_all(std::chrono::nanoseconds max_duration) override; + + RCLCPP_PUBLIC + bool execute_ready_executables( + const rclcpp::executors::ExecutorEntitiesCollection & collection, + rclcpp::WaitResult & wait_result, + bool spin_once); + +protected: +/** + * @brief Executes ready executables from wait set. + * @param spin_once if true executes only the first ready executable. + * @return true if any executable was ready. + */ +RCLCPP_PUBLIC +bool +execute_ready_executables(bool spin_once = false); + +RCLCPP_PUBLIC +void +spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive); + +RCLCPP_PUBLIC +void +spin_once_impl(std::chrono::nanoseconds timeout) override; + private: RCLCPP_DISABLE_COPY(StaticSingleThreadedExecutor) + + std::atomic_bool entities_need_rebuild; }; } // namespace executors diff --git a/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp b/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp index 435ab6ea9f..76111a4df2 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp @@ -277,24 +277,22 @@ class SequentialSynchronization : public detail::SynchronizationPolicyCommon auto time_left_to_wait_ns = this->calculate_time_left_to_wait(time_to_wait_ns, start); // Then wait for entities to become ready. - { - rcl_ret_t ret = rcl_wait(&rcl_wait_set, time_left_to_wait_ns.count()); - if (RCL_RET_OK == ret) { - // Something has become ready in the wait set, and since this class - // did not add anything to it, it is a user entity that is ready. - { - return create_wait_result(WaitResultKind::Ready); - } - } else if (RCL_RET_TIMEOUT == ret) { - // The wait set timed out, exit the loop. - break; - } else if (RCL_RET_WAIT_SET_EMPTY == ret) { - // Wait set was empty, return Empty. - return create_wait_result(WaitResultKind::Empty); - } else { - // Some other error case, throw. - rclcpp::exceptions::throw_from_rcl_error(ret); + rcl_ret_t ret = rcl_wait(&rcl_wait_set, time_left_to_wait_ns.count()); + if (RCL_RET_OK == ret) { + // Something has become ready in the wait set, and since this class + // did not add anything to it, it is a user entity that is ready. + { + return create_wait_result(WaitResultKind::Ready); } + } else if (RCL_RET_TIMEOUT == ret) { + // The wait set timed out, exit the loop. + break; + } else if (RCL_RET_WAIT_SET_EMPTY == ret) { + // Wait set was empty, return Empty. + return create_wait_result(WaitResultKind::Empty); + } else { + // Some other error case, throw. + rclcpp::exceptions::throw_from_rcl_error(ret); } } while (should_loop()); diff --git a/rclcpp/include/rclcpp/wait_set_template.hpp b/rclcpp/include/rclcpp/wait_set_template.hpp index c139913be8..b3f41f8ed5 100644 --- a/rclcpp/include/rclcpp/wait_set_template.hpp +++ b/rclcpp/include/rclcpp/wait_set_template.hpp @@ -661,7 +661,6 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli this->storage_acquire_ownerships(); RCPPUTILS_SCOPE_EXIT({this->storage_release_ownerships();}); - // this method comes from the SynchronizationPolicy return this->template sync_wait>( // pass along the time_to_wait duration as nanoseconds diff --git a/rclcpp/src/rclcpp/any_executable.cpp b/rclcpp/src/rclcpp/any_executable.cpp index 4cee74692f..e6f0e235a2 100644 --- a/rclcpp/src/rclcpp/any_executable.cpp +++ b/rclcpp/src/rclcpp/any_executable.cpp @@ -17,56 +17,18 @@ using rclcpp::AnyExecutable; RCLCPP_PUBLIC -AnyExecutable::AnyExecutable() = default; - -RCLCPP_PUBLIC -AnyExecutable::AnyExecutable( - const rclcpp::SubscriptionBase::SharedPtr & subscription, - const rclcpp::CallbackGroup::SharedPtr & callback_group): - subscription(subscription), - callback_group(callback_group) -{ -} - -RCLCPP_PUBLIC -AnyExecutable::AnyExecutable( - const rclcpp::TimerBase::SharedPtr & timer, - const rclcpp::CallbackGroup::SharedPtr & callback_group): - timer(timer), - callback_group(callback_group) -{ -} - -RCLCPP_PUBLIC -AnyExecutable::AnyExecutable( - const rclcpp::ServiceBase::SharedPtr & service, - const rclcpp::CallbackGroup::SharedPtr & callback_group): - service(service), - callback_group(callback_group) +AnyExecutable::AnyExecutable() +: subscription(nullptr), + timer(nullptr), + service(nullptr), + client(nullptr), + waitable(nullptr), + callback_group(nullptr), + node_base(nullptr), + data(nullptr) { - } -RCLCPP_PUBLIC -AnyExecutable::AnyExecutable( - const rclcpp::ClientBase::SharedPtr & client, - const rclcpp::CallbackGroup::SharedPtr & callback_group): - client(client), - callback_group(callback_group) -{ -} - -RCLCPP_PUBLIC -AnyExecutable::AnyExecutable( - const rclcpp::Waitable::SharedPtr & waitable, - const rclcpp::CallbackGroup::SharedPtr & callback_group, - const std::shared_ptr & data): - waitable(waitable), - callback_group(callback_group), - data(data) -{} - - AnyExecutable::~AnyExecutable() { // Make sure that discarded (taken but not executed) AnyExecutable's have diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 891fa4005e..6203c0192d 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -41,6 +41,8 @@ using namespace std::chrono_literals; using rclcpp::Executor; +/// Mask to indicate to the waitset to only add the subscription. +/// The events and intraprocess waitable are already added via the callback group. static constexpr rclcpp::SubscriptionWaitSetMask kDefaultSubscriptionMask = {true, false, false}; Executor::Executor(const rclcpp::ExecutorOptions & options) @@ -52,8 +54,8 @@ Executor::Executor(const rclcpp::ExecutorOptions & options) this->collect_entities(); })), collector_(notify_waitable_), - current_notify_waitable_(notify_waitable_), - wait_set_({}, {}, {}, {}, {}, {}, options.context) + wait_set_({}, {}, {}, {}, {}, {}, options.context), + current_notify_waitable_(notify_waitable_) { // Store the context for later use. context_ = options.context; @@ -76,7 +78,6 @@ Executor::~Executor() notify_waitable_->remove_guard_condition(interrupt_guard_condition_); notify_waitable_->remove_guard_condition(shutdown_guard_condition_); - current_collection_.timers.update( {}, {}, [this](auto timer) {wait_set_.remove_timer(timer);}); @@ -504,22 +505,27 @@ Executor::execute_client( void Executor::collect_entities() { - + // Get the current list of available waitables from the collector. rclcpp::executors::ExecutorEntitiesCollection collection; this->collector_.update_collections(); auto callback_groups = this->collector_.get_all_callback_groups(); rclcpp::executors::build_entities_collection(callback_groups, collection); + std::lock_guard guard(mutex_); + + // Make a copy of notify waitable so we can continue to mutate the original + // one outside of the execute loop. + // This prevents the collection of guard conditions in the waitable from changing + // while we are waiting on it. if (notify_waitable_) { - // Make a copy of notify waitable so we can continue to mutate the original - // one outside of the execute loop. *current_notify_waitable_.get() = *notify_waitable_.get(); auto notify_waitable = std::static_pointer_cast(current_notify_waitable_); collection.waitables.insert({notify_waitable.get(), {notify_waitable, {}}}); } - std::lock_guard guard(mutex_); + // Update each of the groups of entities in the current collection, adding or removing + // from the wait set as necessary. current_collection_.timers.update( collection.timers, [this](auto timer) {wait_set_.add_timer(timer);}, @@ -573,7 +579,7 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) "rclcpp", "empty wait set received in wait(). This should never happen."); } - rclcpp::executors::ready_executables(current_collection_, wait_result, ready_executables_); + ready_executables_ = rclcpp::executors::ready_executables(current_collection_, wait_result); } bool @@ -609,7 +615,6 @@ Executor::get_next_executable(AnyExecutable & any_executable, std::chrono::nanos // If there are none if (!success) { // Wait for subscriptions or timers to work on - std::cout << "wait_for_work" << std::endl; wait_for_work(timeout); if (!spinning.load()) { return false; diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp index b77c3d8683..bddc475aa0 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp @@ -94,36 +94,29 @@ build_entities_collection( } } -size_t +std::deque ready_executables( const ExecutorEntitiesCollection & collection, - rclcpp::WaitResult & wait_result, - std::deque & executables + rclcpp::WaitResult & wait_result ) { + std::deque executables; + if (wait_result.kind() != rclcpp::WaitResultKind::Ready) { - return 0; + return executables; } - size_t added = 0; auto rcl_wait_set = wait_result.get_wait_set().get_rcl_wait_set(); - struct CachedCallbackGroup - { - rclcpp::CallbackGroup::SharedPtr ptr = nullptr; - bool can_be_taken_from = false; - }; - - std::map> group_map; - auto group_cache = [&group_map](const rclcpp::CallbackGroup::WeakPtr & weak_cbg_ptr) -> const CachedCallbackGroup & + // Cache shared pointers to groups to avoid extra work re-locking them + std::map> group_map; + auto group_cache = [&group_map](const rclcpp::CallbackGroup::WeakPtr & weak_cbg_ptr) { if (group_map.count(weak_cbg_ptr) == 0) { - CachedCallbackGroup temp; - temp.ptr = weak_cbg_ptr.lock(); - if (temp.ptr) - temp.can_be_taken_from = temp.ptr->can_be_taken_from().load(); - group_map.insert({weak_cbg_ptr, temp}); + group_map.insert({weak_cbg_ptr, weak_cbg_ptr.lock()}); } return group_map.find(weak_cbg_ptr)->second; }; @@ -138,15 +131,16 @@ ready_executables( continue; } auto group_info = group_cache(entity_iter->second.callback_group); - if (group_info.ptr && !group_info.can_be_taken_from) { + if (group_info && !group_info->can_be_taken_from().load()) { continue; } - if (!entity->call()) { continue; } - executables.push_back({entity, group_info.ptr}); - added++; + rclcpp::AnyExecutable exec; + exec.timer = entity; + exec.callback_group = group_info; + executables.push_back(exec); } } @@ -158,14 +152,14 @@ ready_executables( if (!entity) { continue; } - auto group_info = group_cache(entity_iter->second.callback_group); - if (group_info.ptr && !group_info.can_be_taken_from) { + if (group_info && !group_info->can_be_taken_from().load()) { continue; } - - executables.push_back({entity, group_info.ptr}); - added++; + rclcpp::AnyExecutable exec; + exec.subscription = entity; + exec.callback_group = group_info; + executables.push_back(exec); } } @@ -177,15 +171,17 @@ ready_executables( if (!entity) { continue; } - const auto & [callback_group, can_be_taken_from] = group_cache(entity_iter->second.callback_group); - if (callback_group && !can_be_taken_from) { + auto group_info = group_cache(entity_iter->second.callback_group); + if (group_info && !group_info->can_be_taken_from().load()) { continue; } - executables.push_back({entity, callback_group}); + rclcpp::AnyExecutable exec; + exec.service = entity; + exec.callback_group = group_info; + executables.push_back(exec); } } - for (size_t ii = 0; ii < rcl_wait_set.size_of_clients; ++ii) { if (nullptr == rcl_wait_set.clients[ii]) {continue;} auto entity_iter = collection.clients.find(rcl_wait_set.clients[ii]); @@ -194,14 +190,14 @@ ready_executables( if (!entity) { continue; } - auto group_info = group_cache(entity_iter->second.callback_group); - if (group_info.ptr && !group_info.can_be_taken_from) { + if (group_info && !group_info->can_be_taken_from().load()) { continue; } - - executables.push_back({entity, group_info.ptr}); - added++; + rclcpp::AnyExecutable exec; + exec.client = entity; + exec.callback_group = group_info; + executables.push_back(exec); } } @@ -210,20 +206,20 @@ ready_executables( if (!waitable) { continue; } - if (!waitable->is_ready(&rcl_wait_set)) { continue; } - auto group_info = group_cache(entry.callback_group); - if (group_info.ptr && !group_info.can_be_taken_from) { + if (group_info && !group_info->can_be_taken_from().load()) { continue; } - - executables.push_back({waitable, group_info.ptr, waitable->take_data()}); - added++; + rclcpp::AnyExecutable exec; + exec.waitable = waitable; + exec.callback_group = group_info; + exec.data = waitable->take_data(); + executables.push_back(exec); } - return added; + return executables; } } // namespace executors diff --git a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp index b315087210..259824e291 100644 --- a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp @@ -37,8 +37,8 @@ SingleThreadedExecutor::spin() wait_for_work(); + /// Get the ready executables in a thread-safe way. std::deque to_exec; - { std::lock_guard guard(mutex_); to_exec = this->ready_executables_; @@ -55,7 +55,5 @@ SingleThreadedExecutor::spin() } execute_any_executable(exec); } - - FrameMark; } } diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp index b315087210..2aaa7b6996 100644 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp @@ -13,20 +13,26 @@ // limitations under the License. #include "rclcpp/executors/executor_entities_collection.hpp" +#include "rclcpp/executors/executor_notify_waitable.hpp" #include "rcpputils/scope_exit.hpp" -#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/executors/static_single_threaded_executor.hpp" #include "rclcpp/any_executable.hpp" -using rclcpp::executors::SingleThreadedExecutor; +using rclcpp::executors::StaticSingleThreadedExecutor; -SingleThreadedExecutor::SingleThreadedExecutor(const rclcpp::ExecutorOptions & options) -: rclcpp::Executor(options) {} +StaticSingleThreadedExecutor::StaticSingleThreadedExecutor(const rclcpp::ExecutorOptions & options) +: rclcpp::Executor(options) +{ + notify_waitable_ = std::make_shared([this](){ + this->entities_need_rebuild.store(true); + }); +} -SingleThreadedExecutor::~SingleThreadedExecutor() {} +StaticSingleThreadedExecutor::~StaticSingleThreadedExecutor() {} void -SingleThreadedExecutor::spin() +StaticSingleThreadedExecutor::spin() { if (spinning.exchange(true)) { throw std::runtime_error("spin() called while already spinning"); @@ -34,28 +40,203 @@ SingleThreadedExecutor::spin() RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); while (rclcpp::ok(this->context_) && spinning.load()) { + std::deque to_exec; - wait_for_work(); + if (current_collection_.empty() || this->entities_need_rebuild.load()) { + this->collect_entities(); + this->entities_need_rebuild.store(false); + } - std::deque to_exec; + std::lock_guard guard(mutex_); + auto wait_result = wait_set_.wait(std::chrono::nanoseconds(-1)); + if (wait_result.kind() == WaitResultKind::Empty) { + RCUTILS_LOG_WARN_NAMED( + "rclcpp", + "empty wait set received in wait(). This should never happen."); + continue; + } + execute_ready_executables(current_collection_, wait_result, false); + } +} + +void +StaticSingleThreadedExecutor::spin_some(std::chrono::nanoseconds max_duration) +{ + // In this context a 0 input max_duration means no duration limit + if (std::chrono::nanoseconds(0) == max_duration) { + max_duration = std::chrono::nanoseconds::max(); + } + + return this->spin_some_impl(max_duration, false); +} + +void +StaticSingleThreadedExecutor::spin_all(std::chrono::nanoseconds max_duration) +{ + if (max_duration < std::chrono::nanoseconds(0)) { + throw std::invalid_argument("max_duration must be greater than or equal to 0"); + } + return this->spin_some_impl(max_duration, true); +} + +void +StaticSingleThreadedExecutor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive) +{ + auto start = std::chrono::steady_clock::now(); + auto max_duration_not_elapsed = [max_duration, start]() { + if (std::chrono::nanoseconds(0) == max_duration) { + // told to spin forever if need be + return true; + } else if (std::chrono::steady_clock::now() - start < max_duration) { + // told to spin only for some maximum amount of time + return true; + } + // spun too long + return false; + }; + + if (spinning.exchange(true)) { + throw std::runtime_error("spin_some() called while already spinning"); + } + RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); + + while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) { + // Get executables that are ready now + if (current_collection_.empty() || this->entities_need_rebuild.load()) { + this->collect_entities(); + this->entities_need_rebuild.store(false); + } + + std::lock_guard guard(mutex_); + auto wait_result = wait_set_.wait(std::chrono::nanoseconds(0)); + if (wait_result.kind() == WaitResultKind::Empty) { + RCUTILS_LOG_WARN_NAMED( + "rclcpp", + "empty wait set received in wait(). This should never happen."); + continue; + } + + // Execute ready executables + bool work_available = execute_ready_executables(current_collection_, wait_result, false); + if (!work_available || !exhaustive) { + break; + } + } +} + +void +StaticSingleThreadedExecutor::spin_once_impl(std::chrono::nanoseconds timeout) +{ + if (rclcpp::ok(context_) && spinning.load()) { + if (current_collection_.empty() || this->entities_need_rebuild.load()) { + this->collect_entities(); + this->entities_need_rebuild.store(false); + } + + std::lock_guard guard(mutex_); + auto wait_result = wait_set_.wait(std::chrono::nanoseconds(timeout)); + if (wait_result.kind() == WaitResultKind::Empty) { + RCUTILS_LOG_WARN_NAMED( + "rclcpp", + "empty wait set received in wait(). This should never happen."); + return; + } + + // Execute ready executables + execute_ready_executables(current_collection_, wait_result, true); + } +} + +bool StaticSingleThreadedExecutor::execute_ready_executables( + const rclcpp::executors::ExecutorEntitiesCollection & collection, + rclcpp::WaitResult & wait_result, + bool spin_once) +{ + bool any_ready_executable = false; + + if (wait_result.kind() != rclcpp::WaitResultKind::Ready) { + return any_ready_executable; + } - { - std::lock_guard guard(mutex_); - to_exec = this->ready_executables_; - this->ready_executables_.clear(); + 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 (nullptr == rcl_wait_set.timers[ii]) {continue;} + auto entity_iter = collection.timers.find(rcl_wait_set.timers[ii]); + if (entity_iter != collection.timers.end()) { + auto entity = entity_iter->second.entity.lock(); + if (!entity) { + continue; + } + if (!entity->call()) { + continue; + } + execute_timer(entity); + if (spin_once) + return true; + any_ready_executable = true; + } + } + + for (size_t ii = 0; ii < rcl_wait_set.size_of_subscriptions; ++ii) { + if (nullptr == rcl_wait_set.subscriptions[ii]) {continue;} + auto entity_iter = collection.subscriptions.find(rcl_wait_set.subscriptions[ii]); + if (entity_iter != collection.subscriptions.end()) { + auto entity = entity_iter->second.entity.lock(); + if (!entity) { + continue; + } + execute_subscription(entity); + if (spin_once) + return true; + any_ready_executable = true; + } + } + + for (size_t ii = 0; ii < rcl_wait_set.size_of_services; ++ii) { + if (nullptr == rcl_wait_set.services[ii]) {continue;} + auto entity_iter = collection.services.find(rcl_wait_set.services[ii]); + if (entity_iter != collection.services.end()) { + auto entity = entity_iter->second.entity.lock(); + if (!entity) { + continue; + } + execute_service(entity); + if (spin_once) + return true; + any_ready_executable = true; } + } - for (auto & exec: to_exec) - { - if (exec.callback_group && - exec.callback_group->type() == CallbackGroupType::MutuallyExclusive) - { - assert(any_executable.callback_group->can_be_taken_from().load()); - exec.callback_group->can_be_taken_from().store(false); + for (size_t ii = 0; ii < rcl_wait_set.size_of_clients; ++ii) { + if (nullptr == rcl_wait_set.clients[ii]) {continue;} + auto entity_iter = collection.clients.find(rcl_wait_set.clients[ii]); + if (entity_iter != collection.clients.end()) { + auto entity = entity_iter->second.entity.lock(); + if (!entity) { + continue; } - execute_any_executable(exec); + execute_client(entity); + if (spin_once) + return true; + any_ready_executable = true; + } + } + + for (auto & [handle, entry] : collection.waitables) { + auto waitable = entry.entity.lock(); + if (!waitable) { + continue; + } + if (!waitable->is_ready(&rcl_wait_set)) { + continue; } - FrameMark; + auto data = waitable->take_data(); + waitable->execute(data); + if (spin_once) + return true; + any_ready_executable = true; } + return any_ready_executable; } From d2d271b8a01e2f701ae6569dd47a1495a71d432e Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 11 Apr 2023 12:09:34 -0500 Subject: [PATCH 28/74] Reduce diff Signed-off-by: Michael Carroll --- .../static_single_threaded_executor.hpp | 31 +++++++++---------- .../subscription_intra_process_buffer.hpp | 1 - .../detail/storage_policy_common.hpp | 1 - .../sequential_synchronization.hpp | 4 +-- rclcpp/src/rclcpp/any_executable.cpp | 3 +- rclcpp/src/rclcpp/callback_group.cpp | 7 +++-- 6 files changed, 21 insertions(+), 26 deletions(-) diff --git a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp index b4932c8f02..5651fe6328 100644 --- a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp @@ -43,7 +43,7 @@ namespace executors class StaticSingleThreadedExecutor : public rclcpp::Executor { public: - RCLCPP_SMART_PTR_DEFINITIONS(SingleThreadedExecutor) + RCLCPP_SMART_PTR_DEFINITIONS(StaticSingleThreadedExecutor) /// Default constructor. See the default constructor for Executor. RCLCPP_PUBLIC @@ -101,29 +101,26 @@ class StaticSingleThreadedExecutor : public rclcpp::Executor void spin_all(std::chrono::nanoseconds max_duration) override; + +protected: + /** + * @brief Executes ready executables from wait set. + * @param spin_once if true executes only the first ready executable. + * @return true if any executable was ready. + */ RCLCPP_PUBLIC bool execute_ready_executables( const rclcpp::executors::ExecutorEntitiesCollection & collection, rclcpp::WaitResult & wait_result, bool spin_once); -protected: -/** - * @brief Executes ready executables from wait set. - * @param spin_once if true executes only the first ready executable. - * @return true if any executable was ready. - */ -RCLCPP_PUBLIC -bool -execute_ready_executables(bool spin_once = false); - -RCLCPP_PUBLIC -void -spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive); + RCLCPP_PUBLIC + void + spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive); -RCLCPP_PUBLIC -void -spin_once_impl(std::chrono::nanoseconds timeout) override; + RCLCPP_PUBLIC + void + spin_once_impl(std::chrono::nanoseconds timeout) override; private: RCLCPP_DISABLE_COPY(StaticSingleThreadedExecutor) diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp index 7577160046..3c71512677 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp @@ -167,7 +167,6 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff void trigger_guard_condition() override { - std::cout << "trigger_guard_condition" << std::endl; this->gc_.trigger(); } diff --git a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp index bfc51a6244..94346f537c 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp @@ -15,7 +15,6 @@ #ifndef RCLCPP__WAIT_SET_POLICIES__DETAIL__STORAGE_POLICY_COMMON_HPP_ #define RCLCPP__WAIT_SET_POLICIES__DETAIL__STORAGE_POLICY_COMMON_HPP_ -#include #include #include #include diff --git a/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp b/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp index 76111a4df2..be2e569c40 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp @@ -281,9 +281,7 @@ class SequentialSynchronization : public detail::SynchronizationPolicyCommon if (RCL_RET_OK == ret) { // Something has become ready in the wait set, and since this class // did not add anything to it, it is a user entity that is ready. - { - return create_wait_result(WaitResultKind::Ready); - } + return create_wait_result(WaitResultKind::Ready); } else if (RCL_RET_TIMEOUT == ret) { // The wait set timed out, exit the loop. break; diff --git a/rclcpp/src/rclcpp/any_executable.cpp b/rclcpp/src/rclcpp/any_executable.cpp index e6f0e235a2..da020942bc 100644 --- a/rclcpp/src/rclcpp/any_executable.cpp +++ b/rclcpp/src/rclcpp/any_executable.cpp @@ -26,8 +26,7 @@ AnyExecutable::AnyExecutable() callback_group(nullptr), node_base(nullptr), data(nullptr) -{ -} +{} AnyExecutable::~AnyExecutable() { diff --git a/rclcpp/src/rclcpp/callback_group.cpp b/rclcpp/src/rclcpp/callback_group.cpp index 7ed3895b4a..679a241588 100644 --- a/rclcpp/src/rclcpp/callback_group.cpp +++ b/rclcpp/src/rclcpp/callback_group.cpp @@ -65,8 +65,11 @@ CallbackGroup::type() const size_t CallbackGroup::size() const { - return subscription_ptrs_.size() + service_ptrs_.size() + client_ptrs_.size() + timer_ptrs_.size() + waitable_ptrs_.size(); - + return subscription_ptrs_.size() + + service_ptrs_.size() + + client_ptrs_.size() + + timer_ptrs_.size() + + waitable_ptrs_.size(); } void CallbackGroup::collect_all_ptrs( From 200f733a8f162da4dae5b20d1f1d51a9000dea85 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 11 Apr 2023 12:13:23 -0500 Subject: [PATCH 29/74] Uncrustify Signed-off-by: Michael Carroll --- rclcpp/include/rclcpp/executor.hpp | 6 +++-- .../static_single_threaded_executor.hpp | 1 - .../detail/storage_policy_common.hpp | 6 ++--- rclcpp/src/rclcpp/executor.cpp | 8 +++--- .../executor_entities_collection.cpp | 15 +++++------ .../executors/multi_threaded_executor.cpp | 4 +-- .../executors/single_threaded_executor.cpp | 3 +-- .../static_single_threaded_executor.cpp | 26 ++++++++++++------- 8 files changed, 37 insertions(+), 32 deletions(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 1b94411fb9..21700e5a4c 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -564,10 +564,12 @@ class Executor rclcpp::WaitSet wait_set_ RCPPUTILS_TSA_GUARDED_BY(mutex_); /// Hold the current state of the collection being waited on by the waitset - rclcpp::executors::ExecutorEntitiesCollection current_collection_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + rclcpp::executors::ExecutorEntitiesCollection current_collection_ RCPPUTILS_TSA_GUARDED_BY( + mutex_); /// Hold the current state of the notify waitable being waited on by the waitset - std::shared_ptr current_notify_waitable_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + std::shared_ptr current_notify_waitable_ + RCPPUTILS_TSA_GUARDED_BY(mutex_); /// Hold the list of executables currently available to be executed. std::deque ready_executables_ RCPPUTILS_TSA_GUARDED_BY(mutex_); diff --git a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp index 5651fe6328..df19361e9b 100644 --- a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp @@ -101,7 +101,6 @@ class StaticSingleThreadedExecutor : public rclcpp::Executor void spin_all(std::chrono::nanoseconds max_duration) override; - protected: /** * @brief Executes ready executables from wait set. diff --git a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp index 94346f537c..d785a261a7 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp @@ -257,9 +257,9 @@ class StoragePolicyCommon } rcl_ret_t ret = rcl_wait_set_add_subscription( - &rcl_wait_set_, - subscription_entry.subscription->get_subscription_handle().get(), - nullptr); + &rcl_wait_set_, + subscription_entry.subscription->get_subscription_handle().get(), + nullptr); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 6203c0192d..39bbc94f7a 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -145,7 +145,7 @@ Executor::add_callback_group( if (!spinning.load()) { this->collect_entities(); - } else if (notify ){ + } else if (notify) { try { interrupt_guard_condition_->trigger(); } catch (const rclcpp::exceptions::RCLError & ex) { @@ -163,7 +163,7 @@ Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_pt if (!spinning.load()) { this->collect_entities(); - } else if (notify ){ + } else if (notify) { try { interrupt_guard_condition_->trigger(); } catch (const rclcpp::exceptions::RCLError & ex) { @@ -183,7 +183,7 @@ Executor::remove_callback_group( if (!spinning.load()) { this->collect_entities(); - } else if (notify ){ + } else if (notify) { try { interrupt_guard_condition_->trigger(); } catch (const rclcpp::exceptions::RCLError & ex) { @@ -207,7 +207,7 @@ Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node if (!spinning.load()) { this->collect_entities(); - } else if (notify ){ + } else if (notify) { try { interrupt_guard_condition_->trigger(); } catch (const rclcpp::exceptions::RCLError & ex) { diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp index bddc475aa0..0c13b9c3a4 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp @@ -110,16 +110,15 @@ ready_executables( // Cache shared pointers to groups to avoid extra work re-locking them std::map> group_map; + rclcpp::CallbackGroup::SharedPtr, + std::owner_less> group_map; auto group_cache = [&group_map](const rclcpp::CallbackGroup::WeakPtr & weak_cbg_ptr) - { - if (group_map.count(weak_cbg_ptr) == 0) { - group_map.insert({weak_cbg_ptr, weak_cbg_ptr.lock()}); - } - return group_map.find(weak_cbg_ptr)->second; - }; + if (group_map.count(weak_cbg_ptr) == 0) { + group_map.insert({weak_cbg_ptr, weak_cbg_ptr.lock()}); + } + return group_map.find(weak_cbg_ptr)->second; + }; for (size_t ii = 0; ii < rcl_wait_set.size_of_timers; ++ii) { diff --git a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp index ee5dcdadad..0cb4b83768 100644 --- a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp @@ -100,8 +100,8 @@ MultiThreadedExecutor::run(size_t this_thread_number) execute_any_executable(any_exec); if (any_exec.callback_group && - any_exec.callback_group->type() == CallbackGroupType::MutuallyExclusive && - any_exec.callback_group->size() > 1) + any_exec.callback_group->type() == CallbackGroupType::MutuallyExclusive && + any_exec.callback_group->size() > 1) { try { interrupt_guard_condition_->trigger(); diff --git a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp index 259824e291..a4efb4030c 100644 --- a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp @@ -45,8 +45,7 @@ SingleThreadedExecutor::spin() this->ready_executables_.clear(); } - for (auto & exec: to_exec) - { + for (auto & exec: to_exec) { if (exec.callback_group && exec.callback_group->type() == CallbackGroupType::MutuallyExclusive) { diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp index 2aaa7b6996..5ed2d37e76 100644 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp @@ -24,9 +24,10 @@ using rclcpp::executors::StaticSingleThreadedExecutor; StaticSingleThreadedExecutor::StaticSingleThreadedExecutor(const rclcpp::ExecutorOptions & options) : rclcpp::Executor(options) { - notify_waitable_ = std::make_shared([this](){ + notify_waitable_ = std::make_shared( + [this]() { this->entities_need_rebuild.store(true); - }); + }); } StaticSingleThreadedExecutor::~StaticSingleThreadedExecutor() {} @@ -148,9 +149,9 @@ StaticSingleThreadedExecutor::spin_once_impl(std::chrono::nanoseconds timeout) } bool StaticSingleThreadedExecutor::execute_ready_executables( - const rclcpp::executors::ExecutorEntitiesCollection & collection, - rclcpp::WaitResult & wait_result, - bool spin_once) + const rclcpp::executors::ExecutorEntitiesCollection & collection, + rclcpp::WaitResult & wait_result, + bool spin_once) { bool any_ready_executable = false; @@ -172,8 +173,9 @@ bool StaticSingleThreadedExecutor::execute_ready_executables( continue; } execute_timer(entity); - if (spin_once) + if (spin_once) { return true; + } any_ready_executable = true; } } @@ -187,8 +189,9 @@ bool StaticSingleThreadedExecutor::execute_ready_executables( continue; } execute_subscription(entity); - if (spin_once) + if (spin_once) { return true; + } any_ready_executable = true; } } @@ -202,8 +205,9 @@ bool StaticSingleThreadedExecutor::execute_ready_executables( continue; } execute_service(entity); - if (spin_once) + if (spin_once) { return true; + } any_ready_executable = true; } } @@ -217,8 +221,9 @@ bool StaticSingleThreadedExecutor::execute_ready_executables( continue; } execute_client(entity); - if (spin_once) + if (spin_once) { return true; + } any_ready_executable = true; } } @@ -234,8 +239,9 @@ bool StaticSingleThreadedExecutor::execute_ready_executables( auto data = waitable->take_data(); waitable->execute(data); - if (spin_once) + if (spin_once) { return true; + } any_ready_executable = true; } return any_ready_executable; From 985c1f4e81d56cd4424e717153a6983404a4f894 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 11 Apr 2023 12:21:58 -0500 Subject: [PATCH 30/74] Restore tests Signed-off-by: Michael Carroll --- rclcpp/test/benchmark/benchmark_executor.cpp | 88 +++++++++++++++++++ .../test/rclcpp/executors/test_executors.cpp | 9 +- .../test_add_callback_groups_to_executor.cpp | 10 ++- 3 files changed, 101 insertions(+), 6 deletions(-) diff --git a/rclcpp/test/benchmark/benchmark_executor.cpp b/rclcpp/test/benchmark/benchmark_executor.cpp index c2b42752f2..65bb3a1007 100644 --- a/rclcpp/test/benchmark/benchmark_executor.cpp +++ b/rclcpp/test/benchmark/benchmark_executor.cpp @@ -189,6 +189,71 @@ BENCHMARK_F(PerformanceTestExecutorSimple, multi_thread_executor_remove_node)(be } } +BENCHMARK_F( + PerformanceTestExecutorSimple, + static_single_thread_executor_add_node)(benchmark::State & st) +{ + rclcpp::executors::StaticSingleThreadedExecutor executor; + for (auto _ : st) { + (void)_; + executor.add_node(node); + st.PauseTiming(); + executor.remove_node(node); + st.ResumeTiming(); + } +} + +BENCHMARK_F( + PerformanceTestExecutorSimple, + static_single_thread_executor_remove_node)(benchmark::State & st) +{ + rclcpp::executors::StaticSingleThreadedExecutor executor; + for (auto _ : st) { + (void)_; + st.PauseTiming(); + executor.add_node(node); + st.ResumeTiming(); + executor.remove_node(node); + } +} + +BENCHMARK_F( + PerformanceTestExecutorSimple, + static_single_thread_executor_spin_until_future_complete)(benchmark::State & st) +{ + rclcpp::executors::StaticSingleThreadedExecutor executor; + // test success of an immediately finishing future + std::promise promise; + std::future future = promise.get_future(); + promise.set_value(true); + auto shared_future = future.share(); + + auto ret = executor.spin_until_future_complete(shared_future, 100ms); + if (ret != rclcpp::FutureReturnCode::SUCCESS) { + st.SkipWithError(rcutils_get_error_string().str); + } + + reset_heap_counters(); + + for (auto _ : st) { + (void)_; + // static_single_thread_executor has a special design. We need to add/remove the node each + // time you call spin + st.PauseTiming(); + executor.add_node(node); + st.ResumeTiming(); + + ret = executor.spin_until_future_complete(shared_future, 100ms); + if (ret != rclcpp::FutureReturnCode::SUCCESS) { + st.SkipWithError(rcutils_get_error_string().str); + break; + } + st.PauseTiming(); + executor.remove_node(node); + st.ResumeTiming(); + } +} + BENCHMARK_F( PerformanceTestExecutorSimple, single_thread_executor_spin_node_until_future_complete)(benchmark::State & st) @@ -249,6 +314,29 @@ BENCHMARK_F( } } +BENCHMARK_F( + PerformanceTestExecutorSimple, + static_single_thread_executor_spin_node_until_future_complete)(benchmark::State & st) +{ + rclcpp::executors::StaticSingleThreadedExecutor executor; + // test success of an immediately finishing future + std::promise promise; + std::future future = promise.get_future(); + promise.set_value(true); + auto shared_future = future.share(); + + reset_heap_counters(); + + for (auto _ : st) { + (void)_; + auto ret = rclcpp::executors::spin_node_until_future_complete( + executor, node, shared_future, 1s); + if (ret != rclcpp::FutureReturnCode::SUCCESS) { + st.SkipWithError(rcutils_get_error_string().str); + break; + } + } +} BENCHMARK_F(PerformanceTestExecutorSimple, spin_until_future_complete)(benchmark::State & st) { diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 1fee748b78..1fa2cbb4dd 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -91,7 +91,8 @@ class TestExecutorsStable : public TestExecutors {}; using ExecutorTypes = ::testing::Types< rclcpp::executors::SingleThreadedExecutor, - rclcpp::executors::MultiThreadedExecutor>; + rclcpp::executors::MultiThreadedExecutor, + rclcpp::executors::StaticSingleThreadedExecutor>; class ExecutorTypeNames { @@ -108,6 +109,10 @@ class ExecutorTypeNames return "MultiThreadedExecutor"; } + if (std::is_same()) { + return "StaticSingleThreadedExecutor"; + } + return ""; } }; @@ -151,7 +156,7 @@ TYPED_TEST(TestExecutorsStable, addTemporaryNode) { } // Sleep for a short time to verify executor.spin() is going, and didn't throw. - std::thread spinner([&]() {executor.spin();}); + std::thread spinner([&]() {EXPECT_NO_THROW(executor.spin());}); std::this_thread::sleep_for(50ms); executor.cancel(); diff --git a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp index b53f47ffd5..02fa0b7a94 100644 --- a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp +++ b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp @@ -49,7 +49,8 @@ class TestAddCallbackGroupsToExecutor : public ::testing::Test using ExecutorTypes = ::testing::Types< rclcpp::executors::SingleThreadedExecutor, - rclcpp::executors::MultiThreadedExecutor>; + rclcpp::executors::MultiThreadedExecutor, + rclcpp::executors::StaticSingleThreadedExecutor>; class ExecutorTypeNames { @@ -66,6 +67,10 @@ class ExecutorTypeNames return "MultiThreadedExecutor"; } + if (std::is_same()) { + return "StaticSingleThreadedExecutor"; + } + return ""; } }; @@ -132,11 +137,9 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, remove_callback_groups) { 2s, timer_callback, cb_grp); ExecutorType executor; executor.add_callback_group(cb_grp, node->get_node_base_interface()); - const rclcpp::QoS qos(10); auto options = rclcpp::SubscriptionOptions(); auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; - rclcpp::CallbackGroup::SharedPtr cb_grp2 = node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive); options.callback_group = cb_grp2; @@ -144,7 +147,6 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, remove_callback_groups) { node->create_subscription("topic_name", qos, callback, options); executor.add_callback_group(cb_grp2, node->get_node_base_interface()); - executor.remove_callback_group(cb_grp); ASSERT_EQ(executor.get_all_callback_groups().size(), 1u); executor.remove_callback_group(cb_grp2); From 03471fc97a9d38288bfe5a2409a61c8e1a07770c Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 11 Apr 2023 12:55:37 -0500 Subject: [PATCH 31/74] Back to weak_ptr and reduce test time Signed-off-by: Michael Carroll --- .../executors/executor_notify_waitable.hpp | 4 ++-- rclcpp/src/rclcpp/executor.cpp | 3 +-- .../executors/executor_notify_waitable.cpp | 20 ++++++++++--------- .../src/rclcpp/node_interfaces/node_base.cpp | 1 + .../test_add_callback_groups_to_executor.cpp | 6 +++--- 5 files changed, 18 insertions(+), 16 deletions(-) diff --git a/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp b/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp index bf6ae26089..9fb44e78c6 100644 --- a/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp +++ b/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp @@ -120,8 +120,8 @@ class ExecutorNotifyWaitable : public rclcpp::Waitable std::mutex guard_condition_mutex_; /// The collection of guard conditions to be waited on. - std::set> notify_guard_conditions_; + std::set> notify_guard_conditions_; }; } // namespace executors diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 39bbc94f7a..6e2e8a7aa0 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -518,8 +518,7 @@ Executor::collect_entities() // This prevents the collection of guard conditions in the waitable from changing // while we are waiting on it. if (notify_waitable_) { - *current_notify_waitable_.get() = *notify_waitable_.get(); - + current_notify_waitable_ = std::make_shared(*notify_waitable_); auto notify_waitable = std::static_pointer_cast(current_notify_waitable_); collection.waitables.insert({notify_waitable.get(), {notify_waitable, {}}}); } diff --git a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp index 9b9445b270..606a37b00f 100644 --- a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp +++ b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp @@ -46,9 +46,11 @@ 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(); + for (auto weak_guard_condition : this->notify_guard_conditions_) { + auto guard_condition = weak_guard_condition.lock(); + if (!guard_condition) continue; + 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); @@ -64,6 +66,7 @@ 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]; @@ -71,7 +74,8 @@ ExecutorNotifyWaitable::is_ready(rcl_wait_set_t * wait_set) if (nullptr == rcl_guard_condition) { continue; } - for (auto guard_condition : this->notify_guard_conditions_) { + 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; } @@ -97,9 +101,8 @@ void ExecutorNotifyWaitable::add_guard_condition(rclcpp::GuardCondition::WeakPtr weak_guard_condition) { std::lock_guard lock(guard_condition_mutex_); - auto guard_condition = weak_guard_condition.lock(); - if (guard_condition && notify_guard_conditions_.count(guard_condition) == 0) { - notify_guard_conditions_.insert(guard_condition); + if (notify_guard_conditions_.count(weak_guard_condition) == 0) { + notify_guard_conditions_.insert(weak_guard_condition); } } @@ -107,9 +110,8 @@ void ExecutorNotifyWaitable::remove_guard_condition(rclcpp::GuardCondition::WeakPtr weak_guard_condition) { std::lock_guard lock(guard_condition_mutex_); - auto guard_condition = weak_guard_condition.lock(); - if (notify_guard_conditions_.count(guard_condition) != 0) { - notify_guard_conditions_.erase(guard_condition); + if (notify_guard_conditions_.count(weak_guard_condition) != 0) { + notify_guard_conditions_.erase(weak_guard_condition); } } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index 4575d705a8..826f8b7b8b 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -208,6 +208,7 @@ NodeBase::create_callback_group( automatically_add_to_executor_with_node); std::lock_guard lock(callback_groups_mutex_); callback_groups_.push_back(group); + this->trigger_notify_guard_condition(); return group; } diff --git a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp index 02fa0b7a94..70e85d88ce 100644 --- a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp +++ b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp @@ -191,17 +191,17 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups_after_add_node_t rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive); rclcpp::TimerBase::SharedPtr timer_ = node->create_wall_timer( - 2s, timer_callback, cb_grp); + 1s, timer_callback, cb_grp); rclcpp::CallbackGroup::SharedPtr cb_grp2 = node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false); auto timer2_callback = []() {}; rclcpp::TimerBase::SharedPtr timer2_ = node->create_wall_timer( - 2s, timer2_callback, cb_grp2); + 1s, timer2_callback, cb_grp2); rclcpp::CallbackGroup::SharedPtr cb_grp3 = node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, true); auto timer3_callback = []() {}; rclcpp::TimerBase::SharedPtr timer3_ = node->create_wall_timer( - 2s, timer3_callback, cb_grp3); + 1s, timer3_callback, cb_grp3); executor.spin(); } From 5c70cb6808b594f1558be75b76294535adca0a2c Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 11 Apr 2023 13:10:44 -0500 Subject: [PATCH 32/74] reduce diff and lint Signed-off-by: Michael Carroll --- .../wait_set_policies/detail/storage_policy_common.hpp | 7 +++---- rclcpp/src/rclcpp/executor.cpp | 3 ++- rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp | 2 +- rclcpp/src/rclcpp/executors/single_threaded_executor.cpp | 3 +-- 4 files changed, 7 insertions(+), 8 deletions(-) diff --git a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp index d785a261a7..bedd116793 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp @@ -191,7 +191,7 @@ class StoragePolicyCommon size_t clients_from_waitables = 0; size_t services_from_waitables = 0; size_t events_from_waitables = 0; - for (const auto & waitable_entry: waitables) { + for (const auto & waitable_entry : waitables) { if (!waitable_entry.waitable) { // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. if (HasStrongOwnership) { @@ -232,7 +232,6 @@ class StoragePolicyCommon needs_resize_ = false; } - // Now clear the wait set, but only if it was not resized, as resizing also // clears the wait set. if (!was_resized) { @@ -243,7 +242,7 @@ class StoragePolicyCommon } // Add subscriptions. - for (const auto & subscription_entry: subscriptions) { + for (const auto & subscription_entry : subscriptions) { if (!subscription_entry.subscription) { // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. if (HasStrongOwnership) { @@ -365,7 +364,7 @@ class StoragePolicyCommon } // Add waitables. - for (auto & waitable_entry: waitables) { + for (auto & waitable_entry : waitables) { if (!waitable_entry.waitable) { // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. if (HasStrongOwnership) { diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 6e2e8a7aa0..e28bd143c3 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -518,7 +518,8 @@ Executor::collect_entities() // This prevents the collection of guard conditions in the waitable from changing // while we are waiting on it. if (notify_waitable_) { - current_notify_waitable_ = std::make_shared(*notify_waitable_); + current_notify_waitable_ = std::make_shared( + *notify_waitable_); auto notify_waitable = std::static_pointer_cast(current_notify_waitable_); collection.waitables.insert({notify_waitable.get(), {notify_waitable, {}}}); } diff --git a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp index 606a37b00f..55694d35ea 100644 --- a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp +++ b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp @@ -48,7 +48,7 @@ ExecutorNotifyWaitable::add_to_wait_set(rcl_wait_set_t * wait_set) std::lock_guard lock(guard_condition_mutex_); for (auto weak_guard_condition : this->notify_guard_conditions_) { auto guard_condition = weak_guard_condition.lock(); - if (!guard_condition) continue; + if (!guard_condition) {continue;} auto rcl_guard_condition = &guard_condition->get_rcl_guard_condition(); rcl_ret_t ret = rcl_wait_set_add_guard_condition( diff --git a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp index a4efb4030c..f2d6a58ee0 100644 --- a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp @@ -34,7 +34,6 @@ SingleThreadedExecutor::spin() RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); while (rclcpp::ok(this->context_) && spinning.load()) { - wait_for_work(); /// Get the ready executables in a thread-safe way. @@ -45,7 +44,7 @@ SingleThreadedExecutor::spin() this->ready_executables_.clear(); } - for (auto & exec: to_exec) { + for (auto & exec : to_exec) { if (exec.callback_group && exec.callback_group->type() == CallbackGroupType::MutuallyExclusive) { From 31d25fc0f7bfeafe308e02ecfda98c69db6dcf38 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 11 Apr 2023 14:56:58 -0500 Subject: [PATCH 33/74] Restore static single threaded tests that weren't working before Signed-off-by: Michael Carroll --- .../detail/storage_policy_common.hpp | 12 +++---- .../sequential_synchronization.hpp | 2 +- rclcpp/src/rclcpp/executor.cpp | 21 ++++++++----- .../executors/executor_entities_collector.cpp | 6 +++- .../static_single_threaded_executor.cpp | 2 ++ .../test/rclcpp/executors/test_executors.cpp | 14 +-------- rclcpp/test/rclcpp/test_executor.cpp | 31 +++++++------------ 7 files changed, 40 insertions(+), 48 deletions(-) diff --git a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp index bedd116793..aa1844922a 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp @@ -221,7 +221,7 @@ class StoragePolicyCommon events_from_waitables ); if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't resize the wait set"); } was_resized = true; // Assumption: the calling code ensures this function is not called @@ -237,7 +237,7 @@ class StoragePolicyCommon if (!was_resized) { rcl_ret_t ret = rcl_wait_set_clear(&rcl_wait_set_); if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't clear the wait set"); } } @@ -260,7 +260,7 @@ class StoragePolicyCommon subscription_entry.subscription->get_subscription_handle().get(), nullptr); if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't fill wait set"); } } @@ -285,7 +285,7 @@ class StoragePolicyCommon &guard_condition->get_rcl_guard_condition(), nullptr); if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't fill wait set"); } } }; @@ -314,7 +314,7 @@ class StoragePolicyCommon timer->get_timer_handle().get(), nullptr); if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't fill wait set"); } } @@ -359,7 +359,7 @@ class StoragePolicyCommon service->get_service_handle().get(), nullptr); if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't fill wait set"); } } diff --git a/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp b/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp index be2e569c40..4afc2a1b27 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp @@ -290,7 +290,7 @@ class SequentialSynchronization : public detail::SynchronizationPolicyCommon return create_wait_result(WaitResultKind::Empty); } else { // Some other error case, throw. - rclcpp::exceptions::throw_from_rcl_error(ret); + rclcpp::exceptions::throw_from_rcl_error(ret, "rcl_wait() failed"); } } while (should_loop()); diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index e28bd143c3..44c448a7bb 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -145,7 +145,9 @@ Executor::add_callback_group( if (!spinning.load()) { this->collect_entities(); - } else if (notify) { + } + + if (notify) { try { interrupt_guard_condition_->trigger(); } catch (const rclcpp::exceptions::RCLError & ex) { @@ -163,13 +165,15 @@ Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_pt if (!spinning.load()) { this->collect_entities(); - } else if (notify) { + } + + if (notify) { try { interrupt_guard_condition_->trigger(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( std::string( - "Failed to trigger guard condition on callback group add: ") + ex.what()); + "Failed to trigger guard condition on node add: ") + ex.what()); } } } @@ -183,13 +187,14 @@ Executor::remove_callback_group( if (!spinning.load()) { this->collect_entities(); - } else if (notify) { + } + if (notify) { try { interrupt_guard_condition_->trigger(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( std::string( - "Failed to trigger guard condition on callback group add: ") + ex.what()); + "Failed to trigger guard condition on callback group remove: ") + ex.what()); } } } @@ -207,13 +212,15 @@ Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node if (!spinning.load()) { this->collect_entities(); - } else if (notify) { + } + + if (notify) { try { interrupt_guard_condition_->trigger(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( std::string( - "Failed to trigger guard condition on callback group add: ") + ex.what()); + "Failed to trigger guard condition on node remove: ") + ex.what()); } } } diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp index 525e0e87f0..938ed2857b 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp @@ -304,7 +304,11 @@ ExecutorEntitiesCollector::process_queues() if (node_it != weak_nodes_.end()) { remove_weak_node(node_it); } else { - throw std::runtime_error("Node needs to be associated with this executor."); + // The node may have been destroyed and removed from the colletion before + // we processed the queues. Don't throw if the pointer is already expired. + if (!weak_node_ptr.expired()) { + throw std::runtime_error("Node needs to be associated with this executor."); + } } auto node_ptr = weak_node_ptr.lock(); diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp index 5ed2d37e76..194bb168d0 100644 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp @@ -28,6 +28,8 @@ StaticSingleThreadedExecutor::StaticSingleThreadedExecutor(const rclcpp::Executo [this]() { this->entities_need_rebuild.store(true); }); + notify_waitable_->add_guard_condition(interrupt_guard_condition_); + notify_waitable_->add_guard_condition(shutdown_guard_condition_); } StaticSingleThreadedExecutor::~StaticSingleThreadedExecutor() {} diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 1fa2cbb4dd..e44a37efd1 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -83,8 +83,6 @@ class TestExecutors : public ::testing::Test int callback_count; }; -// spin_all and spin_some are not implemented correctly in StaticSingleThreadedExecutor, see: -// https://github.com/ros2/rclcpp/issues/1219 for tracking template class TestExecutorsStable : public TestExecutors {}; @@ -121,14 +119,6 @@ class ExecutorTypeNames // is updated. TYPED_TEST_SUITE(TestExecutors, ExecutorTypes, ExecutorTypeNames); -// StaticSingleThreadedExecutor is not included in these tests for now, due to: -// https://github.com/ros2/rclcpp/issues/1219 -using StandardExecutors = - ::testing::Types< - rclcpp::executors::SingleThreadedExecutor, - rclcpp::executors::MultiThreadedExecutor>; -TYPED_TEST_SUITE(TestExecutorsStable, StandardExecutors, ExecutorTypeNames); - // Make sure that executors detach from nodes when destructing TYPED_TEST(TestExecutors, detachOnDestruction) { using ExecutorType = TypeParam; @@ -143,9 +133,7 @@ TYPED_TEST(TestExecutors, detachOnDestruction) { } // Make sure that the executor can automatically remove expired nodes correctly -// Currently fails for StaticSingleThreadedExecutor so it is being skipped, see: -// https://github.com/ros2/rclcpp/issues/1231 -TYPED_TEST(TestExecutorsStable, addTemporaryNode) { +TYPED_TEST(TestExecutors, addTemporaryNode) { using ExecutorType = TypeParam; ExecutorType executor; diff --git a/rclcpp/test/rclcpp/test_executor.cpp b/rclcpp/test/rclcpp/test_executor.cpp index e36b060d0b..ce9986402e 100644 --- a/rclcpp/test/rclcpp/test_executor.cpp +++ b/rclcpp/test/rclcpp/test_executor.cpp @@ -173,7 +173,7 @@ TEST_F(TestExecutor, remove_node_not_associated) { RCLCPP_EXPECT_THROW_EQ( dummy.remove_node(node->get_node_base_interface(), false), - std::runtime_error("Node needs to be associated with an executor.")); + std::runtime_error("Node '/ns/node' needs to be associated with an executor.")); } TEST_F(TestExecutor, remove_node_associated_with_different_executor) { @@ -187,7 +187,7 @@ TEST_F(TestExecutor, remove_node_associated_with_different_executor) { RCLCPP_EXPECT_THROW_EQ( dummy2.remove_node(node1->get_node_base_interface(), false), - std::runtime_error("Node needs to be associated with this executor.")); + std::runtime_error("Node '/ns/node1' needs to be associated with this executor.")); } TEST_F(TestExecutor, spin_node_once_nanoseconds) { @@ -304,24 +304,14 @@ TEST_F(TestExecutor, cancel_failed_trigger_guard_condition) { std::runtime_error("Failed to trigger guard condition in cancel: error not set")); } -TEST_F(TestExecutor, spin_once_failed_trigger_guard_condition) { - DummyExecutor dummy; - auto node = std::make_shared("node", "ns"); - auto timer = - node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}); - - dummy.add_node(node); - // Wait for the wall timer to have expired. - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); +TEST_F(TestExecutor, create_executor_fail_wait_set_clear) { + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_clear, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( - dummy.spin_once(std::chrono::milliseconds(1)), - std::runtime_error( - "Failed to trigger guard condition from execute_any_executable: error not set")); + DummyExecutor dummy, + std::runtime_error("Couldn't clear the wait set: error not set")); } -TEST_F(TestExecutor, spin_some_fail_wait_set_clear) { +TEST_F(TestExecutor, spin_all_fail_wait_set_clear) { DummyExecutor dummy; auto node = std::make_shared("node", "ns"); auto timer = @@ -329,9 +319,10 @@ TEST_F(TestExecutor, spin_some_fail_wait_set_clear) { dummy.add_node(node); auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_clear, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( - dummy.spin_some(std::chrono::milliseconds(1)), - std::runtime_error("Couldn't clear wait set: error not set")); + dummy.spin_all(std::chrono::milliseconds(1)), + std::runtime_error("Couldn't clear the wait set: error not set")); } TEST_F(TestExecutor, spin_some_fail_wait_set_resize) { @@ -359,7 +350,7 @@ TEST_F(TestExecutor, spin_some_fail_add_handles_to_wait_set) { RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( dummy.spin_some(std::chrono::milliseconds(1)), - std::runtime_error("Couldn't fill wait set")); + std::runtime_error("Couldn't fill wait set: error not set")); } TEST_F(TestExecutor, spin_some_fail_wait) { From 7a81a8fb8ac88588206a60662ef977c43b262138 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 11 Apr 2023 15:19:54 -0500 Subject: [PATCH 34/74] Restore more tests Signed-off-by: Michael Carroll --- rclcpp/include/rclcpp/executors.hpp | 3 +++ .../executors/static_single_threaded_executor.hpp | 11 +++++++++-- .../src/rclcpp/executors/single_threaded_executor.cpp | 3 ++- .../executors/static_single_threaded_executor.cpp | 2 +- .../rclcpp/test_add_callback_groups_to_executor.cpp | 6 +++--- rclcpp/test/rclcpp/test_executor.cpp | 7 +++++++ 6 files changed, 25 insertions(+), 7 deletions(-) diff --git a/rclcpp/include/rclcpp/executors.hpp b/rclcpp/include/rclcpp/executors.hpp index bf822cab62..36fb0d63cf 100644 --- a/rclcpp/include/rclcpp/executors.hpp +++ b/rclcpp/include/rclcpp/executors.hpp @@ -51,6 +51,9 @@ spin(rclcpp::Node::SharedPtr node_ptr); namespace executors { +using rclcpp::executors::MultiThreadedExecutor; +using rclcpp::executors::SingleThreadedExecutor; + /// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted. /** * \param[in] executor The executor which will spin the node. diff --git a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp index df19361e9b..71f1819161 100644 --- a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp @@ -15,7 +15,12 @@ #ifndef RCLCPP__EXECUTORS__STATIC_SINGLE_THREADED_EXECUTOR_HPP_ #define RCLCPP__EXECUTORS__STATIC_SINGLE_THREADED_EXECUTOR_HPP_ +#include +#include +#include + #include "rclcpp/executor.hpp" +#include "rclcpp/executors/executor_entities_collection.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" namespace rclcpp @@ -39,7 +44,6 @@ namespace executors * exec.spin(); * exec.remove_node(node); */ - class StaticSingleThreadedExecutor : public rclcpp::Executor { public: @@ -104,11 +108,14 @@ class StaticSingleThreadedExecutor : public rclcpp::Executor protected: /** * @brief Executes ready executables from wait set. + * @param collection entities to evaluate for ready executables. + * @param wait_result result to check for ready executables. * @param spin_once if true executes only the first ready executable. * @return true if any executable was ready. */ RCLCPP_PUBLIC - bool execute_ready_executables( + bool + execute_ready_executables( const rclcpp::executors::ExecutorEntitiesCollection & collection, rclcpp::WaitResult & wait_result, bool spin_once); diff --git a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp index f2d6a58ee0..06f287a85e 100644 --- a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp @@ -32,7 +32,6 @@ SingleThreadedExecutor::spin() throw std::runtime_error("spin() called while already spinning"); } RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); - while (rclcpp::ok(this->context_) && spinning.load()) { wait_for_work(); @@ -44,7 +43,9 @@ SingleThreadedExecutor::spin() this->ready_executables_.clear(); } + // Execute all available executables before return to wait for work for (auto & exec : to_exec) { + // Block mutually exclusive callback group if (exec.callback_group && exec.callback_group->type() == CallbackGroupType::MutuallyExclusive) { diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp index 194bb168d0..43117ab054 100644 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp @@ -1,4 +1,4 @@ -// Copyright 2015 Open Source Robotics Foundation, Inc. +// Copyright 2019 Nobleo Technology // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp index 70e85d88ce..02fa0b7a94 100644 --- a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp +++ b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp @@ -191,17 +191,17 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups_after_add_node_t rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive); rclcpp::TimerBase::SharedPtr timer_ = node->create_wall_timer( - 1s, timer_callback, cb_grp); + 2s, timer_callback, cb_grp); rclcpp::CallbackGroup::SharedPtr cb_grp2 = node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false); auto timer2_callback = []() {}; rclcpp::TimerBase::SharedPtr timer2_ = node->create_wall_timer( - 1s, timer2_callback, cb_grp2); + 2s, timer2_callback, cb_grp2); rclcpp::CallbackGroup::SharedPtr cb_grp3 = node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, true); auto timer3_callback = []() {}; rclcpp::TimerBase::SharedPtr timer3_ = node->create_wall_timer( - 1s, timer3_callback, cb_grp3); + 2s, timer3_callback, cb_grp3); executor.spin(); } diff --git a/rclcpp/test/rclcpp/test_executor.cpp b/rclcpp/test/rclcpp/test_executor.cpp index ce9986402e..b856d19b39 100644 --- a/rclcpp/test/rclcpp/test_executor.cpp +++ b/rclcpp/test/rclcpp/test_executor.cpp @@ -109,6 +109,13 @@ TEST_F(TestExecutor, constructor_bad_guard_condition_init) { rclcpp::exceptions::RCLError); } +TEST_F(TestExecutor, constructor_bad_wait_set_init) { + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_init, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + static_cast(std::make_unique()), + std::runtime_error("Failed to create wait set in Executor constructor: error not set")); +} + TEST_F(TestExecutor, add_callback_group_twice) { DummyExecutor dummy; auto node = std::make_shared("node", "ns"); From 38387e0a29934cd86e97a67eab82c1483989579e Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 11 Apr 2023 16:24:07 -0500 Subject: [PATCH 35/74] Fix multithreaded test Signed-off-by: Michael Carroll --- rclcpp/include/rclcpp/executor.hpp | 2 ++ .../static_single_threaded_executor.hpp | 2 -- rclcpp/src/rclcpp/executor.cpp | 14 ++++++---- .../executors/executor_entities_collector.cpp | 1 + .../executors/executor_notify_waitable.cpp | 6 ++--- .../static_single_threaded_executor.cpp | 26 ++++++++----------- .../test/rclcpp/executors/test_executors.cpp | 8 +++--- 7 files changed, 31 insertions(+), 28 deletions(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 21700e5a4c..c01ff7f10b 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -557,6 +557,8 @@ class Executor */ std::shared_ptr notify_waitable_; + std::atomic_bool entities_need_rebuild_; + /// Collector used to associate executable entities from nodes and guard conditions rclcpp::executors::ExecutorEntitiesCollector collector_; diff --git a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp index 71f1819161..8d01496d30 100644 --- a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp @@ -130,8 +130,6 @@ class StaticSingleThreadedExecutor : public rclcpp::Executor private: RCLCPP_DISABLE_COPY(StaticSingleThreadedExecutor) - - std::atomic_bool entities_need_rebuild; }; } // namespace executors diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 44c448a7bb..653a73de2a 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -51,7 +51,7 @@ Executor::Executor(const rclcpp::ExecutorOptions & options) shutdown_guard_condition_(std::make_shared(options.context)), notify_waitable_(std::make_shared( [this]() { - this->collect_entities(); + this->entities_need_rebuild_.store(true); })), collector_(notify_waitable_), wait_set_({}, {}, {}, {}, {}, {}, options.context), @@ -144,6 +144,7 @@ Executor::add_callback_group( this->collector_.add_callback_group(group_ptr); if (!spinning.load()) { + std::lock_guard guard(mutex_); this->collect_entities(); } @@ -164,6 +165,7 @@ Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_pt this->collector_.add_node(node_ptr); if (!spinning.load()) { + std::lock_guard guard(mutex_); this->collect_entities(); } @@ -186,6 +188,7 @@ Executor::remove_callback_group( this->collector_.remove_callback_group(group_ptr); if (!spinning.load()) { + std::lock_guard guard(mutex_); this->collect_entities(); } if (notify) { @@ -211,6 +214,7 @@ Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node this->collector_.remove_node(node_ptr); if (!spinning.load()) { + std::lock_guard guard(mutex_); this->collect_entities(); } @@ -518,8 +522,6 @@ Executor::collect_entities() auto callback_groups = this->collector_.get_all_callback_groups(); rclcpp::executors::build_entities_collection(callback_groups, collection); - std::lock_guard guard(mutex_); - // Make a copy of notify waitable so we can continue to mutate the original // one outside of the execute loop. // This prevents the collection of guard conditions in the waitable from changing @@ -568,6 +570,8 @@ Executor::collect_entities() wait_set_.add_waitable(waitable); }, [this](auto waitable) {wait_set_.remove_waitable(waitable);}); + + this->entities_need_rebuild_.store(false); } void @@ -575,11 +579,11 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) { TRACEPOINT(rclcpp_executor_wait_for_work, timeout.count()); - if (current_collection_.empty()) { + std::lock_guard guard(mutex_); + if (current_collection_.empty() || this->entities_need_rebuild_.load()) { this->collect_entities(); } - std::lock_guard guard(mutex_); auto wait_result = wait_set_.wait(timeout); if (wait_result.kind() == WaitResultKind::Empty) { RCUTILS_LOG_WARN_NAMED( diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp index 938ed2857b..ede657f08c 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp @@ -278,6 +278,7 @@ ExecutorEntitiesCollector::add_callback_group_to_collection( // Store node guard condition in map and add it to the notify waitable 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); } diff --git a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp index 55694d35ea..8a317a357f 100644 --- a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp +++ b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp @@ -50,10 +50,10 @@ ExecutorNotifyWaitable::add_to_wait_set(rcl_wait_set_t * wait_set) auto guard_condition = weak_guard_condition.lock(); if (!guard_condition) {continue;} - auto rcl_guard_condition = &guard_condition->get_rcl_guard_condition(); + rcl_guard_condition_t * cond = &guard_condition->get_rcl_guard_condition(); + rcl_ret_t ret = rcl_wait_set_add_guard_condition( - wait_set, - rcl_guard_condition, NULL); + wait_set, cond, NULL); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error( diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp index 43117ab054..c5930fac12 100644 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp @@ -24,12 +24,6 @@ using rclcpp::executors::StaticSingleThreadedExecutor; StaticSingleThreadedExecutor::StaticSingleThreadedExecutor(const rclcpp::ExecutorOptions & options) : rclcpp::Executor(options) { - notify_waitable_ = std::make_shared( - [this]() { - this->entities_need_rebuild.store(true); - }); - notify_waitable_->add_guard_condition(interrupt_guard_condition_); - notify_waitable_->add_guard_condition(shutdown_guard_condition_); } StaticSingleThreadedExecutor::~StaticSingleThreadedExecutor() {} @@ -42,15 +36,17 @@ StaticSingleThreadedExecutor::spin() } RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); + // This is essentially the contents of the rclcpp::Executor::wait_for_work method, + // except we need to keep the wait result to reproduce the StaticSingleThreadedExecutor + // behavior. while (rclcpp::ok(this->context_) && spinning.load()) { std::deque to_exec; - if (current_collection_.empty() || this->entities_need_rebuild.load()) { + std::lock_guard guard(mutex_); + if (current_collection_.empty() || this->entities_need_rebuild_.load()) { this->collect_entities(); - this->entities_need_rebuild.store(false); } - std::lock_guard guard(mutex_); auto wait_result = wait_set_.wait(std::chrono::nanoseconds(-1)); if (wait_result.kind() == WaitResultKind::Empty) { RCUTILS_LOG_WARN_NAMED( @@ -105,12 +101,11 @@ StaticSingleThreadedExecutor::spin_some_impl(std::chrono::nanoseconds max_durati while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) { // Get executables that are ready now - if (current_collection_.empty() || this->entities_need_rebuild.load()) { + std::lock_guard guard(mutex_); + if (current_collection_.empty() || this->entities_need_rebuild_.load()) { this->collect_entities(); - this->entities_need_rebuild.store(false); } - std::lock_guard guard(mutex_); auto wait_result = wait_set_.wait(std::chrono::nanoseconds(0)); if (wait_result.kind() == WaitResultKind::Empty) { RCUTILS_LOG_WARN_NAMED( @@ -131,12 +126,11 @@ void StaticSingleThreadedExecutor::spin_once_impl(std::chrono::nanoseconds timeout) { if (rclcpp::ok(context_) && spinning.load()) { - if (current_collection_.empty() || this->entities_need_rebuild.load()) { + std::lock_guard guard(mutex_); + if (current_collection_.empty() || this->entities_need_rebuild_.load()) { this->collect_entities(); - this->entities_need_rebuild.store(false); } - std::lock_guard guard(mutex_); auto wait_result = wait_set_.wait(std::chrono::nanoseconds(timeout)); if (wait_result.kind() == WaitResultKind::Empty) { RCUTILS_LOG_WARN_NAMED( @@ -150,6 +144,8 @@ StaticSingleThreadedExecutor::spin_once_impl(std::chrono::nanoseconds timeout) } } +// This preserves the "scheduling semantics" of the StaticSingleThreadedExecutor +// from the original implementation. bool StaticSingleThreadedExecutor::execute_ready_executables( const rclcpp::executors::ExecutorEntitiesCollection & collection, rclcpp::WaitResult & wait_result, diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index e44a37efd1..5b92957890 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -189,14 +189,16 @@ TYPED_TEST(TestExecutors, spinWhileAlreadySpinning) { ExecutorType executor; executor.add_node(this->node); - bool timer_completed = false; - auto timer = this->node->create_wall_timer(1ms, [&]() {timer_completed = true;}); + std::atomic_bool timer_completed = false; + auto timer = this->node->create_wall_timer(1ms, [&]() { + timer_completed.store(true); + }); std::thread spinner([&]() {executor.spin();}); // Sleep for a short time to verify executor.spin() is going, and didn't throw. auto start = std::chrono::steady_clock::now(); - while (!timer_completed && (std::chrono::steady_clock::now() - start) < 10s) { + while (!timer_completed.load() && (std::chrono::steady_clock::now() - start) < 10s) { std::this_thread::sleep_for(1ms); } From a2f397715e3498af60dd8867e4282e678ca5a336 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 11 Apr 2023 17:53:53 -0500 Subject: [PATCH 36/74] Fix assert Signed-off-by: Michael Carroll --- rclcpp/src/rclcpp/executors/single_threaded_executor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp index 06f287a85e..7b2adc044c 100644 --- a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp @@ -49,7 +49,7 @@ SingleThreadedExecutor::spin() if (exec.callback_group && exec.callback_group->type() == CallbackGroupType::MutuallyExclusive) { - assert(any_executable.callback_group->can_be_taken_from().load()); + assert(exec.callback_group->can_be_taken_from().load()); exec.callback_group->can_be_taken_from().store(false); } execute_any_executable(exec); From 1ad6ad66cfc8c963c00b5c8ce8fb70548254f67f Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 12 Apr 2023 08:49:47 -0500 Subject: [PATCH 37/74] Fix constructor test Signed-off-by: Michael Carroll --- .../wait_set_policies/detail/storage_policy_common.hpp | 2 +- rclcpp/test/rclcpp/executors/test_executors.cpp | 5 +++-- rclcpp/test/rclcpp/test_executor.cpp | 2 +- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp index aa1844922a..c4d69613e2 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp @@ -104,7 +104,7 @@ class StoragePolicyCommon // TODO(wjwwood): support custom allocator, maybe restrict to polymorphic allocator rcl_get_default_allocator()); if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); + rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to create wait set"); } // (Re)build the wait set for the first time. diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 5b92957890..4e55e792b6 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -190,9 +190,10 @@ TYPED_TEST(TestExecutors, spinWhileAlreadySpinning) { executor.add_node(this->node); std::atomic_bool timer_completed = false; - auto timer = this->node->create_wall_timer(1ms, [&]() { + auto timer = this->node->create_wall_timer( + 1ms, [&]() { timer_completed.store(true); - }); + }); std::thread spinner([&]() {executor.spin();}); // Sleep for a short time to verify executor.spin() is going, and didn't throw. diff --git a/rclcpp/test/rclcpp/test_executor.cpp b/rclcpp/test/rclcpp/test_executor.cpp index b856d19b39..71503a0f86 100644 --- a/rclcpp/test/rclcpp/test_executor.cpp +++ b/rclcpp/test/rclcpp/test_executor.cpp @@ -113,7 +113,7 @@ TEST_F(TestExecutor, constructor_bad_wait_set_init) { auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_init, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( static_cast(std::make_unique()), - std::runtime_error("Failed to create wait set in Executor constructor: error not set")); + std::runtime_error("Failed to create wait set: error not set")); } TEST_F(TestExecutor, add_callback_group_twice) { From cd56124c1480a127126ad028a9e938833f0e22b1 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 12 Apr 2023 12:29:04 -0500 Subject: [PATCH 38/74] Change ready_executables signature back Signed-off-by: Michael Carroll --- .../executors/executor_entities_collection.hpp | 8 +++++--- rclcpp/src/rclcpp/executor.cpp | 13 ++++++++++--- .../executors/executor_entities_collection.cpp | 17 +++++++++++------ 3 files changed, 26 insertions(+), 12 deletions(-) diff --git a/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp b/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp index 2829140bd2..53e489209c 100644 --- a/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp +++ b/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp @@ -198,12 +198,14 @@ build_entities_collection( * * \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. + * \param[inout] queue of executables to append new ready executables to + * \return number of new ready executables */ -std::deque +size_t ready_executables( const ExecutorEntitiesCollection & collection, - rclcpp::WaitResult & wait_result); + rclcpp::WaitResult & wait_result, + std::deque & executables); } // namespace executors } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 653a73de2a..bde2fccca5 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -294,19 +294,26 @@ Executor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive) } RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); bool work_available = false; + + while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) { AnyExecutable any_exec; + + { + std::lock_guard guard(mutex_); + work_available = ready_executables_.size() > 0; + } + if (!work_available) { wait_for_work(std::chrono::milliseconds::zero()); } + if (get_next_ready_executable(any_exec)) { execute_any_executable(any_exec); - work_available = true; } else { if (!work_available || !exhaustive) { break; } - work_available = false; } } } @@ -590,7 +597,7 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) "rclcpp", "empty wait set received in wait(). This should never happen."); } - ready_executables_ = rclcpp::executors::ready_executables(current_collection_, wait_result); + rclcpp::executors::ready_executables(current_collection_, wait_result, ready_executables_); } bool diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp index 0c13b9c3a4..9a2c1b97bf 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp @@ -94,19 +94,19 @@ build_entities_collection( } } -std::deque +size_t ready_executables( const ExecutorEntitiesCollection & collection, - rclcpp::WaitResult & wait_result + rclcpp::WaitResult & wait_result, + std::deque & executables ) { - std::deque executables; - if (wait_result.kind() != rclcpp::WaitResultKind::Ready) { - return executables; + return 0; } auto rcl_wait_set = wait_result.get_wait_set().get_rcl_wait_set(); + size_t added = 0; // Cache shared pointers to groups to avoid extra work re-locking them std::maptake_data(); executables.push_back(exec); + added++; } - return executables; + return added; } } // namespace executors From 3a80b86164ac784e9f1ed645a8d0996b7995376d Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 12 Apr 2023 13:07:07 -0500 Subject: [PATCH 39/74] Don't enforce removing callback groups before nodes Signed-off-by: Michael Carroll --- .../src/rclcpp/executors/executor_entities_collector.cpp | 8 -------- 1 file changed, 8 deletions(-) diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp index ede657f08c..5a1d24c333 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp @@ -153,12 +153,7 @@ 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 lock(mutex_); bool associated = manually_added_groups_.count(group_ptr) != 0; bool add_queued = pending_manually_added_groups_.count(group_ptr) != 0; @@ -254,9 +249,6 @@ ExecutorEntitiesCollector::remove_weak_callback_group( 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); } From 6379f0cfa0940a69a54f59652116f859f1e11a16 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 12 Apr 2023 18:11:19 -0500 Subject: [PATCH 40/74] 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 41/74] 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 42/74] 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; } From fcc33e96922bf64f336568fc1d41646a4c251643 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 12 Apr 2023 20:00:48 -0500 Subject: [PATCH 43/74] Fix spin_some/spin_all implementation Signed-off-by: Michael Carroll --- rclcpp/include/rclcpp/callback_group.hpp | 1 - rclcpp/src/rclcpp/executor.cpp | 47 ++++++++++++++++-------- rclcpp/test/rclcpp/test_executor.cpp | 6 +++ 3 files changed, 38 insertions(+), 16 deletions(-) diff --git a/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp index b908d6a7d3..9de840175d 100644 --- a/rclcpp/include/rclcpp/callback_group.hpp +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -196,7 +196,6 @@ class CallbackGroup * allow a second take (eg mutual exclusion) * \return a reference to the flag */ - RCLCPP_PUBLIC std::atomic_bool & can_be_taken_from(); diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index bde2fccca5..01da6a3b0d 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include #include @@ -293,28 +294,44 @@ Executor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive) throw std::runtime_error("spin_some() called while already spinning"); } RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); - bool work_available = false; + size_t work_in_queue = 0; + bool has_waited = false; + + { + std::lock_guard lock(mutex_); + work_in_queue = ready_executables_.size(); + } + // The logic below is to guarantee that we: + // a) run all of the work in the queue before we spin the first time + // b) spin at least once + // c) run all of the work in the queue after we spin while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) { AnyExecutable any_exec; - - { - std::lock_guard guard(mutex_); - work_available = ready_executables_.size() > 0; - } - - if (!work_available) { - wait_for_work(std::chrono::milliseconds::zero()); - } - - if (get_next_ready_executable(any_exec)) { - execute_any_executable(any_exec); - } else { - if (!work_available || !exhaustive) { + if (work_in_queue > 0) { + // If there is work in the queue, then execute it + // This covers the case that there are things left in the queue from a + // previous spin. + if (get_next_ready_executable(any_exec)) { + execute_any_executable(any_exec); + } + } else if (!has_waited && !work_in_queue) { + // Once the ready queue is empty, then we need to wait at least once. + wait_for_work(std::chrono::milliseconds(0)); + has_waited = true; + } else if (has_waited && !work_in_queue) { + // Once we have emptied the ready queue, but have already waited: + if (!exhaustive) { + // In the case of spin some, then we can exit break; + } else { + // In the case of spin all, then we will allow ourselves to wait again. + has_waited = false; } } + std::lock_guard lock(mutex_); + work_in_queue = ready_executables_.size(); } } diff --git a/rclcpp/test/rclcpp/test_executor.cpp b/rclcpp/test/rclcpp/test_executor.cpp index 71503a0f86..5ee9573c3f 100644 --- a/rclcpp/test/rclcpp/test_executor.cpp +++ b/rclcpp/test/rclcpp/test_executor.cpp @@ -151,9 +151,15 @@ TEST_F(TestExecutor, remove_callback_group_null_node) { node.reset(); + + /** + * TODO(mjcarroll): Assert this when we are enforcing that nodes must be destroyed + * after their created callback groups. RCLCPP_EXPECT_THROW_EQ( dummy.remove_callback_group(cb_group, false), std::runtime_error("Node must not be deleted before its callback group(s).")); + */ + EXPECT_NO_THROW(dummy.remove_callback_group(cb_group, false)); } TEST_F(TestExecutor, remove_callback_group_failed_trigger_guard_condition) { From 64cba3b78131fe6fcbb1207ba18b4c9ea72c3eff Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Thu, 13 Apr 2023 09:56:24 -0500 Subject: [PATCH 44/74] Restore single threaded executor Signed-off-by: Michael Carroll --- .../executors/single_threaded_executor.cpp | 24 +++---------------- 1 file changed, 3 insertions(+), 21 deletions(-) diff --git a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp index 7b2adc044c..e7f311c147 100644 --- a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "rclcpp/executors/executor_entities_collection.hpp" #include "rcpputils/scope_exit.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" @@ -33,26 +32,9 @@ SingleThreadedExecutor::spin() } RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); while (rclcpp::ok(this->context_) && spinning.load()) { - wait_for_work(); - - /// Get the ready executables in a thread-safe way. - std::deque to_exec; - { - std::lock_guard guard(mutex_); - to_exec = this->ready_executables_; - this->ready_executables_.clear(); - } - - // Execute all available executables before return to wait for work - for (auto & exec : to_exec) { - // Block mutually exclusive callback group - if (exec.callback_group && - exec.callback_group->type() == CallbackGroupType::MutuallyExclusive) - { - assert(exec.callback_group->can_be_taken_from().load()); - exec.callback_group->can_be_taken_from().store(false); - } - execute_any_executable(exec); + rclcpp::AnyExecutable any_executable; + if (get_next_executable(any_executable)) { + execute_any_executable(any_executable); } } } From ad5931b129c5c279b1742577341e41184adecaed Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Mon, 17 Apr 2023 22:21:54 +0000 Subject: [PATCH 45/74] Picking ABI-incompatible executor changes Signed-off-by: Michael Carroll --- rclcpp/include/rclcpp/callback_group.hpp | 7 + rclcpp/src/rclcpp/callback_group.cpp | 10 + .../executor_entities_collection.cpp | 193 ++++++++++-------- 3 files changed, 127 insertions(+), 83 deletions(-) diff --git a/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp index 97579fcf8c..43c7daa888 100644 --- a/rclcpp/include/rclcpp/callback_group.hpp +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -180,6 +180,13 @@ class CallbackGroup return _find_ptrs_if_impl(func, waitable_ptrs_); } + /// Get the total number of entities in this callback group. + /** + * \return the number of entities in the callback group. + */ + RCLCPP_PUBLIC + size_t size() 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 753a441332..7b600f2835 100644 --- a/rclcpp/src/rclcpp/callback_group.cpp +++ b/rclcpp/src/rclcpp/callback_group.cpp @@ -56,6 +56,16 @@ CallbackGroup::type() const return type_; } +size_t +CallbackGroup::size() const +{ + return ( + subscription_ptrs_.size() + + service_ptrs_.size() + + client_ptrs_.size() + + timer_ptrs_.size() + + waitable_ptrs_.size()); +} void CallbackGroup::collect_all_ptrs( std::function sub_func, std::function service_func, diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp index 567b28014e..4fd6790916 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp @@ -94,109 +94,136 @@ build_entities_collection( } } -template -void check_ready( - EntityCollectionType & collection, - std::deque & executables, - size_t size_of_waited_entities, - typename EntityCollectionType::Key * waited_entities, - std::function fill_executable) +size_t +ready_executables( + const ExecutorEntitiesCollection & collection, + rclcpp::WaitResult & wait_result, + std::deque & executables +) { - for (size_t ii = 0; ii < size_of_waited_entities; ++ii) { - if (!waited_entities[ii]) {continue;} - auto entity_iter = collection.find(waited_entities[ii]); - if (entity_iter != collection.end()) { + size_t added = 0; + if (wait_result.kind() != rclcpp::WaitResultKind::Ready) { + return added; + } + auto rcl_wait_set = wait_result.get_wait_set().get_rcl_wait_set(); + + // Cache shared pointers to groups to avoid extra work re-locking them + std::map> group_map; + + auto group_cache = [&group_map](const rclcpp::CallbackGroup::WeakPtr & weak_cbg_ptr) + { + if (group_map.count(weak_cbg_ptr) == 0) { + group_map.insert({weak_cbg_ptr, weak_cbg_ptr.lock()}); + } + return group_map.find(weak_cbg_ptr)->second; + }; + + for (size_t ii = 0; ii < rcl_wait_set.size_of_timers; ++ii) { + if (nullptr == rcl_wait_set.timers[ii]) {continue;} + auto entity_iter = collection.timers.find(rcl_wait_set.timers[ii]); + if (entity_iter != collection.timers.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()) { + auto group_info = group_cache(entity_iter->second.callback_group); + if (group_info && !group_info->can_be_taken_from().load()) { + continue; + } + if (!entity->call()) { continue; } rclcpp::AnyExecutable exec; + exec.timer = entity; + exec.callback_group = group_info; + executables.push_back(exec); + added++; + } + } - exec.callback_group = callback_group; - if (fill_executable(exec, entity)) { - executables.push_back(exec); + for (size_t ii = 0; ii < rcl_wait_set.size_of_subscriptions; ++ii) { + if (nullptr == rcl_wait_set.subscriptions[ii]) {continue;} + auto entity_iter = collection.subscriptions.find(rcl_wait_set.subscriptions[ii]); + if (entity_iter != collection.subscriptions.end()) { + auto entity = entity_iter->second.entity.lock(); + if (!entity) { + continue; } + auto group_info = group_cache(entity_iter->second.callback_group); + if (group_info && !group_info->can_be_taken_from().load()) { + continue; + } + rclcpp::AnyExecutable exec; + exec.subscription = entity; + exec.callback_group = group_info; + executables.push_back(exec); + added++; } } -} -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(); - check_ready( - collection.timers, - ret, - rcl_wait_set.size_of_timers, - rcl_wait_set.timers, - [](rclcpp::AnyExecutable & exec, auto timer) { - if (!timer->call()) { - return false; + for (size_t ii = 0; ii < rcl_wait_set.size_of_services; ++ii) { + if (nullptr == rcl_wait_set.services[ii]) {continue;} + auto entity_iter = collection.services.find(rcl_wait_set.services[ii]); + if (entity_iter != collection.services.end()) { + auto entity = entity_iter->second.entity.lock(); + if (!entity) { + continue; } - exec.timer = timer; - return true; - }); - - check_ready( - collection.subscriptions, - ret, - rcl_wait_set.size_of_subscriptions, - rcl_wait_set.subscriptions, - [](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, - [](rclcpp::AnyExecutable & exec, auto service) { - exec.service = service; - return true; - }); - - check_ready( - collection.clients, - ret, - rcl_wait_set.size_of_clients, - rcl_wait_set.clients, - [](rclcpp::AnyExecutable & exec, auto client) { - exec.client = client; - return true; - }); - - for (auto & [handle, entry] : collection.waitables) { - 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()) { + auto group_info = group_cache(entity_iter->second.callback_group); + if (group_info && !group_info->can_be_taken_from().load()) { continue; } + rclcpp::AnyExecutable exec; + exec.service = entity; + exec.callback_group = group_info; + executables.push_back(exec); + added++; + } + } + for (size_t ii = 0; ii < rcl_wait_set.size_of_clients; ++ii) { + if (nullptr == rcl_wait_set.clients[ii]) {continue;} + auto entity_iter = collection.clients.find(rcl_wait_set.clients[ii]); + if (entity_iter != collection.clients.end()) { + auto entity = entity_iter->second.entity.lock(); + if (!entity) { + continue; + } + auto group_info = group_cache(entity_iter->second.callback_group); + if (group_info && !group_info->can_be_taken_from().load()) { + continue; + } rclcpp::AnyExecutable exec; - exec.waitable = waitable; - exec.callback_group = group; - ret.push_back(exec); + exec.client = entity; + exec.callback_group = group_info; + executables.push_back(exec); + added++; + } + } + + for (auto & [handle, entry] : collection.waitables) { + auto waitable = entry.entity.lock(); + if (!waitable) { + continue; + } + if (!waitable->is_ready(&rcl_wait_set)) { + continue; } + auto group_info = group_cache(entry.callback_group); + if (group_info && !group_info->can_be_taken_from().load()) { + continue; + } + rclcpp::AnyExecutable exec; + exec.waitable = waitable; + exec.callback_group = group_info; + exec.data = waitable->take_data(); + executables.push_back(exec); + added++; } - return ret; + + return added; } } // namespace executors From acfc0e29fbf30dc43f9ec82e2886d4749baf66da Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Mon, 17 Apr 2023 22:54:51 +0000 Subject: [PATCH 46/74] Add PIMPL Signed-off-by: Michael Carroll --- rclcpp/include/rclcpp/executor.hpp | 4 ++++ .../executors/executor_entities_collection.hpp | 9 +++++---- rclcpp/src/rclcpp/callback_group.cpp | 4 ++-- rclcpp/src/rclcpp/executor.cpp | 5 ++++- .../executors/executor_entities_collection.cpp | 16 ++++++++-------- 5 files changed, 23 insertions(+), 15 deletions(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 3e654faa54..2d5ca2149a 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -51,6 +51,7 @@ typedef std::map impl_; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp b/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp index 98a92ccdd8..166bb99119 100644 --- a/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp +++ b/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp @@ -198,14 +198,15 @@ build_entities_collection( * * \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. + * \param[inout] queue of executables to append new ready executables to + * \return number of new ready executables */ -std::deque +size_t ready_executables( const ExecutorEntitiesCollection & collection, - rclcpp::WaitResult & wait_result + rclcpp::WaitResult & wait_result, + std::deque & executables ); - } // namespace executors } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/callback_group.cpp b/rclcpp/src/rclcpp/callback_group.cpp index 7b600f2835..77f6c87bd9 100644 --- a/rclcpp/src/rclcpp/callback_group.cpp +++ b/rclcpp/src/rclcpp/callback_group.cpp @@ -59,12 +59,12 @@ CallbackGroup::type() const size_t CallbackGroup::size() const { - return ( + return subscription_ptrs_.size() + service_ptrs_.size() + client_ptrs_.size() + timer_ptrs_.size() + - waitable_ptrs_.size()); + waitable_ptrs_.size(); } void CallbackGroup::collect_all_ptrs( std::function sub_func, diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index ccd1b61016..a7a2b8278b 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -41,11 +41,14 @@ using namespace std::chrono_literals; using rclcpp::exceptions::throw_from_rcl_error; using rclcpp::Executor; +class rclcpp::ExecutorImplementation {}; + Executor::Executor(const rclcpp::ExecutorOptions & options) : spinning(false), interrupt_guard_condition_(std::make_shared(options.context)), shutdown_guard_condition_(std::make_shared(options.context)), - memory_strategy_(options.memory_strategy) + memory_strategy_(options.memory_strategy), + impl_(std::make_unique()) { // Store the context for later use. context_ = options.context; diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp index 4fd6790916..88a878824a 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp @@ -20,12 +20,13 @@ 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() @@ -38,7 +39,6 @@ void ExecutorEntitiesCollection::clear() waitables.clear(); } - void build_entities_collection( const std::vector & callback_groups, @@ -94,7 +94,7 @@ build_entities_collection( } } -size_t +size_t ready_executables( const ExecutorEntitiesCollection & collection, rclcpp::WaitResult & wait_result, From c6612eced4e6c3907ed54502945812fc730e577e Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 18 Apr 2023 02:55:38 +0000 Subject: [PATCH 47/74] Additional waitset prune Signed-off-by: Michael Carroll --- rclcpp/src/rclcpp/executor.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 8ff513462a..af38312d1d 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -627,6 +627,9 @@ Executor::collect_entities() }, [this](auto waitable) {wait_set_.remove_waitable(waitable);}); + // In the case that an entity already has an expired weak pointer + // before being removed from the waitset, additionally prune the waitset. + this->wait_set_.prune_deleted_entities(); this->entities_need_rebuild_.store(false); } From 670843a774ada7c3422086171a3dc6303deac250 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 25 Apr 2023 16:19:39 +0000 Subject: [PATCH 48/74] Fix bad merge Signed-off-by: Michael Carroll --- rclcpp/src/rclcpp/executor.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 76fd979397..7465aee303 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -42,7 +42,6 @@ using namespace std::chrono_literals; using rclcpp::Executor; -class rclcpp::ExecutorImplementation {}; /// Mask to indicate to the waitset to only add the subscription. /// The events and intraprocess waitable are already added via the callback group. From e364d892683ac9ebaf613c94815b443d51e75fb5 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Thu, 27 Apr 2023 22:40:39 +0000 Subject: [PATCH 49/74] Expand test timeout Signed-off-by: Michael Carroll --- rclcpp/test/rclcpp/test_timers_manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/test/rclcpp/test_timers_manager.cpp b/rclcpp/test/rclcpp/test_timers_manager.cpp index 635ec322c0..750f86a8e5 100644 --- a/rclcpp/test/rclcpp/test_timers_manager.cpp +++ b/rclcpp/test/rclcpp/test_timers_manager.cpp @@ -296,7 +296,7 @@ TEST_F(TestTimersManager, timers_thread) // Run timers thread for a while timers_manager->start(); - std::this_thread::sleep_for(5ms); + std::this_thread::sleep_for(10ms); timers_manager->stop(); EXPECT_LT(1u, t1_runs); From d63d677e41e80afe687d7fc15a10c9f61fe82d98 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 2 May 2023 13:41:36 +0000 Subject: [PATCH 50/74] Introduce method to clear expired entities from a collection Signed-off-by: Michael Carroll --- .../executor_entities_collection.hpp | 6 +++++ .../executor_entities_collection.cpp | 24 +++++++++++++++++++ 2 files changed, 30 insertions(+) diff --git a/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp b/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp index 166bb99119..517894a2a2 100644 --- a/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp +++ b/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp @@ -178,6 +178,12 @@ struct ExecutorEntitiesCollection /// Clear the entities collection void clear(); + + /// Remove entities that have expired weak ownership + /** + * \return The total number of removed entities + */ + size_t remove_expired_entities(); }; /// Build an entities collection from callback groups diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp index 9f57059372..52a643cb81 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp @@ -39,6 +39,30 @@ void ExecutorEntitiesCollection::clear() waitables.clear(); } +size_t ExecutorEntitiesCollection::remove_expired_entities() +{ + auto remove_entities = [](auto & collection) -> size_t { + size_t removed = 0; + for (auto it = collection.begin(); it != collection.end(); ) { + if (it->second.entity.expired()) { + ++removed; + it = collection.erase(it); + } else { + ++it; + } + } + return removed; + }; + + return + remove_entities(subscriptions) + + remove_entities(timers) + + remove_entities(guard_conditions) + + remove_entities(clients) + + remove_entities(services) + + remove_entities(waitables); +} + void build_entities_collection( const std::vector & callback_groups, From 4fe9f68e8b6eacb219684e95bbab381780084b40 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 29 Nov 2023 15:16:52 +0000 Subject: [PATCH 51/74] Make sure to call remove_expired_entities(). Signed-off-by: Chris Lalancette --- rclcpp/src/rclcpp/executor.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 15dc75a76a..2bd7a6dc97 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -611,6 +611,10 @@ Executor::collect_entities() collection.waitables.insert({notify_waitable.get(), {notify_waitable, {}}}); } + // We must remove expired entities here, so that we don't continue to use older entities. + // See https://github.com/ros2/rclcpp/issues/2180 for more information. + current_collection_.remove_expired_entities(); + // Update each of the groups of entities in the current collection, adding or removing // from the wait set as necessary. current_collection_.timers.update( From eea79ada55387e944228b7c685dbeaf692b121c5 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Thu, 30 Nov 2023 16:41:04 +0000 Subject: [PATCH 52/74] Prune queued work when callback group is removed Signed-off-by: Michael Carroll --- rclcpp/src/rclcpp/executor.cpp | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 2bd7a6dc97..bd3940e13d 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -657,6 +657,26 @@ Executor::collect_entities() // before being removed from the waitset, additionally prune the waitset. this->wait_set_.prune_deleted_entities(); this->entities_need_rebuild_.store(false); + + if (!this->ready_executables_.empty()) + { + std::unordered_set groups; + for (const auto &weak_group : callback_groups) + { + auto group = weak_group.lock(); + if (group) + groups.insert(group); + } + + this->ready_executables_.erase( + std::remove_if( + this->ready_executables_.begin(), + this->ready_executables_.end(), + [groups](auto exec){ + return groups.count(exec.callback_group) == 0; + }), + this->ready_executables_.end()); + } } void From 37550f8cc1ec74b85c4e08bdfa43dbfdde03629a Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Thu, 30 Nov 2023 16:44:28 +0000 Subject: [PATCH 53/74] Prune subscriptions from dynamic storage Signed-off-by: Michael Carroll --- rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp index 7b573e6501..4a33b5468f 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp @@ -386,6 +386,7 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo return weak_ptr.expired(); }; // remove guard conditions which have been deleted + subscriptions_.erase(std::remove_if(subscriptions_.begin(), subscriptions_.end(), p), subscriptions_.end()); guard_conditions_.erase( std::remove_if(guard_conditions_.begin(), guard_conditions_.end(), p), guard_conditions_.end()); From 760c8feee5e265a7a47160fded86006b27b3033d Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 30 Nov 2023 17:48:50 +0000 Subject: [PATCH 54/74] Styles fixes. Signed-off-by: Chris Lalancette --- .../rclcpp/wait_set_policies/dynamic_storage.hpp | 3 ++- rclcpp/src/rclcpp/executor.cpp | 11 +++++------ 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp index 4a33b5468f..79df17cfec 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp @@ -386,7 +386,8 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo return weak_ptr.expired(); }; // remove guard conditions which have been deleted - subscriptions_.erase(std::remove_if(subscriptions_.begin(), subscriptions_.end(), p), subscriptions_.end()); + subscriptions_.erase( + std::remove_if(subscriptions_.begin(), subscriptions_.end(), p), subscriptions_.end()); guard_conditions_.erase( std::remove_if(guard_conditions_.begin(), guard_conditions_.end(), p), guard_conditions_.end()); diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index bd3940e13d..bb12252097 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -658,21 +658,20 @@ Executor::collect_entities() this->wait_set_.prune_deleted_entities(); this->entities_need_rebuild_.store(false); - if (!this->ready_executables_.empty()) - { + if (!this->ready_executables_.empty()) { std::unordered_set groups; - for (const auto &weak_group : callback_groups) - { + for (const auto & weak_group : callback_groups) { auto group = weak_group.lock(); - if (group) + if (group) { groups.insert(group); + } } this->ready_executables_.erase( std::remove_if( this->ready_executables_.begin(), this->ready_executables_.end(), - [groups](auto exec){ + [groups](auto exec) { return groups.count(exec.callback_group) == 0; }), this->ready_executables_.end()); From af43e2af8ec5e91a183391490cd22d6a98751a92 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 5 Dec 2023 20:24:11 +0000 Subject: [PATCH 55/74] Re-trigger guard conditions Signed-off-by: Michael Carroll --- rclcpp/include/rclcpp/any_executable.hpp | 2 +- .../subscription_intra_process_buffer.hpp | 12 ++++++++++++ rclcpp/src/rclcpp/any_executable.cpp | 3 +-- rclcpp/src/rclcpp/executor.cpp | 3 ++- .../executors/executor_entities_collection.cpp | 2 +- 5 files changed, 17 insertions(+), 5 deletions(-) diff --git a/rclcpp/include/rclcpp/any_executable.hpp b/rclcpp/include/rclcpp/any_executable.hpp index 5d4064f452..acb84a7a51 100644 --- a/rclcpp/include/rclcpp/any_executable.hpp +++ b/rclcpp/include/rclcpp/any_executable.hpp @@ -47,7 +47,7 @@ struct AnyExecutable // These are used to keep the scope on the containing items rclcpp::CallbackGroup::SharedPtr callback_group; rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base; - std::shared_ptr data; + std::function()> take_data; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp index f564cbb047..295cefdde1 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp @@ -30,6 +30,7 @@ #include "rclcpp/experimental/ros_message_intra_process_buffer.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/type_support_decl.hpp" +#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp" #include "tracetools/tracetools.h" @@ -99,6 +100,17 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff static_cast(this)); } + void + add_to_wait_set(rcl_wait_set_t * wait_set) override + { + fprintf(stderr, "add_to_wait_set: %s %d\n", this->get_topic_name(), this->buffer_->has_data()); + if (this->buffer_->has_data()) + { + this->trigger_guard_condition(); + } + detail::add_guard_condition_to_rcl_wait_set(*wait_set, this->gc_); + } + bool is_ready(rcl_wait_set_t * wait_set) override { diff --git a/rclcpp/src/rclcpp/any_executable.cpp b/rclcpp/src/rclcpp/any_executable.cpp index da020942bc..e804873536 100644 --- a/rclcpp/src/rclcpp/any_executable.cpp +++ b/rclcpp/src/rclcpp/any_executable.cpp @@ -24,8 +24,7 @@ AnyExecutable::AnyExecutable() client(nullptr), waitable(nullptr), callback_group(nullptr), - node_base(nullptr), - data(nullptr) + node_base(nullptr) {} AnyExecutable::~AnyExecutable() diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index bd3940e13d..a560c90bee 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -412,7 +412,8 @@ Executor::execute_any_executable(AnyExecutable & any_exec) execute_client(any_exec.client); } if (any_exec.waitable) { - any_exec.waitable->execute(any_exec.data); + auto data = any_exec.take_data(); + any_exec.waitable->execute(data); } // Reset the callback_group, regardless of type diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp index 52a643cb81..6ab8dcb1a0 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp @@ -242,7 +242,7 @@ ready_executables( rclcpp::AnyExecutable exec; exec.waitable = waitable; exec.callback_group = group_info; - exec.data = waitable->take_data(); + exec.take_data = [waitable](){return waitable->take_data();}; executables.push_back(exec); added++; } From 4ce164519596a126056a84025338d41e298b85c0 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 5 Dec 2023 20:28:58 +0000 Subject: [PATCH 56/74] Condense to just use watiable.take_data Signed-off-by: Michael Carroll --- rclcpp/include/rclcpp/any_executable.hpp | 1 - .../rclcpp/experimental/subscription_intra_process_buffer.hpp | 1 - rclcpp/src/rclcpp/executor.cpp | 2 +- rclcpp/src/rclcpp/executors/executor_entities_collection.cpp | 3 +-- 4 files changed, 2 insertions(+), 5 deletions(-) diff --git a/rclcpp/include/rclcpp/any_executable.hpp b/rclcpp/include/rclcpp/any_executable.hpp index acb84a7a51..244293ac83 100644 --- a/rclcpp/include/rclcpp/any_executable.hpp +++ b/rclcpp/include/rclcpp/any_executable.hpp @@ -47,7 +47,6 @@ struct AnyExecutable // These are used to keep the scope on the containing items rclcpp::CallbackGroup::SharedPtr callback_group; rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base; - std::function()> take_data; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp index 295cefdde1..65c0c7fdb8 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp @@ -103,7 +103,6 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff void add_to_wait_set(rcl_wait_set_t * wait_set) override { - fprintf(stderr, "add_to_wait_set: %s %d\n", this->get_topic_name(), this->buffer_->has_data()); if (this->buffer_->has_data()) { this->trigger_guard_condition(); diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 411108f9f1..56c6de03a6 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -412,7 +412,7 @@ Executor::execute_any_executable(AnyExecutable & any_exec) execute_client(any_exec.client); } if (any_exec.waitable) { - auto data = any_exec.take_data(); + auto data = any_exec.waitable->take_data(); any_exec.waitable->execute(data); } diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp index 6ab8dcb1a0..b6e030d340 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp @@ -227,7 +227,7 @@ ready_executables( } } - for (auto & [handle, entry] : collection.waitables) { + for (const auto & [handle, entry] : collection.waitables) { auto waitable = entry.entity.lock(); if (!waitable) { continue; @@ -242,7 +242,6 @@ ready_executables( rclcpp::AnyExecutable exec; exec.waitable = waitable; exec.callback_group = group_info; - exec.take_data = [waitable](){return waitable->take_data();}; executables.push_back(exec); added++; } From 662f44046faa823b4df927cba2f7c9821c2242a4 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 6 Dec 2023 17:07:08 +0000 Subject: [PATCH 57/74] Lint Signed-off-by: Michael Carroll --- .../rclcpp/experimental/subscription_intra_process_buffer.hpp | 3 +-- .../rclcpp/wait_set_policies/detail/storage_policy_common.hpp | 1 - 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp index 65c0c7fdb8..abe5142581 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp @@ -103,8 +103,7 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff void add_to_wait_set(rcl_wait_set_t * wait_set) override { - if (this->buffer_->has_data()) - { + if (this->buffer_->has_data()) { this->trigger_guard_condition(); } detail::add_guard_condition_to_rcl_wait_set(*wait_set, this->gc_); diff --git a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp index c4d69613e2..44331e7687 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp @@ -340,7 +340,6 @@ class StoragePolicyCommon } } - // Add services. for (const auto & service : services) { if (!service) { From 02d9cd65854916852819ac980d450533f6f3f20a Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Fri, 8 Dec 2023 15:46:23 +0000 Subject: [PATCH 58/74] Address reviewer comments (nits) Signed-off-by: Michael Carroll --- rclcpp/include/rclcpp/executor.hpp | 4 ++-- .../detail/storage_policy_common.hpp | 14 +++++++------- rclcpp/src/rclcpp/any_executable.cpp | 1 - rclcpp/src/rclcpp/executor.cpp | 4 +--- .../rclcpp/executors/executor_notify_waitable.cpp | 3 +-- 5 files changed, 11 insertions(+), 15 deletions(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 402e7aa874..b273bfb361 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -569,7 +569,7 @@ class Executor * This waitable contains the interrupt and shutdown guard condition, as well * as the guard condition associated with each node and callback group. * By default, if any change is detected in the monitored entities, the notify - * waitable will awake the executor and rebuild the collections. + * waitable will awake the executor and rebuild the collections. */ std::shared_ptr notify_waitable_; @@ -578,7 +578,7 @@ class Executor /// Collector used to associate executable entities from nodes and guard conditions rclcpp::executors::ExecutorEntitiesCollector collector_; - /// Waitset to be waited on. + /// WaitSet to be waited on. rclcpp::WaitSet wait_set_ RCPPUTILS_TSA_GUARDED_BY(mutex_); /// Hold the current state of the collection being waited on by the waitset diff --git a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp index 44331e7687..bd6a5c1281 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp @@ -203,13 +203,13 @@ class StoragePolicyCommon needs_pruning_ = true; continue; } - const auto & waitable = waitable_entry.waitable; - subscriptions_from_waitables += waitable->get_number_of_ready_subscriptions(); - guard_conditions_from_waitables += waitable->get_number_of_ready_guard_conditions(); - timers_from_waitables += waitable->get_number_of_ready_timers(); - clients_from_waitables += waitable->get_number_of_ready_clients(); - services_from_waitables += waitable->get_number_of_ready_services(); - events_from_waitables += waitable->get_number_of_ready_events(); + auto & waitable = *waitable_entry.waitable; + subscriptions_from_waitables += waitable.get_number_of_ready_subscriptions(); + guard_conditions_from_waitables += waitable.get_number_of_ready_guard_conditions(); + timers_from_waitables += waitable.get_number_of_ready_timers(); + clients_from_waitables += waitable.get_number_of_ready_clients(); + services_from_waitables += waitable.get_number_of_ready_services(); + events_from_waitables += waitable.get_number_of_ready_events(); } rcl_ret_t ret = rcl_wait_set_resize( &rcl_wait_set_, diff --git a/rclcpp/src/rclcpp/any_executable.cpp b/rclcpp/src/rclcpp/any_executable.cpp index e804873536..2449cbe1f7 100644 --- a/rclcpp/src/rclcpp/any_executable.cpp +++ b/rclcpp/src/rclcpp/any_executable.cpp @@ -16,7 +16,6 @@ using rclcpp::AnyExecutable; -RCLCPP_PUBLIC AnyExecutable::AnyExecutable() : subscription(nullptr), timer(nullptr), diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 56c6de03a6..cedff33819 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -649,9 +649,7 @@ Executor::collect_entities() current_collection_.waitables.update( collection.waitables, - [this](auto waitable) { - wait_set_.add_waitable(waitable); - }, + [this](auto waitable) {wait_set_.add_waitable(waitable);}, [this](auto waitable) {wait_set_.remove_waitable(waitable);}); // In the case that an entity already has an expired weak pointer diff --git a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp index f759903563..8517eb7cd0 100644 --- a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp +++ b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp @@ -52,8 +52,7 @@ ExecutorNotifyWaitable::add_to_wait_set(rcl_wait_set_t * wait_set) rcl_guard_condition_t * cond = &guard_condition->get_rcl_guard_condition(); - rcl_ret_t ret = rcl_wait_set_add_guard_condition( - wait_set, cond, NULL); + rcl_ret_t ret = rcl_wait_set_add_guard_condition(wait_set, cond, NULL); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error( From 8ed094e867e4540bcb6271f0db0df07605c084e3 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Fri, 8 Dec 2023 17:53:57 +0000 Subject: [PATCH 59/74] Lock mutex when copying Signed-off-by: Michael Carroll --- .../include/rclcpp/executors/executor_notify_waitable.hpp | 4 ++-- rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp | 8 +++++--- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp b/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp index eee8e59793..2b43fecca1 100644 --- a/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp +++ b/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp @@ -48,11 +48,11 @@ class ExecutorNotifyWaitable : public rclcpp::Waitable ~ExecutorNotifyWaitable() override = default; RCLCPP_PUBLIC - ExecutorNotifyWaitable(const ExecutorNotifyWaitable & other); + ExecutorNotifyWaitable(ExecutorNotifyWaitable & other); RCLCPP_PUBLIC - ExecutorNotifyWaitable & operator=(const ExecutorNotifyWaitable & other); + ExecutorNotifyWaitable & operator=(ExecutorNotifyWaitable & other); /// Add conditions to the wait set /** diff --git a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp index 8517eb7cd0..057aee9672 100644 --- a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp +++ b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp @@ -27,15 +27,17 @@ ExecutorNotifyWaitable::ExecutorNotifyWaitable(std::function on_exec { } -ExecutorNotifyWaitable::ExecutorNotifyWaitable(const ExecutorNotifyWaitable & other) -: ExecutorNotifyWaitable(other.execute_callback_) +ExecutorNotifyWaitable::ExecutorNotifyWaitable(ExecutorNotifyWaitable & other) { + std::lock_guard lock(other.guard_condition_mutex_); + this->execute_callback_ = other.execute_callback_; this->notify_guard_conditions_ = other.notify_guard_conditions_; } -ExecutorNotifyWaitable & ExecutorNotifyWaitable::operator=(const ExecutorNotifyWaitable & other) +ExecutorNotifyWaitable & ExecutorNotifyWaitable::operator=(ExecutorNotifyWaitable & other) { if (this != &other) { + std::lock_guard lock(other.guard_condition_mutex_); this->execute_callback_ = other.execute_callback_; this->notify_guard_conditions_ = other.notify_guard_conditions_; } From 1376bd78c5691dc6650f8b010ab4df2587a86bae Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Thu, 11 Jan 2024 23:25:30 +0000 Subject: [PATCH 60/74] Refactors to static single threaded based on reviewers Signed-off-by: Michael Carroll --- .../static_single_threaded_executor.hpp | 6 +- .../static_single_threaded_executor.cpp | 177 ++++++++---------- 2 files changed, 80 insertions(+), 103 deletions(-) diff --git a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp index 8d01496d30..6f22909caf 100644 --- a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp @@ -113,21 +113,21 @@ class StaticSingleThreadedExecutor : public rclcpp::Executor * @param spin_once if true executes only the first ready executable. * @return true if any executable was ready. */ - RCLCPP_PUBLIC bool execute_ready_executables( const rclcpp::executors::ExecutorEntitiesCollection & collection, rclcpp::WaitResult & wait_result, bool spin_once); - RCLCPP_PUBLIC void spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive); - RCLCPP_PUBLIC void spin_once_impl(std::chrono::nanoseconds timeout) override; + std::optional> + collect_and_wait(std::chrono::nanoseconds timeout); + private: RCLCPP_DISABLE_COPY(StaticSingleThreadedExecutor) }; diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp index c5930fac12..50f79d7952 100644 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp @@ -65,7 +65,6 @@ StaticSingleThreadedExecutor::spin_some(std::chrono::nanoseconds max_duration) if (std::chrono::nanoseconds(0) == max_duration) { max_duration = std::chrono::nanoseconds::max(); } - return this->spin_some_impl(max_duration, false); } @@ -83,41 +82,28 @@ StaticSingleThreadedExecutor::spin_some_impl(std::chrono::nanoseconds max_durati { auto start = std::chrono::steady_clock::now(); auto max_duration_not_elapsed = [max_duration, start]() { - if (std::chrono::nanoseconds(0) == max_duration) { - // told to spin forever if need be - return true; - } else if (std::chrono::steady_clock::now() - start < max_duration) { - // told to spin only for some maximum amount of time - return true; - } - // spun too long - return false; - }; + const auto spin_forever = std::chrono::nanoseconds(0) == max_duration; + const auto cur_duration = std::chrono::steady_clock::now() - start; + return spin_forever || (cur_duration < max_duration); + }; if (spinning.exchange(true)) { throw std::runtime_error("spin_some() called while already spinning"); } - RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); + RCPPUTILS_SCOPE_EXIT(this->spinning.store(false);); while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) { // Get executables that are ready now std::lock_guard guard(mutex_); - if (current_collection_.empty() || this->entities_need_rebuild_.load()) { - this->collect_entities(); - } - - auto wait_result = wait_set_.wait(std::chrono::nanoseconds(0)); - if (wait_result.kind() == WaitResultKind::Empty) { - RCUTILS_LOG_WARN_NAMED( - "rclcpp", - "empty wait set received in wait(). This should never happen."); - continue; - } - // Execute ready executables - bool work_available = execute_ready_executables(current_collection_, wait_result, false); - if (!work_available || !exhaustive) { - break; + auto wait_result = this->collect_and_wait(std::chrono::nanoseconds(0)); + if (wait_result.has_value()) + { + // Execute ready executables + bool work_available = this->execute_ready_executables(current_collection_, wait_result.value(), false); + if (!work_available || !exhaustive) { + break; + } } } } @@ -127,21 +113,28 @@ StaticSingleThreadedExecutor::spin_once_impl(std::chrono::nanoseconds timeout) { if (rclcpp::ok(context_) && spinning.load()) { std::lock_guard guard(mutex_); - if (current_collection_.empty() || this->entities_need_rebuild_.load()) { - this->collect_entities(); - } - - auto wait_result = wait_set_.wait(std::chrono::nanoseconds(timeout)); - if (wait_result.kind() == WaitResultKind::Empty) { - RCUTILS_LOG_WARN_NAMED( - "rclcpp", - "empty wait set received in wait(). This should never happen."); - return; + auto wait_result = this->collect_and_wait(timeout); + if (wait_result.has_value()) + { + this->execute_ready_executables(current_collection_, wait_result.value(), true); } + } +} - // Execute ready executables - execute_ready_executables(current_collection_, wait_result, true); +std::optional> +StaticSingleThreadedExecutor::collect_and_wait(std::chrono::nanoseconds timeout) +{ + if (current_collection_.empty() || this->entities_need_rebuild_.load()) { + this->collect_entities(); + } + auto wait_result = wait_set_.wait(std::chrono::nanoseconds(timeout)); + if (wait_result.kind() == WaitResultKind::Empty) { + RCUTILS_LOG_WARN_NAMED( + "rclcpp", + "empty wait set received in wait(). This should never happen."); + return {}; } + return wait_result; } // This preserves the "scheduling semantics" of the StaticSingleThreadedExecutor @@ -152,6 +145,7 @@ bool StaticSingleThreadedExecutor::execute_ready_executables( bool spin_once) { bool any_ready_executable = false; + bool executable_run = false; if (wait_result.kind() != rclcpp::WaitResultKind::Ready) { return any_ready_executable; @@ -159,72 +153,55 @@ bool StaticSingleThreadedExecutor::execute_ready_executables( 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 (nullptr == rcl_wait_set.timers[ii]) {continue;} - auto entity_iter = collection.timers.find(rcl_wait_set.timers[ii]); - if (entity_iter != collection.timers.end()) { - auto entity = entity_iter->second.entity.lock(); - if (!entity) { - continue; - } - if (!entity->call()) { - continue; + auto iterate_collection = [spin_once, &any_ready_executable](auto entities, auto entities_size, auto collection, auto entity_callback){ + for (size_t ii = 0; ii < entities_size; ++ii) { + if (nullptr == entities[ii]) {continue;} + auto entity_iter = collection.find(entities[ii]); + if (entity_iter != collection.end()) { + auto entity = entity_iter->second.entity.lock(); + if (!entity) { + continue; + } + if (!entity_callback(entity)) + continue; + any_ready_executable = true; + if (spin_once) { + return; + } } - execute_timer(entity); - if (spin_once) { - return true; - } - any_ready_executable = true; } - } + }; - for (size_t ii = 0; ii < rcl_wait_set.size_of_subscriptions; ++ii) { - if (nullptr == rcl_wait_set.subscriptions[ii]) {continue;} - auto entity_iter = collection.subscriptions.find(rcl_wait_set.subscriptions[ii]); - if (entity_iter != collection.subscriptions.end()) { - auto entity = entity_iter->second.entity.lock(); - if (!entity) { - continue; - } - execute_subscription(entity); - if (spin_once) { - return true; - } - any_ready_executable = true; - } - } + iterate_collection(rcl_wait_set.timers, rcl_wait_set.size_of_timers, collection.timers, + [](auto timer){ + if (!timer->call()) + return false; + execute_timer(timer); + return true; + }); + if (spin_once && any_ready_executable) { return true; } - for (size_t ii = 0; ii < rcl_wait_set.size_of_services; ++ii) { - if (nullptr == rcl_wait_set.services[ii]) {continue;} - auto entity_iter = collection.services.find(rcl_wait_set.services[ii]); - if (entity_iter != collection.services.end()) { - auto entity = entity_iter->second.entity.lock(); - if (!entity) { - continue; - } - execute_service(entity); - if (spin_once) { - return true; - } - any_ready_executable = true; - } - } + iterate_collection(rcl_wait_set.subscriptions, rcl_wait_set.size_of_subscriptions, collection.subscriptions, + [](auto subscription){ + execute_subscription(subscription); + return true; + }); + if (spin_once && any_ready_executable) { return true; } + + iterate_collection(rcl_wait_set.services, rcl_wait_set.size_of_services, collection.services, + [](auto service){ + execute_service(service); + return true; + }); + if (spin_once && any_ready_executable) { return true; } + + iterate_collection(rcl_wait_set.clients, rcl_wait_set.size_of_clients, collection.clients, + [](auto client){ + execute_client(client); + return true; + }); + if (spin_once && any_ready_executable) { return true; } - for (size_t ii = 0; ii < rcl_wait_set.size_of_clients; ++ii) { - if (nullptr == rcl_wait_set.clients[ii]) {continue;} - auto entity_iter = collection.clients.find(rcl_wait_set.clients[ii]); - if (entity_iter != collection.clients.end()) { - auto entity = entity_iter->second.entity.lock(); - if (!entity) { - continue; - } - execute_client(entity); - if (spin_once) { - return true; - } - any_ready_executable = true; - } - } for (auto & [handle, entry] : collection.waitables) { auto waitable = entry.entity.lock(); From 6208333110fecb90a9633016095aa9250314be8a Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Thu, 11 Jan 2024 23:36:04 +0000 Subject: [PATCH 61/74] More small refactoring Signed-off-by: Michael Carroll --- .../static_single_threaded_executor.cpp | 86 ++++++++++--------- 1 file changed, 45 insertions(+), 41 deletions(-) diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp index 50f79d7952..083aa8a259 100644 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp @@ -145,65 +145,69 @@ bool StaticSingleThreadedExecutor::execute_ready_executables( bool spin_once) { bool any_ready_executable = false; - bool executable_run = false; - if (wait_result.kind() != rclcpp::WaitResultKind::Ready) { return any_ready_executable; } auto rcl_wait_set = wait_result.get_wait_set().get_rcl_wait_set(); - auto iterate_collection = [spin_once, &any_ready_executable](auto entities, auto entities_size, auto collection, auto entity_callback){ - for (size_t ii = 0; ii < entities_size; ++ii) { - if (nullptr == entities[ii]) {continue;} - auto entity_iter = collection.find(entities[ii]); - if (entity_iter != collection.end()) { - auto entity = entity_iter->second.entity.lock(); - if (!entity) { - continue; - } - if (!entity_callback(entity)) - continue; - any_ready_executable = true; - if (spin_once) { - return; + // Helper function to iterate over the waitset result and current collection to check for readiness + // + // In the case that spin_once is set, this function will exit after the first entity in the collection + // is executed, regardless if additional work is available and ready. + // + // \param[in] entities - rcl_wait_set handles to iterate over (eg rcl_wait_set.timers) + // \param[in] size_of_entities - size of rcl_wait_set handles (eg rcl_wait_set.size_of_timers) + // \param[in] collection - collection of associated weak_ptrs to be waited on + // \param[in] entity_callback - function of the form bool(std:;shared_ptr), + // returns true if the entity was executed, + // false otherwise + auto iterate_collection = [spin_once, &any_ready_executable]( + auto entities, auto size_of_entities, auto collection, auto entity_callback){ + for (size_t ii = 0; ii < size_of_entities; ++ii) { + if (nullptr == entities[ii]) {continue;} + auto entity_iter = collection.find(entities[ii]); + if (entity_iter != collection.end()) { + auto entity = entity_iter->second.entity.lock(); + if (!entity) { + continue; + } + if (!entity_callback(entity)) + continue; + any_ready_executable = true; + if (spin_once) { + return; + } } } - } }; - iterate_collection(rcl_wait_set.timers, rcl_wait_set.size_of_timers, collection.timers, - [](auto timer){ - if (!timer->call()) - return false; - execute_timer(timer); - return true; - }); - if (spin_once && any_ready_executable) { return true; } + // Timer must check if call() is true before executing + auto timer_callback = [](auto timer){ + if (!timer->call()) + return false; + execute_timer(timer); + return true; + }; - iterate_collection(rcl_wait_set.subscriptions, rcl_wait_set.size_of_subscriptions, collection.subscriptions, - [](auto subscription){ - execute_subscription(subscription); - return true; - }); + auto subscription_callback = [](auto subscription){execute_subscription(subscription); return true;}; + auto service_callback = [](auto service){execute_service(service); return true;}; + auto client_callback = [](auto client){execute_client(client); return true;}; + + iterate_collection(rcl_wait_set.timers, rcl_wait_set.size_of_timers, collection.timers, timer_callback); if (spin_once && any_ready_executable) { return true; } - iterate_collection(rcl_wait_set.services, rcl_wait_set.size_of_services, collection.services, - [](auto service){ - execute_service(service); - return true; - }); + iterate_collection(rcl_wait_set.subscriptions, rcl_wait_set.size_of_subscriptions, collection.subscriptions, subscription_callback); if (spin_once && any_ready_executable) { return true; } - iterate_collection(rcl_wait_set.clients, rcl_wait_set.size_of_clients, collection.clients, - [](auto client){ - execute_client(client); - return true; - }); + iterate_collection(rcl_wait_set.services, rcl_wait_set.size_of_services, collection.services, service_callback); if (spin_once && any_ready_executable) { return true; } + iterate_collection(rcl_wait_set.clients, rcl_wait_set.size_of_clients, collection.clients, client_callback); + if (spin_once && any_ready_executable) { return true; } - for (auto & [handle, entry] : collection.waitables) { + // Waitables require custom logic, so iterate_collection cannot be used. + for (const auto & [handle, entry] : collection.waitables) { auto waitable = entry.entity.lock(); if (!waitable) { continue; From 6b671aab91dc21271ffea33cd293dfe20e080e7f Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 23 Jan 2024 16:43:47 +0000 Subject: [PATCH 62/74] Lint Signed-off-by: Michael Carroll --- rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp index 0cb4b83768..3649afa4b1 100644 --- a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp @@ -55,7 +55,7 @@ MultiThreadedExecutor::spin() if (spinning.exchange(true)) { throw std::runtime_error("spin() called while already spinning"); } - RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); + RCPPUTILS_SCOPE_EXIT(this->spinning.store(false);); std::vector threads; size_t thread_id = 0; { @@ -100,8 +100,7 @@ MultiThreadedExecutor::run(size_t this_thread_number) execute_any_executable(any_exec); if (any_exec.callback_group && - any_exec.callback_group->type() == CallbackGroupType::MutuallyExclusive && - any_exec.callback_group->size() > 1) + any_exec.callback_group->type() == CallbackGroupType::MutuallyExclusive) { try { interrupt_guard_condition_->trigger(); From 527d284cac17ca554dd6e1928518d717b8d77933 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 23 Jan 2024 17:53:22 +0000 Subject: [PATCH 63/74] Lint Signed-off-by: Michael Carroll --- .../executors/multi_threaded_executor.cpp | 2 +- .../static_single_threaded_executor.cpp | 86 +++++++++++-------- 2 files changed, 50 insertions(+), 38 deletions(-) diff --git a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp index 3649afa4b1..2d5c76e817 100644 --- a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp @@ -100,7 +100,7 @@ MultiThreadedExecutor::run(size_t this_thread_number) execute_any_executable(any_exec); if (any_exec.callback_group && - any_exec.callback_group->type() == CallbackGroupType::MutuallyExclusive) + any_exec.callback_group->type() == CallbackGroupType::MutuallyExclusive) { try { interrupt_guard_condition_->trigger(); diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp index 083aa8a259..a03dedbefc 100644 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp @@ -82,10 +82,10 @@ StaticSingleThreadedExecutor::spin_some_impl(std::chrono::nanoseconds max_durati { auto start = std::chrono::steady_clock::now(); auto max_duration_not_elapsed = [max_duration, start]() { - const auto spin_forever = std::chrono::nanoseconds(0) == max_duration; - const auto cur_duration = std::chrono::steady_clock::now() - start; - return spin_forever || (cur_duration < max_duration); - }; + const auto spin_forever = std::chrono::nanoseconds(0) == max_duration; + const auto cur_duration = std::chrono::steady_clock::now() - start; + return spin_forever || (cur_duration < max_duration); + }; if (spinning.exchange(true)) { throw std::runtime_error("spin_some() called while already spinning"); @@ -97,10 +97,11 @@ StaticSingleThreadedExecutor::spin_some_impl(std::chrono::nanoseconds max_durati std::lock_guard guard(mutex_); auto wait_result = this->collect_and_wait(std::chrono::nanoseconds(0)); - if (wait_result.has_value()) - { + if (wait_result.has_value()) { // Execute ready executables - bool work_available = this->execute_ready_executables(current_collection_, wait_result.value(), false); + bool work_available = this->execute_ready_executables( + current_collection_, + wait_result.value(), false); if (!work_available || !exhaustive) { break; } @@ -114,8 +115,7 @@ StaticSingleThreadedExecutor::spin_once_impl(std::chrono::nanoseconds timeout) if (rclcpp::ok(context_) && spinning.load()) { std::lock_guard guard(mutex_); auto wait_result = this->collect_and_wait(timeout); - if (wait_result.has_value()) - { + if (wait_result.has_value()) { this->execute_ready_executables(current_collection_, wait_result.value(), true); } } @@ -151,10 +151,10 @@ bool StaticSingleThreadedExecutor::execute_ready_executables( auto rcl_wait_set = wait_result.get_wait_set().get_rcl_wait_set(); - // Helper function to iterate over the waitset result and current collection to check for readiness + // Helper function to iterate over the waitset result and collection to check for readiness // - // In the case that spin_once is set, this function will exit after the first entity in the collection - // is executed, regardless if additional work is available and ready. + // In the case that spin_once is set, this function will exit after the first entity in the + // collectionis executed, regardless if additional work is available and ready. // // \param[in] entities - rcl_wait_set handles to iterate over (eg rcl_wait_set.timers) // \param[in] size_of_entities - size of rcl_wait_set handles (eg rcl_wait_set.size_of_timers) @@ -163,7 +163,7 @@ bool StaticSingleThreadedExecutor::execute_ready_executables( // returns true if the entity was executed, // false otherwise auto iterate_collection = [spin_once, &any_ready_executable]( - auto entities, auto size_of_entities, auto collection, auto entity_callback){ + auto entities, auto size_of_entities, auto collection, auto entity_callback) { for (size_t ii = 0; ii < size_of_entities; ++ii) { if (nullptr == entities[ii]) {continue;} auto entity_iter = collection.find(entities[ii]); @@ -172,39 +172,51 @@ bool StaticSingleThreadedExecutor::execute_ready_executables( if (!entity) { continue; } - if (!entity_callback(entity)) + if (!entity_callback(entity)) { continue; + } any_ready_executable = true; if (spin_once) { return; } } } - }; + }; // Timer must check if call() is true before executing - auto timer_callback = [](auto timer){ - if (!timer->call()) - return false; - execute_timer(timer); - return true; - }; - - auto subscription_callback = [](auto subscription){execute_subscription(subscription); return true;}; - auto service_callback = [](auto service){execute_service(service); return true;}; - auto client_callback = [](auto client){execute_client(client); return true;}; - - iterate_collection(rcl_wait_set.timers, rcl_wait_set.size_of_timers, collection.timers, timer_callback); - if (spin_once && any_ready_executable) { return true; } - - iterate_collection(rcl_wait_set.subscriptions, rcl_wait_set.size_of_subscriptions, collection.subscriptions, subscription_callback); - if (spin_once && any_ready_executable) { return true; } - - iterate_collection(rcl_wait_set.services, rcl_wait_set.size_of_services, collection.services, service_callback); - if (spin_once && any_ready_executable) { return true; } - - iterate_collection(rcl_wait_set.clients, rcl_wait_set.size_of_clients, collection.clients, client_callback); - if (spin_once && any_ready_executable) { return true; } + auto timer_callback = [](auto timer) { + if (!timer->call()) { + return false; + } + execute_timer(timer); + return true; + }; + + auto subscription_callback = [](auto subscription) { + execute_subscription(subscription); return true; + }; + auto service_callback = [](auto service) {execute_service(service); return true;}; + auto client_callback = [](auto client) {execute_client(client); return true;}; + + iterate_collection( + rcl_wait_set.timers, rcl_wait_set.size_of_timers, collection.timers, + timer_callback); + if (spin_once && any_ready_executable) {return true;} + + iterate_collection( + rcl_wait_set.subscriptions, rcl_wait_set.size_of_subscriptions, + collection.subscriptions, subscription_callback); + if (spin_once && any_ready_executable) {return true;} + + iterate_collection( + rcl_wait_set.services, rcl_wait_set.size_of_services, collection.services, + service_callback); + if (spin_once && any_ready_executable) {return true;} + + iterate_collection( + rcl_wait_set.clients, rcl_wait_set.size_of_clients, collection.clients, + client_callback); + if (spin_once && any_ready_executable) {return true;} // Waitables require custom logic, so iterate_collection cannot be used. for (const auto & [handle, entry] : collection.waitables) { From 92aff2c36d062c5e5de819d00f6c8546c3b11845 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Fri, 26 Jan 2024 17:44:52 +0000 Subject: [PATCH 64/74] Add ready executable accessors to WaitResult Signed-off-by: Michael Carroll --- rclcpp/include/rclcpp/wait_result.hpp | 176 ++++++++++++++++++++++++++ 1 file changed, 176 insertions(+) diff --git a/rclcpp/include/rclcpp/wait_result.hpp b/rclcpp/include/rclcpp/wait_result.hpp index e879043d04..161e92a7c3 100644 --- a/rclcpp/include/rclcpp/wait_result.hpp +++ b/rclcpp/include/rclcpp/wait_result.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include "rcl/wait.h" @@ -24,6 +25,8 @@ #include "rclcpp/macros.hpp" #include "rclcpp/wait_result_kind.hpp" +#include "rclcpp/timer.hpp" + namespace rclcpp { @@ -134,6 +137,179 @@ class WaitResult final } } + size_t ready_timers() const + { + auto ready = 0; + if (this->kind() == WaitResultKind::Ready) { + const auto rcl_wait_set = wait_set_pointer_->get_rcl_wait_set(); + for (size_t ii = 0; ii < rcl_wait_set.size_of_timers; ++ii) { + if (rcl_wait_set.timers[ii] != nullptr && + wait_set_pointer_->timers[ii]->call()) { + ready++; + } + } + } + return ready; + } + + size_t ready_subscriptions() const + { + auto ready = 0; + if (this->kind() == WaitResultKind::Ready) { + const auto rcl_wait_set = wait_set_pointer_->get_rcl_wait_set(); + for (size_t ii = 0; ii < rcl_wait_set.size_of_subscriptions; ++ii) { + if (rcl_wait_set.subscriptions[ii] != nullptr) { + ready++; + } + } + } + return ready; + } + + size_t ready_services() const + { + auto ready = 0; + if (this->kind() == WaitResultKind::Ready) { + const auto rcl_wait_set = wait_set_pointer_->get_rcl_wait_set(); + for (size_t ii = 0; ii < rcl_wait_set.size_of_services; ++ii) { + if (rcl_wait_set.services[ii] != nullptr) { + ready++; + } + } + } + return ready; + } + + size_t ready_clients() const + { + auto ready = 0; + if (this->kind() == WaitResultKind::Ready) { + const auto rcl_wait_set = wait_set_pointer_->get_rcl_wait_set(); + for (size_t ii = 0; ii < rcl_wait_set.size_of_clients; ++ii) { + if (rcl_wait_set.clients[ii] != nullptr) { + ready++; + } + } + } + return ready; + } + + size_t ready_waitables() const + { + auto ready = 0; + if (this->kind() == WaitResultKind::Ready) { + const auto rcl_wait_set = wait_set_pointer_->get_rcl_wait_set(); + for (size_t ii = 0; ii < rcl_wait_set.size_of_waitables; ++ii) { + if (rcl_wait_set.waitables[ii] != nullptr && + wait_set_pointer_->waitable->is_ready(&rcl_wait_set)) { + ready++; + } + } + } + return ready; + } + + size_t ready_count() const + { + return ready_timers() + + ready_subscriptions() + + ready_services() + + ready_clients() + + ready_waitables(); + } + + std::shared_ptr next_ready_timer() + { + auto ret = std::shared_ptr{nullptr}; + if (this->kind() == WaitResultKind::Ready) + { + auto & rcl_wait_set = wait_set_pointer_->storage_get_rcl_wait_set(); + for (size_t ii = 0; ii < rcl_wait_set.size_of_timers; ++ii) + { + if (rcl_wait_set.timers[ii] != nullptr && + wait_set_pointer_->timers[ii]->call()) { + ret = wait_set_pointer_->timers[ii]; + rcl_wait_set.timers[ii] = nullptr; + break; + } + } + } + return ret; + } + + std::shared_ptr next_ready_subscription() + { + auto ret = std::shared_ptr{nullptr}; + if (this->kind() == WaitResultKind::Ready) + { + auto & rcl_wait_set = wait_set_pointer_->storage_get_rcl_wait_set(); + for (size_t ii = 0; ii < rcl_wait_set.size_of_subscriptions; ++ii) + { + if (rcl_wait_set.subscription[ii] != nullptr) { + ret = wait_set_pointer_->subscriptions[ii]; + rcl_wait_set.subscriptions[ii] = nullptr; + break; + } + } + } + return ret; + } + + std::shared_ptr next_ready_service() + { + auto ret = std::shared_ptr{nullptr}; + if (this->kind() == WaitResultKind::Ready) + { + auto & rcl_wait_set = wait_set_pointer_->storage_get_rcl_wait_set(); + for (size_t ii = 0; ii < rcl_wait_set.size_of_services; ++ii) + { + if (rcl_wait_set.service[ii] != nullptr) { + ret = wait_set_pointer_->service[ii]; + rcl_wait_set.services[ii] = nullptr; + break; + } + } + } + return ret; + } + + std::shared_ptr next_ready_client() + { + auto ret = std::shared_ptr{nullptr}; + if (this->kind() == WaitResultKind::Ready) + { + auto & rcl_wait_set = wait_set_pointer_->storage_get_rcl_wait_set(); + for (size_t ii = 0; ii < rcl_wait_set.size_of_clients; ++ii) + { + if (rcl_wait_set.client[ii] != nullptr) { + ret = wait_set_pointer_->client[ii]; + rcl_wait_set.clients[ii] = nullptr; + break; + } + } + } + return ret; + } + + std::shared_ptr next_ready_waitable() + { + auto ret = std::shared_ptr{nullptr}; + if (this->kind() == WaitResultKind::Ready) + { + auto rcl_wait_set = get_wait_set().get_rcl_wait_set(); + for (size_t ii = 0; ii < rcl_wait_set.size_of_waitables; ++ii) + { + if (rcl_wait_set.waitable[ii] != nullptr && + wait_set_pointer_->waitables->is_ready(&rcl_wait_set)) { + ret = wait_set_pointer_->waitable[ii]; + rcl_wait_set.waitables[ii] = nullptr; + break; + } + } + } + return ret; + } + private: RCLCPP_DISABLE_COPY(WaitResult) From bf224d6ae2fd60f57ec20a874772cf8ce6417dd6 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 30 Jan 2024 16:04:14 +0000 Subject: [PATCH 65/74] Make use of accessors from wait_set Signed-off-by: Michael Carroll --- rclcpp/include/rclcpp/executor.hpp | 4 +- rclcpp/include/rclcpp/wait_result.hpp | 75 ++++----- .../detail/storage_policy_common.hpp | 22 +++ .../wait_set_policies/dynamic_storage.hpp | 35 +++++ .../wait_set_policies/static_storage.hpp | 35 +++++ rclcpp/src/rclcpp/executor.cpp | 142 +++++++++--------- 6 files changed, 200 insertions(+), 113 deletions(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index b273bfb361..aa2750100e 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -580,6 +580,7 @@ class Executor /// WaitSet to be waited on. rclcpp::WaitSet wait_set_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + std::optional> wait_result_ RCPPUTILS_TSA_GUARDED_BY(mutex_); /// Hold the current state of the collection being waited on by the waitset rclcpp::executors::ExecutorEntitiesCollection current_collection_ RCPPUTILS_TSA_GUARDED_BY( @@ -589,9 +590,6 @@ class Executor std::shared_ptr current_notify_waitable_ RCPPUTILS_TSA_GUARDED_BY(mutex_); - /// Hold the list of executables currently available to be executed. - std::deque ready_executables_ RCPPUTILS_TSA_GUARDED_BY(mutex_); - /// shutdown callback handle registered to Context rclcpp::OnShutdownCallbackHandle shutdown_callback_handle_; diff --git a/rclcpp/include/rclcpp/wait_result.hpp b/rclcpp/include/rclcpp/wait_result.hpp index 161e92a7c3..2dca2abf25 100644 --- a/rclcpp/include/rclcpp/wait_result.hpp +++ b/rclcpp/include/rclcpp/wait_result.hpp @@ -144,8 +144,9 @@ class WaitResult final const auto rcl_wait_set = wait_set_pointer_->get_rcl_wait_set(); for (size_t ii = 0; ii < rcl_wait_set.size_of_timers; ++ii) { if (rcl_wait_set.timers[ii] != nullptr && - wait_set_pointer_->timers[ii]->call()) { - ready++; + wait_set_pointer_->timers(ii)->call()) + { + ready++; } } } @@ -159,7 +160,7 @@ class WaitResult final const auto rcl_wait_set = wait_set_pointer_->get_rcl_wait_set(); for (size_t ii = 0; ii < rcl_wait_set.size_of_subscriptions; ++ii) { if (rcl_wait_set.subscriptions[ii] != nullptr) { - ready++; + ready++; } } } @@ -173,7 +174,7 @@ class WaitResult final const auto rcl_wait_set = wait_set_pointer_->get_rcl_wait_set(); for (size_t ii = 0; ii < rcl_wait_set.size_of_services; ++ii) { if (rcl_wait_set.services[ii] != nullptr) { - ready++; + ready++; } } } @@ -187,7 +188,7 @@ class WaitResult final const auto rcl_wait_set = wait_set_pointer_->get_rcl_wait_set(); for (size_t ii = 0; ii < rcl_wait_set.size_of_clients; ++ii) { if (rcl_wait_set.clients[ii] != nullptr) { - ready++; + ready++; } } } @@ -201,8 +202,9 @@ class WaitResult final const auto rcl_wait_set = wait_set_pointer_->get_rcl_wait_set(); for (size_t ii = 0; ii < rcl_wait_set.size_of_waitables; ++ii) { if (rcl_wait_set.waitables[ii] != nullptr && - wait_set_pointer_->waitable->is_ready(&rcl_wait_set)) { - ready++; + wait_set_pointer_->waitables[ii]->is_ready(&rcl_wait_set)) + { + ready++; } } } @@ -221,16 +223,15 @@ class WaitResult final std::shared_ptr next_ready_timer() { auto ret = std::shared_ptr{nullptr}; - if (this->kind() == WaitResultKind::Ready) - { + if (this->kind() == WaitResultKind::Ready) { auto & rcl_wait_set = wait_set_pointer_->storage_get_rcl_wait_set(); - for (size_t ii = 0; ii < rcl_wait_set.size_of_timers; ++ii) - { + for (size_t ii = 0; ii < rcl_wait_set.size_of_timers; ++ii) { if (rcl_wait_set.timers[ii] != nullptr && - wait_set_pointer_->timers[ii]->call()) { - ret = wait_set_pointer_->timers[ii]; - rcl_wait_set.timers[ii] = nullptr; - break; + wait_set_pointer_->timers(ii)->call()) + { + ret = wait_set_pointer_->timers(ii); + rcl_wait_set.timers[ii] = nullptr; + break; } } } @@ -240,13 +241,11 @@ class WaitResult final std::shared_ptr next_ready_subscription() { auto ret = std::shared_ptr{nullptr}; - if (this->kind() == WaitResultKind::Ready) - { + if (this->kind() == WaitResultKind::Ready) { auto & rcl_wait_set = wait_set_pointer_->storage_get_rcl_wait_set(); - for (size_t ii = 0; ii < rcl_wait_set.size_of_subscriptions; ++ii) - { - if (rcl_wait_set.subscription[ii] != nullptr) { - ret = wait_set_pointer_->subscriptions[ii]; + for (size_t ii = 0; ii < rcl_wait_set.size_of_subscriptions; ++ii) { + if (rcl_wait_set.subscriptions[ii] != nullptr) { + ret = wait_set_pointer_->subscriptions(ii); rcl_wait_set.subscriptions[ii] = nullptr; break; } @@ -258,13 +257,11 @@ class WaitResult final std::shared_ptr next_ready_service() { auto ret = std::shared_ptr{nullptr}; - if (this->kind() == WaitResultKind::Ready) - { + if (this->kind() == WaitResultKind::Ready) { auto & rcl_wait_set = wait_set_pointer_->storage_get_rcl_wait_set(); - for (size_t ii = 0; ii < rcl_wait_set.size_of_services; ++ii) - { - if (rcl_wait_set.service[ii] != nullptr) { - ret = wait_set_pointer_->service[ii]; + for (size_t ii = 0; ii < rcl_wait_set.size_of_services; ++ii) { + if (rcl_wait_set.services[ii] != nullptr) { + ret = wait_set_pointer_->services(ii); rcl_wait_set.services[ii] = nullptr; break; } @@ -276,13 +273,11 @@ class WaitResult final std::shared_ptr next_ready_client() { auto ret = std::shared_ptr{nullptr}; - if (this->kind() == WaitResultKind::Ready) - { + if (this->kind() == WaitResultKind::Ready) { auto & rcl_wait_set = wait_set_pointer_->storage_get_rcl_wait_set(); - for (size_t ii = 0; ii < rcl_wait_set.size_of_clients; ++ii) - { - if (rcl_wait_set.client[ii] != nullptr) { - ret = wait_set_pointer_->client[ii]; + for (size_t ii = 0; ii < rcl_wait_set.size_of_clients; ++ii) { + if (rcl_wait_set.clients[ii] != nullptr) { + ret = wait_set_pointer_->clients(ii); rcl_wait_set.clients[ii] = nullptr; break; } @@ -293,16 +288,12 @@ class WaitResult final std::shared_ptr next_ready_waitable() { - auto ret = std::shared_ptr{nullptr}; - if (this->kind() == WaitResultKind::Ready) - { + auto ret = std::shared_ptr{nullptr}; + if (this->kind() == WaitResultKind::Ready) { auto rcl_wait_set = get_wait_set().get_rcl_wait_set(); - for (size_t ii = 0; ii < rcl_wait_set.size_of_waitables; ++ii) - { - if (rcl_wait_set.waitable[ii] != nullptr && - wait_set_pointer_->waitables->is_ready(&rcl_wait_set)) { - ret = wait_set_pointer_->waitable[ii]; - rcl_wait_set.waitables[ii] = nullptr; + for (size_t ii = 0; ii < wait_set_pointer_->size_of_waitables(); ++ii) { + if (wait_set_pointer_->waitables(ii)->is_ready(&rcl_wait_set)) { + ret = wait_set_pointer_->waitables(ii); break; } } diff --git a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp index bd6a5c1281..48afae1658 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp @@ -397,6 +397,28 @@ class StoragePolicyCommon needs_resize_ = true; } + template + typename SubscriptionsIterable::value_type + subscriptions(size_t) const {return nullptr;} + + template + typename TimersIterable::value_type + timers(size_t) const {return nullptr;} + + template + typename ClientsIterable::value_type + clients(size_t) const {return nullptr;} + + template + typename ServicesIterable::value_type + services(size_t) const {return nullptr;} + + size_t size_of_waitables() const {return 0;} + + template + typename WaitablesIterable::value_type + waitables(size_t) const {return nullptr;} + rcl_wait_set_t rcl_wait_set_; rclcpp::Context::SharedPtr context_; diff --git a/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp index 79df17cfec..a5f27596f4 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp @@ -453,6 +453,41 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo reset_all(shared_waitables_); } + std::shared_ptr + subscriptions(size_t ii) const + { + return shared_subscriptions_[ii].subscription; + } + + std::shared_ptr + timers(size_t ii) const + { + return shared_timers_[ii]; + } + + std::shared_ptr + clients(size_t ii) const + { + return shared_clients_[ii]; + } + + std::shared_ptr + services(size_t ii) const + { + return shared_services_[ii]; + } + + size_t size_of_waitables() const + { + return shared_waitables_.size(); + } + + std::shared_ptr + waitables(size_t ii) const + { + return shared_waitables_[ii].waitable; + } + size_t ownership_reference_counter_ = 0; SequenceOfWeakSubscriptions subscriptions_; diff --git a/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp b/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp index 434947c19f..04c8a3f403 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp @@ -188,6 +188,41 @@ class StaticStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCom // Explicitly do nothing. } + typename ArrayOfSubscriptions::value_type + subscriptions(size_t ii) const + { + return subscriptions_[ii]; + } + + typename ArrayOfTimers::value_type + timers(size_t ii) const + { + return timers_[ii]; + } + + typename ArrayOfClients::value_type + clients(size_t ii) const + { + return clients_[ii]; + } + + typename ArrayOfServices::value_type + services(size_t ii) const + { + return services_[ii]; + } + + size_t size_of_waitables() const + { + return waitables_.size(); + } + + typename ArrayOfWaitables::value_type + waitables(size_t ii) const + { + return waitables_[ii]; + } + const ArrayOfSubscriptions subscriptions_; const ArrayOfGuardConditions guard_conditions_; const ArrayOfTimers timers_; diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index cedff33819..21d1d57ff3 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -315,43 +315,25 @@ Executor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive) } RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); - size_t work_in_queue = 0; - bool has_waited = false; - - { - std::lock_guard lock(mutex_); - work_in_queue = ready_executables_.size(); - } - // The logic below is to guarantee that we: - // a) run all of the work in the queue before we spin the first time - // b) spin at least once - // c) run all of the work in the queue after we spin - while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) { - AnyExecutable any_exec; - if (work_in_queue > 0) { - // If there is work in the queue, then execute it - // This covers the case that there are things left in the queue from a - // previous spin. - if (get_next_ready_executable(any_exec)) { - execute_any_executable(any_exec); - } - } else if (!has_waited && !work_in_queue) { - // Once the ready queue is empty, then we need to wait at least once. + if (!wait_result_.has_value()) { wait_for_work(std::chrono::milliseconds(0)); - has_waited = true; - } else if (has_waited && !work_in_queue) { - // Once we have emptied the ready queue, but have already waited: - if (!exhaustive) { - // In the case of spin some, then we can exit - break; - } else { - // In the case of spin all, then we will allow ourselves to wait again. - has_waited = false; - } } - std::lock_guard lock(mutex_); - work_in_queue = ready_executables_.size(); + + AnyExecutable any_exec; + if (get_next_ready_executable(any_exec)) { + execute_any_executable(any_exec); + } else { + // If nothing is ready, reset the result to signal we are + // ready to wait again + wait_result_.reset(); + } + + if (!wait_result_.has_value() && !exhaustive) { + // In the case of spin some, then we can exit + // In the case of spin all, then we will allow ourselves to wait again. + break; + } } } @@ -558,7 +540,6 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) throw std::runtime_error("Delivered message kind is not supported"); } } - return; } void @@ -595,6 +576,9 @@ Executor::execute_client( void Executor::collect_entities() { + // Updating the entity collection and waitset expires any active result + this->wait_result_.reset(); + // Get the current list of available waitables from the collector. rclcpp::executors::ExecutorEntitiesCollection collection; this->collector_.update_collections(); @@ -656,45 +640,28 @@ Executor::collect_entities() // before being removed from the waitset, additionally prune the waitset. this->wait_set_.prune_deleted_entities(); this->entities_need_rebuild_.store(false); - - if (!this->ready_executables_.empty()) { - std::unordered_set groups; - for (const auto & weak_group : callback_groups) { - auto group = weak_group.lock(); - if (group) { - groups.insert(group); - } - } - - this->ready_executables_.erase( - std::remove_if( - this->ready_executables_.begin(), - this->ready_executables_.end(), - [groups](auto exec) { - return groups.count(exec.callback_group) == 0; - }), - this->ready_executables_.end()); - } } void Executor::wait_for_work(std::chrono::nanoseconds timeout) { TRACETOOLS_TRACEPOINT(rclcpp_executor_wait_for_work, timeout.count()); + + // Clear any previous wait result + this->wait_result_.reset(); + { std::lock_guard guard(mutex_); if (current_collection_.empty() || this->entities_need_rebuild_.load()) { this->collect_entities(); } } - - auto wait_result = wait_set_.wait(timeout); - if (wait_result.kind() == WaitResultKind::Empty) { + this->wait_result_.emplace(wait_set_.wait(timeout)); + if (!this->wait_result_ || this->wait_result_->kind() == WaitResultKind::Empty) { RCUTILS_LOG_WARN_NAMED( "rclcpp", "empty wait set received in wait(). This should never happen."); } - rclcpp::executors::ready_executables(current_collection_, wait_result, ready_executables_); } bool @@ -702,22 +669,61 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable) { TRACETOOLS_TRACEPOINT(rclcpp_executor_get_next_ready); - std::lock_guard guard(mutex_); - if (ready_executables_.size() == 0) { + if (!wait_result_.has_value() || wait_result_->kind() != rclcpp::WaitResultKind::Ready) { return false; } - any_executable = ready_executables_.front(); - ready_executables_.pop_front(); + if (auto timer = wait_result_->next_ready_timer()) { + auto entity_iter = current_collection_.timers.find(timer->get_timer_handle().get()); + if (entity_iter != current_collection_.timers.end()) { + if (auto callback_group = entity_iter->second.callback_group.lock()) { + any_executable.timer = timer; + any_executable.callback_group = callback_group; + } + } + } else if (auto subscription = wait_result_->next_ready_subscription()) { + auto entity_iter = current_collection_.subscriptions.find( + subscription->get_subscription_handle().get()); + if (entity_iter != current_collection_.subscriptions.end()) { + if (auto callback_group = entity_iter->second.callback_group.lock()) { + any_executable.subscription = subscription; + any_executable.callback_group = callback_group; + } + } + } else if (auto service = wait_result_->next_ready_service()) { + auto entity_iter = current_collection_.services.find(service->get_service_handle().get()); + if (entity_iter != current_collection_.services.end()) { + if (auto callback_group = entity_iter->second.callback_group.lock()) { + any_executable.service = service; + any_executable.callback_group = callback_group; + } + } + } else if (auto client = wait_result_->next_ready_client()) { + auto entity_iter = current_collection_.clients.find(client->get_client_handle().get()); + if (entity_iter != current_collection_.clients.end()) { + if (auto callback_group = entity_iter->second.callback_group.lock()) { + any_executable.client = client; + any_executable.callback_group = callback_group; + } + } + } else if (auto waitable = wait_result_->next_ready_waitable()) { + auto entity_iter = current_collection_.waitables.find(waitable.get()); + if (entity_iter != current_collection_.waitables.end()) { + if (auto callback_group = entity_iter->second.callback_group.lock()) { + any_executable.waitable = waitable; + any_executable.callback_group = callback_group; + } + } + } - if (any_executable.callback_group && - any_executable.callback_group->type() == CallbackGroupType::MutuallyExclusive) - { - assert(any_executable.callback_group->can_be_taken_from().load()); - any_executable.callback_group->can_be_taken_from().store(false); + if (any_executable.callback_group) { + if (any_executable.callback_group->type() == CallbackGroupType::MutuallyExclusive) { + assert(any_executable.callback_group->can_be_taken_from().load()); + any_executable.callback_group->can_be_taken_from().store(false); + } } - return true; + return any_executable.callback_group != nullptr; } bool From ddac8215f56b05f3bdea8dd8acfa70477b2d3379 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Mon, 12 Feb 2024 14:20:16 +0000 Subject: [PATCH 66/74] Fix tests Signed-off-by: Michael Carroll --- rclcpp/include/rclcpp/any_executable.hpp | 5 +- rclcpp/include/rclcpp/executor.hpp | 1 - rclcpp/include/rclcpp/wait_result.hpp | 119 +++++------------- rclcpp/include/rclcpp/wait_result_kind.hpp | 1 + rclcpp/include/rclcpp/wait_set_template.hpp | 19 +++ rclcpp/src/rclcpp/executor.cpp | 94 ++++++++++---- .../executors/executor_notify_waitable.cpp | 5 +- .../executors/single_threaded_executor.cpp | 5 + .../test/rclcpp/executors/test_executors.cpp | 6 +- 9 files changed, 130 insertions(+), 125 deletions(-) diff --git a/rclcpp/include/rclcpp/any_executable.hpp b/rclcpp/include/rclcpp/any_executable.hpp index 244293ac83..e4e9eaecb0 100644 --- a/rclcpp/include/rclcpp/any_executable.hpp +++ b/rclcpp/include/rclcpp/any_executable.hpp @@ -45,8 +45,9 @@ struct AnyExecutable rclcpp::ClientBase::SharedPtr client; rclcpp::Waitable::SharedPtr waitable; // These are used to keep the scope on the containing items - rclcpp::CallbackGroup::SharedPtr callback_group; - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base; + rclcpp::CallbackGroup::SharedPtr callback_group {nullptr}; + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base {nullptr}; + std::shared_ptr data {nullptr}; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index aa2750100e..1ced42be4b 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -19,7 +19,6 @@ #include #include #include -#include #include #include #include diff --git a/rclcpp/include/rclcpp/wait_result.hpp b/rclcpp/include/rclcpp/wait_result.hpp index 2dca2abf25..0b5227327e 100644 --- a/rclcpp/include/rclcpp/wait_result.hpp +++ b/rclcpp/include/rclcpp/wait_result.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -137,91 +138,9 @@ class WaitResult final } } - size_t ready_timers() const - { - auto ready = 0; - if (this->kind() == WaitResultKind::Ready) { - const auto rcl_wait_set = wait_set_pointer_->get_rcl_wait_set(); - for (size_t ii = 0; ii < rcl_wait_set.size_of_timers; ++ii) { - if (rcl_wait_set.timers[ii] != nullptr && - wait_set_pointer_->timers(ii)->call()) - { - ready++; - } - } - } - return ready; - } - - size_t ready_subscriptions() const - { - auto ready = 0; - if (this->kind() == WaitResultKind::Ready) { - const auto rcl_wait_set = wait_set_pointer_->get_rcl_wait_set(); - for (size_t ii = 0; ii < rcl_wait_set.size_of_subscriptions; ++ii) { - if (rcl_wait_set.subscriptions[ii] != nullptr) { - ready++; - } - } - } - return ready; - } - - size_t ready_services() const - { - auto ready = 0; - if (this->kind() == WaitResultKind::Ready) { - const auto rcl_wait_set = wait_set_pointer_->get_rcl_wait_set(); - for (size_t ii = 0; ii < rcl_wait_set.size_of_services; ++ii) { - if (rcl_wait_set.services[ii] != nullptr) { - ready++; - } - } - } - return ready; - } - - size_t ready_clients() const - { - auto ready = 0; - if (this->kind() == WaitResultKind::Ready) { - const auto rcl_wait_set = wait_set_pointer_->get_rcl_wait_set(); - for (size_t ii = 0; ii < rcl_wait_set.size_of_clients; ++ii) { - if (rcl_wait_set.clients[ii] != nullptr) { - ready++; - } - } - } - return ready; - } - - size_t ready_waitables() const - { - auto ready = 0; - if (this->kind() == WaitResultKind::Ready) { - const auto rcl_wait_set = wait_set_pointer_->get_rcl_wait_set(); - for (size_t ii = 0; ii < rcl_wait_set.size_of_waitables; ++ii) { - if (rcl_wait_set.waitables[ii] != nullptr && - wait_set_pointer_->waitables[ii]->is_ready(&rcl_wait_set)) - { - ready++; - } - } - } - return ready; - } - - size_t ready_count() const - { - return ready_timers() + - ready_subscriptions() + - ready_services() + - ready_clients() + - ready_waitables(); - } - std::shared_ptr next_ready_timer() { + check_wait_result_dirty(); auto ret = std::shared_ptr{nullptr}; if (this->kind() == WaitResultKind::Ready) { auto & rcl_wait_set = wait_set_pointer_->storage_get_rcl_wait_set(); @@ -240,6 +159,7 @@ class WaitResult final std::shared_ptr next_ready_subscription() { + check_wait_result_dirty(); auto ret = std::shared_ptr{nullptr}; if (this->kind() == WaitResultKind::Ready) { auto & rcl_wait_set = wait_set_pointer_->storage_get_rcl_wait_set(); @@ -256,6 +176,7 @@ class WaitResult final std::shared_ptr next_ready_service() { + check_wait_result_dirty(); auto ret = std::shared_ptr{nullptr}; if (this->kind() == WaitResultKind::Ready) { auto & rcl_wait_set = wait_set_pointer_->storage_get_rcl_wait_set(); @@ -272,12 +193,14 @@ class WaitResult final std::shared_ptr next_ready_client() { + check_wait_result_dirty(); auto ret = std::shared_ptr{nullptr}; if (this->kind() == WaitResultKind::Ready) { auto & rcl_wait_set = wait_set_pointer_->storage_get_rcl_wait_set(); for (size_t ii = 0; ii < rcl_wait_set.size_of_clients; ++ii) { if (rcl_wait_set.clients[ii] != nullptr) { ret = wait_set_pointer_->clients(ii); + rcl_wait_set.clients[ii] = nullptr; break; } @@ -288,17 +211,22 @@ class WaitResult final std::shared_ptr next_ready_waitable() { - auto ret = std::shared_ptr{nullptr}; + check_wait_result_dirty(); + auto waitable = std::shared_ptr{nullptr}; + auto data = std::shared_ptr{nullptr}; + if (this->kind() == WaitResultKind::Ready) { auto rcl_wait_set = get_wait_set().get_rcl_wait_set(); - for (size_t ii = 0; ii < wait_set_pointer_->size_of_waitables(); ++ii) { - if (wait_set_pointer_->waitables(ii)->is_ready(&rcl_wait_set)) { - ret = wait_set_pointer_->waitables(ii); - break; + while (next_waitable_index_ < wait_set_pointer_->size_of_waitables()) { + auto cur_waitable = wait_set_pointer_->waitables(next_waitable_index_); + if (cur_waitable != nullptr && cur_waitable->is_ready(&rcl_wait_set)) { + waitable = cur_waitable; } + next_waitable_index_++; } } - return ret; + + return waitable; } private: @@ -321,9 +249,20 @@ class WaitResult final wait_set_pointer_->wait_result_acquire(); } - const WaitResultKind wait_result_kind_; + void check_wait_result_dirty() + { + // In the case that the waitset was modified while the result was out, + // we must mark the wait result as no longer valid + if (wait_set_pointer_ && wait_set_pointer_->wait_result_dirty_) { + this->wait_result_kind_ = WaitResultKind::Invalid; + } + } + + WaitResultKind wait_result_kind_; WaitSetT * wait_set_pointer_ = nullptr; + + size_t next_waitable_index_ = 0; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/wait_result_kind.hpp b/rclcpp/include/rclcpp/wait_result_kind.hpp index 3ce65bf4f3..7980d1d127 100644 --- a/rclcpp/include/rclcpp/wait_result_kind.hpp +++ b/rclcpp/include/rclcpp/wait_result_kind.hpp @@ -26,6 +26,7 @@ enum RCLCPP_PUBLIC WaitResultKind Ready, //storage_add_subscription(std::move(local_subscription)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} } if (mask.include_events) { for (auto key_event_pair : inner_subscription->get_event_handlers()) { @@ -164,6 +165,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli throw std::runtime_error("subscription event already associated with a wait set"); } this->storage_add_waitable(std::move(event), std::move(local_subscription)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} } } if (mask.include_intra_process_waitable) { @@ -180,6 +182,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli this->storage_add_waitable( std::move(inner_subscription->get_intra_process_waitable()), std::move(local_subscription)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} } } }); @@ -224,6 +227,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli auto local_subscription = inner_subscription; local_subscription->exchange_in_use_by_wait_set_state(local_subscription.get(), false); this->storage_remove_subscription(std::move(local_subscription)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} } if (mask.include_events) { for (auto key_event_pair : inner_subscription->get_event_handlers()) { @@ -231,6 +235,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli auto local_subscription = inner_subscription; local_subscription->exchange_in_use_by_wait_set_state(event.get(), false); this->storage_remove_waitable(std::move(event)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} } } if (mask.include_intra_process_waitable) { @@ -239,6 +244,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // This is the case when intra process is enabled for the subscription. inner_subscription->exchange_in_use_by_wait_set_state(local_waitable.get(), false); this->storage_remove_waitable(std::move(local_waitable)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} } } }); @@ -289,6 +295,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // fixed sized storage policies. // It will throw if the guard condition has already been added. this->storage_add_guard_condition(std::move(inner_guard_condition)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} }); } @@ -326,6 +333,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // fixed sized storage policies. // It will throw if the guard condition is not in the wait set. this->storage_remove_guard_condition(std::move(inner_guard_condition)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} }); } @@ -357,6 +365,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // fixed sized storage policies. // It will throw if the timer has already been added. this->storage_add_timer(std::move(inner_timer)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} }); } @@ -384,6 +393,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // fixed sized storage policies. // It will throw if the timer is not in the wait set. this->storage_remove_timer(std::move(inner_timer)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} }); } @@ -415,6 +425,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // fixed sized storage policies. // It will throw if the client has already been added. this->storage_add_client(std::move(inner_client)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} }); } @@ -442,6 +453,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // fixed sized storage policies. // It will throw if the client is not in the wait set. this->storage_remove_client(std::move(inner_client)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} }); } @@ -473,6 +485,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // fixed sized storage policies. // It will throw if the service has already been added. this->storage_add_service(std::move(inner_service)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} }); } @@ -500,6 +513,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // fixed sized storage policies. // It will throw if the service is not in the wait set. this->storage_remove_service(std::move(inner_service)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} }); } @@ -551,6 +565,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // fixed sized storage policies. // It will throw if the waitable has already been added. this->storage_add_waitable(std::move(inner_waitable), std::move(associated_entity)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} }); } @@ -578,6 +593,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // fixed sized storage policies. // It will throw if the waitable is not in the wait set. this->storage_remove_waitable(std::move(inner_waitable)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} }); } @@ -715,6 +731,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli throw std::runtime_error("wait_result_acquire() called while already holding"); } wait_result_holding_ = true; + wait_result_dirty_ = false; // this method comes from the SynchronizationPolicy this->sync_wait_result_acquire(); // this method comes from the StoragePolicy @@ -734,6 +751,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli throw std::runtime_error("wait_result_release() called while not holding"); } wait_result_holding_ = false; + wait_result_dirty_ = false; // this method comes from the StoragePolicy this->storage_release_ownerships(); // this method comes from the SynchronizationPolicy @@ -741,6 +759,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli } bool wait_result_holding_ = false; + bool wait_result_dirty_ = false; }; } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 21d1d57ff3..335055a5db 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -53,18 +53,17 @@ Executor::Executor(const rclcpp::ExecutorOptions & options) : spinning(false), interrupt_guard_condition_(std::make_shared(options.context)), shutdown_guard_condition_(std::make_shared(options.context)), + context_(options.context), notify_waitable_(std::make_shared( [this]() { this->entities_need_rebuild_.store(true); })), + entities_need_rebuild_(true), collector_(notify_waitable_), wait_set_({}, {}, {}, {}, {}, {}, options.context), current_notify_waitable_(notify_waitable_), impl_(std::make_unique()) { - // Store the context for later use. - context_ = options.context; - shutdown_callback_handle_ = context_->add_on_shutdown_callback( [weak_gc = std::weak_ptr{shutdown_guard_condition_}]() { auto strong_gc = weak_gc.lock(); @@ -394,8 +393,7 @@ Executor::execute_any_executable(AnyExecutable & any_exec) execute_client(any_exec.client); } if (any_exec.waitable) { - auto data = any_exec.waitable->take_data(); - any_exec.waitable->execute(data); + any_exec.waitable->execute(any_exec.data); } // Reset the callback_group, regardless of type @@ -669,49 +667,90 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable) { TRACETOOLS_TRACEPOINT(rclcpp_executor_get_next_ready); + bool valid_executable = false; + if (!wait_result_.has_value() || wait_result_->kind() != rclcpp::WaitResultKind::Ready) { return false; } - if (auto timer = wait_result_->next_ready_timer()) { - auto entity_iter = current_collection_.timers.find(timer->get_timer_handle().get()); - if (entity_iter != current_collection_.timers.end()) { - if (auto callback_group = entity_iter->second.callback_group.lock()) { + if (!valid_executable) { + while (auto timer = wait_result_->next_ready_timer()) { + auto entity_iter = current_collection_.timers.find(timer->get_timer_handle().get()); + if (entity_iter != current_collection_.timers.end()) { + auto callback_group = entity_iter->second.callback_group.lock(); + if (callback_group && !callback_group->can_be_taken_from()) { + continue; + } any_executable.timer = timer; any_executable.callback_group = callback_group; + valid_executable = true; + break; } } - } else if (auto subscription = wait_result_->next_ready_subscription()) { - auto entity_iter = current_collection_.subscriptions.find( - subscription->get_subscription_handle().get()); - if (entity_iter != current_collection_.subscriptions.end()) { - if (auto callback_group = entity_iter->second.callback_group.lock()) { + } + + if (!valid_executable) { + while (auto subscription = wait_result_->next_ready_subscription()) { + auto entity_iter = current_collection_.subscriptions.find( + subscription->get_subscription_handle().get()); + if (entity_iter != current_collection_.subscriptions.end()) { + auto callback_group = entity_iter->second.callback_group.lock(); + if (callback_group && !callback_group->can_be_taken_from()) { + continue; + } any_executable.subscription = subscription; any_executable.callback_group = callback_group; + valid_executable = true; + break; } } - } else if (auto service = wait_result_->next_ready_service()) { - auto entity_iter = current_collection_.services.find(service->get_service_handle().get()); - if (entity_iter != current_collection_.services.end()) { - if (auto callback_group = entity_iter->second.callback_group.lock()) { + } + + if (!valid_executable) { + while (auto service = wait_result_->next_ready_service()) { + auto entity_iter = current_collection_.services.find(service->get_service_handle().get()); + if (entity_iter != current_collection_.services.end()) { + auto callback_group = entity_iter->second.callback_group.lock(); + if (callback_group && !callback_group->can_be_taken_from()) { + continue; + } any_executable.service = service; any_executable.callback_group = callback_group; + valid_executable = true; + break; } } - } else if (auto client = wait_result_->next_ready_client()) { - auto entity_iter = current_collection_.clients.find(client->get_client_handle().get()); - if (entity_iter != current_collection_.clients.end()) { - if (auto callback_group = entity_iter->second.callback_group.lock()) { + } + + if (!valid_executable) { + while (auto client = wait_result_->next_ready_client()) { + auto entity_iter = current_collection_.clients.find(client->get_client_handle().get()); + if (entity_iter != current_collection_.clients.end()) { + auto callback_group = entity_iter->second.callback_group.lock(); + if (callback_group && !callback_group->can_be_taken_from()) { + continue; + } any_executable.client = client; any_executable.callback_group = callback_group; + valid_executable = true; + break; } } - } else if (auto waitable = wait_result_->next_ready_waitable()) { - auto entity_iter = current_collection_.waitables.find(waitable.get()); - if (entity_iter != current_collection_.waitables.end()) { - if (auto callback_group = entity_iter->second.callback_group.lock()) { + } + + if (!valid_executable) { + while (auto waitable = wait_result_->next_ready_waitable()) { + auto entity_iter = current_collection_.waitables.find(waitable.get()); + if (entity_iter != current_collection_.waitables.end()) { + auto callback_group = entity_iter->second.callback_group.lock(); + if (callback_group && !callback_group->can_be_taken_from()) { + continue; + } any_executable.waitable = waitable; any_executable.callback_group = callback_group; + any_executable.data = waitable->take_data(); + valid_executable = true; + break; } } } @@ -723,7 +762,8 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable) } } - return any_executable.callback_group != nullptr; + + return valid_executable; } bool diff --git a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp index 057aee9672..85bedcead1 100644 --- a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp +++ b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp @@ -70,15 +70,16 @@ ExecutorNotifyWaitable::is_ready(rcl_wait_set_t * wait_set) 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]; + const auto * rcl_guard_condition = wait_set->guard_conditions[ii]; if (nullptr == rcl_guard_condition) { continue; } - for (auto weak_guard_condition : this->notify_guard_conditions_) { + for (const 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; + break; } } } diff --git a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp index e7f311c147..975733b497 100644 --- a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp @@ -31,6 +31,11 @@ SingleThreadedExecutor::spin() throw std::runtime_error("spin() called while already spinning"); } RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); + + // Clear any previous result and rebuild the waitset + this->wait_result_.reset(); + this->entities_need_rebuild_ = true; + while (rclcpp::ok(this->context_) && spinning.load()) { rclcpp::AnyExecutable any_executable; if (get_next_executable(any_executable)) { diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 453b39bd5d..50c6b55bdf 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -425,13 +425,13 @@ class TestWaitable : public rclcpp::Waitable get_number_of_ready_guard_conditions() override {return 1;} size_t - get_count() + get_count() const { return count_; } private: - size_t count_ = 0; + std::atomic count_ = 0; rclcpp::GuardCondition gc_; }; @@ -488,7 +488,7 @@ TYPED_TEST(TestExecutors, spinSome) // Long timeout, doesn't block test from finishing because spin_some should exit after the // first one completes. - bool spin_exited = false; + std::atomic spin_exited = false; std::thread spinner([&spin_exited, &executor, this]() { executor.spin_some(1s); executor.remove_node(this->node, true); From 1a9784dd28a21378b062633d7f687d79d4c7e065 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 13 Feb 2024 00:21:31 +0000 Subject: [PATCH 67/74] Fix more tests Signed-off-by: Michael Carroll --- rclcpp/include/rclcpp/wait_result.hpp | 8 ++--- .../detail/storage_policy_common.hpp | 8 +++-- .../wait_set_policies/dynamic_storage.hpp | 30 +++++++++++++++---- .../wait_set_policies/static_storage.hpp | 30 +++++++++++++++---- .../test_static_single_threaded_executor.cpp | 3 +- 5 files changed, 62 insertions(+), 17 deletions(-) diff --git a/rclcpp/include/rclcpp/wait_result.hpp b/rclcpp/include/rclcpp/wait_result.hpp index 0b5227327e..604088cd67 100644 --- a/rclcpp/include/rclcpp/wait_result.hpp +++ b/rclcpp/include/rclcpp/wait_result.hpp @@ -144,7 +144,7 @@ class WaitResult final auto ret = std::shared_ptr{nullptr}; if (this->kind() == WaitResultKind::Ready) { auto & rcl_wait_set = wait_set_pointer_->storage_get_rcl_wait_set(); - for (size_t ii = 0; ii < rcl_wait_set.size_of_timers; ++ii) { + for (size_t ii = 0; ii < wait_set_pointer_->size_of_timers(); ++ii) { if (rcl_wait_set.timers[ii] != nullptr && wait_set_pointer_->timers(ii)->call()) { @@ -163,7 +163,7 @@ class WaitResult final auto ret = std::shared_ptr{nullptr}; if (this->kind() == WaitResultKind::Ready) { auto & rcl_wait_set = wait_set_pointer_->storage_get_rcl_wait_set(); - for (size_t ii = 0; ii < rcl_wait_set.size_of_subscriptions; ++ii) { + for (size_t ii = 0; ii < wait_set_pointer_->size_of_subscriptions(); ++ii) { if (rcl_wait_set.subscriptions[ii] != nullptr) { ret = wait_set_pointer_->subscriptions(ii); rcl_wait_set.subscriptions[ii] = nullptr; @@ -180,7 +180,7 @@ class WaitResult final auto ret = std::shared_ptr{nullptr}; if (this->kind() == WaitResultKind::Ready) { auto & rcl_wait_set = wait_set_pointer_->storage_get_rcl_wait_set(); - for (size_t ii = 0; ii < rcl_wait_set.size_of_services; ++ii) { + for (size_t ii = 0; ii < wait_set_pointer_->size_of_services(); ++ii) { if (rcl_wait_set.services[ii] != nullptr) { ret = wait_set_pointer_->services(ii); rcl_wait_set.services[ii] = nullptr; @@ -197,7 +197,7 @@ class WaitResult final auto ret = std::shared_ptr{nullptr}; if (this->kind() == WaitResultKind::Ready) { auto & rcl_wait_set = wait_set_pointer_->storage_get_rcl_wait_set(); - for (size_t ii = 0; ii < rcl_wait_set.size_of_clients; ++ii) { + for (size_t ii = 0; ii < wait_set_pointer_->size_of_clients(); ++ii) { if (rcl_wait_set.clients[ii] != nullptr) { ret = wait_set_pointer_->clients(ii); diff --git a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp index 48afae1658..42293a3e89 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp @@ -397,6 +397,12 @@ class StoragePolicyCommon needs_resize_ = true; } + size_t size_of_subscriptions() const {return 0;} + size_t size_of_timers() const {return 0;} + size_t size_of_clients() const {return 0;} + size_t size_of_services() const {return 0;} + size_t size_of_waitables() const {return 0;} + template typename SubscriptionsIterable::value_type subscriptions(size_t) const {return nullptr;} @@ -413,8 +419,6 @@ class StoragePolicyCommon typename ServicesIterable::value_type services(size_t) const {return nullptr;} - size_t size_of_waitables() const {return 0;} - template typename WaitablesIterable::value_type waitables(size_t) const {return nullptr;} diff --git a/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp index a5f27596f4..8f97596218 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp @@ -453,6 +453,31 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo reset_all(shared_waitables_); } + size_t size_of_subscriptions() const + { + return shared_subscriptions_.size(); + } + + size_t size_of_timers() const + { + return shared_timers_.size(); + } + + size_t size_of_clients() const + { + return shared_clients_.size(); + } + + size_t size_of_services() const + { + return shared_services_.size(); + } + + size_t size_of_waitables() const + { + return shared_waitables_.size(); + } + std::shared_ptr subscriptions(size_t ii) const { @@ -477,11 +502,6 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo return shared_services_[ii]; } - size_t size_of_waitables() const - { - return shared_waitables_.size(); - } - std::shared_ptr waitables(size_t ii) const { diff --git a/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp b/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp index 04c8a3f403..7f5cad74ad 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp @@ -188,6 +188,31 @@ class StaticStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCom // Explicitly do nothing. } + size_t size_of_subscriptions() const + { + return subscriptions_.size(); + } + + size_t size_of_timers() const + { + return timers_.size(); + } + + size_t size_of_clients() const + { + return clients_.size(); + } + + size_t size_of_services() const + { + return services_.size(); + } + + size_t size_of_waitables() const + { + return waitables_.size(); + } + typename ArrayOfSubscriptions::value_type subscriptions(size_t ii) const { @@ -212,11 +237,6 @@ class StaticStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCom return services_[ii]; } - size_t size_of_waitables() const - { - return waitables_.size(); - } - typename ArrayOfWaitables::value_type waitables(size_t ii) const { diff --git a/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp b/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp index 819788b461..1a0f3f88c4 100644 --- a/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp +++ b/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp @@ -86,7 +86,8 @@ TEST_F(TestStaticSingleThreadedExecutor, remove_callback_group_trigger_guard_fai "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( executor.remove_callback_group(cb_group, true), - std::runtime_error("Failed to trigger guard condition on callback group remove: error not set")); + std::runtime_error( + "Failed to trigger guard condition on callback group remove: error not set")); } } From 892f70adfc6a00e481dd9e1690c4834fe82d52e7 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 13 Feb 2024 15:08:12 +0000 Subject: [PATCH 68/74] Tidy up single threaded executor implementation Signed-off-by: Michael Carroll --- .../static_single_threaded_executor.cpp | 117 ++++++------------ 1 file changed, 39 insertions(+), 78 deletions(-) diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp index a03dedbefc..5643b99a74 100644 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp @@ -149,91 +149,52 @@ bool StaticSingleThreadedExecutor::execute_ready_executables( return any_ready_executable; } - auto rcl_wait_set = wait_result.get_wait_set().get_rcl_wait_set(); - - // Helper function to iterate over the waitset result and collection to check for readiness - // - // In the case that spin_once is set, this function will exit after the first entity in the - // collectionis executed, regardless if additional work is available and ready. - // - // \param[in] entities - rcl_wait_set handles to iterate over (eg rcl_wait_set.timers) - // \param[in] size_of_entities - size of rcl_wait_set handles (eg rcl_wait_set.size_of_timers) - // \param[in] collection - collection of associated weak_ptrs to be waited on - // \param[in] entity_callback - function of the form bool(std:;shared_ptr), - // returns true if the entity was executed, - // false otherwise - auto iterate_collection = [spin_once, &any_ready_executable]( - auto entities, auto size_of_entities, auto collection, auto entity_callback) { - for (size_t ii = 0; ii < size_of_entities; ++ii) { - if (nullptr == entities[ii]) {continue;} - auto entity_iter = collection.find(entities[ii]); - if (entity_iter != collection.end()) { - auto entity = entity_iter->second.entity.lock(); - if (!entity) { - continue; - } - if (!entity_callback(entity)) { - continue; - } - any_ready_executable = true; - if (spin_once) { - return; - } - } - } - }; + while (auto subscription = wait_result.next_ready_subscription()) { + auto entity_iter = collection.subscriptions.find(subscription->get_subscription_handle().get()); + if (entity_iter != collection.subscriptions.end()) { + execute_subscription(subscription); + any_ready_executable = true; + if (spin_once) {return any_ready_executable;} + } + } - // Timer must check if call() is true before executing - auto timer_callback = [](auto timer) { - if (!timer->call()) { - return false; + while (auto timer = wait_result.next_ready_timer()) { + auto entity_iter = collection.timers.find(timer->get_timer_handle().get()); + if (entity_iter != collection.timers.end()) { + if (timer->call()) { + execute_timer(timer); + any_ready_executable = true; + if (spin_once) {return any_ready_executable;} } - execute_timer(timer); - return true; - }; + } + } - auto subscription_callback = [](auto subscription) { - execute_subscription(subscription); return true; - }; - auto service_callback = [](auto service) {execute_service(service); return true;}; - auto client_callback = [](auto client) {execute_client(client); return true;}; - - iterate_collection( - rcl_wait_set.timers, rcl_wait_set.size_of_timers, collection.timers, - timer_callback); - if (spin_once && any_ready_executable) {return true;} - - iterate_collection( - rcl_wait_set.subscriptions, rcl_wait_set.size_of_subscriptions, - collection.subscriptions, subscription_callback); - if (spin_once && any_ready_executable) {return true;} - - iterate_collection( - rcl_wait_set.services, rcl_wait_set.size_of_services, collection.services, - service_callback); - if (spin_once && any_ready_executable) {return true;} - - iterate_collection( - rcl_wait_set.clients, rcl_wait_set.size_of_clients, collection.clients, - client_callback); - if (spin_once && any_ready_executable) {return true;} - - // Waitables require custom logic, so iterate_collection cannot be used. - for (const auto & [handle, entry] : collection.waitables) { - auto waitable = entry.entity.lock(); - if (!waitable) { - continue; + while (auto client = wait_result.next_ready_client()) { + auto entity_iter = collection.clients.find(client->get_client_handle().get()); + if (entity_iter != collection.clients.end()) { + execute_client(client); + any_ready_executable = true; + if (spin_once) {return any_ready_executable;} } - if (!waitable->is_ready(&rcl_wait_set)) { - continue; + } + + while (auto service = wait_result.next_ready_service()) { + auto entity_iter = collection.services.find(service->get_service_handle().get()); + if (entity_iter != collection.services.end()) { + execute_service(service); + any_ready_executable = true; + if (spin_once) {return any_ready_executable;} } + } - auto data = waitable->take_data(); - waitable->execute(data); - if (spin_once) { - return true; + while (auto waitable = wait_result.next_ready_waitable()) { + auto entity_iter = collection.waitables.find(waitable.get()); + if (entity_iter != collection.waitables.end()) { + auto data = waitable->take_data(); + waitable->execute(data); + any_ready_executable = true; + if (spin_once) {return any_ready_executable;} } - any_ready_executable = true; } return any_ready_executable; } From d254d0c801e5e07c21b9d5fecc0182df9ad5abd7 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 27 Feb 2024 03:17:08 +0000 Subject: [PATCH 69/74] Don't null out timer, rely on call Signed-off-by: Michael Carroll --- rclcpp/include/rclcpp/wait_result.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/rclcpp/include/rclcpp/wait_result.hpp b/rclcpp/include/rclcpp/wait_result.hpp index 604088cd67..b1230bb63d 100644 --- a/rclcpp/include/rclcpp/wait_result.hpp +++ b/rclcpp/include/rclcpp/wait_result.hpp @@ -149,7 +149,6 @@ class WaitResult final wait_set_pointer_->timers(ii)->call()) { ret = wait_set_pointer_->timers(ii); - rcl_wait_set.timers[ii] = nullptr; break; } } From 57693595edd9a99a795d8f55f3c0d728817ac648 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 19 Mar 2024 05:20:34 -0700 Subject: [PATCH 70/74] change how timers are checked from wait result in executors Signed-off-by: William Woodall --- rclcpp/include/rclcpp/callback_group.hpp | 6 +- rclcpp/include/rclcpp/wait_result.hpp | 119 +++++++++++++----- rclcpp/src/rclcpp/executor.cpp | 20 ++- .../static_single_threaded_executor.cpp | 28 ++--- .../test/rclcpp/executors/test_executors.cpp | 21 +++- .../test_add_callback_groups_to_executor.cpp | 8 +- 6 files changed, 142 insertions(+), 60 deletions(-) diff --git a/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp index d716199cfd..d8c529d56f 100644 --- a/rclcpp/include/rclcpp/callback_group.hpp +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -184,7 +184,8 @@ class CallbackGroup * \return the number of entities in the callback group. */ RCLCPP_PUBLIC - size_t size() const; + size_t + size() const; /// Return a reference to the 'can be taken' atomic boolean. /** @@ -216,7 +217,8 @@ class CallbackGroup * \param[in] waitable_fuinc Function to execute for each waitable */ RCLCPP_PUBLIC - void collect_all_ptrs( + void + collect_all_ptrs( std::function sub_func, std::function service_func, std::function client_func, diff --git a/rclcpp/include/rclcpp/wait_result.hpp b/rclcpp/include/rclcpp/wait_result.hpp index b1230bb63d..7b87c25c1c 100644 --- a/rclcpp/include/rclcpp/wait_result.hpp +++ b/rclcpp/include/rclcpp/wait_result.hpp @@ -20,12 +20,16 @@ #include #include #include +#include #include "rcl/wait.h" #include "rclcpp/macros.hpp" #include "rclcpp/wait_result_kind.hpp" +#include "rclcpp/client.hpp" +#include "rclcpp/service.hpp" +#include "rclcpp/subscription_base.hpp" #include "rclcpp/timer.hpp" namespace rclcpp @@ -138,33 +142,80 @@ class WaitResult final } } - std::shared_ptr next_ready_timer() + /// Get the next ready timer and its index in the wait result, but do not clear it. + /** + * The returned timer is not cleared automatically, as it the case with the + * other next_ready_*()-like functions. + * Instead, this function returns the timer and the index that identifies it + * in the wait result, so that it can be cleared (marked as taken or used) + * in a separate step with clear_timer_with_index(). + * This is necessary in some multi-threaded executor implementations. + * + * If the timer is not cleared using the index, subsequent calls to this + * function will return the same timer. + * + * If there is no ready timer, then nullptr will be returned and the index + * will be invalid and should not be used. + * + * \param[in] start_index index at which to start searching for the next ready + * timer in the wait result. If the start_index is out of bounds for the + * list of timers in the wait result, then {nullptr, start_index} will be + * returned. Defaults to 0. + * \return next ready timer pointer and its index in the wait result, or + * {nullptr, start_index} if none was found. + */ + std::pair, size_t> + peak_next_ready_timer(size_t start_index = 0) { check_wait_result_dirty(); auto ret = std::shared_ptr{nullptr}; + size_t ii = start_index; if (this->kind() == WaitResultKind::Ready) { - auto & rcl_wait_set = wait_set_pointer_->storage_get_rcl_wait_set(); - for (size_t ii = 0; ii < wait_set_pointer_->size_of_timers(); ++ii) { - if (rcl_wait_set.timers[ii] != nullptr && - wait_set_pointer_->timers(ii)->call()) - { - ret = wait_set_pointer_->timers(ii); + auto & wait_set = this->get_wait_set(); + auto & rcl_wait_set = wait_set.storage_get_rcl_wait_set(); + for (; ii < wait_set.size_of_timers(); ++ii) { + if (rcl_wait_set.timers[ii] != nullptr) { + ret = wait_set.timers(ii); break; } } } - return ret; + return {ret, ii}; + } + + /// Clear the timer at the given index. + /** + * Clearing a timer from the wait result prevents it from being returned by + * the peak_next_ready_timer() on subsequent calls. + * + * The index should come from the peak_next_ready_timer() function, and + * should only be used with this function if the timer pointer was valid. + * + * \throws std::out_of_range if the given index is out of range + */ + void + clear_timer_with_index(size_t index) + { + auto & wait_set = this->get_wait_set(); + auto & rcl_wait_set = wait_set.storage_get_rcl_wait_set(); + if (index >= wait_set.size_of_timers()) { + throw std::out_of_range("given timer index is out of range"); + } + rcl_wait_set.timers[index] = nullptr; } - std::shared_ptr next_ready_subscription() + /// Get the next ready subscription, clearing it from the wait result. + std::shared_ptr + next_ready_subscription() { check_wait_result_dirty(); auto ret = std::shared_ptr{nullptr}; if (this->kind() == WaitResultKind::Ready) { - auto & rcl_wait_set = wait_set_pointer_->storage_get_rcl_wait_set(); - for (size_t ii = 0; ii < wait_set_pointer_->size_of_subscriptions(); ++ii) { + auto & wait_set = this->get_wait_set(); + auto & rcl_wait_set = wait_set.storage_get_rcl_wait_set(); + for (size_t ii = 0; ii < wait_set.size_of_subscriptions(); ++ii) { if (rcl_wait_set.subscriptions[ii] != nullptr) { - ret = wait_set_pointer_->subscriptions(ii); + ret = wait_set.subscriptions(ii); rcl_wait_set.subscriptions[ii] = nullptr; break; } @@ -173,15 +224,18 @@ class WaitResult final return ret; } - std::shared_ptr next_ready_service() + /// Get the next ready service, clearing it from the wait result. + std::shared_ptr + next_ready_service() { check_wait_result_dirty(); auto ret = std::shared_ptr{nullptr}; if (this->kind() == WaitResultKind::Ready) { - auto & rcl_wait_set = wait_set_pointer_->storage_get_rcl_wait_set(); - for (size_t ii = 0; ii < wait_set_pointer_->size_of_services(); ++ii) { + auto & wait_set = this->get_wait_set(); + auto & rcl_wait_set = wait_set.storage_get_rcl_wait_set(); + for (size_t ii = 0; ii < wait_set.size_of_services(); ++ii) { if (rcl_wait_set.services[ii] != nullptr) { - ret = wait_set_pointer_->services(ii); + ret = wait_set.services(ii); rcl_wait_set.services[ii] = nullptr; break; } @@ -190,16 +244,18 @@ class WaitResult final return ret; } - std::shared_ptr next_ready_client() + /// Get the next ready client, clearing it from the wait result. + std::shared_ptr + next_ready_client() { check_wait_result_dirty(); auto ret = std::shared_ptr{nullptr}; if (this->kind() == WaitResultKind::Ready) { - auto & rcl_wait_set = wait_set_pointer_->storage_get_rcl_wait_set(); - for (size_t ii = 0; ii < wait_set_pointer_->size_of_clients(); ++ii) { + auto & wait_set = this->get_wait_set(); + auto & rcl_wait_set = wait_set.storage_get_rcl_wait_set(); + for (size_t ii = 0; ii < wait_set.size_of_clients(); ++ii) { if (rcl_wait_set.clients[ii] != nullptr) { - ret = wait_set_pointer_->clients(ii); - + ret = wait_set.clients(ii); rcl_wait_set.clients[ii] = nullptr; break; } @@ -208,16 +264,19 @@ class WaitResult final return ret; } - std::shared_ptr next_ready_waitable() + /// Get the next ready waitable, clearing it from the wait result. + std::shared_ptr + next_ready_waitable() { check_wait_result_dirty(); auto waitable = std::shared_ptr{nullptr}; auto data = std::shared_ptr{nullptr}; if (this->kind() == WaitResultKind::Ready) { - auto rcl_wait_set = get_wait_set().get_rcl_wait_set(); - while (next_waitable_index_ < wait_set_pointer_->size_of_waitables()) { - auto cur_waitable = wait_set_pointer_->waitables(next_waitable_index_); + auto & wait_set = this->get_wait_set(); + auto rcl_wait_set = wait_set.get_rcl_wait_set(); + while (next_waitable_index_ < wait_set.size_of_waitables()) { + auto cur_waitable = wait_set.waitables(next_waitable_index_); if (cur_waitable != nullptr && cur_waitable->is_ready(&rcl_wait_set)) { waitable = cur_waitable; } @@ -245,14 +304,16 @@ class WaitResult final // Should be enforced by the static factory methods on this class. assert(WaitResultKind::Ready == wait_result_kind); // Secure thread-safety (if provided) and shared ownership (if needed). - wait_set_pointer_->wait_result_acquire(); + this->get_wait_set().wait_result_acquire(); } - void check_wait_result_dirty() + /// Check if the wait result is invalid because the wait set was modified. + void + check_wait_result_dirty() { - // In the case that the waitset was modified while the result was out, + // In the case that the wait set was modified while the result was out, // we must mark the wait result as no longer valid - if (wait_set_pointer_ && wait_set_pointer_->wait_result_dirty_) { + if (wait_set_pointer_ && this->get_wait_set().wait_result_dirty_) { this->wait_result_kind_ = WaitResultKind::Invalid; } } diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 335055a5db..27568c218a 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -559,8 +559,7 @@ Executor::execute_service(rclcpp::ServiceBase::SharedPtr service) } void -Executor::execute_client( - rclcpp::ClientBase::SharedPtr client) +Executor::execute_client(rclcpp::ClientBase::SharedPtr client) { auto request_header = client->create_request_header(); std::shared_ptr response = client->create_response(); @@ -674,13 +673,28 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable) } if (!valid_executable) { - while (auto timer = wait_result_->next_ready_timer()) { + size_t current_timer_index = 0; + while (true) { + auto [timer, timer_index] = wait_result_->peak_next_ready_timer(current_timer_index); + if (nullptr == timer) { + break; + } + current_timer_index = timer_index; auto entity_iter = current_collection_.timers.find(timer->get_timer_handle().get()); if (entity_iter != current_collection_.timers.end()) { auto callback_group = entity_iter->second.callback_group.lock(); if (callback_group && !callback_group->can_be_taken_from()) { continue; } + // At this point the timer is either ready for execution or was perhaps + // it was canceled, based on the result of call(), but either way it + // should not be checked again from peak_next_ready_timer(), so clear + // it from the wait result. + wait_result_->clear_timer_with_index(current_timer_index); + // Check that the timer should be called still, i.e. it wasn't canceled. + if (!timer->call()) { + continue; + } any_executable.timer = timer; any_executable.callback_group = callback_group; valid_executable = true; diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp index 5643b99a74..4ddefc11f4 100644 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp @@ -40,21 +40,7 @@ StaticSingleThreadedExecutor::spin() // except we need to keep the wait result to reproduce the StaticSingleThreadedExecutor // behavior. while (rclcpp::ok(this->context_) && spinning.load()) { - std::deque to_exec; - - std::lock_guard guard(mutex_); - if (current_collection_.empty() || this->entities_need_rebuild_.load()) { - this->collect_entities(); - } - - auto wait_result = wait_set_.wait(std::chrono::nanoseconds(-1)); - if (wait_result.kind() == WaitResultKind::Empty) { - RCUTILS_LOG_WARN_NAMED( - "rclcpp", - "empty wait set received in wait(). This should never happen."); - continue; - } - execute_ready_executables(current_collection_, wait_result, false); + this->spin_once_impl(std::chrono::nanoseconds(-1)); } } @@ -101,7 +87,8 @@ StaticSingleThreadedExecutor::spin_some_impl(std::chrono::nanoseconds max_durati // Execute ready executables bool work_available = this->execute_ready_executables( current_collection_, - wait_result.value(), false); + wait_result.value(), + false); if (!work_available || !exhaustive) { break; } @@ -158,9 +145,16 @@ bool StaticSingleThreadedExecutor::execute_ready_executables( } } - while (auto timer = wait_result.next_ready_timer()) { + size_t current_timer_index = 0; + while (true) { + auto [timer, timer_index] = wait_result.peak_next_ready_timer(current_timer_index); + if (nullptr == timer) { + break; + } + current_timer_index = timer_index; auto entity_iter = collection.timers.find(timer->get_timer_handle().get()); if (entity_iter != collection.timers.end()) { + wait_result.clear_timer_with_index(current_timer_index); if (timer->call()) { execute_timer(timer); any_ready_executable = true; diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 2a310916e8..0d5e97c229 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -172,7 +172,6 @@ TYPED_TEST(TestExecutors, spinWhileAlreadySpinning) { using ExecutorType = TypeParam; ExecutorType executor; - executor.add_node(this->node); std::atomic_bool timer_completed = false; auto timer = this->node->create_wall_timer( @@ -180,9 +179,10 @@ TYPED_TEST(TestExecutors, spinWhileAlreadySpinning) timer_completed.store(true); }); + executor.add_node(this->node); std::thread spinner([&]() {executor.spin();}); - // Sleep for a short time to verify executor.spin() is going, and didn't throw. + // Sleep for a short time to verify executor.spin() is going, and didn't throw. auto start = std::chrono::steady_clock::now(); while (!timer_completed.load() && (std::chrono::steady_clock::now() - start) < 10s) { std::this_thread::sleep_for(1ms); @@ -339,11 +339,18 @@ class TestWaitable : public rclcpp::Waitable void add_to_wait_set(rcl_wait_set_t * wait_set) override { + if (trigger_count_ > 0) { + // Keep the gc triggered until the trigger count is reduced back to zero. + // This is necessary if trigger() results in the wait set waking, but not + // executing this waitable, in which case it needs to be re-triggered. + gc_.trigger(); + } rclcpp::detail::add_guard_condition_to_rcl_wait_set(*wait_set, gc_); } void trigger() { + trigger_count_++; gc_.trigger(); } @@ -371,6 +378,7 @@ class TestWaitable : public rclcpp::Waitable execute(std::shared_ptr & data) override { (void) data; + trigger_count_--; count_++; std::this_thread::sleep_for(3ms); } @@ -400,6 +408,7 @@ class TestWaitable : public rclcpp::Waitable } private: + std::atomic trigger_count_ = 0; std::atomic count_ = 0; rclcpp::GuardCondition gc_; }; @@ -461,7 +470,7 @@ TYPED_TEST(TestExecutors, spinSome) std::thread spinner([&spin_exited, &executor, this]() { executor.spin_some(1s); executor.remove_node(this->node, true); - spin_exited = true; + spin_exited.store(true); }); // Do some work until sufficient calls to the waitable occur, but keep going until either @@ -469,8 +478,8 @@ TYPED_TEST(TestExecutors, spinSome) auto start = std::chrono::steady_clock::now(); while ( my_waitable->get_count() <= 1 && - !spin_exited && - (std::chrono::steady_clock::now() - start < 1s)) + !spin_exited.load() && + std::chrono::steady_clock::now() - start < 10s) { my_waitable->trigger(); this->publisher->publish(test_msgs::msg::Empty()); @@ -480,7 +489,7 @@ TYPED_TEST(TestExecutors, spinSome) // the first iteration of the while loop EXPECT_LE(1u, my_waitable->get_count()); waitable_interfaces->remove_waitable(my_waitable, nullptr); - EXPECT_TRUE(spin_exited); + EXPECT_TRUE(spin_exited.load()); // Cancel if it hasn't exited already. executor.cancel(); diff --git a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp index 9d52d14035..27bdf2482c 100644 --- a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp +++ b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp @@ -204,10 +204,11 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups_after_add_node_t auto node = std::make_shared("my_node", "/ns"); executor.add_node(node->get_node_base_interface()); ASSERT_EQ(executor.get_all_callback_groups().size(), 1u); - std::atomic_int timer_count {0}; + std::atomic_size_t timer_count {0}; auto timer_callback = [&executor, &timer_count]() { + printf("in timer_callback(%zu)\n", timer_count.load()); if (timer_count > 0) { - ASSERT_EQ(executor.get_all_callback_groups().size(), 3u); + ASSERT_GT(executor.get_all_callback_groups().size(), 1u); executor.cancel(); } timer_count++; @@ -215,7 +216,7 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups_after_add_node_t rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive); rclcpp::TimerBase::SharedPtr timer_ = node->create_wall_timer( - 2s, timer_callback, cb_grp); + 1s, timer_callback, cb_grp); rclcpp::CallbackGroup::SharedPtr cb_grp2 = node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false); auto timer2_callback = []() {}; @@ -227,6 +228,7 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups_after_add_node_t rclcpp::TimerBase::SharedPtr timer3_ = node->create_wall_timer( 2s, timer3_callback, cb_grp3); executor.spin(); + ASSERT_GT(timer_count.load(), 0u); } /* From 15f321f1151335d8391d68275241ecda8d0c5d87 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 20 Mar 2024 16:01:27 -0700 Subject: [PATCH 71/74] peak -> peek Signed-off-by: William Woodall --- rclcpp/include/rclcpp/wait_result.hpp | 6 +++--- rclcpp/src/rclcpp/executor.cpp | 4 ++-- .../rclcpp/executors/static_single_threaded_executor.cpp | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/rclcpp/include/rclcpp/wait_result.hpp b/rclcpp/include/rclcpp/wait_result.hpp index 7b87c25c1c..2bb29cbd19 100644 --- a/rclcpp/include/rclcpp/wait_result.hpp +++ b/rclcpp/include/rclcpp/wait_result.hpp @@ -165,7 +165,7 @@ class WaitResult final * {nullptr, start_index} if none was found. */ std::pair, size_t> - peak_next_ready_timer(size_t start_index = 0) + peek_next_ready_timer(size_t start_index = 0) { check_wait_result_dirty(); auto ret = std::shared_ptr{nullptr}; @@ -186,9 +186,9 @@ class WaitResult final /// Clear the timer at the given index. /** * Clearing a timer from the wait result prevents it from being returned by - * the peak_next_ready_timer() on subsequent calls. + * the peek_next_ready_timer() on subsequent calls. * - * The index should come from the peak_next_ready_timer() function, and + * The index should come from the peek_next_ready_timer() function, and * should only be used with this function if the timer pointer was valid. * * \throws std::out_of_range if the given index is out of range diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 27568c218a..4351efb10a 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -675,7 +675,7 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable) if (!valid_executable) { size_t current_timer_index = 0; while (true) { - auto [timer, timer_index] = wait_result_->peak_next_ready_timer(current_timer_index); + auto [timer, timer_index] = wait_result_->peek_next_ready_timer(current_timer_index); if (nullptr == timer) { break; } @@ -688,7 +688,7 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable) } // At this point the timer is either ready for execution or was perhaps // it was canceled, based on the result of call(), but either way it - // should not be checked again from peak_next_ready_timer(), so clear + // should not be checked again from peek_next_ready_timer(), so clear // it from the wait result. wait_result_->clear_timer_with_index(current_timer_index); // Check that the timer should be called still, i.e. it wasn't canceled. diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp index 4ddefc11f4..dce22bd441 100644 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp @@ -147,7 +147,7 @@ bool StaticSingleThreadedExecutor::execute_ready_executables( size_t current_timer_index = 0; while (true) { - auto [timer, timer_index] = wait_result.peak_next_ready_timer(current_timer_index); + auto [timer, timer_index] = wait_result.peek_next_ready_timer(current_timer_index); if (nullptr == timer) { break; } From 836946e23f2f1c5b6e71fc985b98c279b1c0d9a0 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 26 Mar 2024 04:03:53 -0700 Subject: [PATCH 72/74] fix bug in next_waitable logic Signed-off-by: William Woodall --- rclcpp/include/rclcpp/wait_result.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/wait_result.hpp b/rclcpp/include/rclcpp/wait_result.hpp index 2bb29cbd19..429eb1dd25 100644 --- a/rclcpp/include/rclcpp/wait_result.hpp +++ b/rclcpp/include/rclcpp/wait_result.hpp @@ -276,11 +276,11 @@ class WaitResult final auto & wait_set = this->get_wait_set(); auto rcl_wait_set = wait_set.get_rcl_wait_set(); while (next_waitable_index_ < wait_set.size_of_waitables()) { - auto cur_waitable = wait_set.waitables(next_waitable_index_); + auto cur_waitable = wait_set.waitables(next_waitable_index_++); if (cur_waitable != nullptr && cur_waitable->is_ready(&rcl_wait_set)) { waitable = cur_waitable; + break; } - next_waitable_index_++; } } From 2a88295fbac788f90cdf955608f5a00c5ef774e7 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 26 Mar 2024 05:58:15 -0700 Subject: [PATCH 73/74] fix bug in StaticSTE that broke the add callback groups to executor tests Signed-off-by: William Woodall --- rclcpp/src/rclcpp/executor.cpp | 4 ++++ .../executors/executor_entities_collector.cpp | 18 ++++++++++++++++++ .../static_single_threaded_executor.cpp | 1 - .../test_add_callback_groups_to_executor.cpp | 17 ++++++++++++----- 4 files changed, 34 insertions(+), 6 deletions(-) diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 4351efb10a..8200dc86a2 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -72,6 +72,10 @@ Executor::Executor(const rclcpp::ExecutorOptions & options) } }); + notify_waitable_->set_on_ready_callback([this](auto, auto) { + this->entities_need_rebuild_.store(true); + }); + notify_waitable_->add_guard_condition(interrupt_guard_condition_); notify_waitable_->add_guard_condition(shutdown_guard_condition_); } diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp index 3bcf831493..702716a758 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp @@ -61,6 +61,12 @@ ExecutorEntitiesCollector::~ExecutorEntitiesCollector() if (group_ptr) { group_ptr->get_associated_with_executor_atomic().store(false); } + // Disassociate the guard condition from the executor notify waitable + auto guard_condition_it = weak_groups_to_guard_conditions_.find(weak_group_ptr); + 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); + } } pending_manually_added_groups_.clear(); pending_manually_removed_groups_.clear(); @@ -144,6 +150,11 @@ ExecutorEntitiesCollector::add_callback_group(rclcpp::CallbackGroup::SharedPtr g } this->pending_manually_added_groups_.insert(group_ptr); + + // Store callback group notify guard condition in map and add it to the notify waitable + 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); } void @@ -341,6 +352,13 @@ ExecutorEntitiesCollector::process_queues() auto group_ptr = weak_group_ptr.lock(); if (group_ptr) { this->add_callback_group_to_collection(group_ptr, manually_added_groups_); + } else { + // Disassociate the guard condition from the executor notify waitable + auto guard_condition_it = weak_groups_to_guard_conditions_.find(weak_group_ptr); + 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); + } } } pending_manually_added_groups_.clear(); diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp index dce22bd441..3348f422f9 100644 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp @@ -13,7 +13,6 @@ // limitations under the License. #include "rclcpp/executors/executor_entities_collection.hpp" -#include "rclcpp/executors/executor_notify_waitable.hpp" #include "rcpputils/scope_exit.hpp" #include "rclcpp/executors/static_single_threaded_executor.hpp" diff --git a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp index 27bdf2482c..c2bb9cd8fb 100644 --- a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp +++ b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp @@ -200,18 +200,25 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups_after_add_node_t { using ExecutorType = TypeParam; + auto count_callback_groups_in_node = [](auto node) { + size_t num = 0; + node->get_node_base_interface()->for_each_callback_group([&num](auto) { + num++; + }); + return num; + }; + ExecutorType executor; auto node = std::make_shared("my_node", "/ns"); executor.add_node(node->get_node_base_interface()); - ASSERT_EQ(executor.get_all_callback_groups().size(), 1u); + ASSERT_EQ(executor.get_all_callback_groups().size(), count_callback_groups_in_node(node)); std::atomic_size_t timer_count {0}; auto timer_callback = [&executor, &timer_count]() { - printf("in timer_callback(%zu)\n", timer_count.load()); - if (timer_count > 0) { - ASSERT_GT(executor.get_all_callback_groups().size(), 1u); + auto cur_timer_count = timer_count++; + printf("in timer_callback(%zu)\n", cur_timer_count); + if (cur_timer_count > 0) { executor.cancel(); } - timer_count++; }; rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive); From 245bb50cfb7be8407c0fcfa3c514fb50dfb6c7e8 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 26 Mar 2024 06:00:08 -0700 Subject: [PATCH 74/74] style Signed-off-by: William Woodall --- rclcpp/src/rclcpp/executor.cpp | 7 ++++--- .../test/rclcpp/test_add_callback_groups_to_executor.cpp | 3 ++- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 8200dc86a2..776e6de4ea 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -72,9 +72,10 @@ Executor::Executor(const rclcpp::ExecutorOptions & options) } }); - notify_waitable_->set_on_ready_callback([this](auto, auto) { - this->entities_need_rebuild_.store(true); - }); + notify_waitable_->set_on_ready_callback( + [this](auto, auto) { + this->entities_need_rebuild_.store(true); + }); notify_waitable_->add_guard_condition(interrupt_guard_condition_); notify_waitable_->add_guard_condition(shutdown_guard_condition_); diff --git a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp index c2bb9cd8fb..fefec6c8fa 100644 --- a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp +++ b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp @@ -202,7 +202,8 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups_after_add_node_t auto count_callback_groups_in_node = [](auto node) { size_t num = 0; - node->get_node_base_interface()->for_each_callback_group([&num](auto) { + node->get_node_base_interface()->for_each_callback_group( + [&num](auto) { num++; }); return num;