15 #ifndef RCLCPP__WAIT_SET_POLICIES__DETAIL__STORAGE_POLICY_COMMON_HPP_
16 #define RCLCPP__WAIT_SET_POLICIES__DETAIL__STORAGE_POLICY_COMMON_HPP_
32 namespace wait_set_policies
38 template<
bool HasStrongOwnership>
43 class SubscriptionsIterable,
44 class GuardConditionsIterable,
45 class ExtraGuardConditionsIterable,
47 class ClientsIterable,
48 class ServicesIterable,
49 class WaitablesIterable
53 const SubscriptionsIterable & subscriptions,
54 const GuardConditionsIterable & guard_conditions,
55 const ExtraGuardConditionsIterable & extra_guard_conditions,
56 const TimersIterable & timers,
57 const ClientsIterable & clients,
58 const ServicesIterable & services,
59 const WaitablesIterable & waitables,
60 rclcpp::Context::SharedPtr context
65 if (
nullptr == context) {
69 size_t subscriptions_from_waitables = 0;
70 size_t guard_conditions_from_waitables = 0;
71 size_t timers_from_waitables = 0;
72 size_t clients_from_waitables = 0;
73 size_t services_from_waitables = 0;
74 size_t events_from_waitables = 0;
75 for (
const auto & waitable_entry : waitables) {
77 if (
nullptr == waitable_ptr_pair.second) {
78 if (HasStrongOwnership) {
95 rcl_ret_t ret = rcl_wait_set_init(
97 subscriptions.size() + subscriptions_from_waitables,
98 guard_conditions.size() + extra_guard_conditions.size() + guard_conditions_from_waitables,
99 timers.size() + timers_from_waitables,
100 clients.size() + clients_from_waitables,
101 services.size() + services_from_waitables,
102 events_from_waitables,
105 rcl_get_default_allocator());
106 if (RCL_RET_OK != ret) {
114 extra_guard_conditions,
124 if (RCL_RET_OK != ret) {
130 "Error in destruction of rcl wait set: %s", exception.
what());
135 template<
class EntityT>
139 return {
nullptr, shared_pointer.
get()};
142 template<
class EntityT>
146 auto shared_pointer = weak_pointer.
lock();
147 return {shared_pointer, shared_pointer.get()};
159 class SubscriptionsIterable,
160 class GuardConditionsIterable,
161 class ExtraGuardConditionsIterable,
162 class TimersIterable,
163 class ClientsIterable,
164 class ServicesIterable,
165 class WaitablesIterable
169 const SubscriptionsIterable & subscriptions,
170 const GuardConditionsIterable & guard_conditions,
171 const ExtraGuardConditionsIterable & extra_guard_conditions,
172 const TimersIterable & timers,
173 const ClientsIterable & clients,
174 const ServicesIterable & services,
175 const WaitablesIterable & waitables
178 bool was_resized =
false;
188 size_t subscriptions_from_waitables = 0;
189 size_t guard_conditions_from_waitables = 0;
190 size_t timers_from_waitables = 0;
191 size_t clients_from_waitables = 0;
192 size_t services_from_waitables = 0;
193 size_t events_from_waitables = 0;
194 for (
const auto & waitable_entry : waitables) {
196 if (
nullptr == waitable_ptr_pair.second) {
198 if (HasStrongOwnership) {
215 rcl_ret_t ret = rcl_wait_set_resize(
217 subscriptions.size() + subscriptions_from_waitables,
218 guard_conditions.size() + extra_guard_conditions.size() + guard_conditions_from_waitables,
219 timers.size() + timers_from_waitables,
220 clients.size() + clients_from_waitables,
221 services.size() + services_from_waitables,
222 events_from_waitables
224 if (RCL_RET_OK != ret) {
240 if (RCL_RET_OK != ret) {
246 for (
const auto & subscription_entry : subscriptions) {
247 auto subscription_ptr_pair =
249 if (
nullptr == subscription_ptr_pair.second) {
251 if (HasStrongOwnership) {
260 rcl_ret_t ret = rcl_wait_set_add_subscription(
262 subscription_ptr_pair.second->get_subscription_handle().get(),
264 if (RCL_RET_OK != ret) {
270 auto add_guard_conditions =
271 [
this](
const auto & inner_guard_conditions)
273 for (
const auto & guard_condition : inner_guard_conditions) {
275 if (
nullptr == guard_condition_ptr_pair.second) {
277 if (HasStrongOwnership) {
286 rcl_ret_t ret = rcl_wait_set_add_guard_condition(
288 &guard_condition_ptr_pair.second->get_rcl_guard_condition(),
290 if (RCL_RET_OK != ret) {
297 add_guard_conditions(guard_conditions);
300 add_guard_conditions(extra_guard_conditions);
303 for (
const auto & timer : timers) {
305 if (
nullptr == timer_ptr_pair.second) {
307 if (HasStrongOwnership) {
316 rcl_ret_t ret = rcl_wait_set_add_timer(
318 timer_ptr_pair.second->get_timer_handle().get(),
320 if (RCL_RET_OK != ret) {
326 for (
const auto & client : clients) {
328 if (
nullptr == client_ptr_pair.second) {
330 if (HasStrongOwnership) {
339 rcl_ret_t ret = rcl_wait_set_add_client(
341 client_ptr_pair.second->get_client_handle().get(),
343 if (RCL_RET_OK != ret) {
349 for (
const auto & service : services) {
351 if (
nullptr == service_ptr_pair.second) {
353 if (HasStrongOwnership) {
362 rcl_ret_t ret = rcl_wait_set_add_service(
364 service_ptr_pair.second->get_service_handle().get(),
366 if (RCL_RET_OK != ret) {
372 for (
auto & waitable_entry : waitables) {
374 if (
nullptr == waitable_ptr_pair.second) {
376 if (HasStrongOwnership) {
422 #endif // RCLCPP__WAIT_SET_POLICIES__DETAIL__STORAGE_POLICY_COMMON_HPP_