rclcpp  master
C++ ROS Client Library API
allocator_memory_strategy.hpp
Go to the documentation of this file.
1 // Copyright 2015 Open Source Robotics Foundation, Inc.
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 
15 #ifndef RCLCPP__STRATEGIES__ALLOCATOR_MEMORY_STRATEGY_HPP_
16 #define RCLCPP__STRATEGIES__ALLOCATOR_MEMORY_STRATEGY_HPP_
17 
18 #include <memory>
19 #include <vector>
20 
21 #include "rcl/allocator.h"
22 
25 #include "rclcpp/node.hpp"
27 
28 #include "rcutils/logging_macros.h"
29 
30 #include "rmw/types.h"
31 
32 namespace rclcpp
33 {
34 namespace memory_strategies
35 {
36 namespace allocator_memory_strategy
37 {
38 
40 
45 template<typename Alloc = std::allocator<void>>
47 {
48 public:
50 
51  using VoidAllocTraits = typename allocator::AllocRebind<void *, Alloc>;
52  using VoidAlloc = typename VoidAllocTraits::allocator_type;
53 
54  explicit AllocatorMemoryStrategy(std::shared_ptr<Alloc> allocator)
55  {
56  allocator_ = std::make_shared<VoidAlloc>(*allocator.get());
57  }
58 
60  {
61  allocator_ = std::make_shared<VoidAlloc>();
62  }
63 
64  void add_guard_condition(const rcl_guard_condition_t * guard_condition)
65  {
66  for (const auto & existing_guard_condition : guard_conditions_) {
67  if (existing_guard_condition == guard_condition) {
68  return;
69  }
70  }
71  guard_conditions_.push_back(guard_condition);
72  }
73 
74  void remove_guard_condition(const rcl_guard_condition_t * guard_condition)
75  {
76  for (auto it = guard_conditions_.begin(); it != guard_conditions_.end(); ++it) {
77  if (*it == guard_condition) {
78  guard_conditions_.erase(it);
79  break;
80  }
81  }
82  }
83 
85  {
86  subscription_handles_.clear();
87  service_handles_.clear();
88  client_handles_.clear();
89  timer_handles_.clear();
90  waitable_handles_.clear();
91  }
92 
93  virtual void remove_null_handles(rcl_wait_set_t * wait_set)
94  {
95  // TODO(jacobperron): Check if wait set sizes are what we expect them to be?
96  // e.g. wait_set->size_of_clients == client_handles_.size()
97 
98  // Important to use subscription_handles_.size() instead of wait set's size since
99  // there may be more subscriptions in the wait set due to Waitables added to the end.
100  // The same logic applies for other entities.
101  for (size_t i = 0; i < subscription_handles_.size(); ++i) {
102  if (!wait_set->subscriptions[i]) {
103  subscription_handles_[i].reset();
104  }
105  }
106  for (size_t i = 0; i < service_handles_.size(); ++i) {
107  if (!wait_set->services[i]) {
108  service_handles_[i].reset();
109  }
110  }
111  for (size_t i = 0; i < client_handles_.size(); ++i) {
112  if (!wait_set->clients[i]) {
113  client_handles_[i].reset();
114  }
115  }
116  for (size_t i = 0; i < timer_handles_.size(); ++i) {
117  if (!wait_set->timers[i]) {
118  timer_handles_[i].reset();
119  }
120  }
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();
124  }
125  }
126 
127  subscription_handles_.erase(
128  std::remove(subscription_handles_.begin(), subscription_handles_.end(), nullptr),
129  subscription_handles_.end()
130  );
131 
132  service_handles_.erase(
133  std::remove(service_handles_.begin(), service_handles_.end(), nullptr),
134  service_handles_.end()
135  );
136 
137  client_handles_.erase(
138  std::remove(client_handles_.begin(), client_handles_.end(), nullptr),
139  client_handles_.end()
140  );
141 
142  timer_handles_.erase(
143  std::remove(timer_handles_.begin(), timer_handles_.end(), nullptr),
144  timer_handles_.end()
145  );
146 
147  waitable_handles_.erase(
148  std::remove(waitable_handles_.begin(), waitable_handles_.end(), nullptr),
149  waitable_handles_.end()
150  );
151  }
152 
153  bool collect_entities(const WeakNodeList & weak_nodes)
154  {
155  bool has_invalid_weak_nodes = false;
156  for (auto & weak_node : weak_nodes) {
157  auto node = weak_node.lock();
158  if (!node) {
159  has_invalid_weak_nodes = true;
160  continue;
161  }
162  for (auto & weak_group : node->get_callback_groups()) {
163  auto group = weak_group.lock();
164  if (!group || !group->can_be_taken_from().load()) {
165  continue;
166  }
167  for (auto & weak_subscription : group->get_subscription_ptrs()) {
168  auto subscription = weak_subscription.lock();
169  if (subscription) {
170  subscription_handles_.push_back(subscription->get_subscription_handle());
171  if (subscription->get_intra_process_subscription_handle()) {
172  subscription_handles_.push_back(
173  subscription->get_intra_process_subscription_handle());
174  }
175  }
176  }
177  for (auto & weak_service : group->get_service_ptrs()) {
178  auto service = weak_service.lock();
179  if (service) {
180  service_handles_.push_back(service->get_service_handle());
181  }
182  }
183  for (auto & weak_client : group->get_client_ptrs()) {
184  auto client = weak_client.lock();
185  if (client) {
186  client_handles_.push_back(client->get_client_handle());
187  }
188  }
189  for (auto & weak_timer : group->get_timer_ptrs()) {
190  auto timer = weak_timer.lock();
191  if (timer) {
192  timer_handles_.push_back(timer->get_timer_handle());
193  }
194  }
195  for (auto & weak_waitable : group->get_waitable_ptrs()) {
196  auto waitable = weak_waitable.lock();
197  if (waitable) {
198  waitable_handles_.push_back(waitable);
199  }
200  }
201  }
202  }
203  return has_invalid_weak_nodes;
204  }
205 
207  {
208  for (auto subscription : subscription_handles_) {
209  if (rcl_wait_set_add_subscription(wait_set, subscription.get(), NULL) != RCL_RET_OK) {
211  "rclcpp",
212  "Couldn't add subscription to wait set: %s", rcl_get_error_string().str);
213  return false;
214  }
215  }
216 
217  for (auto client : client_handles_) {
218  if (rcl_wait_set_add_client(wait_set, client.get(), NULL) != RCL_RET_OK) {
220  "rclcpp",
221  "Couldn't add client to wait set: %s", rcl_get_error_string().str);
222  return false;
223  }
224  }
225 
226  for (auto service : service_handles_) {
227  if (rcl_wait_set_add_service(wait_set, service.get(), NULL) != RCL_RET_OK) {
229  "rclcpp",
230  "Couldn't add service to wait set: %s", rcl_get_error_string().str);
231  return false;
232  }
233  }
234 
235  for (auto timer : timer_handles_) {
236  if (rcl_wait_set_add_timer(wait_set, timer.get(), NULL) != RCL_RET_OK) {
238  "rclcpp",
239  "Couldn't add timer to wait set: %s", rcl_get_error_string().str);
240  return false;
241  }
242  }
243 
244  for (auto guard_condition : guard_conditions_) {
245  if (rcl_wait_set_add_guard_condition(wait_set, guard_condition, NULL) != RCL_RET_OK) {
247  "rclcpp",
248  "Couldn't add guard_condition to wait set: %s",
249  rcl_get_error_string().str);
250  return false;
251  }
252  }
253 
254  for (auto waitable : waitable_handles_) {
255  if (!waitable->add_to_wait_set(wait_set)) {
257  "rclcpp",
258  "Couldn't add waitable to wait set: %s", rcl_get_error_string().str);
259  return false;
260  }
261  }
262  return true;
263  }
264 
265  virtual void
267  executor::AnyExecutable & any_exec,
268  const WeakNodeList & weak_nodes)
269  {
270  auto it = subscription_handles_.begin();
271  while (it != subscription_handles_.end()) {
272  auto subscription = get_subscription_by_handle(*it, weak_nodes);
273  if (subscription) {
274  // Figure out if this is for intra-process or not.
275  bool is_intra_process = false;
276  if (subscription->get_intra_process_subscription_handle()) {
277  is_intra_process = subscription->get_intra_process_subscription_handle() == *it;
278  }
279  // Find the group for this handle and see if it can be serviced
280  auto group = get_group_by_subscription(subscription, weak_nodes);
281  if (!group) {
282  // Group was not found, meaning the subscription is not valid...
283  // Remove it from the ready list and continue looking
284  it = subscription_handles_.erase(it);
285  continue;
286  }
287  if (!group->can_be_taken_from().load()) {
288  // Group is mutually exclusive and is being used, so skip it for now
289  // Leave it to be checked next time, but continue searching
290  ++it;
291  continue;
292  }
293  // Otherwise it is safe to set and return the any_exec
294  if (is_intra_process) {
295  any_exec.subscription_intra_process = subscription;
296  } else {
297  any_exec.subscription = subscription;
298  }
299  any_exec.callback_group = group;
300  any_exec.node_base = get_node_by_group(group, weak_nodes);
301  subscription_handles_.erase(it);
302  return;
303  }
304  // Else, the subscription is no longer valid, remove it and continue
305  it = subscription_handles_.erase(it);
306  }
307  }
308 
309  virtual void
311  executor::AnyExecutable & any_exec,
312  const WeakNodeList & weak_nodes)
313  {
314  auto it = service_handles_.begin();
315  while (it != service_handles_.end()) {
316  auto service = get_service_by_handle(*it, weak_nodes);
317  if (service) {
318  // Find the group for this handle and see if it can be serviced
319  auto group = get_group_by_service(service, weak_nodes);
320  if (!group) {
321  // Group was not found, meaning the service is not valid...
322  // Remove it from the ready list and continue looking
323  it = service_handles_.erase(it);
324  continue;
325  }
326  if (!group->can_be_taken_from().load()) {
327  // Group is mutually exclusive and is being used, so skip it for now
328  // Leave it to be checked next time, but continue searching
329  ++it;
330  continue;
331  }
332  // Otherwise it is safe to set and return the any_exec
333  any_exec.service = service;
334  any_exec.callback_group = group;
335  any_exec.node_base = get_node_by_group(group, weak_nodes);
336  service_handles_.erase(it);
337  return;
338  }
339  // Else, the service is no longer valid, remove it and continue
340  it = service_handles_.erase(it);
341  }
342  }
343 
344  virtual void
345  get_next_client(executor::AnyExecutable & any_exec, const WeakNodeList & weak_nodes)
346  {
347  auto it = client_handles_.begin();
348  while (it != client_handles_.end()) {
349  auto client = get_client_by_handle(*it, weak_nodes);
350  if (client) {
351  // Find the group for this handle and see if it can be serviced
352  auto group = get_group_by_client(client, weak_nodes);
353  if (!group) {
354  // Group was not found, meaning the service is not valid...
355  // Remove it from the ready list and continue looking
356  it = client_handles_.erase(it);
357  continue;
358  }
359  if (!group->can_be_taken_from().load()) {
360  // Group is mutually exclusive and is being used, so skip it for now
361  // Leave it to be checked next time, but continue searching
362  ++it;
363  continue;
364  }
365  // Otherwise it is safe to set and return the any_exec
366  any_exec.client = client;
367  any_exec.callback_group = group;
368  any_exec.node_base = get_node_by_group(group, weak_nodes);
369  client_handles_.erase(it);
370  return;
371  }
372  // Else, the service is no longer valid, remove it and continue
373  it = client_handles_.erase(it);
374  }
375  }
376 
377  virtual void
379  {
380  auto it = waitable_handles_.begin();
381  while (it != waitable_handles_.end()) {
382  auto waitable = *it;
383  if (waitable) {
384  // Find the group for this handle and see if it can be serviced
385  auto group = get_group_by_waitable(waitable, weak_nodes);
386  if (!group) {
387  // Group was not found, meaning the waitable is not valid...
388  // Remove it from the ready list and continue looking
389  it = waitable_handles_.erase(it);
390  continue;
391  }
392  if (!group->can_be_taken_from().load()) {
393  // Group is mutually exclusive and is being used, so skip it for now
394  // Leave it to be checked next time, but continue searching
395  ++it;
396  continue;
397  }
398  // Otherwise it is safe to set and return the any_exec
399  any_exec.waitable = waitable;
400  any_exec.callback_group = group;
401  any_exec.node_base = get_node_by_group(group, weak_nodes);
402  waitable_handles_.erase(it);
403  return;
404  }
405  // Else, the waitable is no longer valid, remove it and continue
406  it = waitable_handles_.erase(it);
407  }
408  }
409 
411  {
412  return rclcpp::allocator::get_rcl_allocator<void *, VoidAlloc>(*allocator_.get());
413  }
414 
416  {
417  size_t number_of_subscriptions = subscription_handles_.size();
418  for (auto waitable : waitable_handles_) {
419  number_of_subscriptions += waitable->get_number_of_ready_subscriptions();
420  }
421  return number_of_subscriptions;
422  }
423 
425  {
426  size_t number_of_services = service_handles_.size();
427  for (auto waitable : waitable_handles_) {
428  number_of_services += waitable->get_number_of_ready_services();
429  }
430  return number_of_services;
431  }
432 
433  size_t number_of_ready_events() const
434  {
435  size_t number_of_events = 0;
436  for (auto waitable : waitable_handles_) {
437  number_of_events += waitable->get_number_of_ready_events();
438  }
439  return number_of_events;
440  }
441 
442  size_t number_of_ready_clients() const
443  {
444  size_t number_of_clients = client_handles_.size();
445  for (auto waitable : waitable_handles_) {
446  number_of_clients += waitable->get_number_of_ready_clients();
447  }
448  return number_of_clients;
449  }
450 
452  {
453  size_t number_of_guard_conditions = guard_conditions_.size();
454  for (auto waitable : waitable_handles_) {
455  number_of_guard_conditions += waitable->get_number_of_ready_guard_conditions();
456  }
458  }
459 
460  size_t number_of_ready_timers() const
461  {
462  size_t number_of_timers = timer_handles_.size();
463  for (auto waitable : waitable_handles_) {
464  number_of_timers += waitable->get_number_of_ready_timers();
465  }
466  return number_of_timers;
467  }
468 
469  size_t number_of_waitables() const
470  {
471  return waitable_handles_.size();
472  }
473 
474 private:
475  template<typename T>
476  using VectorRebind =
478 
480 
485  VectorRebind<std::shared_ptr<Waitable>> waitable_handles_;
486 
487  std::shared_ptr<VoidAlloc> allocator_;
488 };
489 
490 } // namespace allocator_memory_strategy
491 } // namespace memory_strategies
492 } // namespace rclcpp
493 
494 #endif // RCLCPP__STRATEGIES__ALLOCATOR_MEMORY_STRATEGY_HPP_
#define rcl_get_error_string
const rcl_service_t ** services
rclcpp::ServiceBase::SharedPtr service
Definition: any_executable.hpp:47
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base
Definition: any_executable.hpp:52
Delegate for handling memory allocations while the Executor is executing.
Definition: memory_strategy.hpp:41
rclcpp::Waitable::SharedPtr waitable
Definition: any_executable.hpp:49
size_t number_of_guard_conditions() const
Definition: allocator_memory_strategy.hpp:451
rclcpp::SubscriptionBase::SharedPtr subscription_intra_process
Definition: any_executable.hpp:45
Definition: any_executable.hpp:35
virtual rcl_allocator_t get_allocator()
Definition: allocator_memory_strategy.hpp:410
This header provides the get_node_topics_interface() template function.
Definition: allocator_common.hpp:24
rclcpp::ClientBase::SharedPtr client
Definition: any_executable.hpp:48
size_t number_of_ready_subscriptions() const
Definition: allocator_memory_strategy.hpp:415
size_t number_of_ready_services() const
Definition: allocator_memory_strategy.hpp:424
const rcl_client_t ** clients
AllocatorMemoryStrategy()
Definition: allocator_memory_strategy.hpp:59
bool collect_entities(const WeakNodeList &weak_nodes)
Definition: allocator_memory_strategy.hpp:153
T remove(T... args)
typename VoidAllocTraits::allocator_type VoidAlloc
Definition: allocator_memory_strategy.hpp:52
const rcl_timer_t ** timers
virtual void get_next_service(executor::AnyExecutable &any_exec, const WeakNodeList &weak_nodes)
Definition: allocator_memory_strategy.hpp:310
#define RCL_RET_OK
rclcpp::SubscriptionBase::SharedPtr subscription
Definition: any_executable.hpp:44
typename std::allocator_traits< Alloc >::template rebind_traits< T > AllocRebind
Definition: allocator_common.hpp:30
virtual void get_next_waitable(executor::AnyExecutable &any_exec, const WeakNodeList &weak_nodes)
Definition: allocator_memory_strategy.hpp:378
static rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group, const WeakNodeList &weak_nodes)
#define RCLCPP_SMART_PTR_DEFINITIONS(...)
Definition: macros.hpp:36
virtual void get_next_client(executor::AnyExecutable &any_exec, const WeakNodeList &weak_nodes)
Definition: allocator_memory_strategy.hpp:345
virtual void get_next_subscription(executor::AnyExecutable &any_exec, const WeakNodeList &weak_nodes)
Definition: allocator_memory_strategy.hpp:266
void add_guard_condition(const rcl_guard_condition_t *guard_condition)
Definition: allocator_memory_strategy.hpp:64
static rclcpp::callback_group::CallbackGroup::SharedPtr get_group_by_waitable(rclcpp::Waitable::SharedPtr waitable, const WeakNodeList &weak_nodes)
rcl_ret_t rcl_wait_set_add_timer(rcl_wait_set_t *wait_set, const rcl_timer_t *timer, size_t *index)
static rclcpp::callback_group::CallbackGroup::SharedPtr get_group_by_client(rclcpp::ClientBase::SharedPtr client, const WeakNodeList &weak_nodes)
size_t number_of_ready_clients() const
Definition: allocator_memory_strategy.hpp:442
typename allocator::AllocRebind< void *, Alloc > VoidAllocTraits
Definition: allocator_memory_strategy.hpp:51
rcl_ret_t rcl_wait_set_add_service(rcl_wait_set_t *wait_set, const rcl_service_t *service, size_t *index)
#define RCUTILS_LOG_ERROR_NAMED(name,...)
rclcpp::callback_group::CallbackGroup::SharedPtr callback_group
Definition: any_executable.hpp:51
T get(T... args)
virtual void remove_null_handles(rcl_wait_set_t *wait_set)
Definition: allocator_memory_strategy.hpp:93
static rclcpp::callback_group::CallbackGroup::SharedPtr get_group_by_service(rclcpp::ServiceBase::SharedPtr service, const WeakNodeList &weak_nodes)
void remove_guard_condition(const rcl_guard_condition_t *guard_condition)
Definition: allocator_memory_strategy.hpp:74
void clear_handles()
Definition: allocator_memory_strategy.hpp:84
static rclcpp::callback_group::CallbackGroup::SharedPtr get_group_by_subscription(rclcpp::SubscriptionBase::SharedPtr subscription, const WeakNodeList &weak_nodes)
static rclcpp::SubscriptionBase::SharedPtr get_subscription_by_handle(std::shared_ptr< const rcl_subscription_t > subscriber_handle, const WeakNodeList &weak_nodes)
rcl_ret_t rcl_wait_set_add_subscription(rcl_wait_set_t *wait_set, const rcl_subscription_t *subscription, size_t *index)
size_t number_of_waitables() const
Definition: allocator_memory_strategy.hpp:469
rcl_ret_t rcl_wait_set_add_client(rcl_wait_set_t *wait_set, const rcl_client_t *client, size_t *index)
static rclcpp::ServiceBase::SharedPtr get_service_by_handle(std::shared_ptr< const rcl_service_t > service_handle, const WeakNodeList &weak_nodes)
static rclcpp::ClientBase::SharedPtr get_client_by_handle(std::shared_ptr< const rcl_client_t > client_handle, const WeakNodeList &weak_nodes)
size_t number_of_ready_timers() const
Definition: allocator_memory_strategy.hpp:460
size_t number_of_ready_events() const
Definition: allocator_memory_strategy.hpp:433
bool add_handles_to_wait_set(rcl_wait_set_t *wait_set)
Definition: allocator_memory_strategy.hpp:206
Delegate for handling memory allocations while the Executor is executing.
Definition: allocator_memory_strategy.hpp:46
const rcl_subscription_t ** subscriptions
rcl_ret_t rcl_wait_set_add_guard_condition(rcl_wait_set_t *wait_set, const rcl_guard_condition_t *guard_condition, size_t *index)