From a62957284edc929ba29afdeb75153a36cd6ca815 Mon Sep 17 00:00:00 2001 From: Akihisa Nagata <54956813+asa-naki@users.noreply.github.com> Date: Mon, 23 Oct 2023 15:17:40 +0900 Subject: [PATCH] feat(system_error_monitor): add ignore hartbeat timeout in initializing state (#972) * add ignore hartbeat timeout in initializing state Signed-off-by: asa-naki * fix typo * Update comment * ci(pre-commit): autofix * fix typo * ci(pre-commit): autofix * update comment * ci(pre-commit): autofix --------- Signed-off-by: asa-naki Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/system_error_monitor_core.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) 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;