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 1a90a55c73187..434009eb69d30 100644 --- a/system/system_error_monitor/src/system_error_monitor_core.cpp +++ b/system/system_error_monitor/src/system_error_monitor_core.cpp @@ -478,8 +478,16 @@ void AutowareErrorMonitor::onTimer() } return; } - + // Heartbeat in AutowareState,diag_array times out during AutowareState INITIALIZING due to high + // processing load,add a disable function to avoid Emergencies in isDataHeartbeatTimeout() in + // AutowareState INITIALIZING. if (isDataHeartbeatTimeout()) { + if ((autoware_state_->state == autoware_auto_system_msgs::msg::AutowareState::INITIALIZING)) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), + "ignore heartbeat timeout in initializing state"); + return; + } updateTimeoutHazardStatus(); publishHazardStatus(hazard_status_); return;