From 3108a010d6c7d09d051372f47355966d9e4e67ab Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 18 Mar 2022 10:35:37 +0900 Subject: [PATCH] feat(system_error_monitor): output emergency reason for debugging (#510) * feat(system_error_monitor): output emergency reason for debugging Signed-off-by: Takayuki Murooka * print all emergency reasons with 5 sec throttle using macros Signed-off-by: Takayuki Murooka * not use macros Signed-off-by: Takayuki Murooka * use const std::shared_ptr for argument of clock Signed-off-by: Takayuki Murooka * use enum class for DebugLevel Signed-off-by: Takayuki Murooka * resolved review's comment Signed-off-by: Takayuki Murooka --- .../src/system_error_monitor_core.cpp | 37 +++++++++++++++++++ 1 file changed, 37 insertions(+) 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 12fdafce41708..1a0b696bb36f4 100644 --- a/system/system_error_monitor/src/system_error_monitor_core.cpp +++ b/system/system_error_monitor/src/system_error_monitor_core.cpp @@ -28,6 +28,35 @@ namespace { +enum class DebugLevel { DEBUG, INFO, WARN, ERROR, FATAL }; + +template +void logThrottledNamed( + const std::string & logger_name, const rclcpp::Clock::SharedPtr clock, const double duration_ms, + const std::string & message) +{ + static std::unordered_map last_output_time; + if (last_output_time.count(logger_name) != 0) { + const auto time_from_last_output = clock->now() - last_output_time.at(logger_name); + if (time_from_last_output.seconds() * 1000.0 < duration_ms) { + return; + } + } + + last_output_time[logger_name] = clock->now(); + if constexpr (debug_level == DebugLevel::DEBUG) { + RCLCPP_DEBUG(rclcpp::get_logger(logger_name), message.c_str()); + } else if constexpr (debug_level == DebugLevel::INFO) { + RCLCPP_INFO(rclcpp::get_logger(logger_name), message.c_str()); + } else if constexpr (debug_level == DebugLevel::WARN) { + RCLCPP_WARN(rclcpp::get_logger(logger_name), message.c_str()); + } else if constexpr (debug_level == DebugLevel::ERROR) { + RCLCPP_ERROR(rclcpp::get_logger(logger_name), message.c_str()); + } else if constexpr (debug_level == DebugLevel::FATAL) { + RCLCPP_FATAL(rclcpp::get_logger(logger_name), message.c_str()); + } +} + std::vector split(const std::string & str, const char delim) { std::vector elems; @@ -111,9 +140,17 @@ diagnostic_msgs::msg::DiagnosticArray convertHazardStatusToDiagnosticArray( diag_array.status.push_back(decorateDiag(hazard_diag, "[Safe Fault]")); } for (const auto & hazard_diag : hazard_status.diag_latent_fault) { + const std::string logger_name = "system_error_monitor " + hazard_diag.name; + logThrottledNamed( + logger_name, clock, 5000, "[Latent Fault]: " + hazard_diag.message); + diag_array.status.push_back(decorateDiag(hazard_diag, "[Latent Fault]")); } for (const auto & hazard_diag : hazard_status.diag_single_point_fault) { + const std::string logger_name = "system_error_monitor " + hazard_diag.name; + logThrottledNamed( + logger_name, clock, 5000, "[Single Point Fault]: " + hazard_diag.message); + diag_array.status.push_back(decorateDiag(hazard_diag, "[Single Point Fault]")); }