From 166a23986296ddfa5884d405651ac3d26f1e5660 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Tue, 17 Dec 2024 14:45:56 +0900 Subject: [PATCH] Remove macro `DEFINE_STATE_PREDICATE` Signed-off-by: yamacir-kit --- .../concealer/field_operator_application.hpp | 19 --------------- .../concealer/service_with_validation.hpp | 2 +- .../concealer/transition_assertion.hpp | 23 ++++++++++--------- .../src/field_operator_application.cpp | 16 ++++++------- 4 files changed, 21 insertions(+), 39 deletions(-) diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index b1a6d2a067b..db11a70de50 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -146,25 +146,6 @@ struct FieldOperatorApplication : public rclcpp::Node, ~FieldOperatorApplication(); - /* - NOTE: This predicate should not take the state being compared as an - argument or template parameter. Otherwise, code using this class would - need to have knowledge of the Autoware state type. - */ -#define DEFINE_STATE_PREDICATE(NAME, VALUE) \ - auto is##NAME() const noexcept { return autoware_state == #VALUE; } \ - static_assert(true, "") - - DEFINE_STATE_PREDICATE(Initializing, INITIALIZING_VEHICLE); - DEFINE_STATE_PREDICATE(WaitingForRoute, WAITING_FOR_ROUTE); - DEFINE_STATE_PREDICATE(Planning, PLANNING); - DEFINE_STATE_PREDICATE(WaitingForEngage, WAITING_FOR_ENGAGE); - DEFINE_STATE_PREDICATE(Driving, DRIVING); - DEFINE_STATE_PREDICATE(ArrivedGoal, ARRIVAL_GOAL); - DEFINE_STATE_PREDICATE(Finalizing, FINALIZING); - -#undef DEFINE_STATE_PREDICATE - auto spinSome() -> void; auto engage() -> void; diff --git a/external/concealer/include/concealer/service_with_validation.hpp b/external/concealer/include/concealer/service_with_validation.hpp index 14382070d07..6b49df8d0f0 100644 --- a/external/concealer/include/concealer/service_with_validation.hpp +++ b/external/concealer/include/concealer/service_with_validation.hpp @@ -135,7 +135,7 @@ class ServiceWithValidation } } - throw common::scenario_simulator_exception::Error( + throw common::scenario_simulator_exception::AutowareError( "Requested the service ", std::quoted(service_name), " ", attempts_count, " times, but was not successful."); } diff --git a/external/concealer/include/concealer/transition_assertion.hpp b/external/concealer/include/concealer/transition_assertion.hpp index 39121175319..ce28408a7f1 100644 --- a/external/concealer/include/concealer/transition_assertion.hpp +++ b/external/concealer/include/concealer/transition_assertion.hpp @@ -25,6 +25,7 @@ namespace concealer template struct TransitionAssertion { +protected: const std::chrono::steady_clock::time_point start; const std::chrono::seconds initialize_duration; @@ -42,16 +43,16 @@ struct TransitionAssertion #define DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(STATE) \ template \ - auto waitForAutowareStateToBe##STATE( \ + auto waitForAutowareStateToBe_##STATE( \ Thunk && thunk = [] {}, Interval interval = std::chrono::seconds(1)) \ { \ for (thunk(); not static_cast(*this).is_stop_requested.load() and \ - not static_cast(*this).is##STATE(); \ + static_cast(*this).autoware_state != #STATE; \ rclcpp::GenericRate(interval).sleep()) { \ if ( \ have_never_been_engaged and \ start + initialize_duration <= std::chrono::steady_clock::now()) { \ - const auto state = static_cast(*this).getAutowareStateName(); \ + const auto state = static_cast(*this).autoware_state; \ throw common::AutowareError( \ "Simulator waited for the Autoware state to transition to " #STATE \ ", but time is up. The current Autoware state is ", \ @@ -60,19 +61,19 @@ struct TransitionAssertion thunk(); \ } \ } \ - if constexpr (std::string_view(#STATE) == std::string_view("Driving")) { \ + if constexpr (std::string_view(#STATE) == std::string_view("DRIVING")) { \ have_never_been_engaged = false; \ } \ } \ static_assert(true) - DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(Initializing); - DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(WaitingForRoute); - DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(Planning); - DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(WaitingForEngage); - DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(Driving); - DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(ArrivedGoal); - DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(Finalizing); + DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(INITIALIZING); + DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(WAITING_FOR_ROUTE); + DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(PLANNING); + DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(WAITING_FOR_ENGAGE); + 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); #undef DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE }; diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index 5bad86ff9ec..1c7cf0d04e7 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -311,12 +311,12 @@ auto FieldOperatorApplication::enableAutowareControl() -> void auto FieldOperatorApplication::engage() -> void { task_queue.delay([this]() { - waitForAutowareStateToBeDriving([this]() { + waitForAutowareStateToBe_DRIVING([this]() { auto request = std::make_shared(); request->engage = true; try { return requestEngage(request, 1); - } catch (const common::Error &) { + } catch (const common::AutowareError &) { return; // Ignore error because this service is validated by Autoware state transition. } }); @@ -326,13 +326,13 @@ auto FieldOperatorApplication::engage() -> void auto FieldOperatorApplication::engageable() const -> bool { rethrow(); - return task_queue.exhausted() and isWaitingForEngage(); + return task_queue.exhausted() and autoware_state == "WAITING_FOR_ENGAGE"; } auto FieldOperatorApplication::engaged() const -> bool { rethrow(); - return task_queue.exhausted() and isDriving(); + return task_queue.exhausted() and autoware_state == "DRIVING"; } auto FieldOperatorApplication::getWaypoints() const -> traffic_simulator_msgs::msg::WaypointsArray @@ -350,7 +350,7 @@ auto FieldOperatorApplication::initialize(const geometry_msgs::msg::Pose & initi { if (not std::exchange(initialize_was_called, true)) { task_queue.delay([this, initial_pose]() { - waitForAutowareStateToBeWaitingForRoute([&]() { + waitForAutowareStateToBe_WAITING_FOR_ROUTE([&]() { #if __has_include() if (getLocalizationState().state != LocalizationInitializationState::UNINITIALIZED) { return; @@ -366,7 +366,7 @@ auto FieldOperatorApplication::initialize(const geometry_msgs::msg::Pose & initi request->pose.push_back(initial_pose_msg); try { return requestInitialPose(request, 1); - } catch (const common::Error &) { + } catch (const common::AutowareError &) { return; // Ignore error because this service is validated by Autoware state transition. } }); @@ -380,7 +380,7 @@ auto FieldOperatorApplication::plan(const std::vector(); @@ -414,7 +414,7 @@ auto FieldOperatorApplication::plan(const std::vector