Merge branch 'master' into RJD-1201/fix_quick_start

external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp

@@ -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                             \
   {                                                          \

external/concealer/include/concealer/transition_assertion.hpp

@@ -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);

external/concealer/src/field_operator_application_for_autoware_universe.cpp

@@ -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);
   });
 }
 

openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp

@@ -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>

openscenario/openscenario_interpreter/src/syntax/storyboard_element_state_condition.cpp

@@ -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.
   */
 

simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp

@@ -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 &){};
 
@@ -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;
 

simulation/traffic_simulator/src/entity/entity_base.cpp

@@ -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)
 {