Skip to content

Commit

Permalink
Merge pull request #1369 from tier4/fix/acquire-position-action
Browse files Browse the repository at this point in the history 
Fix/acquire position action
  • Loading branch information
@yamacir-kit
yamacir-kit authored Sep 26, 2024
2 parents f0b2eb3 + 8b6e2e8 commit 695dbf2
Show file tree
Hide file tree
Showing 8 changed files with 345 additions and 101 deletions.
5 changes: 5 additions & 0 deletions external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -84,6 +84,11 @@ class FieldOperatorApplicationFor<AutowareUniverse>

auto receiveEmergencyState(const tier4_external_api_msgs::msg::Emergency & msg) -> void;

/*
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 \
{ \
Expand Down
126 changes: 38 additions & 88 deletions external/concealer/include/concealer/transition_assertion.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,111 +15,61 @@
#ifndef CONCEALER__TRANSITION_ASSERTION_HPP_
#define CONCEALER__TRANSITION_ASSERTION_HPP_

#include <autoware_auto_system_msgs/msg/autoware_state.hpp>
#include <chrono>
#include <rclcpp/node.hpp>
#include <rclcpp/rate.hpp>
#include <scenario_simulator_exception/exception.hpp>

namespace concealer
{
template <typename T>
auto getParameter(const std::string & name, T value = {})
{
rclcpp::Node node{"get_parameter", "simulation"};

node.declare_parameter<T>(name, value);
node.get_parameter<T>(name, value);

return value;
}

template <typename Autoware>
struct TransitionAssertion
{
using clock_type = std::chrono::steady_clock;
using rate_type = rclcpp::GenericRate<clock_type>;
const std::chrono::steady_clock::time_point start;

const clock_type::time_point start;
const std::chrono::seconds initialize_duration;

explicit TransitionAssertion()
: start(clock_type::now()),
initialize_duration(std::chrono::seconds(getParameter<int>("initialize_duration")))
{
}
bool have_never_been_engaged = true;

auto makeTransitionError(const std::string & expected) const
explicit TransitionAssertion()
: start(std::chrono::steady_clock::now()), initialize_duration([]() {
auto node = rclcpp::Node("get_parameter", "simulation");
node.declare_parameter("initialize_duration", 0);
return node.get_parameter("initialize_duration").as_int();
}())
{
const auto current_state = asAutoware().getAutowareStateName();
using namespace std::chrono;
return common::AutowareError(
"Simulator waited for the Autoware state to transition to ", expected,
", but time is up. The current Autoware state is ",
(current_state.empty() ? "NOT PUBLISHED YET" : current_state), ".");
}

auto asAutoware() -> Autoware & { return static_cast<Autoware &>(*this); }
auto asAutoware() const -> const Autoware & { return static_cast<const Autoware &>(*this); }

#define DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE_WITH_INITIALIZE_TIME_LIMIT(STATE) \
template <typename Thunk = void (*)()> \
void waitForAutowareStateToBe##STATE( \
Thunk && thunk = [] {}, std::chrono::seconds interval = std::chrono::seconds(1)) \
{ \
using std::chrono::duration_cast; \
using std::chrono::seconds; \
rate_type rate(interval); \
for (thunk(); not asAutoware().isStopRequested() and not asAutoware().is##STATE(); \
rate.sleep()) { \
auto now = clock_type::now(); \
if (start + initialize_duration <= now) { \
throw makeTransitionError(#STATE); \
} else { \
RCLCPP_INFO_STREAM( \
asAutoware().get_logger(), \
"Simulator waiting for Autoware state to be " #STATE " (remain: " \
<< duration_cast<seconds>(start + initialize_duration - now).count() << ")."); \
thunk(); \
} \
} \
RCLCPP_INFO_STREAM( \
asAutoware().get_logger(), \
"Autoware is " << asAutoware().getAutowareStateName() << " now."); \
} \
static_assert(true, "")

#define DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(STATE) \
template <typename Thunk = void (*)()> \
void waitForAutowareStateToBe##STATE( \
Thunk && thunk = [] {}, std::chrono::seconds interval = std::chrono::seconds(1)) \
{ \
using std::chrono::duration_cast; \
using std::chrono::seconds; \
rate_type rate(interval); \
for (thunk(); not asAutoware().isStopRequested() and not asAutoware().is##STATE(); \
rate.sleep()) { \
auto now = clock_type::now(); \
RCLCPP_INFO_STREAM( \
asAutoware().get_logger(), \
"Simulator waiting for Autoware state to be " #STATE " (remain: " \
<< duration_cast<seconds>(start + initialize_duration - now).count() << ")."); \
thunk(); \
} \
RCLCPP_INFO_STREAM( \
asAutoware().get_logger(), \
"Autoware is " << asAutoware().getAutowareStateName() << " now."); \
} \
static_assert(true, "")

// XXX: The time limit must be ignored in waitForAutowareStateToBeDriving() because the current
// implementation attempts to transition to Driving state after initialize_duration seconds have
// elapsed.

DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE_WITH_INITIALIZE_TIME_LIMIT(Initializing);
DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE_WITH_INITIALIZE_TIME_LIMIT(WaitingForRoute);
DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE_WITH_INITIALIZE_TIME_LIMIT(Planning);
DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE_WITH_INITIALIZE_TIME_LIMIT(WaitingForEngage);
#define DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(STATE) \
template <typename Thunk = void (*)(), typename Interval = std::chrono::seconds> \
auto waitForAutowareStateToBe##STATE( \
Thunk && thunk = [] {}, Interval interval = std::chrono::seconds(1)) \
{ \
for (thunk(); not static_cast<const Autoware &>(*this).isStopRequested() and \
not static_cast<const Autoware &>(*this).is##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(); \
throw common::AutowareError( \
"Simulator waited for the Autoware state to transition to " #STATE \
", but time is up. The current Autoware state is ", \
(state.empty() ? "not published yet" : state)); \
} else { \
thunk(); \
} \
} \
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);
Expand Down
7 changes: 6 additions & 1 deletion external/concealer/src/field_operator_application_for_autoware_universe.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -350,7 +350,12 @@ auto FieldOperatorApplicationFor<AutowareUniverse>::plan(
auto FieldOperatorApplicationFor<AutowareUniverse>::clearRoute() -> void
{
task_queue.delay([this] {
requestClearRoute(std::make_shared<autoware_adapi_v1_msgs::srv::ClearRoute::Request>());
/*
Since this service tends to be available long after the launch of
Autoware, set the attempts_count to a high value. There is no technical
basis for the number 30.
*/
requestClearRoute(std::make_shared<autoware_adapi_v1_msgs::srv::ClearRoute::Request>(), 30);
});
}

Expand Down
5 changes: 3 additions & 2 deletions openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -284,9 +284,10 @@ class SimulatorCore
{
protected:
template <typename... Ts>
static auto applyAcquirePositionAction(Ts &&... xs)
static auto applyAcquirePositionAction(const std::string & entity_ref, Ts &&... xs)
{
return core->requestAcquirePosition(std::forward<decltype(xs)>(xs)...);
core->requestClearRoute(entity_ref);
return core->requestAcquirePosition(entity_ref, std::forward<decltype(xs)>(xs)...);
}

template <typename... Ts>
Expand Down
2 changes: 1 addition & 1 deletion openscenario/openscenario_interpreter/src/syntax/storyboard_element_state_condition.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@ StoryboardElementStateCondition::StoryboardElementStateCondition(
Note that there is no guarantee that the StoryboardElement will remain in
the same state as when the notification is made until
StoryboardElementStateCondition::evaluate is executed after the callback
is called. The In other words, the StoryboardElement may immediately
is called. In other words, the StoryboardElement may immediately
transition to the next state after calling the callback function.
*/

Expand Down
4 changes: 2 additions & 2 deletions simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -141,7 +141,7 @@ class EntityBase

virtual void requestAssignRoute(const std::vector<geometry_msgs::msg::Pose> &) = 0;

virtual void requestLaneChange(const lanelet::Id){};
virtual void requestLaneChange(const lanelet::Id) {}

virtual void requestLaneChange(const traffic_simulator::lane_change::Parameter &){};

Expand All @@ -164,7 +164,7 @@ class EntityBase

virtual void requestSpeedChange(const speed_change::RelativeTargetSpeed &, bool);

virtual void requestClearRoute();
virtual void requestClearRoute() {}

virtual auto isControlledBySimulator() const -> bool;

Expand Down
7 changes: 0 additions & 7 deletions simulation/traffic_simulator/src/entity/entity_base.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -218,13 +218,6 @@ void EntityBase::requestSpeedChangeWithConstantAcceleration(
}
}

void EntityBase::requestClearRoute()
{
THROW_SEMANTIC_ERROR(
"requestClearRoute is only supported for EgoEntity. The specified Entity is not an EgoEntity. "
"Please check the scenario carefully.");
}

void EntityBase::requestSpeedChangeWithTimeConstraint(
const double target_speed, const speed_change::Transition transition, double acceleration_time)
{
Expand Down
Loading

0 comments on commit 695dbf2

Please sign in to comment.