Merge pull request #1584 from tier4/fix/concealer-7/transition

Author: HansRobo

external/concealer/include/concealer/field_operator_application.hpp

@@ -124,14 +124,19 @@ struct FieldOperatorApplication : public rclcpp::Node
 
   template <typename Thunk = void (*)()>
   auto waitForAutowareStateToBe(
-    const LegacyAutowareState & state, Thunk thunk = [] {})
+    const LegacyAutowareState & from_state, const LegacyAutowareState & to_state,
+    Thunk thunk = [] {})
   {
     thunk();
 
-    while (not finalized.load() and getLegacyAutowareState().value != state.value) {
+    auto not_to_be = [&](auto current_state) {
+      return from_state.value <= current_state.value and current_state.value < to_state.value;
+    };
+
+    while (not finalized.load() and not_to_be(getLegacyAutowareState())) {
       if (time_limit <= std::chrono::steady_clock::now()) {
         throw common::AutowareError(
-          "Simulator waited for the Autoware state to transition to ", state,
+          "Simulator waited for the Autoware state to transition to ", to_state,
           ", but time is up. The current Autoware state is ", getLegacyAutowareState());
       } else {
         thunk();

external/concealer/include/concealer/service.hpp

@@ -41,7 +41,7 @@ class Service
   template <typename Node>
   explicit Service(
     const std::string & name, Node & node,
-    const std::chrono::nanoseconds & interval = std::chrono::seconds(1))
+    const std::chrono::nanoseconds & interval = std::chrono::seconds(3))
   : name(name),
     client(node.template create_client<T>(name, rmw_qos_profile_default)),
     interval(interval)

external/concealer/src/field_operator_application.cpp

@@ -269,8 +269,19 @@ auto FieldOperatorApplication::engage() -> void
           "The simulator attempted to request Autoware to engage, but was aborted because "
           "Autoware's current state is ",
           state, ".");
+      case LegacyAutowareState::initializing:
+        // The initial pose has been sent but has not yet reached Autoware.
+        waitForAutowareStateToBe(
+          LegacyAutowareState::initializing, LegacyAutowareState::waiting_for_route);
+        [[fallthrough]];
+      case LegacyAutowareState::waiting_for_route:
+        // The route has been sent but has not yet reached Autoware.
+        waitForAutowareStateToBe(
+          LegacyAutowareState::waiting_for_route, LegacyAutowareState::planning);
+        [[fallthrough]];
       case LegacyAutowareState::planning:
-        waitForAutowareStateToBe(LegacyAutowareState::waiting_for_engage);
+        waitForAutowareStateToBe(
+          LegacyAutowareState::planning, LegacyAutowareState::waiting_for_engage);
         [[fallthrough]];
       case LegacyAutowareState::waiting_for_engage:
         requestEngage(
@@ -280,8 +291,12 @@ auto FieldOperatorApplication::engage() -> void
             return request;
           }(),
           30);
+        waitForAutowareStateToBe(
+          LegacyAutowareState::waiting_for_engage, LegacyAutowareState::driving);
         time_limit = std::decay_t<decltype(time_limit)>::max();
         break;
+      case LegacyAutowareState::driving:
+        break;
     }
   });
 }
@@ -309,6 +324,10 @@ auto FieldOperatorApplication::initialize(const geometry_msgs::msg::Pose & initi
             "The simulator attempted to initialize Autoware, but aborted because Autoware's "
             "current state is ",
             state, ".");
+        case LegacyAutowareState::undefined:
+          waitForAutowareStateToBe(
+            LegacyAutowareState::undefined, LegacyAutowareState::initializing);
+          [[fallthrough]];
         case LegacyAutowareState::initializing:
           requestInitialPose(
             [&]() {
@@ -324,6 +343,8 @@ auto FieldOperatorApplication::initialize(const geometry_msgs::msg::Pose & initi
               return request;
             }(),
             30);
+          waitForAutowareStateToBe(
+            LegacyAutowareState::initializing, LegacyAutowareState::waiting_for_route);
           break;
       }
     });
@@ -343,8 +364,9 @@ auto FieldOperatorApplication::plan(const std::vector<geometry_msgs::msg::PoseSt
           "current state is ",
           state, ".");
       case LegacyAutowareState::initializing:
+        // The initial pose has been sent but has not yet reached Autoware.
       case LegacyAutowareState::arrived_goal:
-        waitForAutowareStateToBe(LegacyAutowareState::waiting_for_route);
+        waitForAutowareStateToBe(state, LegacyAutowareState::waiting_for_route);
         [[fallthrough]];
       case LegacyAutowareState::waiting_for_route:
         requestSetRoutePoints(
@@ -369,7 +391,7 @@ auto FieldOperatorApplication::plan(const std::vector<geometry_msgs::msg::PoseSt
 
                [1] https://github.com/autowarefoundation/autoware_adapi_msgs/commit/805f8ebd3ca24564844df9889feeaf183101fbef
                [2] https://github.com/autowarefoundation/autoware_adapi_msgs/commit/cf310bd038673b6cbef3ae3b61dfe607212de419
-             */
+            */
             if constexpr (
               DetectMember_option<SetRoutePoints::Request>::value and
               DetectMember_allow_goal_modification<
@@ -381,6 +403,10 @@ auto FieldOperatorApplication::plan(const std::vector<geometry_msgs::msg::PoseSt
             return request;
           }(),
           30);
+        waitForAutowareStateToBe(
+          LegacyAutowareState::waiting_for_route, LegacyAutowareState::planning);
+        waitForAutowareStateToBe(
+          LegacyAutowareState::planning, LegacyAutowareState::waiting_for_engage);
         break;
     }
   });