Refactor/concealer 7

external/concealer/include/concealer/field_operator_application.hpp

@@ -17,20 +17,16 @@
 
 #include <sys/wait.h>
 
-#if __has_include(<autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>)
-#include <autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>
-#endif
-
 #include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
 #include <autoware_adapi_v1_msgs/srv/change_operation_mode.hpp>
 #include <autoware_adapi_v1_msgs/srv/clear_route.hpp>
 #include <autoware_adapi_v1_msgs/srv/initialize_localization.hpp>
 #include <autoware_adapi_v1_msgs/srv/set_route_points.hpp>
 #include <autoware_control_msgs/msg/control.hpp>
-#include <autoware_system_msgs/msg/autoware_state.hpp>
 #include <autoware_vehicle_msgs/msg/gear_command.hpp>
 #include <autoware_vehicle_msgs/msg/turn_indicators_command.hpp>
 #include <concealer/autoware_universe.hpp>
+#include <concealer/legacy_autoware_state.hpp>
 #include <concealer/path_with_lane_id.hpp>
 #include <concealer/publisher.hpp>
 #include <concealer/service.hpp>
@@ -63,8 +59,6 @@ struct FieldOperatorApplication : public rclcpp::Node
 
   std::chrono::steady_clock::time_point time_limit;
 
-  std::string autoware_state = "LAUNCHING";
-
   std::string minimum_risk_maneuver_state;
 
   std::string minimum_risk_maneuver_behavior;
@@ -74,8 +68,16 @@ struct FieldOperatorApplication : public rclcpp::Node
   using Control                         = autoware_control_msgs::msg::Control;
   using CooperateStatusArray            = tier4_rtc_msgs::msg::CooperateStatusArray;
   using Emergency                       = tier4_external_api_msgs::msg::Emergency;
+#if __has_include(<autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>)
   using LocalizationInitializationState = autoware_adapi_v1_msgs::msg::LocalizationInitializationState;
+#endif
   using MrmState                        = autoware_adapi_v1_msgs::msg::MrmState;
+#if __has_include(<autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>)
+  using OperationModeState              = autoware_adapi_v1_msgs::msg::OperationModeState;
+#endif
+#if __has_include(<autoware_adapi_v1_msgs/msg/route_state.hpp>)
+  using RouteState                      = autoware_adapi_v1_msgs::msg::RouteState;
+#endif
   using Trajectory                      = tier4_planning_msgs::msg::Trajectory;
   using TurnIndicatorsCommand           = autoware_vehicle_msgs::msg::TurnIndicatorsCommand;
 
@@ -95,10 +97,16 @@ struct FieldOperatorApplication : public rclcpp::Node
 #if __has_include(<autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>)
   Subscriber<LocalizationInitializationState> getLocalizationState;
 #endif
-  Subscriber<MrmState>                 getMrmState;
-  Subscriber<priority::PathWithLaneId> getPathWithLaneId;
-  Subscriber<Trajectory>               getTrajectory;
-  Subscriber<TurnIndicatorsCommand>    getTurnIndicatorsCommand;
+  Subscriber<MrmState>                        getMrmState;
+#if __has_include(<autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>)
+  Subscriber<OperationModeState>              getOperationModeState;
+#endif
+  Subscriber<priority::PathWithLaneId>        getPathWithLaneId;
+#if __has_include(<autoware_adapi_v1_msgs/msg/route_state.hpp>)
+  Subscriber<RouteState>                      getRouteState;
+#endif
+  Subscriber<Trajectory>                      getTrajectory;
+  Subscriber<TurnIndicatorsCommand>           getTurnIndicatorsCommand;
 
   Service<ClearRoute>             requestClearRoute;
   Service<CooperateCommands>      requestCooperateCommands;
@@ -119,16 +127,15 @@ struct FieldOperatorApplication : public rclcpp::Node
 
   template <typename Thunk = void (*)()>
   auto waitForAutowareStateToBe(
-    const std::string & state, Thunk thunk = [] {})
+    const LegacyAutowareState & state, Thunk thunk = [] {})
   {
     thunk();
 
-    while (not finalized.load() and autoware_state != state) {
+    while (not finalized.load() and getLegacyAutowareState().value != state.value) {
       if (time_limit <= std::chrono::steady_clock::now()) {
         throw common::AutowareError(
           "Simulator waited for the Autoware state to transition to ", state,
-          ", but time is up. The current Autoware state is ",
-          (autoware_state.empty() ? "not published yet" : autoware_state));
+          ", but time is up. The current Autoware state is ", getLegacyAutowareState());
       } else {
         thunk();
         rclcpp::GenericRate<std::chrono::steady_clock>(std::chrono::seconds(1)).sleep();
@@ -154,6 +161,8 @@ struct FieldOperatorApplication : public rclcpp::Node
 
   auto clearRoute() -> void;
 
+  auto getLegacyAutowareState() const -> LegacyAutowareState;
+
   auto getWaypoints() const -> traffic_simulator_msgs::msg::WaypointsArray;
 
   auto requestAutoModeForCooperation(const std::string &, bool) -> void;

external/concealer/include/concealer/legacy_autoware_state.hpp

@@ -0,0 +1,177 @@
+// Copyright 2015 TIER IV, Inc. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef CONCEALER__LEGACY_AUTOWARE_STATE_HPP_
+#define CONCEALER__LEGACY_AUTOWARE_STATE_HPP_
+
+#if __has_include(<autoware_system_msgs/msg/autoware_state.hpp>)
+#include <autoware_system_msgs/msg/autoware_state.hpp>
+#endif
+
+#if __has_include(<autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>)
+#include <autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>
+#endif
+
+#if __has_include(<autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>)
+#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
+#endif
+
+#if __has_include(<autoware_adapi_v1_msgs/msg/route_state.hpp>)
+#include <autoware_adapi_v1_msgs/msg/route_state.hpp>
+#endif
+
+namespace concealer
+{
+struct LegacyAutowareState
+{
+  enum value_type {
+    undefined,
+    initializing,
+    waiting_for_route,
+    planning,
+    waiting_for_engage,
+    driving,
+    arrived_goal,
+    finalizing,
+  };
+
+  value_type value;
+
+  constexpr LegacyAutowareState(value_type value) : value(value) {}
+
+#if __has_include(<autoware_system_msgs/msg/autoware_state.hpp>)
+  explicit LegacyAutowareState(const autoware_system_msgs::msg::AutowareState & autoware_state)
+  : value([&]() {
+      switch (autoware_state.state) {
+        case autoware_system_msgs::msg::AutowareState::INITIALIZING:
+          return initializing;
+        case autoware_system_msgs::msg::AutowareState::WAITING_FOR_ROUTE:
+          return waiting_for_route;
+        case autoware_system_msgs::msg::AutowareState::PLANNING:
+          return planning;
+        case autoware_system_msgs::msg::AutowareState::WAITING_FOR_ENGAGE:
+          return waiting_for_engage;
+        case autoware_system_msgs::msg::AutowareState::DRIVING:
+          return driving;
+        case autoware_system_msgs::msg::AutowareState::ARRIVED_GOAL:
+          return arrived_goal;
+        case autoware_system_msgs::msg::AutowareState::FINALIZING:
+          return finalizing;
+      }
+    }())
+  {
+  }
+#endif
+
+#if __has_include(<autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>) and \
+    __has_include(<autoware_adapi_v1_msgs/msg/route_state.hpp>) and \
+    __has_include(<autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>)
+  explicit LegacyAutowareState(
+    const autoware_adapi_v1_msgs::msg::LocalizationInitializationState & localization_state,
+    const autoware_adapi_v1_msgs::msg::RouteState & route_state,
+    const autoware_adapi_v1_msgs::msg::OperationModeState & operation_mode_state,
+    const rclcpp::Time & now)
+  : value([&]() {
+      /*
+         See
+         - https://github.com/autowarefoundation/autoware.universe/blob/e60daf7d1c85208eaac083b90c181e224c2ac513/system/autoware_default_adapi/document/autoware-state.md
+         - https://github.com/autowarefoundation/autoware_universe/blob/e60daf7d1c85208eaac083b90c181e224c2ac513/system/autoware_default_adapi/src/compatibility/autoware_state.cpp#L80-L141
+      */
+      switch (localization_state.state) {
+        case autoware_adapi_v1_msgs::msg::LocalizationInitializationState::UNKNOWN:
+        case autoware_adapi_v1_msgs::msg::LocalizationInitializationState::UNINITIALIZED:
+        case autoware_adapi_v1_msgs::msg::LocalizationInitializationState::INITIALIZING:
+          return initializing;
+
+        case autoware_adapi_v1_msgs::msg::LocalizationInitializationState::INITIALIZED:
+          switch (route_state.state) {
+            case autoware_adapi_v1_msgs::msg::RouteState::UNKNOWN:
+              return initializing;
+
+            case autoware_adapi_v1_msgs::msg::RouteState::ARRIVED:
+              if (const auto duration = now - rclcpp::Time(route_state.stamp);
+                  duration.seconds() < 2.0) {
+                return arrived_goal;
+              }
+              [[fallthrough]];
+
+            case autoware_adapi_v1_msgs::msg::RouteState::UNSET:
+              return waiting_for_route;
+
+            case autoware_adapi_v1_msgs::msg::RouteState::SET:
+            case autoware_adapi_v1_msgs::msg::RouteState::CHANGING:
+              switch (operation_mode_state.mode) {
+                case autoware_adapi_v1_msgs::msg::OperationModeState::UNKNOWN:
+                  return initializing;
+
+                case autoware_adapi_v1_msgs::msg::OperationModeState::AUTONOMOUS:
+                case autoware_adapi_v1_msgs::msg::OperationModeState::LOCAL:
+                case autoware_adapi_v1_msgs::msg::OperationModeState::REMOTE:
+                  if (operation_mode_state.is_autoware_control_enabled) {
+                    return driving;
+                  }
+                  [[fallthrough]];
+
+                case autoware_adapi_v1_msgs::msg::OperationModeState::STOP:
+                  return operation_mode_state.is_autonomous_mode_available ? waiting_for_engage
+                                                                           : planning;
+
+                default:
+                  return undefined;
+              }
+
+            default:
+              return undefined;
+          }
+
+        default:
+          return undefined;
+      }
+    }())
+  {
+  }
+#endif
+
+  constexpr operator const char *() const
+  {
+    switch (value) {
+      case initializing:
+        return "INITIALIZING";
+      case waiting_for_route:
+        return "WAITING_FOR_ROUTE";
+      case planning:
+        return "PLANNING";
+      case waiting_for_engage:
+        return "WAITING_FOR_ENGAGE";
+      case driving:
+        return "DRIVING";
+      case arrived_goal:
+        return "ARRIVED_GOAL";
+      case finalizing:
+        return "FINALIZING";
+      default:
+        return "";
+    }
+  }
+
+  operator std::string() const { return operator const char *(); }
+
+  friend auto operator<<(std::ostream & os, const LegacyAutowareState & state) -> std::ostream &
+  {
+    return os << static_cast<const char *>(state);
+  }
+};
+}  // namespace concealer
+
+#endif  // CONCEALER__LEGACY_AUTOWARE_STATE_HPP_