diff --git a/system/system_error_monitor/config/system_error_monitor.param.yaml b/system/system_error_monitor/config/system_error_monitor.param.yaml index 722eb943bc7d1..fde96aeeebfb2 100644 --- a/system/system_error_monitor/config/system_error_monitor.param.yaml +++ b/system/system_error_monitor/config/system_error_monitor.param.yaml @@ -4,7 +4,7 @@ # 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: @@ -12,6 +12,7 @@ # 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 index db71ffe5c0c90..cd968ce615e0b 100644 --- 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,7 +4,7 @@ # 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: @@ -12,15 +12,16 @@ # 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 index a77462f9c8a30..f033618b2305d 100644 --- 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; diff --git a/system/system_error_monitor/src/system_error_monitor_core.cpp b/system/system_error_monitor/src/system_error_monitor_core.cpp index 8db37d6d62488..1a90a55c73187 100644 --- 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)) {