From 5d085298999a7a98b1512ed23df57c0d35a65a77 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 23 Oct 2023 15:24:47 +0900 Subject: [PATCH] feat(system_error_monitor): add ignore hartbeat timeout in initializing state (backport #972) (#976) 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> (cherry picked from commit 3c22be06610071d2f7fccea82ba02aa5f1ea531a) Co-authored-by: Akihisa Nagata <54956813+asa-naki@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 d6cc771e5dbd2..ff54a502cb404 100644 --- a/system/system_error_monitor/src/system_error_monitor_core.cpp +++ b/system/system_error_monitor/src/system_error_monitor_core.cpp @@ -450,8 +450,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;