Skip to content

Commit

Permalink
Remove macro DEFINE_STATE_PREDICATE
Browse files Browse the repository at this point in the history 
Signed-off-by: yamacir-kit <[email protected]>
  • Loading branch information
@yamacir-kit
yamacir-kit committed Dec 17, 2024
1 parent 0aa6335 commit 166a239
Show file tree
Hide file tree
Showing 4 changed files with 21 additions and 39 deletions.
19 changes: 0 additions & 19 deletions external/concealer/include/concealer/field_operator_application.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
2 changes: 1 addition & 1 deletion external/concealer/include/concealer/service_with_validation.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.");
}
Expand Down
23 changes: 12 additions & 11 deletions external/concealer/include/concealer/transition_assertion.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@ namespace concealer
template <typename Autoware>
struct TransitionAssertion
{
protected:
const std::chrono::steady_clock::time_point start;

const std::chrono::seconds initialize_duration;
Expand All @@ -42,16 +43,16 @@ struct TransitionAssertion

#define DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(STATE) \
template <typename Thunk = void (*)(), typename Interval = std::chrono::seconds> \
auto waitForAutowareStateToBe##STATE( \
auto waitForAutowareStateToBe_##STATE( \
Thunk && thunk = [] {}, Interval interval = std::chrono::seconds(1)) \
{ \
for (thunk(); not static_cast<const Autoware &>(*this).is_stop_requested.load() and \
not static_cast<const Autoware &>(*this).is##STATE(); \
static_cast<const Autoware &>(*this).autoware_state != #STATE; \
rclcpp::GenericRate<std::chrono::steady_clock>(interval).sleep()) { \
if ( \
have_never_been_engaged and \
start + initialize_duration <= std::chrono::steady_clock::now()) { \
const auto state = static_cast<const Autoware &>(*this).getAutowareStateName(); \
const auto state = static_cast<const Autoware &>(*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 ", \
Expand All @@ -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
};
Expand Down
16 changes: 8 additions & 8 deletions external/concealer/src/field_operator_application.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<Engage::Request>();
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.
}
});
Expand All @@ -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
Expand All @@ -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(<autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>)
if (getLocalizationState().state != LocalizationInitializationState::UNINITIALIZED) {
return;
Expand All @@ -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.
}
});
Expand All @@ -380,7 +380,7 @@ auto FieldOperatorApplication::plan(const std::vector<geometry_msgs::msg::PoseSt
assert(not route.empty());

task_queue.delay([this, route] {
waitForAutowareStateToBeWaitingForRoute(); // NOTE: This is assertion.
waitForAutowareStateToBe_WAITING_FOR_ROUTE(); // NOTE: This is assertion.

auto request = std::make_shared<SetRoutePoints::Request>();

Expand Down Expand Up @@ -414,7 +414,7 @@ auto FieldOperatorApplication::plan(const std::vector<geometry_msgs::msg::PoseSt

requestSetRoutePoints(request, 1);

waitForAutowareStateToBeWaitingForEngage();
waitForAutowareStateToBe_WAITING_FOR_ENGAGE();
});
}

Expand Down

0 comments on commit 166a239

Please sign in to comment.