rclcpp  master
C++ ROS Client Library API
sequential_synchronization.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__SEQUENTIAL_SYNCHRONIZATION_HPP_
16 #define RCLCPP__WAIT_SET_POLICIES__SEQUENTIAL_SYNCHRONIZATION_HPP_
17 
18 #include <chrono>
19 #include <functional>
20 #include <memory>
21 #include <utility>
22 
23 #include "rclcpp/client.hpp"
24 #include "rclcpp/exceptions.hpp"
26 #include "rclcpp/macros.hpp"
27 #include "rclcpp/service.hpp"
30 #include "rclcpp/timer.hpp"
32 #include "rclcpp/wait_result.hpp"
35 #include "rclcpp/waitable.hpp"
36 
37 namespace rclcpp
38 {
39 namespace wait_set_policies
40 {
41 
44 {
45 protected:
46  explicit SequentialSynchronization(rclcpp::Context::SharedPtr) {}
47  ~SequentialSynchronization() = default;
48 
50 
56  {
58  return empty;
59  }
60 
62 
65  void
71  > add_subscription_function)
72  {
73  // Explicitly no thread synchronization.
74  add_subscription_function(std::move(subscription), mask);
75  }
76 
78 
81  void
87  > remove_subscription_function)
88  {
89  // Explicitly no thread synchronization.
90  remove_subscription_function(std::move(subscription), mask);
91  }
92 
94 
97  void
99  std::shared_ptr<rclcpp::GuardCondition> && guard_condition,
100  std::function<void(std::shared_ptr<rclcpp::GuardCondition>&&)> add_guard_condition_function)
101  {
102  // Explicitly no thread synchronization.
103  add_guard_condition_function(std::move(guard_condition));
104  }
105 
107 
110  void
112  std::shared_ptr<rclcpp::GuardCondition> && guard_condition,
113  std::function<void(std::shared_ptr<rclcpp::GuardCondition>&&)> remove_guard_condition_function)
114  {
115  // Explicitly no thread synchronization.
116  remove_guard_condition_function(std::move(guard_condition));
117  }
118 
120 
123  void
126  std::function<void(std::shared_ptr<rclcpp::TimerBase>&&)> add_timer_function)
127  {
128  // Explicitly no thread synchronization.
129  add_timer_function(std::move(timer));
130  }
131 
133 
136  void
139  std::function<void(std::shared_ptr<rclcpp::TimerBase>&&)> remove_timer_function)
140  {
141  // Explicitly no thread synchronization.
142  remove_timer_function(std::move(timer));
143  }
144 
146 
149  void
152  std::function<void(std::shared_ptr<rclcpp::ClientBase>&&)> add_client_function)
153  {
154  // Explicitly no thread synchronization.
155  add_client_function(std::move(client));
156  }
157 
159 
162  void
165  std::function<void(std::shared_ptr<rclcpp::ClientBase>&&)> remove_client_function)
166  {
167  // Explicitly no thread synchronization.
168  remove_client_function(std::move(client));
169  }
170 
172 
175  void
178  std::function<void(std::shared_ptr<rclcpp::ServiceBase>&&)> add_service_function)
179  {
180  // Explicitly no thread synchronization.
181  add_service_function(std::move(service));
182  }
183 
185 
188  void
191  std::function<void(std::shared_ptr<rclcpp::ServiceBase>&&)> remove_service_function)
192  {
193  // Explicitly no thread synchronization.
194  remove_service_function(std::move(service));
195  }
196 
198 
201  void
204  std::shared_ptr<void> && associated_entity,
207  > add_waitable_function)
208  {
209  // Explicitly no thread synchronization.
210  add_waitable_function(std::move(waitable), std::move(associated_entity));
211  }
212 
214 
217  void
220  std::function<void(std::shared_ptr<rclcpp::Waitable>&&)> remove_waitable_function)
221  {
222  // Explicitly no thread synchronization.
223  remove_waitable_function(std::move(waitable));
224  }
225 
227 
230  void
231  sync_prune_deleted_entities(std::function<void()> prune_deleted_entities_function)
232  {
233  // Explicitly no thread synchronization.
234  prune_deleted_entities_function();
235  }
236 
238  template<class WaitResultT>
239  WaitResultT
241  std::chrono::nanoseconds time_to_wait_ns,
242  std::function<void()> rebuild_rcl_wait_set,
243  std::function<rcl_wait_set_t & ()> get_rcl_wait_set,
244  std::function<WaitResultT(WaitResultKind wait_result_kind)> create_wait_result)
245  {
246  // Assumption: this function assumes that some measure has been taken to
247  // ensure none of the entities being waited on by the wait set are allowed
248  // to go out of scope and therefore be deleted.
249  // In the case of the StaticStorage policy, this is ensured because it
250  // retains shared ownership of all entites for the duration of its own life.
251  // In the case of the DynamicStorage policy, this is ensured by the function
252  // which calls this function, by acquiring shared ownership of the entites
253  // for the duration of this function.
254 
255  // Setup looping predicate.
256  auto start = std::chrono::steady_clock::now();
257  std::function<bool()> should_loop = this->create_loop_predicate(time_to_wait_ns, start);
258 
259  // Wait until exit condition is met.
260  do {
261  // Rebuild the wait set.
262  // This will resize the wait set if needed, due to e.g. adding or removing
263  // entities since the last wait, but this should never occur in static
264  // storage wait sets since they cannot be changed after construction.
265  // This will also clear the wait set and re-add all the entities, which
266  // prepares it to be waited on again.
267  rebuild_rcl_wait_set();
268 
269  rcl_wait_set_t & rcl_wait_set = get_rcl_wait_set();
270 
271  // Wait unconditionally until timeout condition occurs since we assume
272  // there are no conditions that would require the wait to stop and reset,
273  // like asynchronously adding or removing an entity, i.e. explicitly
274  // providing no thread-safety.
275 
276  // Calculate how much time there is left to wait, unless blocking indefinitely.
277  auto time_left_to_wait_ns = this->calculate_time_left_to_wait(time_to_wait_ns, start);
278 
279  // Then wait for entities to become ready.
280  rcl_ret_t ret = rcl_wait(&rcl_wait_set, time_left_to_wait_ns.count());
281  if (RCL_RET_OK == ret) {
282  // Something has become ready in the wait set, and since this class
283  // did not add anything to it, it is a user entity that is ready.
284  return create_wait_result(WaitResultKind::Ready);
285  } else if (RCL_RET_TIMEOUT == ret) {
286  // The wait set timed out, exit the loop.
287  break;
288  } else if (RCL_RET_WAIT_SET_EMPTY == ret) {
289  // Wait set was empty, return Empty.
290  return create_wait_result(WaitResultKind::Empty);
291  } else {
292  // Some other error case, throw.
294  }
295  } while (should_loop());
296 
297  // Wait did not result in ready items, return timeout.
298  return create_wait_result(WaitResultKind::Timeout);
299  }
300 
301  void
303  {
304  // Explicitly do nothing.
305  }
306 
307  void
309  {
310  // Explicitly do nothing.
311  }
312 };
313 
314 } // namespace wait_set_policies
315 } // namespace rclcpp
316 
317 #endif // RCLCPP__WAIT_SET_POLICIES__SEQUENTIAL_SYNCHRONIZATION_HPP_
rclcpp::wait_set_policies::SequentialSynchronization::sync_remove_guard_condition
void sync_remove_guard_condition(std::shared_ptr< rclcpp::GuardCondition > &&guard_condition, std::function< void(std::shared_ptr< rclcpp::GuardCondition > &&)> remove_guard_condition_function)
Remove guard condition without thread-safety.
Definition: sequential_synchronization.hpp:111
rclcpp::wait_set_policies::SequentialSynchronization::SequentialSynchronization
SequentialSynchronization(rclcpp::Context::SharedPtr)
Definition: sequential_synchronization.hpp:46
synchronization_policy_common.hpp
rclcpp::wait_set_policies::SequentialSynchronization::sync_remove_waitable
void sync_remove_waitable(std::shared_ptr< rclcpp::Waitable > &&waitable, std::function< void(std::shared_ptr< rclcpp::Waitable > &&)> remove_waitable_function)
Remove waitable without thread-safety.
Definition: sequential_synchronization.hpp:218
exceptions.hpp
client.hpp
rclcpp::wait_set_policies::SequentialSynchronization::sync_remove_subscription
void sync_remove_subscription(std::shared_ptr< rclcpp::SubscriptionBase > &&subscription, const rclcpp::SubscriptionWaitSetMask &mask, std::function< void(std::shared_ptr< rclcpp::SubscriptionBase > &&, const rclcpp::SubscriptionWaitSetMask &) > remove_subscription_function)
Remove guard condition without thread-safety.
Definition: sequential_synchronization.hpp:82
std::shared_ptr< rclcpp::SubscriptionBase >
rclcpp::wait_set_policies::detail::SynchronizationPolicyCommon::create_loop_predicate
std::function< bool()> create_loop_predicate(std::chrono::nanoseconds time_to_wait_ns, std::chrono::steady_clock::time_point start)
Definition: synchronization_policy_common.hpp:36
rclcpp::Ready
@ Ready
Definition: wait_result_kind.hpp:26
rclcpp::wait_set_policies::SequentialSynchronization::sync_remove_client
void sync_remove_client(std::shared_ptr< rclcpp::ClientBase > &&client, std::function< void(std::shared_ptr< rclcpp::ClientBase > &&)> remove_client_function)
Remove client without thread-safety.
Definition: sequential_synchronization.hpp:163
rclcpp::wait_set_policies::SequentialSynchronization::sync_add_subscription
void sync_add_subscription(std::shared_ptr< rclcpp::SubscriptionBase > &&subscription, const rclcpp::SubscriptionWaitSetMask &mask, std::function< void(std::shared_ptr< rclcpp::SubscriptionBase > &&, const rclcpp::SubscriptionWaitSetMask &) > add_subscription_function)
Add subscription without thread-safety.
Definition: sequential_synchronization.hpp:66
rclcpp::WaitResultKind
WaitResultKind
Represents the various kinds of results from waiting on a wait set.
Definition: wait_result_kind.hpp:24
std::move
T move(T... args)
rclcpp::wait_set_policies::SequentialSynchronization::sync_remove_timer
void sync_remove_timer(std::shared_ptr< rclcpp::TimerBase > &&timer, std::function< void(std::shared_ptr< rclcpp::TimerBase > &&)> remove_timer_function)
Remove timer without thread-safety.
Definition: sequential_synchronization.hpp:137
rclcpp::wait_set_policies::SequentialSynchronization
WaitSet policy that explicitly provides no thread synchronization.
Definition: sequential_synchronization.hpp:43
rcl_ret_t
rmw_ret_t rcl_ret_t
rclcpp::SubscriptionWaitSetMask
Options used to determine what parts of a subscription get added to or removed from a wait set.
Definition: subscription_wait_set_mask.hpp:24
rclcpp::wait_set_policies::detail::SynchronizationPolicyCommon::calculate_time_left_to_wait
std::chrono::nanoseconds calculate_time_left_to_wait(std::chrono::nanoseconds original_time_to_wait_ns, std::chrono::steady_clock::time_point start)
Definition: synchronization_policy_common.hpp:51
std::chrono::nanoseconds
rclcpp::wait_set_policies::SequentialSynchronization::sync_add_timer
void sync_add_timer(std::shared_ptr< rclcpp::TimerBase > &&timer, std::function< void(std::shared_ptr< rclcpp::TimerBase > &&)> add_timer_function)
Add timer without thread-safety.
Definition: sequential_synchronization.hpp:124
std::function
subscription_base.hpp
guard_condition.hpp
rclcpp
This header provides the get_node_base_interface() template function.
Definition: allocator_common.hpp:24
timer.hpp
rclcpp::wait_set_policies::SequentialSynchronization::sync_wait
WaitResultT sync_wait(std::chrono::nanoseconds time_to_wait_ns, std::function< void()> rebuild_rcl_wait_set, std::function< rcl_wait_set_t &()> get_rcl_wait_set, std::function< WaitResultT(WaitResultKind wait_result_kind)> create_wait_result)
Implements wait without any thread-safety.
Definition: sequential_synchronization.hpp:240
rclcpp::wait_set_policies::SequentialSynchronization::sync_remove_service
void sync_remove_service(std::shared_ptr< rclcpp::ServiceBase > &&service, std::function< void(std::shared_ptr< rclcpp::ServiceBase > &&)> remove_service_function)
Remove service without thread-safety.
Definition: sequential_synchronization.hpp:189
rclcpp::wait_set_policies::SequentialSynchronization::~SequentialSynchronization
~SequentialSynchronization()=default
macros.hpp
subscription_wait_set_mask.hpp
std::array
rclcpp::Timeout
@ Timeout
Definition: wait_result_kind.hpp:27
RCL_RET_WAIT_SET_EMPTY
#define RCL_RET_WAIT_SET_EMPTY
rcl_wait_set_t
rclcpp::wait_set_policies::detail::SynchronizationPolicyCommon
Common structure for synchronization policies.
Definition: synchronization_policy_common.hpp:29
rclcpp::wait_set_policies::SequentialSynchronization::sync_add_waitable
void sync_add_waitable(std::shared_ptr< rclcpp::Waitable > &&waitable, std::shared_ptr< void > &&associated_entity, std::function< void(std::shared_ptr< rclcpp::Waitable > &&, std::shared_ptr< void > &&) > add_waitable_function)
Add waitable without thread-safety.
Definition: sequential_synchronization.hpp:202
rclcpp::wait_set_policies::SequentialSynchronization::sync_add_guard_condition
void sync_add_guard_condition(std::shared_ptr< rclcpp::GuardCondition > &&guard_condition, std::function< void(std::shared_ptr< rclcpp::GuardCondition > &&)> add_guard_condition_function)
Add guard condition without thread-safety.
Definition: sequential_synchronization.hpp:98
wait_result_kind.hpp
rclcpp::wait_set_policies::SequentialSynchronization::sync_wait_result_acquire
void sync_wait_result_acquire()
Definition: sequential_synchronization.hpp:302
visibility_control.hpp
rclcpp::wait_set_policies::SequentialSynchronization::sync_wait_result_release
void sync_wait_result_release()
Definition: sequential_synchronization.hpp:308
rclcpp::Empty
@ Empty
Definition: wait_result_kind.hpp:28
rclcpp::wait_set_policies::SequentialSynchronization::sync_add_client
void sync_add_client(std::shared_ptr< rclcpp::ClientBase > &&client, std::function< void(std::shared_ptr< rclcpp::ClientBase > &&)> add_client_function)
Add client without thread-safety.
Definition: sequential_synchronization.hpp:150
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.
RCL_RET_TIMEOUT
#define RCL_RET_TIMEOUT
rclcpp::wait_set_policies::SequentialSynchronization::sync_prune_deleted_entities
void sync_prune_deleted_entities(std::function< void()> prune_deleted_entities_function)
Prune deleted entities without thread-safety.
Definition: sequential_synchronization.hpp:231
wait_result.hpp
RCL_RET_OK
#define RCL_RET_OK
service.hpp
rcl_wait
rcl_ret_t rcl_wait(rcl_wait_set_t *wait_set, int64_t timeout)
waitable.hpp
rclcpp::wait_set_policies::SequentialSynchronization::get_extra_guard_conditions
const std::array< std::shared_ptr< rclcpp::GuardCondition >, 0 > & get_extra_guard_conditions()
Return any "extra" guard conditions needed to implement the synchronization policy.
Definition: sequential_synchronization.hpp:55
rclcpp::wait_set_policies::SequentialSynchronization::sync_add_service
void sync_add_service(std::shared_ptr< rclcpp::ServiceBase > &&service, std::function< void(std::shared_ptr< rclcpp::ServiceBase > &&)> add_service_function)
Add service without thread-safety.
Definition: sequential_synchronization.hpp:176
std::chrono::steady_clock::now
T now(T... args)