rclcpp  master
C++ ROS Client Library API
storage_policy_common.hpp
Go to the documentation of this file.
1 // Copyright 2020 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__WAIT_SET_POLICIES__DETAIL__STORAGE_POLICY_COMMON_HPP_
16 #define RCLCPP__WAIT_SET_POLICIES__DETAIL__STORAGE_POLICY_COMMON_HPP_
17 
18 #include <memory>
19 #include <stdexcept>
20 #include <utility>
21 
22 #include "rcl/wait.h"
23 
24 #include "rclcpp/exceptions.hpp"
25 #include "rclcpp/logging.hpp"
26 #include "rclcpp/macros.hpp"
28 #include "rclcpp/waitable.hpp"
29 
30 namespace rclcpp
31 {
32 namespace wait_set_policies
33 {
34 namespace detail
35 {
36 
38 template<bool HasStrongOwnership>
40 {
41 protected:
42  template<
43  class SubscriptionsIterable,
44  class GuardConditionsIterable,
45  class ExtraGuardConditionsIterable,
46  class TimersIterable,
47  class ClientsIterable,
48  class ServicesIterable,
49  class WaitablesIterable
50  >
51  explicit
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
61  )
62  : rcl_wait_set_(rcl_get_zero_initialized_wait_set()), context_(context)
63  {
64  // Check context is not nullptr.
65  if (nullptr == context) {
66  throw std::invalid_argument("context is nullptr");
67  }
68  // Accumulate total contributions from waitables.
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) {
76  auto waitable_ptr_pair = get_raw_pointer_from_smart_pointer(waitable_entry.waitable);
77  if (nullptr == waitable_ptr_pair.second) {
78  if (HasStrongOwnership) {
79  throw std::runtime_error("unexpected condition, fixed storage policy needs pruning");
80  }
81  // Flag for pruning.
82  needs_pruning_ = true;
83  continue;
84  }
85 
86  rclcpp::Waitable & waitable = *waitable_ptr_pair.second;
87  subscriptions_from_waitables += waitable.get_number_of_ready_subscriptions();
88  guard_conditions_from_waitables += waitable.get_number_of_ready_guard_conditions();
89  timers_from_waitables += waitable.get_number_of_ready_timers();
90  clients_from_waitables += waitable.get_number_of_ready_clients();
91  services_from_waitables += waitable.get_number_of_ready_services();
92  events_from_waitables += waitable.get_number_of_ready_events();
93  }
94  // Initialize wait set using initial inputs.
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,
103  context_->get_rcl_context().get(),
104  // TODO(wjwwood): support custom allocator, maybe restrict to polymorphic allocator
105  rcl_get_default_allocator());
106  if (RCL_RET_OK != ret) {
108  }
109 
110  // (Re)build the wait set for the first time.
112  subscriptions,
113  guard_conditions,
114  extra_guard_conditions,
115  timers,
116  clients,
117  services,
118  waitables);
119  }
120 
122  {
123  rcl_ret_t ret = rcl_wait_set_fini(&rcl_wait_set_);
124  if (RCL_RET_OK != ret) {
125  try {
127  } catch (const std::exception & exception) {
128  RCLCPP_ERROR(
129  rclcpp::get_logger("rclcpp"),
130  "Error in destruction of rcl wait set: %s", exception.what());
131  }
132  }
133  }
134 
135  template<class EntityT>
138  {
139  return {nullptr, shared_pointer.get()};
140  }
141 
142  template<class EntityT>
145  {
146  auto shared_pointer = weak_pointer.lock();
147  return {shared_pointer, shared_pointer.get()};
148  }
149 
151 
158  template<
159  class SubscriptionsIterable,
160  class GuardConditionsIterable,
161  class ExtraGuardConditionsIterable,
162  class TimersIterable,
163  class ClientsIterable,
164  class ServicesIterable,
165  class WaitablesIterable
166  >
167  void
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
176  )
177  {
178  bool was_resized = false;
179  // Resize the wait set, but only if it needs to be.
180  if (needs_resize_) {
181  // Resizing with rcl_wait_set_resize() is a no-op if nothing has changed,
182  // but tracking the need to resize in this class avoids an unnecessary
183  // library call (rcl is most likely a separate shared library) each wait
184  // loop.
185  // Also, since static storage wait sets will never need resizing, so it
186  // avoids completely redundant calls to this function in that case.
187  // Accumulate total contributions from waitables.
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) {
195  auto waitable_ptr_pair = get_raw_pointer_from_smart_pointer(waitable_entry.waitable);
196  if (nullptr == waitable_ptr_pair.second) {
197  // In this case it was probably stored as a weak_ptr, but is now locking to nullptr.
198  if (HasStrongOwnership) {
199  // This will not happen in fixed sized storage, as it holds
200  // shared ownership the whole time and is never in need of pruning.
201  throw std::runtime_error("unexpected condition, fixed storage policy needs pruning");
202  }
203  // Flag for pruning.
204  needs_pruning_ = true;
205  continue;
206  }
207  rclcpp::Waitable & waitable = *waitable_ptr_pair.second;
208  subscriptions_from_waitables += waitable.get_number_of_ready_subscriptions();
209  guard_conditions_from_waitables += waitable.get_number_of_ready_guard_conditions();
210  timers_from_waitables += waitable.get_number_of_ready_timers();
211  clients_from_waitables += waitable.get_number_of_ready_clients();
212  services_from_waitables += waitable.get_number_of_ready_services();
213  events_from_waitables += waitable.get_number_of_ready_events();
214  }
215  rcl_ret_t ret = rcl_wait_set_resize(
216  &rcl_wait_set_,
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
223  );
224  if (RCL_RET_OK != ret) {
226  }
227  was_resized = true;
228  // Assumption: the calling code ensures this function is not called
229  // concurrently with functions that set this variable to true, either
230  // with documentation (as is the case for the SequentialSychronization
231  // policy), or with synchronization primitives (as is the case with
232  // the ThreadSafeSynchronization policy).
233  needs_resize_ = false;
234  }
235 
236  // Now clear the wait set, but only if it was not resized, as resizing also
237  // clears the wait set.
238  if (!was_resized) {
239  rcl_ret_t ret = rcl_wait_set_clear(&rcl_wait_set_);
240  if (RCL_RET_OK != ret) {
242  }
243  }
244 
245  // Add subscriptions.
246  for (const auto & subscription_entry : subscriptions) {
247  auto subscription_ptr_pair =
248  get_raw_pointer_from_smart_pointer(subscription_entry.subscription);
249  if (nullptr == subscription_ptr_pair.second) {
250  // In this case it was probably stored as a weak_ptr, but is now locking to nullptr.
251  if (HasStrongOwnership) {
252  // This will not happen in fixed sized storage, as it holds
253  // shared ownership the whole time and is never in need of pruning.
254  throw std::runtime_error("unexpected condition, fixed storage policy needs pruning");
255  }
256  // Flag for pruning.
257  needs_pruning_ = true;
258  continue;
259  }
260  rcl_ret_t ret = rcl_wait_set_add_subscription(
261  &rcl_wait_set_,
262  subscription_ptr_pair.second->get_subscription_handle().get(),
263  nullptr);
264  if (RCL_RET_OK != ret) {
266  }
267  }
268 
269  // Setup common code to add guard_conditions.
270  auto add_guard_conditions =
271  [this](const auto & inner_guard_conditions)
272  {
273  for (const auto & guard_condition : inner_guard_conditions) {
274  auto guard_condition_ptr_pair = get_raw_pointer_from_smart_pointer(guard_condition);
275  if (nullptr == guard_condition_ptr_pair.second) {
276  // In this case it was probably stored as a weak_ptr, but is now locking to nullptr.
277  if (HasStrongOwnership) {
278  // This will not happen in fixed sized storage, as it holds
279  // shared ownership the whole time and is never in need of pruning.
280  throw std::runtime_error("unexpected condition, fixed storage policy needs pruning");
281  }
282  // Flag for pruning.
283  needs_pruning_ = true;
284  continue;
285  }
286  rcl_ret_t ret = rcl_wait_set_add_guard_condition(
287  &rcl_wait_set_,
288  &guard_condition_ptr_pair.second->get_rcl_guard_condition(),
289  nullptr);
290  if (RCL_RET_OK != ret) {
292  }
293  }
294  };
295 
296  // Add guard conditions.
297  add_guard_conditions(guard_conditions);
298 
299  // Add extra guard conditions.
300  add_guard_conditions(extra_guard_conditions);
301 
302  // Add timers.
303  for (const auto & timer : timers) {
304  auto timer_ptr_pair = get_raw_pointer_from_smart_pointer(timer);
305  if (nullptr == timer_ptr_pair.second) {
306  // In this case it was probably stored as a weak_ptr, but is now locking to nullptr.
307  if (HasStrongOwnership) {
308  // This will not happen in fixed sized storage, as it holds
309  // shared ownership the whole time and is never in need of pruning.
310  throw std::runtime_error("unexpected condition, fixed storage policy needs pruning");
311  }
312  // Flag for pruning.
313  needs_pruning_ = true;
314  continue;
315  }
316  rcl_ret_t ret = rcl_wait_set_add_timer(
317  &rcl_wait_set_,
318  timer_ptr_pair.second->get_timer_handle().get(),
319  nullptr);
320  if (RCL_RET_OK != ret) {
322  }
323  }
324 
325  // Add clients.
326  for (const auto & client : clients) {
327  auto client_ptr_pair = get_raw_pointer_from_smart_pointer(client);
328  if (nullptr == client_ptr_pair.second) {
329  // In this case it was probably stored as a weak_ptr, but is now locking to nullptr.
330  if (HasStrongOwnership) {
331  // This will not happen in fixed sized storage, as it holds
332  // shared ownership the whole time and is never in need of pruning.
333  throw std::runtime_error("unexpected condition, fixed storage policy needs pruning");
334  }
335  // Flag for pruning.
336  needs_pruning_ = true;
337  continue;
338  }
339  rcl_ret_t ret = rcl_wait_set_add_client(
340  &rcl_wait_set_,
341  client_ptr_pair.second->get_client_handle().get(),
342  nullptr);
343  if (RCL_RET_OK != ret) {
345  }
346  }
347 
348  // Add services.
349  for (const auto & service : services) {
350  auto service_ptr_pair = get_raw_pointer_from_smart_pointer(service);
351  if (nullptr == service_ptr_pair.second) {
352  // In this case it was probably stored as a weak_ptr, but is now locking to nullptr.
353  if (HasStrongOwnership) {
354  // This will not happen in fixed sized storage, as it holds
355  // shared ownership the whole time and is never in need of pruning.
356  throw std::runtime_error("unexpected condition, fixed storage policy needs pruning");
357  }
358  // Flag for pruning.
359  needs_pruning_ = true;
360  continue;
361  }
362  rcl_ret_t ret = rcl_wait_set_add_service(
363  &rcl_wait_set_,
364  service_ptr_pair.second->get_service_handle().get(),
365  nullptr);
366  if (RCL_RET_OK != ret) {
368  }
369  }
370 
371  // Add waitables.
372  for (auto & waitable_entry : waitables) {
373  auto waitable_ptr_pair = get_raw_pointer_from_smart_pointer(waitable_entry.waitable);
374  if (nullptr == waitable_ptr_pair.second) {
375  // In this case it was probably stored as a weak_ptr, but is now locking to nullptr.
376  if (HasStrongOwnership) {
377  // This will not happen in fixed sized storage, as it holds
378  // shared ownership the whole time and is never in need of pruning.
379  throw std::runtime_error("unexpected condition, fixed storage policy needs pruning");
380  }
381  // Flag for pruning.
382  needs_pruning_ = true;
383  continue;
384  }
385  rclcpp::Waitable & waitable = *waitable_ptr_pair.second;
386  bool successful = waitable.add_to_wait_set(&rcl_wait_set_);
387  if (!successful) {
388  throw std::runtime_error("waitable unexpectedly failed to be added to wait set");
389  }
390  }
391  }
392 
393  const rcl_wait_set_t &
395  {
396  return rcl_wait_set_;
397  }
398 
401  {
402  return rcl_wait_set_;
403  }
404 
405  void
407  {
408  needs_resize_ = true;
409  }
410 
412  rclcpp::Context::SharedPtr context_;
413 
414  bool needs_pruning_ = false;
415  bool needs_resize_ = false;
416 };
417 
418 } // namespace detail
419 } // namespace wait_set_policies
420 } // namespace rclcpp
421 
422 #endif // RCLCPP__WAIT_SET_POLICIES__DETAIL__STORAGE_POLICY_COMMON_HPP_
std::weak_ptr::lock
T lock(T... args)
exceptions.hpp
rclcpp::wait_set_policies::detail::StoragePolicyCommon::storage_flag_for_resize
void storage_flag_for_resize()
Definition: storage_policy_common.hpp:406
std::shared_ptr
std::exception
rclcpp::wait_set_policies::detail::StoragePolicyCommon::get_raw_pointer_from_smart_pointer
std::pair< void *, EntityT * > get_raw_pointer_from_smart_pointer(const std::shared_ptr< EntityT > &shared_pointer)
Definition: storage_policy_common.hpp:137
rclcpp::wait_set_policies::detail::StoragePolicyCommon::get_raw_pointer_from_smart_pointer
std::pair< std::shared_ptr< EntityT >, EntityT * > get_raw_pointer_from_smart_pointer(const std::weak_ptr< EntityT > &weak_pointer)
Definition: storage_policy_common.hpp:144
rclcpp::Waitable::get_number_of_ready_subscriptions
virtual size_t get_number_of_ready_subscriptions()
Get the number of ready subscriptions.
rclcpp::Waitable::get_number_of_ready_timers
virtual size_t get_number_of_ready_timers()
Get the number of ready timers.
std::pair
rclcpp::wait_set_policies::detail::StoragePolicyCommon
Common structure for storage policies, which provides rcl wait set access.
Definition: storage_policy_common.hpp:39
rclcpp::get_logger
Logger get_logger(const std::string &name)
Return a named logger.
std::shared_ptr::get
T get(T... args)
rclcpp::wait_set_policies::detail::StoragePolicyCommon::needs_pruning_
bool needs_pruning_
Definition: storage_policy_common.hpp:414
rclcpp::Waitable
Definition: waitable.hpp:28
rclcpp::wait_set_policies::detail::StoragePolicyCommon::context_
rclcpp::Context::SharedPtr context_
Definition: storage_policy_common.hpp:412
rclcpp
This header provides the get_node_base_interface() template function.
Definition: allocator_common.hpp:24
rclcpp::Waitable::get_number_of_ready_events
virtual size_t get_number_of_ready_events()
Get the number of ready events.
rclcpp::Waitable::get_number_of_ready_services
virtual size_t get_number_of_ready_services()
Get the number of ready services.
rclcpp::wait_set_policies::detail::StoragePolicyCommon::needs_resize_
bool needs_resize_
Definition: storage_policy_common.hpp:415
rclcpp::Waitable::get_number_of_ready_clients
virtual size_t get_number_of_ready_clients()
Get the number of ready clients.
macros.hpp
logging.hpp
rclcpp::wait_set_policies::detail::StoragePolicyCommon::rcl_wait_set_
rcl_wait_set_t rcl_wait_set_
Definition: storage_policy_common.hpp:411
rcl_wait_set_t
std::runtime_error
std::invalid_argument
rclcpp::wait_set_policies::detail::StoragePolicyCommon::~StoragePolicyCommon
~StoragePolicyCommon()
Definition: storage_policy_common.hpp:121
std::weak_ptr
visibility_control.hpp
rclcpp::Waitable::add_to_wait_set
virtual bool add_to_wait_set(rcl_wait_set_t *wait_set)=0
Add the Waitable to a wait set.
rclcpp::wait_set_policies::detail::StoragePolicyCommon::StoragePolicyCommon
StoragePolicyCommon(const SubscriptionsIterable &subscriptions, const GuardConditionsIterable &guard_conditions, const ExtraGuardConditionsIterable &extra_guard_conditions, const TimersIterable &timers, const ClientsIterable &clients, const ServicesIterable &services, const WaitablesIterable &waitables, rclcpp::Context::SharedPtr context)
Definition: storage_policy_common.hpp:52
rclcpp::wait_set_policies::detail::StoragePolicyCommon::storage_get_rcl_wait_set
const rcl_wait_set_t & storage_get_rcl_wait_set() const
Definition: storage_policy_common.hpp:394
rclcpp::exceptions::throw_from_rcl_error
void throw_from_rcl_error(rcl_ret_t ret, const std::string &prefix="", const rcl_error_state_t *error_state=nullptr, void(*reset_error)()=rcl_reset_error)
Throw a C++ std::exception which was created based on an rcl error.
rclcpp::wait_set_policies::detail::StoragePolicyCommon::storage_get_rcl_wait_set
rcl_wait_set_t & storage_get_rcl_wait_set()
Definition: storage_policy_common.hpp:400
rclcpp::Waitable::get_number_of_ready_guard_conditions
virtual size_t get_number_of_ready_guard_conditions()
Get the number of ready guard_conditions.
rclcpp::wait_set_policies::detail::StoragePolicyCommon::storage_rebuild_rcl_wait_set_with_sets
void storage_rebuild_rcl_wait_set_with_sets(const SubscriptionsIterable &subscriptions, const GuardConditionsIterable &guard_conditions, const ExtraGuardConditionsIterable &extra_guard_conditions, const TimersIterable &timers, const ClientsIterable &clients, const ServicesIterable &services, const WaitablesIterable &waitables)
Rebuild the wait set, preparing it for the next wait call.
Definition: storage_policy_common.hpp:168
waitable.hpp
std::exception::what
T what(T... args)
RCLCPP_ERROR
#define RCLCPP_ERROR(logger,...)
Definition: logging.hpp:1419