feat(system_error_monitor): add ignore_until_waiting_for_route module (…

…#888) * add ignore_module Signed-off-by: asa-naki <akihisa.nagata@tier4.jp> * add description Signed-off-by: asa-naki <akihisa.nagata@tier4.jp> * ci(pre-commit): autofix * change name ignore_until_waiting_for_route Signed-off-by: asa-naki <akihisa.nagata@tier4.jp> * update description Signed-off-by: asa-naki <akihisa.nagata@tier4.jp> * rename function name and delete planning state Signed-off-by: asa-naki <akihisa.nagata@tier4.jp> * update description Signed-off-by: asa-naki <akihisa.nagata@tier4.jp> * Update --------- Signed-off-by: asa-naki <akihisa.nagata@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp>
tier4 · Aug 14, 2024 · 0b7fae2 · 0b7fae2
1 parent 7f7616a
commit 0b7fae2
Show file tree

Hide file tree

Showing 4 changed files with 40 additions and 7 deletions.
diff --git a/system/system_error_monitor/config/system_error_monitor.param.yaml b/system/system_error_monitor/config/system_error_monitor.param.yaml
@@ -4,14 +4,15 @@
 #   lf_at: diag level where it becomes Latent Fault
 #   spf_at: diag level where it becomes Single Point Fault
 #   auto_recovery: Determines whether the system will automatically recover when it recovers from an error.
-#
+#   ignore_until_waiting_for_route: Determines whether the system will ignore the module until waiting for route.
 # Note:
 # empty-value for sf_at, lf_at and spf_at is "none"
 # default values are:
 #   sf_at: "none"
 #   lf_at: "warn"
 #   spf_at: "error"
 #   auto_recovery: "true"
+#   ignore_until_waiting_for_route: "false"
 ---
 /**:
   ros__parameters:

diff --git a/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml b/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml
@@ -4,23 +4,24 @@
 #   lf_at: diag level where it becomes Latent Fault
 #   spf_at: diag level where it becomes Single Point Fault
 #   auto_recovery: Determines whether the system will automatically recover when it recovers from an error.
-#
+#   ignore_until_waiting_for_route: Determines whether the system will ignore the module until waiting for route.
 # Note:
 # empty-value for sf_at, lf_at and spf_at is "none"
 # default values are:
 #   sf_at: "none"
 #   lf_at: "warn"
 #   spf_at: "error"
 #   auto_recovery: "true"
+#   ignore_until_waiting_for_route: "false"
 ---
 /**:
   ros__parameters:
     required_modules:
       autonomous_driving:
-        /autoware/control/autonomous_driving/node_alive_monitoring: default
+        /autoware/control/autonomous_driving/node_alive_monitoring: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false",ignore_until_waiting_for_route: "true"}
         /autoware/control/autonomous_driving/performance_monitoring/lane_departure: default
         /autoware/control/control_command_gate/node_alive_monitoring: default
-        /autoware/control/autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false"}
+        /autoware/control/autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false",ignore_until_waiting_for_route: "false"}
 
         /autoware/localization/node_alive_monitoring: default
         # /autoware/localization/performance_monitoring/matching_score: { sf_at: "warn", lf_at: "none", spf_at: "none" }
@@ -30,7 +31,7 @@
 
         /autoware/perception/node_alive_monitoring: default
 
-        /autoware/planning/node_alive_monitoring: default
+        /autoware/planning/node_alive_monitoring: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false",ignore_until_waiting_for_route: "true"}
         /autoware/planning/performance_monitoring/trajectory_validation: default
 
         /autoware/sensing/node_alive_monitoring: default
@@ -53,7 +54,7 @@
 
       external_control:
         /autoware/control/control_command_gate/node_alive_monitoring: default
-        /autoware/control/autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false"}
+        /autoware/control/autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false",ignore_until_waiting_for_route: "false"}
         /autoware/control/external_control/external_command_selector/node_alive_monitoring: default
 
         /autoware/system/node_alive_monitoring: default

diff --git a/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp b/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp
@@ -48,6 +48,7 @@ struct DiagConfig
   std::string lf_at;
   std::string spf_at;
   bool auto_recovery;
+  bool ignore_until_waiting_for_route;
 };
 
 using RequiredModules = std::vector<DiagConfig>;

diff --git a/system/system_error_monitor/src/system_error_monitor_core.cpp b/system/system_error_monitor/src/system_error_monitor_core.cpp
@@ -208,6 +208,22 @@ int isInNoFaultCondition(
 
   return false;
 }
+bool ignoreUntilWaitingForRoute(
+  const autoware_auto_system_msgs::msg::AutowareState & autoware_state,
+  const DiagConfig & required_module)
+{
+  using autoware_auto_system_msgs::msg::AutowareState;
+  using tier4_control_msgs::msg::GateMode;
+
+  const auto is_in_autonomous_ignore_state =
+    (autoware_state.state == AutowareState::INITIALIZING) ||
+    (autoware_state.state == AutowareState::WAITING_FOR_ROUTE);
+
+  if (is_in_autonomous_ignore_state && required_module.ignore_until_waiting_for_route) {
+    return true;
+  }
+  return false;
+}
 }  // namespace
 
 AutowareErrorMonitor::AutowareErrorMonitor()
@@ -322,11 +338,22 @@ void AutowareErrorMonitor::loadRequiredModules(const std::string & key)
     std::string auto_recovery_approval_str;
     this->get_parameter_or(auto_recovery_key, auto_recovery_approval_str, std::string("true"));
 
+    const auto ignore_until_waiting_for_route_key =
+      module_name_with_prefix + std::string(".ignore_until_waiting_for_route");
+    std::string ignore_until_waiting_for_route_str;
+    this->get_parameter_or(
+      ignore_until_waiting_for_route_key, ignore_until_waiting_for_route_str, std::string("false"));
+
     // Convert auto_recovery_approval_str to bool
     bool auto_recovery_approval{};
     std::istringstream(auto_recovery_approval_str) >> std::boolalpha >> auto_recovery_approval;
 
-    required_modules.push_back({param_module, sf_at, lf_at, spf_at, auto_recovery_approval});
+    bool ignore_until_waiting_for_route{};
+    std::istringstream(ignore_until_waiting_for_route_str) >> std::boolalpha >>
+      ignore_until_waiting_for_route;
+
+    required_modules.push_back(
+      {param_module, sf_at, lf_at, spf_at, auto_recovery_approval, ignore_until_waiting_for_route});
   }
 
   required_modules_map_.insert(std::make_pair(key, required_modules));
@@ -493,6 +520,9 @@ uint8_t AutowareErrorMonitor::getHazardLevel(
   using autoware_auto_system_msgs::msg::HazardStatus;
 
   if (isOverLevel(diag_level, required_module.spf_at)) {
+    if (ignoreUntilWaitingForRoute(*autoware_state_, required_module)) {
+      return HazardStatus::NO_FAULT;
+    }
     return HazardStatus::SINGLE_POINT_FAULT;
   }
   if (isOverLevel(diag_level, required_module.lf_at)) {