15 #ifndef RCLCPP__STRATEGIES__ALLOCATOR_MEMORY_STRATEGY_HPP_ 16 #define RCLCPP__STRATEGIES__ALLOCATOR_MEMORY_STRATEGY_HPP_ 32 namespace memory_strategies
34 namespace allocator_memory_strategy
43 template<
typename Alloc = std::allocator<
void>>
57 executable_allocator_ = std::make_shared<ExecAlloc>(*allocator.get());
58 allocator_ = std::make_shared<VoidAlloc>(*allocator.get());
63 executable_allocator_ = std::make_shared<ExecAlloc>();
64 allocator_ = std::make_shared<VoidAlloc>();
69 for (
const auto & existing_guard_condition : guard_conditions_) {
70 if (existing_guard_condition == guard_condition) {
74 guard_conditions_.push_back(guard_condition);
79 for (
auto it = guard_conditions_.begin(); it != guard_conditions_.end(); ++it) {
80 if (*it == guard_condition) {
81 guard_conditions_.erase(it);
89 subscription_handles_.clear();
90 service_handles_.clear();
91 client_handles_.clear();
92 timer_handles_.clear();
99 subscription_handles_[i] =
nullptr;
104 service_handles_[i] =
nullptr;
109 client_handles_[i] =
nullptr;
113 if (!wait_set->
timers[i]) {
114 timer_handles_[i] =
nullptr;
118 subscription_handles_.erase(
119 std::remove(subscription_handles_.begin(), subscription_handles_.end(),
nullptr),
120 subscription_handles_.end()
123 service_handles_.erase(
124 std::remove(service_handles_.begin(), service_handles_.end(),
nullptr),
125 service_handles_.end()
128 client_handles_.erase(
129 std::remove(client_handles_.begin(), client_handles_.end(),
nullptr),
130 client_handles_.end()
133 timer_handles_.erase(
134 std::remove(timer_handles_.begin(), timer_handles_.end(),
nullptr),
141 bool has_invalid_weak_nodes =
false;
142 for (
auto & weak_node : weak_nodes) {
143 auto node = weak_node.lock();
145 has_invalid_weak_nodes =
true;
148 for (
auto & weak_group : node->get_callback_groups()) {
149 auto group = weak_group.lock();
150 if (!group || !group->can_be_taken_from().load()) {
153 for (
auto & weak_subscription : group->get_subscription_ptrs()) {
154 auto subscription = weak_subscription.lock();
156 subscription_handles_.push_back(subscription->get_subscription_handle());
157 if (subscription->get_intra_process_subscription_handle()) {
158 subscription_handles_.push_back(
159 subscription->get_intra_process_subscription_handle());
163 for (
auto & weak_service : group->get_service_ptrs()) {
164 auto service = weak_service.lock();
166 service_handles_.push_back(service->get_service_handle());
169 for (
auto & weak_client : group->get_client_ptrs()) {
170 auto client = weak_client.lock();
172 client_handles_.push_back(client->get_client_handle());
175 for (
auto & weak_timer : group->get_timer_ptrs()) {
176 auto timer = weak_timer.lock();
178 timer_handles_.push_back(timer->get_timer_handle());
183 return has_invalid_weak_nodes;
188 for (
auto subscription : subscription_handles_) {
195 for (
auto client : client_handles_) {
202 for (
auto service : service_handles_) {
209 for (
auto timer : timer_handles_) {
216 for (
auto guard_condition : guard_conditions_) {
218 fprintf(stderr,
"Couldn't add guard_condition to waitset: %s\n",
230 return std::allocate_shared<executor::AnyExecutable>(*executable_allocator_.get());
237 auto it = subscription_handles_.begin();
238 while (it != subscription_handles_.end()) {
242 bool is_intra_process =
false;
243 if (subscription->get_intra_process_subscription_handle()) {
244 is_intra_process = subscription->get_intra_process_subscription_handle() == *it;
251 subscription_handles_.erase(it);
254 if (!group->can_be_taken_from().load()) {
261 if (is_intra_process) {
262 any_exec->subscription_intra_process = subscription;
264 any_exec->subscription = subscription;
266 any_exec->callback_group = group;
268 subscription_handles_.erase(it);
272 subscription_handles_.erase(it);
280 auto it = service_handles_.begin();
281 while (it != service_handles_.end()) {
289 service_handles_.erase(it);
292 if (!group->can_be_taken_from().load()) {
299 any_exec->service = service;
300 any_exec->callback_group = group;
302 service_handles_.erase(it);
306 service_handles_.erase(it);
313 auto it = client_handles_.begin();
314 while (it != client_handles_.end()) {
322 client_handles_.erase(it);
325 if (!group->can_be_taken_from().load()) {
332 any_exec->client = client;
333 any_exec->callback_group = group;
335 client_handles_.erase(it);
339 client_handles_.erase(it);
345 return rclcpp::allocator::get_rcl_allocator<void *, VoidAlloc>(*allocator_.get());
350 return subscription_handles_.size();
355 return service_handles_.size();
360 return client_handles_.size();
365 return guard_conditions_.size();
370 return timer_handles_.size();
376 std::vector<T, typename std::allocator_traits<Alloc>::template rebind_alloc<T>>;
378 VectorRebind<const rcl_guard_condition_t *> guard_conditions_;
380 VectorRebind<const rcl_subscription_t *> subscription_handles_;
381 VectorRebind<const rcl_service_t *> service_handles_;
382 VectorRebind<const rcl_client_t *> client_handles_;
383 VectorRebind<const rcl_timer_t *> timer_handles_;
385 std::shared_ptr<ExecAlloc> executable_allocator_;
386 std::shared_ptr<VoidAlloc> allocator_;
393 #endif // RCLCPP__STRATEGIES__ALLOCATOR_MEMORY_STRATEGY_HPP_ static rclcpp::subscription::SubscriptionBase::SharedPtr get_subscription_by_handle(const rcl_subscription_t *subscriber_handle, const WeakNodeVector &weak_nodes)
const rcl_service_t ** services
static rclcpp::client::ClientBase::SharedPtr get_client_by_handle(const rcl_client_t *client_handle, const WeakNodeVector &weak_nodes)
std::vector< rclcpp::node_interfaces::NodeBaseInterface::WeakPtr > WeakNodeVector
Definition: memory_strategy.hpp:44
executor::AnyExecutable::SharedPtr instantiate_next_executable()
Provide a newly initialized AnyExecutable object.
Definition: allocator_memory_strategy.hpp:228
Delegate for handling memory allocations while the Executor is executing.
Definition: memory_strategy.hpp:40
size_t number_of_guard_conditions() const
Definition: allocator_memory_strategy.hpp:363
allocator::Deleter< ExecAlloc, executor::AnyExecutable > ExecDeleter
Definition: allocator_memory_strategy.hpp:51
bool collect_entities(const WeakNodeVector &weak_nodes)
Definition: allocator_memory_strategy.hpp:139
rcl_ret_t rcl_wait_set_add_subscription(rcl_wait_set_t *wait_set, const rcl_subscription_t *subscription)
size_t size_of_subscriptions
virtual rcl_allocator_t get_allocator()
Definition: allocator_memory_strategy.hpp:343
Definition: allocator_common.hpp:24
Definition: parameter.hpp:235
size_t number_of_ready_subscriptions() const
Definition: allocator_memory_strategy.hpp:348
size_t number_of_ready_services() const
Definition: allocator_memory_strategy.hpp:353
const rcl_client_t ** clients
AllocatorMemoryStrategy()
Definition: allocator_memory_strategy.hpp:61
typename VoidAllocTraits::allocator_type VoidAlloc
Definition: allocator_memory_strategy.hpp:53
const rcl_timer_t ** timers
static rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group, const WeakNodeVector &weak_nodes)
static rclcpp::callback_group::CallbackGroup::SharedPtr get_group_by_service(rclcpp::service::ServiceBase::SharedPtr service, const WeakNodeVector &weak_nodes)
static rclcpp::service::ServiceBase::SharedPtr get_service_by_handle(const rcl_service_t *service_handle, const WeakNodeVector &weak_nodes)
bool add_handles_to_waitset(rcl_wait_set_t *wait_set)
Definition: allocator_memory_strategy.hpp:186
rcl_ret_t rcl_wait_set_add_guard_condition(rcl_wait_set_t *wait_set, const rcl_guard_condition_t *guard_condition)
static rclcpp::callback_group::CallbackGroup::SharedPtr get_group_by_subscription(rclcpp::subscription::SubscriptionBase::SharedPtr subscription, const WeakNodeVector &weak_nodes)
typename std::allocator_traits< Alloc >::template rebind_traits< T > AllocRebind
Definition: allocator_common.hpp:30
static rclcpp::callback_group::CallbackGroup::SharedPtr get_group_by_client(rclcpp::client::ClientBase::SharedPtr client, const WeakNodeVector &weak_nodes)
#define RCLCPP_SMART_PTR_DEFINITIONS(...)
Definition: macros.hpp:36
void add_guard_condition(const rcl_guard_condition_t *guard_condition)
Definition: allocator_memory_strategy.hpp:67
#define rcl_get_error_string_safe
size_t number_of_ready_clients() const
Definition: allocator_memory_strategy.hpp:358
rcl_ret_t rcl_wait_set_add_client(rcl_wait_set_t *wait_set, const rcl_client_t *client)
typename allocator::AllocRebind< void *, Alloc > VoidAllocTraits
Definition: allocator_memory_strategy.hpp:52
virtual void remove_null_handles(rcl_wait_set_t *wait_set)
Definition: allocator_memory_strategy.hpp:95
rcl_ret_t rcl_wait_set_add_service(rcl_wait_set_t *wait_set, const rcl_service_t *service)
void remove_guard_condition(const rcl_guard_condition_t *guard_condition)
Definition: allocator_memory_strategy.hpp:77
void clear_handles()
Definition: allocator_memory_strategy.hpp:87
virtual void get_next_service(executor::AnyExecutable::SharedPtr any_exec, const WeakNodeVector &weak_nodes)
Definition: allocator_memory_strategy.hpp:277
typename std::conditional< std::is_same< typename std::allocator_traits< Alloc >::template rebind_alloc< T >, typename std::allocator< void >::template rebind< T >::other >::value, std::default_delete< T >, AllocatorDeleter< Alloc > >::type Deleter
Definition: allocator_deleter.hpp:101
typename ExecAllocTraits::allocator_type ExecAlloc
Definition: allocator_memory_strategy.hpp:50
size_t number_of_ready_timers() const
Definition: allocator_memory_strategy.hpp:368
virtual void get_next_client(executor::AnyExecutable::SharedPtr any_exec, const WeakNodeVector &weak_nodes)
Definition: allocator_memory_strategy.hpp:311
virtual void get_next_subscription(executor::AnyExecutable::SharedPtr any_exec, const WeakNodeVector &weak_nodes)
Definition: allocator_memory_strategy.hpp:234
allocator::AllocRebind< executor::AnyExecutable, Alloc > ExecAllocTraits
Definition: allocator_memory_strategy.hpp:49
rcl_ret_t rcl_wait_set_add_timer(rcl_wait_set_t *wait_set, const rcl_timer_t *timer)
Delegate for handling memory allocations while the Executor is executing.
Definition: allocator_memory_strategy.hpp:44
const rcl_subscription_t ** subscriptions