feat: use /autoware/state in concealer

tier4 · Sep 13, 2024 · bcc3353 · bcc3353
1 parent a6a0383
commit bcc3353
Show file tree

Hide file tree

Showing 4 changed files with 48 additions and 28 deletions.
diff --git a/docs/developer_guide/Communication.md b/docs/developer_guide/Communication.md
@@ -5,7 +5,7 @@
 
 | topic                                                                          | type                                                                                                                                                                               | note                                                                  |
 |--------------------------------------------------------------------------------|------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|-----------------------------------------------------------------------|
-| `/autoware/state`                                                              | [`autoware_auto_system_msgs/msg/AutowareState`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_system_msgs/msg/AutowareState.idl)                       | Used in UserDefinedValueCondition : `currentAutowareState`            |
+| `/autoware/state`                                                              | [`autoware_auto_system_msgs/msg/AutowareState`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_system_msgs/msg/AutowareState.idl) <br/> or [`autoware_system_msgs/msg/AutowareState`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_system_msgs/msg/AutowareState.msg)             | Used in UserDefinedValueCondition : `currentAutowareState`            |
 | `/control/command/control_cmd`                                                 | [`autoware_auto_control_msgs/msg/AckermannControlCommand`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_control_msgs/msg/AckermannControlCommand.idl) |                                                                       |
 | `/control/command/gear_cmd`                                                    | [`autoware_auto_vehicle_msgs/msg/GearCommand`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/GearCommand.idl)                         |                                                                       |
 | `/control/command/turn_indicators_cmd`                                         | [`autoware_auto_vehicle_msgs/msg/TurnIndicatorsCommand`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/TurnIndicatorsCommand.idl)     | Used in UserDefinedValueCondition : `currentTurnIndicatorsState`      |

diff --git a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp
@@ -19,6 +19,14 @@
 #include <autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>
 #endif
 
+#if __has_include(<autoware_system_msgs/msg/autoware_state.hpp>)
+#include <autoware_system_msgs/msg/autoware_state.hpp>
+#endif
+
+#if __has_include(<autoware_auto_system_msgs/msg/autoware_state.hpp>)
+#include <autoware_auto_system_msgs/msg/autoware_state.hpp>
+#endif
+
 #include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
 #include <autoware_adapi_v1_msgs/srv/clear_route.hpp>
 #include <autoware_adapi_v1_msgs/srv/initialize_localization.hpp>
@@ -42,7 +50,6 @@
 #include <tier4_rtc_msgs/msg/cooperate_status_array.hpp>
 #include <tier4_rtc_msgs/srv/auto_mode_with_module.hpp>
 #include <tier4_rtc_msgs/srv/cooperate_commands.hpp>
-#include <tier4_system_msgs/msg/autoware_state.hpp>
 
 namespace concealer
 {
@@ -55,7 +62,12 @@ class FieldOperatorApplicationFor<AutowareUniverse>
 
   // clang-format off
   SubscriberWrapper<autoware_auto_control_msgs::msg::AckermannControlCommand>     getAckermannControlCommand;
-  SubscriberWrapper<tier4_system_msgs::msg::AutowareState, ThreadSafety::safe>    getAutowareState;
+#if __has_include(<autoware_system_msgs/msg/autoware_state.hpp>)
+  SubscriberWrapper<autoware_system_msgs::msg::AutowareState, ThreadSafety::safe>    getAutowareState;
+#endif
+#if __has_include(<autoware_auto_system_msgs/msg/autoware_state.hpp>)
+  SubscriberWrapper<autoware_auto_system_msgs::msg::AutowareState, ThreadSafety::safe> getAutowareAutoState;
+#endif
   SubscriberWrapper<tier4_rtc_msgs::msg::CooperateStatusArray>                    getCooperateStatusArray;
   SubscriberWrapper<tier4_external_api_msgs::msg::Emergency>                      getEmergencyState;
 #if __has_include(<autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>)
@@ -76,6 +88,8 @@ class FieldOperatorApplicationFor<AutowareUniverse>
 
   tier4_rtc_msgs::msg::CooperateStatusArray latest_cooperate_status_array;
 
+  std::string autoware_state;
+
   std::string minimum_risk_maneuver_state;
 
   std::string minimum_risk_maneuver_behavior;
@@ -84,12 +98,8 @@ class FieldOperatorApplicationFor<AutowareUniverse>
 
   auto receiveEmergencyState(const tier4_external_api_msgs::msg::Emergency & msg) -> void;
 
-#define DEFINE_STATE_PREDICATE(NAME, VALUE)                  \
-  auto is##NAME() const noexcept                             \
-  {                                                          \
-    using tier4_system_msgs::msg::AutowareState;             \
-    return getAutowareState().state == AutowareState::VALUE; \
-  }                                                          \
+#define DEFINE_STATE_PREDICATE(NAME, VALUE)                           \
+  auto is##NAME() const noexcept { return autoware_state == #VALUE; } \
   static_assert(true, "")
 
   DEFINE_STATE_PREDICATE(Initializing, INITIALIZING_VEHICLE);
@@ -98,12 +108,33 @@ class FieldOperatorApplicationFor<AutowareUniverse>
   DEFINE_STATE_PREDICATE(WaitingForEngage, WAITING_FOR_ENGAGE);
   DEFINE_STATE_PREDICATE(Driving, DRIVING);
   DEFINE_STATE_PREDICATE(ArrivedGoal, ARRIVAL_GOAL);
-  DEFINE_STATE_PREDICATE(Emergency, EMERGENCY);
   DEFINE_STATE_PREDICATE(Finalizing, FINALIZING);
 
 #undef DEFINE_STATE_PREDICATE
 
 protected:
+  template <typename T>
+  auto getAutowareStateString(std::uint8_t state) const -> std::string
+  {
+#define CASE(IDENTIFIER) \
+  case T::IDENTIFIER:    \
+    return #IDENTIFIER
+
+    switch (state) {
+      CASE(INITIALIZING);
+      CASE(WAITING_FOR_ROUTE);
+      CASE(PLANNING);
+      CASE(WAITING_FOR_ENGAGE);
+      CASE(DRIVING);
+      CASE(ARRIVED_GOAL);
+      CASE(FINALIZING);
+
+      default:
+        return "";
+    }
+
+#undef CASE
+  }
   auto sendSIGINT() -> void override;
 
 public:
@@ -115,7 +146,12 @@ class FieldOperatorApplicationFor<AutowareUniverse>
   : FieldOperatorApplication(std::forward<decltype(xs)>(xs)...),
     // clang-format off
     getAckermannControlCommand("/control/command/control_cmd", rclcpp::QoS(1), *this),
-    getAutowareState("/api/iv_msgs/autoware/state", rclcpp::QoS(1), *this),
+#if __has_include(<autoware_system_msgs/msg/autoware_state.hpp>)
+    getAutowareState("/autoware/state", rclcpp::QoS(1), *this, [this](const auto & v) { autoware_state = getAutowareStateString<autoware_system_msgs::msg::AutowareState>(v.state); }),
+#endif
+#if __has_include(<autoware_auto_system_msgs/msg/autoware_state.hpp>)
+    getAutowareAutoState("/autoware/state", rclcpp::QoS(1), *this, [this](const auto & v) { autoware_state = getAutowareStateString<autoware_auto_system_msgs::msg::AutowareState>(v.state); }),
+#endif
     getCooperateStatusArray("/api/external/get/rtc_status", rclcpp::QoS(1), *this, [this](const auto & v) { latest_cooperate_status_array = v; }),
     getEmergencyState("/api/external/get/emergency", rclcpp::QoS(1), *this, [this](const auto & v) { receiveEmergencyState(v); }),
 #if __has_include(<autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>)

diff --git a/external/concealer/package.xml b/external/concealer/package.xml
@@ -22,7 +22,6 @@
   <depend>tier4_external_api_msgs</depend>
   <depend>tier4_planning_msgs</depend>
   <depend>tier4_rtc_msgs</depend>
-  <depend>tier4_system_msgs</depend>
 
   <!-- Common -->
   <depend>ament_index_cpp</depend>

diff --git a/external/concealer/src/field_operator_application_for_autoware_universe.cpp b/external/concealer/src/field_operator_application_for_autoware_universe.cpp
@@ -409,22 +409,7 @@ auto FieldOperatorApplicationFor<AutowareUniverse>::restrictTargetSpeed(double v
 
 auto FieldOperatorApplicationFor<AutowareUniverse>::getAutowareStateName() const -> std::string
 {
-#define IF(IDENTIFIER, RETURN)                                                         \
-  if (getAutowareState().state == tier4_system_msgs::msg::AutowareState::IDENTIFIER) { \
-    return #RETURN;                                                                    \
-  }
-
-  IF(INITIALIZING_VEHICLE, INITIALIZING)
-  IF(WAITING_FOR_ROUTE, WAITING_FOR_ROUTE)
-  IF(PLANNING, PLANNING)
-  IF(WAITING_FOR_ENGAGE, WAITING_FOR_ENGAGE)
-  IF(DRIVING, DRIVING)
-  IF(ARRIVAL_GOAL, ARRIVED_GOAL)
-  IF(EMERGENCY, EMERGENCY)
-  IF(FINALIZING, FINALIZING)
-
-  return "";
-#undef IF
+  return autoware_state;
 }
 
 auto FieldOperatorApplicationFor<AutowareUniverse>::getEmergencyStateName() const -> std::string