Autoware.Auto
wait_for_matched.hpp
Go to the documentation of this file.
1 // Copyright 2020 Christopher Ho
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 #ifndef MOTION_TESTING_NODES__WAIT_FOR_MATCHED_HPP_
15 #define MOTION_TESTING_NODES__WAIT_FOR_MATCHED_HPP_
16 
18 
19 #include <rclcpp/rclcpp.hpp>
20 
21 #include <chrono>
22 #include <thread>
23 
24 namespace motion
25 {
26 namespace motion_testing_nodes
27 {
28 
29 namespace details
30 {
31 
33 template<typename ConditionF>
34 std::chrono::nanoseconds wait_for_matched(
35  const std::chrono::nanoseconds timeout,
36  const std::chrono::nanoseconds poll_period,
37  const ConditionF & condition)
38 {
39  const auto start = std::chrono::steady_clock::now();
40  const auto end = start + timeout;
41  auto now = start;
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();
47  }
48  if (!matched) {
49  throw std::runtime_error{"Timeout waiting for match"};
50  }
51 
52  return end - now;
53 }
54 
55 
56 } // namespace details
57 
65 MOTION_TESTING_NODES_PUBLIC std::chrono::nanoseconds wait_for_matched(
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});
70 
78 MOTION_TESTING_NODES_PUBLIC std::chrono::nanoseconds wait_for_matched(
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});
83 
84 } // namespace motion_testing_nodes
85 } // namespace motion
86 
87 #endif // MOTION_TESTING_NODES__WAIT_FOR_MATCHED_HPP_
motion::motion_testing_nodes::wait_for_matched
MOTION_TESTING_NODES_PUBLIC std::chrono::nanoseconds wait_for_matched(const rclcpp::PublisherBase &pub, const std::size_t minimum_match_count, const std::chrono::nanoseconds timeout=std::chrono::seconds{5LL}, const std::chrono::nanoseconds poll_period=std::chrono::milliseconds{1LL})
Definition: wait_for_matched.cpp:24
motion::motion_testing_nodes::details::wait_for_matched
std::chrono::nanoseconds wait_for_matched(const std::chrono::nanoseconds timeout, const std::chrono::nanoseconds poll_period, const ConditionF &condition)
Implementation of wait_for_matched.
Definition: wait_for_matched.hpp:34
visibility_control.hpp
motion
Definition: controller_base.hpp:31
get_open_port.end
end
Definition: scripts/get_open_port.py:23