From cf72ad11ae5cd1b2c7a9a70bfcb147fa62e1e3ee Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 8 Mar 2022 09:08:19 -0500 Subject: [PATCH 1/3] Use a single call to collect all CallbackGroup pointers. Believe it or not, the taking and releasing of mutex_ within the CallbackGroup class shows up in profiles. However, when collecting entities we really don't need to take it and drop it between each of subscriptions, services, clients, timers, and waitables. Just take it once at the beginning, collect everything, and then return, which removes a lot of unnecessary work with that mutex. Signed-off-by: Chris Lalancette --- rclcpp/include/rclcpp/callback_group.hpp | 8 ++++ .../strategies/allocator_memory_strategy.hpp | 20 +++------ rclcpp/src/rclcpp/callback_group.cpp | 45 +++++++++++++++++++ 3 files changed, 59 insertions(+), 14 deletions(-) diff --git a/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp index efa8352206..94bceced81 100644 --- a/rclcpp/include/rclcpp/callback_group.hpp +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -138,6 +138,14 @@ class CallbackGroup const CallbackGroupType & type() const; + RCLCPP_PUBLIC + void collect_all_ptrs( + std::function sub_func, + std::function service_func, + std::function client_func, + std::function timer_func, + std::function waitable_func) const; + /// Return a reference to the 'associated with executor' atomic boolean. /** * When a callback group is added to an executor this boolean is checked diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index d0bb9c7df2..05c4f1d4eb 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -164,30 +164,22 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy if (!group || !group->can_be_taken_from().load()) { continue; } - group->find_subscription_ptrs_if( + + group->collect_all_ptrs( [this](const rclcpp::SubscriptionBase::SharedPtr & subscription) { subscription_handles_.push_back(subscription->get_subscription_handle()); - return false; - }); - group->find_service_ptrs_if( + }, [this](const rclcpp::ServiceBase::SharedPtr & service) { service_handles_.push_back(service->get_service_handle()); - return false; - }); - group->find_client_ptrs_if( + }, [this](const rclcpp::ClientBase::SharedPtr & client) { client_handles_.push_back(client->get_client_handle()); - return false; - }); - group->find_timer_ptrs_if( + }, [this](const rclcpp::TimerBase::SharedPtr & timer) { timer_handles_.push_back(timer->get_timer_handle()); - return false; - }); - group->find_waitable_ptrs_if( + }, [this](const rclcpp::Waitable::SharedPtr & waitable) { waitable_handles_.push_back(waitable); - return false; }); } diff --git a/rclcpp/src/rclcpp/callback_group.cpp b/rclcpp/src/rclcpp/callback_group.cpp index 6b38578ba9..4b11156cf9 100644 --- a/rclcpp/src/rclcpp/callback_group.cpp +++ b/rclcpp/src/rclcpp/callback_group.cpp @@ -40,6 +40,51 @@ CallbackGroup::type() const return type_; } +void CallbackGroup::collect_all_ptrs( + std::function sub_func, + std::function service_func, + std::function client_func, + std::function timer_func, + std::function waitable_func) const +{ + std::lock_guard lock(mutex_); + + for (const rclcpp::SubscriptionBase::WeakPtr & weak_ptr : subscription_ptrs_) { + rclcpp::SubscriptionBase::SharedPtr ref_ptr = weak_ptr.lock(); + if (ref_ptr) { + sub_func(ref_ptr); + } + } + + for (const rclcpp::ServiceBase::WeakPtr & weak_ptr : service_ptrs_) { + rclcpp::ServiceBase::SharedPtr ref_ptr = weak_ptr.lock(); + if (ref_ptr) { + service_func(ref_ptr); + } + } + + for (const rclcpp::ClientBase::WeakPtr & weak_ptr : client_ptrs_) { + rclcpp::ClientBase::SharedPtr ref_ptr = weak_ptr.lock(); + if (ref_ptr) { + client_func(ref_ptr); + } + } + + for (const rclcpp::TimerBase::WeakPtr & weak_ptr : timer_ptrs_) { + rclcpp::TimerBase::SharedPtr ref_ptr = weak_ptr.lock(); + if (ref_ptr) { + timer_func(ref_ptr); + } + } + + for (const rclcpp::Waitable::WeakPtr & weak_ptr : waitable_ptrs_) { + rclcpp::Waitable::SharedPtr ref_ptr = weak_ptr.lock(); + if (ref_ptr) { + waitable_func(ref_ptr); + } + } +} + std::atomic_bool & CallbackGroup::get_associated_with_executor_atomic() { From b7d34c4b5b51ccd96eacd2e16a7f765081b83723 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 8 Mar 2022 09:11:40 -0500 Subject: [PATCH 2/3] Pass shared_ptr as constref in the MemoryStrategy class. That way, in the case that the callee doesn't need to take a reference, we avoid creating and destroying a shared_ptr class. This reduces the overhead in using these functions, and seems to work fine in the default case. If a user wants to subclass the MemoryStrategy class, then they can explicitly create a shared_ptr copy by using the copy constructor. Signed-off-by: Chris Lalancette --- rclcpp/include/rclcpp/memory_strategy.hpp | 20 ++++++++++---------- rclcpp/src/rclcpp/memory_strategy.cpp | 20 ++++++++++---------- 2 files changed, 20 insertions(+), 20 deletions(-) diff --git a/rclcpp/include/rclcpp/memory_strategy.hpp b/rclcpp/include/rclcpp/memory_strategy.hpp index 0993c223c9..fb5ba2a63f 100644 --- a/rclcpp/include/rclcpp/memory_strategy.hpp +++ b/rclcpp/include/rclcpp/memory_strategy.hpp @@ -100,52 +100,52 @@ class RCLCPP_PUBLIC MemoryStrategy static rclcpp::SubscriptionBase::SharedPtr get_subscription_by_handle( - std::shared_ptr subscriber_handle, + const std::shared_ptr & subscriber_handle, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); static rclcpp::ServiceBase::SharedPtr get_service_by_handle( - std::shared_ptr service_handle, + const std::shared_ptr & service_handle, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); static rclcpp::ClientBase::SharedPtr get_client_by_handle( - std::shared_ptr client_handle, + const std::shared_ptr & client_handle, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); static rclcpp::TimerBase::SharedPtr get_timer_by_handle( - std::shared_ptr timer_handle, + const std::shared_ptr & timer_handle, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); static rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_by_group( - rclcpp::CallbackGroup::SharedPtr group, + const rclcpp::CallbackGroup::SharedPtr & group, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); static rclcpp::CallbackGroup::SharedPtr get_group_by_subscription( - rclcpp::SubscriptionBase::SharedPtr subscription, + const rclcpp::SubscriptionBase::SharedPtr & subscription, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); static rclcpp::CallbackGroup::SharedPtr get_group_by_service( - rclcpp::ServiceBase::SharedPtr service, + const rclcpp::ServiceBase::SharedPtr & service, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); static rclcpp::CallbackGroup::SharedPtr get_group_by_client( - rclcpp::ClientBase::SharedPtr client, + const rclcpp::ClientBase::SharedPtr & client, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); static rclcpp::CallbackGroup::SharedPtr get_group_by_timer( - rclcpp::TimerBase::SharedPtr timer, + const rclcpp::TimerBase::SharedPtr & timer, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); static rclcpp::CallbackGroup::SharedPtr get_group_by_waitable( - rclcpp::Waitable::SharedPtr waitable, + const rclcpp::Waitable::SharedPtr & waitable, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); }; diff --git a/rclcpp/src/rclcpp/memory_strategy.cpp b/rclcpp/src/rclcpp/memory_strategy.cpp index bd32bdb169..cb69dc0d26 100644 --- a/rclcpp/src/rclcpp/memory_strategy.cpp +++ b/rclcpp/src/rclcpp/memory_strategy.cpp @@ -19,7 +19,7 @@ using rclcpp::memory_strategy::MemoryStrategy; rclcpp::SubscriptionBase::SharedPtr MemoryStrategy::get_subscription_by_handle( - std::shared_ptr subscriber_handle, + const std::shared_ptr & subscriber_handle, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { for (const auto & pair : weak_groups_to_nodes) { @@ -40,7 +40,7 @@ MemoryStrategy::get_subscription_by_handle( rclcpp::ServiceBase::SharedPtr MemoryStrategy::get_service_by_handle( - std::shared_ptr service_handle, + const std::shared_ptr & service_handle, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { for (const auto & pair : weak_groups_to_nodes) { @@ -61,7 +61,7 @@ MemoryStrategy::get_service_by_handle( rclcpp::ClientBase::SharedPtr MemoryStrategy::get_client_by_handle( - std::shared_ptr client_handle, + const std::shared_ptr & client_handle, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { for (const auto & pair : weak_groups_to_nodes) { @@ -82,7 +82,7 @@ MemoryStrategy::get_client_by_handle( rclcpp::TimerBase::SharedPtr MemoryStrategy::get_timer_by_handle( - std::shared_ptr timer_handle, + const std::shared_ptr & timer_handle, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { for (const auto & pair : weak_groups_to_nodes) { @@ -103,7 +103,7 @@ MemoryStrategy::get_timer_by_handle( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr MemoryStrategy::get_node_by_group( - rclcpp::CallbackGroup::SharedPtr group, + const rclcpp::CallbackGroup::SharedPtr & group, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { if (!group) { @@ -121,7 +121,7 @@ MemoryStrategy::get_node_by_group( rclcpp::CallbackGroup::SharedPtr MemoryStrategy::get_group_by_subscription( - rclcpp::SubscriptionBase::SharedPtr subscription, + const rclcpp::SubscriptionBase::SharedPtr & subscription, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { for (const auto & pair : weak_groups_to_nodes) { @@ -143,7 +143,7 @@ MemoryStrategy::get_group_by_subscription( rclcpp::CallbackGroup::SharedPtr MemoryStrategy::get_group_by_service( - rclcpp::ServiceBase::SharedPtr service, + const rclcpp::ServiceBase::SharedPtr & service, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { for (const auto & pair : weak_groups_to_nodes) { @@ -165,7 +165,7 @@ MemoryStrategy::get_group_by_service( rclcpp::CallbackGroup::SharedPtr MemoryStrategy::get_group_by_client( - rclcpp::ClientBase::SharedPtr client, + const rclcpp::ClientBase::SharedPtr & client, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { for (const auto & pair : weak_groups_to_nodes) { @@ -187,7 +187,7 @@ MemoryStrategy::get_group_by_client( rclcpp::CallbackGroup::SharedPtr MemoryStrategy::get_group_by_timer( - rclcpp::TimerBase::SharedPtr timer, + const rclcpp::TimerBase::SharedPtr & timer, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { for (const auto & pair : weak_groups_to_nodes) { @@ -209,7 +209,7 @@ MemoryStrategy::get_group_by_timer( rclcpp::CallbackGroup::SharedPtr MemoryStrategy::get_group_by_waitable( - rclcpp::Waitable::SharedPtr waitable, + const rclcpp::Waitable::SharedPtr & waitable, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { for (const auto & pair : weak_groups_to_nodes) { From 132aeead2fe73d115a48bd21dc0098c105456d89 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 8 Mar 2022 09:18:01 -0500 Subject: [PATCH 3/3] Use constref shared_ptr when iterating. Believe it or not, the creation and destruction of the shared_ptr class itself shows up in profiles in these loops. Since we don't need to hold onto a reference while iterating (the class is already doing that), just use constref wherever we can which drastically reduces the overhead. Signed-off-by: Chris Lalancette --- .../strategies/allocator_memory_strategy.hpp | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 05c4f1d4eb..88698179d4 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -196,7 +196,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy bool add_handles_to_wait_set(rcl_wait_set_t * wait_set) override { - for (auto subscription : subscription_handles_) { + for (const std::shared_ptr & subscription : subscription_handles_) { if (rcl_wait_set_add_subscription(wait_set, subscription.get(), NULL) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", @@ -205,7 +205,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } } - for (auto client : client_handles_) { + for (const std::shared_ptr & client : client_handles_) { if (rcl_wait_set_add_client(wait_set, client.get(), NULL) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", @@ -214,7 +214,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } } - for (auto service : service_handles_) { + for (const std::shared_ptr & service : service_handles_) { if (rcl_wait_set_add_service(wait_set, service.get(), NULL) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", @@ -223,7 +223,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } } - for (auto timer : timer_handles_) { + for (const std::shared_ptr & timer : timer_handles_) { if (rcl_wait_set_add_timer(wait_set, timer.get(), NULL) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", @@ -236,7 +236,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy detail::add_guard_condition_to_rcl_wait_set(*wait_set, *guard_condition); } - for (auto waitable : waitable_handles_) { + for (const std::shared_ptr & waitable : waitable_handles_) { waitable->add_to_wait_set(wait_set); } return true; @@ -394,7 +394,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy { auto it = waitable_handles_.begin(); while (it != waitable_handles_.end()) { - auto waitable = *it; + std::shared_ptr & waitable = *it; if (waitable) { // Find the group for this handle and see if it can be serviced auto group = get_group_by_waitable(waitable, weak_groups_to_nodes); @@ -430,7 +430,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy size_t number_of_ready_subscriptions() const override { size_t number_of_subscriptions = subscription_handles_.size(); - for (auto waitable : waitable_handles_) { + for (const std::shared_ptr & waitable : waitable_handles_) { number_of_subscriptions += waitable->get_number_of_ready_subscriptions(); } return number_of_subscriptions; @@ -439,7 +439,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy size_t number_of_ready_services() const override { size_t number_of_services = service_handles_.size(); - for (auto waitable : waitable_handles_) { + for (const std::shared_ptr & waitable : waitable_handles_) { number_of_services += waitable->get_number_of_ready_services(); } return number_of_services; @@ -448,7 +448,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy size_t number_of_ready_events() const override { size_t number_of_events = 0; - for (auto waitable : waitable_handles_) { + for (const std::shared_ptr & waitable : waitable_handles_) { number_of_events += waitable->get_number_of_ready_events(); } return number_of_events; @@ -457,7 +457,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy size_t number_of_ready_clients() const override { size_t number_of_clients = client_handles_.size(); - for (auto waitable : waitable_handles_) { + for (const std::shared_ptr & waitable : waitable_handles_) { number_of_clients += waitable->get_number_of_ready_clients(); } return number_of_clients; @@ -466,7 +466,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy size_t number_of_guard_conditions() const override { size_t number_of_guard_conditions = guard_conditions_.size(); - for (auto waitable : waitable_handles_) { + for (const std::shared_ptr & waitable : waitable_handles_) { number_of_guard_conditions += waitable->get_number_of_ready_guard_conditions(); } return number_of_guard_conditions; @@ -475,7 +475,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy size_t number_of_ready_timers() const override { size_t number_of_timers = timer_handles_.size(); - for (auto waitable : waitable_handles_) { + for (const std::shared_ptr & waitable : waitable_handles_) { number_of_timers += waitable->get_number_of_ready_timers(); } return number_of_timers;