15 #ifndef RCLCPP__STRATEGIES__ALLOCATOR_MEMORY_STRATEGY_HPP_
16 #define RCLCPP__STRATEGIES__ALLOCATOR_MEMORY_STRATEGY_HPP_
21 #include "rcl/allocator.h"
34 namespace memory_strategies
36 namespace allocator_memory_strategy
45 template<
typename Alloc = std::allocator<
void>>
56 allocator_ = std::make_shared<VoidAlloc>(*allocator.get());
61 allocator_ = std::make_shared<VoidAlloc>();
66 for (
const auto & existing_guard_condition : guard_conditions_) {
67 if (existing_guard_condition == guard_condition) {
71 guard_conditions_.push_back(guard_condition);
76 for (
auto it = guard_conditions_.begin(); it != guard_conditions_.end(); ++it) {
77 if (*it == guard_condition) {
78 guard_conditions_.erase(it);
86 subscription_handles_.clear();
87 service_handles_.clear();
88 client_handles_.clear();
89 timer_handles_.clear();
90 waitable_handles_.clear();
101 for (
size_t i = 0; i < subscription_handles_.size(); ++i) {
103 subscription_handles_[i].reset();
106 for (
size_t i = 0; i < service_handles_.size(); ++i) {
108 service_handles_[i].reset();
111 for (
size_t i = 0; i < client_handles_.size(); ++i) {
113 client_handles_[i].reset();
116 for (
size_t i = 0; i < timer_handles_.size(); ++i) {
117 if (!wait_set->
timers[i]) {
118 timer_handles_[i].reset();
121 for (
size_t i = 0; i < waitable_handles_.size(); ++i) {
122 if (!waitable_handles_[i]->is_ready(wait_set)) {
123 waitable_handles_[i].reset();
127 subscription_handles_.erase(
128 std::remove(subscription_handles_.begin(), subscription_handles_.end(),
nullptr),
129 subscription_handles_.end()
132 service_handles_.erase(
133 std::remove(service_handles_.begin(), service_handles_.end(),
nullptr),
134 service_handles_.end()
137 client_handles_.erase(
138 std::remove(client_handles_.begin(), client_handles_.end(),
nullptr),
139 client_handles_.end()
142 timer_handles_.erase(
143 std::remove(timer_handles_.begin(), timer_handles_.end(),
nullptr),
147 waitable_handles_.erase(
148 std::remove(waitable_handles_.begin(), waitable_handles_.end(),
nullptr),
149 waitable_handles_.end()
155 bool has_invalid_weak_nodes =
false;
156 for (
auto & weak_node : weak_nodes) {
157 auto node = weak_node.lock();
159 has_invalid_weak_nodes =
true;
162 for (
auto & weak_group : node->get_callback_groups()) {
163 auto group = weak_group.lock();
164 if (!group || !group->can_be_taken_from().load()) {
167 group->find_subscription_ptrs_if(
168 [
this](
const rclcpp::SubscriptionBase::SharedPtr & subscription) {
169 subscription_handles_.push_back(subscription->get_subscription_handle());
172 group->find_service_ptrs_if(
173 [
this](
const rclcpp::ServiceBase::SharedPtr & service) {
174 service_handles_.push_back(service->get_service_handle());
177 group->find_client_ptrs_if(
178 [
this](
const rclcpp::ClientBase::SharedPtr & client) {
179 client_handles_.push_back(client->get_client_handle());
182 group->find_timer_ptrs_if(
183 [
this](
const rclcpp::TimerBase::SharedPtr & timer) {
184 timer_handles_.push_back(timer->get_timer_handle());
187 group->find_waitable_ptrs_if(
188 [
this](
const rclcpp::Waitable::SharedPtr & waitable) {
189 waitable_handles_.push_back(waitable);
194 return has_invalid_weak_nodes;
199 if (
nullptr == waitable) {
202 waitable_handles_.push_back(waitable);
207 for (
auto subscription : subscription_handles_) {
208 if (rcl_wait_set_add_subscription(wait_set, subscription.get(), NULL) != RCL_RET_OK) {
211 "Couldn't add subscription to wait set: %s", rcl_get_error_string().str);
216 for (
auto client : client_handles_) {
217 if (rcl_wait_set_add_client(wait_set, client.get(), NULL) != RCL_RET_OK) {
220 "Couldn't add client to wait set: %s", rcl_get_error_string().str);
225 for (
auto service : service_handles_) {
226 if (rcl_wait_set_add_service(wait_set, service.get(), NULL) != RCL_RET_OK) {
229 "Couldn't add service to wait set: %s", rcl_get_error_string().str);
234 for (
auto timer : timer_handles_) {
235 if (rcl_wait_set_add_timer(wait_set, timer.get(), NULL) != RCL_RET_OK) {
238 "Couldn't add timer to wait set: %s", rcl_get_error_string().str);
243 for (
auto guard_condition : guard_conditions_) {
244 if (rcl_wait_set_add_guard_condition(wait_set, guard_condition, NULL) != RCL_RET_OK) {
247 "Couldn't add guard_condition to wait set: %s",
248 rcl_get_error_string().str);
253 for (
auto waitable : waitable_handles_) {
254 if (!waitable->add_to_wait_set(wait_set)) {
257 "Couldn't add waitable to wait set: %s", rcl_get_error_string().str);
269 auto it = subscription_handles_.begin();
270 while (it != subscription_handles_.end()) {
278 it = subscription_handles_.erase(it);
281 if (!group->can_be_taken_from().load()) {
291 subscription_handles_.erase(it);
295 it = subscription_handles_.erase(it);
304 auto it = service_handles_.begin();
305 while (it != service_handles_.end()) {
313 it = service_handles_.erase(it);
316 if (!group->can_be_taken_from().load()) {
326 service_handles_.erase(it);
330 it = service_handles_.erase(it);
337 auto it = client_handles_.begin();
338 while (it != client_handles_.end()) {
346 it = client_handles_.erase(it);
349 if (!group->can_be_taken_from().load()) {
359 client_handles_.erase(it);
363 it = client_handles_.erase(it);
372 auto it = timer_handles_.begin();
373 while (it != timer_handles_.end()) {
381 it = timer_handles_.erase(it);
384 if (!group->can_be_taken_from().load()) {
391 any_exec.
timer = timer;
394 timer_handles_.erase(it);
398 it = timer_handles_.erase(it);
405 auto it = waitable_handles_.begin();
406 while (it != waitable_handles_.end()) {
414 it = waitable_handles_.erase(it);
417 if (!group->can_be_taken_from().load()) {
427 waitable_handles_.erase(it);
431 it = waitable_handles_.erase(it);
437 return rclcpp::allocator::get_rcl_allocator<void *, VoidAlloc>(*allocator_.
get());
442 size_t number_of_subscriptions = subscription_handles_.size();
443 for (
auto waitable : waitable_handles_) {
444 number_of_subscriptions += waitable->get_number_of_ready_subscriptions();
446 return number_of_subscriptions;
451 size_t number_of_services = service_handles_.size();
452 for (
auto waitable : waitable_handles_) {
453 number_of_services += waitable->get_number_of_ready_services();
455 return number_of_services;
460 size_t number_of_events = 0;
461 for (
auto waitable : waitable_handles_) {
462 number_of_events += waitable->get_number_of_ready_events();
464 return number_of_events;
469 size_t number_of_clients = client_handles_.size();
470 for (
auto waitable : waitable_handles_) {
471 number_of_clients += waitable->get_number_of_ready_clients();
473 return number_of_clients;
479 for (
auto waitable : waitable_handles_) {
487 size_t number_of_timers = timer_handles_.size();
488 for (
auto waitable : waitable_handles_) {
489 number_of_timers += waitable->get_number_of_ready_timers();
491 return number_of_timers;
496 return waitable_handles_.size();
504 VectorRebind<const rcl_guard_condition_t *> guard_conditions_;
506 VectorRebind<std::shared_ptr<const rcl_subscription_t>> subscription_handles_;
507 VectorRebind<std::shared_ptr<const rcl_service_t>> service_handles_;
508 VectorRebind<std::shared_ptr<const rcl_client_t>> client_handles_;
509 VectorRebind<std::shared_ptr<const rcl_timer_t>> timer_handles_;
510 VectorRebind<std::shared_ptr<Waitable>> waitable_handles_;
519 #endif // RCLCPP__STRATEGIES__ALLOCATOR_MEMORY_STRATEGY_HPP_