14 #ifndef MOTION_TESTING_NODES__WAIT_FOR_MATCHED_HPP_
15 #define MOTION_TESTING_NODES__WAIT_FOR_MATCHED_HPP_
19 #include <rclcpp/rclcpp.hpp>
26 namespace motion_testing_nodes
33 template<
typename ConditionF>
35 const std::chrono::nanoseconds timeout,
36 const std::chrono::nanoseconds poll_period,
37 const ConditionF & condition)
39 const auto start = std::chrono::steady_clock::now();
40 const auto end = start + timeout;
42 auto matched = condition();
43 while ((now <
end) && (!matched)) {
44 std::this_thread::sleep_for(poll_period);
45 matched = condition();
46 now = std::chrono::steady_clock::now();
49 throw std::runtime_error{
"Timeout waiting for match"};
66 const rclcpp::PublisherBase & pub,
67 const std::size_t minimum_match_count,
68 const std::chrono::nanoseconds timeout = std::chrono::seconds{5LL},
69 const std::chrono::nanoseconds poll_period = std::chrono::milliseconds{1LL});
79 const rclcpp::SubscriptionBase & sub,
80 const std::size_t minimum_match_count,
81 const std::chrono::nanoseconds timeout = std::chrono::seconds{5LL},
82 const std::chrono::nanoseconds poll_period = std::chrono::milliseconds{1LL});
87 #endif // MOTION_TESTING_NODES__WAIT_FOR_MATCHED_HPP_