15 #ifndef CONCEALER__TRANSITION_ASSERTION_HPP_
16 #define CONCEALER__TRANSITION_ASSERTION_HPP_
19 #include <rclcpp/node.hpp>
20 #include <rclcpp/rate.hpp>
25 template <
typename Autoware>
29 const std::chrono::steady_clock::time_point
start;
37 auto node = rclcpp::Node(
"get_parameter",
"simulation");
38 node.declare_parameter(
"initialize_duration", 0);
39 return node.get_parameter(
"initialize_duration").as_int();
44 #define DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(STATE) \
45 template <typename Thunk = void (*)(), typename Interval = std::chrono::seconds> \
46 auto waitForAutowareStateToBe_##STATE( \
47 Thunk && thunk = [] {}, Interval interval = std::chrono::seconds(1)) \
49 for (thunk(); not static_cast<const Autoware &>(*this).is_stop_requested.load() and \
50 static_cast<const Autoware &>(*this).autoware_state != #STATE; \
51 rclcpp::GenericRate<std::chrono::steady_clock>(interval).sleep()) { \
53 have_never_been_engaged and \
54 start + initialize_duration <= std::chrono::steady_clock::now()) { \
55 const auto state = static_cast<const Autoware &>(*this).autoware_state; \
56 throw common::AutowareError( \
57 "Simulator waited for the Autoware state to transition to " #STATE \
58 ", but time is up. The current Autoware state is ", \
59 (state.empty() ? "not published yet" : state)); \
64 if constexpr (std::string_view(#STATE) == std::string_view("DRIVING")) { \
65 have_never_been_engaged = false; \
78 #undef DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE
Definition: autoware_universe.hpp:40
Definition: transition_assertion.hpp:27
DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(WAITING_FOR_ENGAGE)
DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(INITIALIZING)
DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(DRIVING)
DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(ARRIVED_GOAL)
DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(FINALIZING)
TransitionAssertion()
Definition: transition_assertion.hpp:35
DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(PLANNING)
bool have_never_been_engaged
Definition: transition_assertion.hpp:33
const std::chrono::seconds initialize_duration
Definition: transition_assertion.hpp:31
const std::chrono::steady_clock::time_point start
Definition: transition_assertion.hpp:29
DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(WAITING_FOR_ROUTE)