Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Micro-optimizations in rclcpp #1896

Merged
merged 3 commits into from
Mar 9, 2022
Merged

Micro-optimizations in rclcpp #1896

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
cf72ad1
Use a single call to collect all CallbackGroup pointers.
clalancette Mar 8, 2022
b7d34c4
Pass shared_ptr as constref in the MemoryStrategy class.
clalancette Mar 8, 2022
132aeea
Use constref shared_ptr when iterating.
clalancette Mar 8, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 8 additions & 0 deletions rclcpp/include/rclcpp/callback_group.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -138,6 +138,14 @@ class CallbackGroup
const CallbackGroupType &
type() const;

RCLCPP_PUBLIC
void collect_all_ptrs(
std::function<void(const rclcpp::SubscriptionBase::SharedPtr &)> sub_func,
std::function<void(const rclcpp::ServiceBase::SharedPtr &)> service_func,
std::function<void(const rclcpp::ClientBase::SharedPtr &)> client_func,
std::function<void(const rclcpp::TimerBase::SharedPtr &)> timer_func,
std::function<void(const rclcpp::Waitable::SharedPtr &)> 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
Expand Down
20 changes: 10 additions & 10 deletions rclcpp/include/rclcpp/memory_strategy.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -100,52 +100,52 @@ class RCLCPP_PUBLIC MemoryStrategy

static rclcpp::SubscriptionBase::SharedPtr
get_subscription_by_handle(
std::shared_ptr<const rcl_subscription_t> subscriber_handle,
const std::shared_ptr<const rcl_subscription_t> & subscriber_handle,
const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);

static rclcpp::ServiceBase::SharedPtr
get_service_by_handle(
std::shared_ptr<const rcl_service_t> service_handle,
const std::shared_ptr<const rcl_service_t> & service_handle,
const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);

static rclcpp::ClientBase::SharedPtr
get_client_by_handle(
std::shared_ptr<const rcl_client_t> client_handle,
const std::shared_ptr<const rcl_client_t> & client_handle,
const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);

static rclcpp::TimerBase::SharedPtr
get_timer_by_handle(
std::shared_ptr<const rcl_timer_t> timer_handle,
const std::shared_ptr<const rcl_timer_t> & 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);
};

Expand Down
44 changes: 18 additions & 26 deletions rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
});
}

Expand All @@ -204,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<const rcl_subscription_t> & subscription : subscription_handles_) {
if (rcl_wait_set_add_subscription(wait_set, subscription.get(), NULL) != RCL_RET_OK) {
RCUTILS_LOG_ERROR_NAMED(
"rclcpp",
Expand All @@ -213,7 +205,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
}
}

for (auto client : client_handles_) {
for (const std::shared_ptr<const rcl_client_t> & client : client_handles_) {
if (rcl_wait_set_add_client(wait_set, client.get(), NULL) != RCL_RET_OK) {
RCUTILS_LOG_ERROR_NAMED(
"rclcpp",
Expand All @@ -222,7 +214,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
}
}

for (auto service : service_handles_) {
for (const std::shared_ptr<const rcl_service_t> & service : service_handles_) {
if (rcl_wait_set_add_service(wait_set, service.get(), NULL) != RCL_RET_OK) {
RCUTILS_LOG_ERROR_NAMED(
"rclcpp",
Expand All @@ -231,7 +223,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
}
}

for (auto timer : timer_handles_) {
for (const std::shared_ptr<const rcl_timer_t> & timer : timer_handles_) {
if (rcl_wait_set_add_timer(wait_set, timer.get(), NULL) != RCL_RET_OK) {
RCUTILS_LOG_ERROR_NAMED(
"rclcpp",
Expand All @@ -244,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 : waitable_handles_) {
waitable->add_to_wait_set(wait_set);
}
return true;
Expand Down Expand Up @@ -402,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> & 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);
Expand Down Expand Up @@ -438,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 : waitable_handles_) {
number_of_subscriptions += waitable->get_number_of_ready_subscriptions();
}
return number_of_subscriptions;
Expand All @@ -447,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 : waitable_handles_) {
number_of_services += waitable->get_number_of_ready_services();
}
return number_of_services;
Expand All @@ -456,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 : waitable_handles_) {
number_of_events += waitable->get_number_of_ready_events();
}
return number_of_events;
Expand All @@ -465,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 : waitable_handles_) {
number_of_clients += waitable->get_number_of_ready_clients();
}
return number_of_clients;
Expand All @@ -474,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 : waitable_handles_) {
number_of_guard_conditions += waitable->get_number_of_ready_guard_conditions();
}
return number_of_guard_conditions;
Expand All @@ -483,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 : waitable_handles_) {
number_of_timers += waitable->get_number_of_ready_timers();
}
return number_of_timers;
Expand Down
45 changes: 45 additions & 0 deletions rclcpp/src/rclcpp/callback_group.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,51 @@ CallbackGroup::type() const
return type_;
}

void CallbackGroup::collect_all_ptrs(
std::function<void(const rclcpp::SubscriptionBase::SharedPtr &)> sub_func,
std::function<void(const rclcpp::ServiceBase::SharedPtr &)> service_func,
std::function<void(const rclcpp::ClientBase::SharedPtr &)> client_func,
std::function<void(const rclcpp::TimerBase::SharedPtr &)> timer_func,
std::function<void(const rclcpp::Waitable::SharedPtr &)> waitable_func) const
{
std::lock_guard<std::mutex> 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()
{
Expand Down
20 changes: 10 additions & 10 deletions rclcpp/src/rclcpp/memory_strategy.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@ using rclcpp::memory_strategy::MemoryStrategy;

rclcpp::SubscriptionBase::SharedPtr
MemoryStrategy::get_subscription_by_handle(
std::shared_ptr<const rcl_subscription_t> subscriber_handle,
const std::shared_ptr<const rcl_subscription_t> & subscriber_handle,
const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
{
for (const auto & pair : weak_groups_to_nodes) {
Expand All @@ -40,7 +40,7 @@ MemoryStrategy::get_subscription_by_handle(

rclcpp::ServiceBase::SharedPtr
MemoryStrategy::get_service_by_handle(
std::shared_ptr<const rcl_service_t> service_handle,
const std::shared_ptr<const rcl_service_t> & service_handle,
const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
{
for (const auto & pair : weak_groups_to_nodes) {
Expand All @@ -61,7 +61,7 @@ MemoryStrategy::get_service_by_handle(

rclcpp::ClientBase::SharedPtr
MemoryStrategy::get_client_by_handle(
std::shared_ptr<const rcl_client_t> client_handle,
const std::shared_ptr<const rcl_client_t> & client_handle,
const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
{
for (const auto & pair : weak_groups_to_nodes) {
Expand All @@ -82,7 +82,7 @@ MemoryStrategy::get_client_by_handle(

rclcpp::TimerBase::SharedPtr
MemoryStrategy::get_timer_by_handle(
std::shared_ptr<const rcl_timer_t> timer_handle,
const std::shared_ptr<const rcl_timer_t> & timer_handle,
const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
{
for (const auto & pair : weak_groups_to_nodes) {
Expand All @@ -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) {
Expand All @@ -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) {
Expand All @@ -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) {
Expand All @@ -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) {
Expand All @@ -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) {
Expand All @@ -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) {
Expand Down